search

peci1 / robot_body_filter

Filters the robot's body out of point clouds and laser scans.
BSD 3-Clause "New" or "Revised" License
82 stars 21 forks source link

Conflict with non-released updates in geometric_shapes@noetic-devel #21

Closed fmessmer closed 2 years ago

fmessmer commented 2 years ago

I'm trying to use this filter on our robot to filter robot_body parts from a PointCloud

While I got it to work on real HW, I experience issues when using the same filter pipeline/config with sensor data simulated in gazebo - see "Details" below Has anybody used this filter together with gazebo, too?

First I thought the simulated PointCloud message might be the problem or it might be related to filtering meshes. But I get a filtered output PointCloud from the robot_body_filter before the SegFault - thus I dont think it's the data And I get the same SegFault when only filtering SolidPrimitives (i.e. Boxes)

So eventually, I think this is related to maybe tf being published at a slower rate in gazebo (real-time factor!)? Or maybe the robot_body_filter node not considering sim_time: true or something

I also don't understand the log messages...

[ INFO] [1652874275.464236263, 8.168000000]: /robot_body_filter(TFFramesWatchdog::searchForReachableFrames): TFFramesWatchdog (torso_cam3d_bottom_depth_optical_frame): Frame test_link became reachable at 8.168000000

actually sounds like something good happening....but it SegFaults right after that!

Are there any parameters that I could play with to relax potential tf timeouts? Any additional hints to debug this SegFault? Thanks!

``` started roslaunch server http://alpamayo:38357/ SUMMARY ======== PARAMETERS * /robot_body_filter/cloud_filter_chain: [{'name': 'robot_... * /rosdistro: noetic * /rosversion: 1.15.14 NODES / robot_body_filter (sensor_filters/pointcloud2_filter_chain) ROS_MASTER_URI=http://localhost:11311 process[robot_body_filter-1]: started with pid [1104702] GNU gdb (Ubuntu 9.2-0ubuntu1~20.04.1) 9.2 Copyright (C) 2020 Free Software Foundation, Inc. License GPLv3+: GNU GPL version 3 or later This is free software: you are free to change and redistribute it. There is NO WARRANTY, to the extent permitted by law. Type "show copying" and "show warranty" for details. This GDB was configured as "x86_64-linux-gnu". Type "show configuration" for configuration details. For bug reporting instructions, please see: . Find the GDB manual and other documentation resources online at: . For help, type "help". Type "apropos word" to search for commands related to "word"... Reading symbols from /opt/ros/noetic/lib/sensor_filters/pointcloud2_filter_chain... --Type for more, q to quit, c to continue without paging--c (No debugging symbols found in /opt/ros/noetic/lib/sensor_filters/pointcloud2_filter_chain) Starting program: /opt/ros/noetic/lib/sensor_filters/pointcloud2_filter_chain \~input:=/torso_cam3d_bottom/depth/points \~output:=/torso_cam3d_bottom/depth/points_filtered __name:=robot_body_filter __log:=/home/fxm/.ros/log/ae38a35c-d69f-11ec-a079-e98e37d8e7f7/robot_body_filter-1.log [Thread debugging using libthread_db enabled] Using host libthread_db library "/lib/x86_64-linux-gnu/libthread_db.so.1". [New Thread 0x7ffff42ac700 (LWP 1104822)] [New Thread 0x7ffff3aab700 (LWP 1104823)] [New Thread 0x7ffff32aa700 (LWP 1104824)] [New Thread 0x7ffff2aa9700 (LWP 1104825)] [ INFO] [1652874266.266266917, 7.160000000]: /robot_body_filter(FilterBase::getParamVerbose): robot_body_filter_containment: Found parameter: sensor/point_by_point, value: False [ INFO] [1652874266.268948633, 7.160000000]: /robot_body_filter(FilterBase::getParamVerbose): robot_body_filter_containment: Found parameter: transforms/buffer_length, value: 60.000000 s [New Thread 0x7fffe197e700 (LWP 1104873)] [ INFO] [1652874266.302806741, 7.163000000]: /robot_body_filter(FilterBase::getParamVerbose): robot_body_filter_containment: Found parameter: frames/fixed, value: base_link [ INFO] [1652874266.303274481, 7.163000000]: /robot_body_filter(FilterBase::getParamVerbose): robot_body_filter_containment: Found parameter: frames/sensor, value: torso_cam3d_bottom_depth_optical_frame [ INFO] [1652874266.306058089, 7.163000000]: /robot_body_filter(FilterBase::getParamVerbose): robot_body_filter_containment: Found parameter: frames/filtering, value: torso_cam3d_bottom_depth_optical_frame [ INFO] [1652874266.306634509, 7.163000000]: /robot_body_filter(FilterBase::getParamVerbose): robot_body_filter_containment: Found parameter: sensor/min_distance, value: 0.000000 m [ INFO] [1652874266.307093232, 7.163000000]: /robot_body_filter(FilterBase::getParamVerbose): robot_body_filter_containment: Found parameter: sensor/max_distance, value: 2.000000 m [ INFO] [1652874266.307797563, 7.163000000]: /robot_body_filter(FilterBase::getParamVerbose): robot_body_filter_containment: Found parameter: body_model/robot_description_param, value: /robot_description [ INFO] [1652874266.308176273, 7.163000000]: /robot_body_filter(FilterBase::getParamVerbose): robot_body_filter_containment: Found parameter: filter/keep_clouds_organized, value: True [ INFO] [1652874266.309143428, 7.163000000]: /robot_body_filter(FilterBase::getParamVerbose): robot_body_filter_containment: Found parameter: filter/model_pose_update_interval, value: 0.000000 s [ INFO] [1652874266.309552967, 7.163000000]: /robot_body_filter(FilterBase::getParamVerbose): robot_body_filter_containment: Found parameter: filter/do_clipping, value: True [ INFO] [1652874266.311286367, 7.164000000]: /robot_body_filter(FilterBase::getParamVerbose): robot_body_filter_containment: Found parameter: filter/do_contains_test, value: True [ INFO] [1652874266.312016289, 7.164000000]: /robot_body_filter(FilterBase::getParamVerbose): robot_body_filter_containment: Found parameter: filter/do_shadow_test, value: False [ INFO] [1652874266.312961398, 7.164000000]: /robot_body_filter(FilterBase::getParamVerbose): robot_body_filter_containment: Found parameter: filter/max_shadow_distance, value: 2.000000 m [ INFO] [1652874266.313842679, 7.164000000]: /robot_body_filter(FilterBase::getParamVerbose): robot_body_filter_containment: Found parameter: transforms/timeout/reachable, value: 0.100000 s [ INFO] [1652874266.314885319, 7.164000000]: /robot_body_filter(FilterBase::getParamVerbose): robot_body_filter_containment: Found parameter: transforms/timeout/unreachable, value: 0.200000 s [ INFO] [1652874266.315764613, 7.164000000]: /robot_body_filter(FilterBase::getParamVerbose): robot_body_filter_containment: Found parameter: transforms/require_all_reachable, value: False [ INFO] [1652874266.316376708, 7.164000000]: /robot_body_filter(FilterBase::getParamVerbose): robot_body_filter_containment: Parameter bounding_sphere/publish_cut_out_pointcloud not defined, assigning default: False [ INFO] [1652874266.316769610, 7.164000000]: /robot_body_filter(FilterBase::getParamVerbose): robot_body_filter_containment: Found parameter: bounding_box/publish_cut_out_pointcloud, value: False [ INFO] [1652874266.316943156, 7.164000000]: /robot_body_filter(FilterBase::getParamVerbose): robot_body_filter_containment: Parameter oriented_bounding_box/publish_cut_out_pointcloud not defined, assigning default: False [ INFO] [1652874266.316974232, 7.164000000]: /robot_body_filter(FilterBase::getParamVerbose): robot_body_filter_containment: Parameter local_bounding_box/publish_cut_out_pointcloud not defined, assigning default: False [ INFO] [1652874266.317001224, 7.164000000]: /robot_body_filter(FilterBase::getParamVerbose): robot_body_filter_containment: Parameter bounding_sphere/compute not defined, assigning default: False [ INFO] [1652874266.317031035, 7.164000000]: /robot_body_filter(FilterBase::getParamVerbose): robot_body_filter_containment: Found parameter: bounding_box/compute, value: False [ INFO] [1652874266.317055972, 7.164000000]: /robot_body_filter(FilterBase::getParamVerbose): robot_body_filter_containment: Parameter oriented_bounding_box/compute not defined, assigning default: False [ INFO] [1652874266.317895075, 7.164000000]: /robot_body_filter(FilterBase::getParamVerbose): robot_body_filter_containment: Parameter local_bounding_box/compute not defined, assigning default: False [ INFO] [1652874266.318058339, 7.164000000]: /robot_body_filter(FilterBase::getParamVerbose): robot_body_filter_containment: Parameter bounding_sphere/debug not defined, assigning default: False [ INFO] [1652874266.319147916, 7.164000000]: /robot_body_filter(FilterBase::getParamVerbose): robot_body_filter_containment: Parameter bounding_box/debug not defined, assigning default: False [ INFO] [1652874266.319917465, 7.164000000]: /robot_body_filter(FilterBase::getParamVerbose): robot_body_filter_containment: Parameter oriented_bounding_box/debug not defined, assigning default: False [ INFO] [1652874266.321123209, 7.164000000]: /robot_body_filter(FilterBase::getParamVerbose): robot_body_filter_containment: Parameter local_bounding_box/debug not defined, assigning default: False [ INFO] [1652874266.322318696, 7.164000000]: /robot_body_filter(FilterBase::getParamVerbose): robot_body_filter_containment: Parameter bounding_sphere/marker not defined, assigning default: False [ INFO] [1652874266.323932030, 7.164000000]: /robot_body_filter(FilterBase::getParamVerbose): robot_body_filter_containment: Parameter bounding_box/marker not defined, assigning default: False [ INFO] [1652874266.324288608, 7.164000000]: /robot_body_filter(FilterBase::getParamVerbose): robot_body_filter_containment: Parameter oriented_bounding_box/marker not defined, assigning default: False [ INFO] [1652874266.324397226, 7.164000000]: /robot_body_filter(FilterBase::getParamVerbose): robot_body_filter_containment: Parameter local_bounding_box/marker not defined, assigning default: False [ INFO] [1652874266.324551261, 7.164000000]: /robot_body_filter(FilterBase::getParamVerbose): robot_body_filter_containment: Parameter local_bounding_box/frame_id not defined, assigning default: base_link [ INFO] [1652874266.324662172, 7.164000000]: /robot_body_filter(FilterBase::getParamVerbose): robot_body_filter_containment: Found parameter: debug/pcl/inside, value: False [ INFO] [1652874266.324806651, 7.164000000]: /robot_body_filter(FilterBase::getParamVerbose): robot_body_filter_containment: Found parameter: debug/pcl/clip, value: False [ INFO] [1652874266.325027957, 7.164000000]: /robot_body_filter(FilterBase::getParamVerbose): robot_body_filter_containment: Found parameter: debug/pcl/shadow, value: False [ INFO] [1652874266.325918933, 7.164000000]: /robot_body_filter(FilterBase::getParamVerbose): robot_body_filter_containment: Found parameter: debug/marker/contains, value: False [ INFO] [1652874266.326025054, 7.164000000]: /robot_body_filter(FilterBase::getParamVerbose): robot_body_filter_containment: Found parameter: debug/marker/shadow, value: False [ INFO] [1652874266.326129102, 7.164000000]: /robot_body_filter(FilterBase::getParamVerbose): robot_body_filter_containment: Parameter debug/marker/bounding_sphere not defined, assigning default: False [ INFO] [1652874266.327593080, 7.164000000]: /robot_body_filter(FilterBase::getParamVerbose): robot_body_filter_containment: Parameter debug/marker/bounding_box not defined, assigning default: False [ INFO] [1652874266.328036678, 7.164000000]: /robot_body_filter(FilterBase::getParamVerbose): robot_body_filter_containment: Found parameter: body_model/inflation/padding, value: 0.030000 m [ INFO] [1652874266.328162408, 7.164000000]: /robot_body_filter(FilterBase::getParamVerbose): robot_body_filter_containment: Found parameter: body_model/inflation/scale, value: 1.000000 [ INFO] [1652874266.328274965, 7.164000000]: /robot_body_filter(FilterBase::getParamVerbose): robot_body_filter_containment: Parameter body_model/inflation/contains_test/padding not defined, assigning default: 0.030000 m [ INFO] [1652874266.328490782, 7.164000000]: /robot_body_filter(FilterBase::getParamVerbose): robot_body_filter_containment: Parameter body_model/inflation/contains_test/scale not defined, assigning default: 1.000000 [ INFO] [1652874266.329814176, 7.164000000]: /robot_body_filter(FilterBase::getParamVerbose): robot_body_filter_containment: Parameter body_model/inflation/shadow_test/padding not defined, assigning default: 0.030000 m [ INFO] [1652874266.330097885, 7.164000000]: /robot_body_filter(FilterBase::getParamVerbose): robot_body_filter_containment: Parameter body_model/inflation/shadow_test/scale not defined, assigning default: 1.000000 [ INFO] [1652874266.330207487, 7.164000000]: /robot_body_filter(FilterBase::getParamVerbose): robot_body_filter_containment: Parameter body_model/inflation/bounding_sphere/padding not defined, assigning default: 0.030000 m [ INFO] [1652874266.330403855, 7.164000000]: /robot_body_filter(FilterBase::getParamVerbose): robot_body_filter_containment: Parameter body_model/inflation/bounding_sphere/scale not defined, assigning default: 1.000000 [ INFO] [1652874266.330581312, 7.164000000]: /robot_body_filter(FilterBase::getParamVerbose): robot_body_filter_containment: Parameter body_model/inflation/bounding_box/padding not defined, assigning default: 0.030000 m [ INFO] [1652874266.331063924, 7.164000000]: /robot_body_filter(FilterBase::getParamVerbose): robot_body_filter_containment: Parameter body_model/inflation/bounding_box/scale not defined, assigning default: 1.000000 [ INFO] [1652874266.331248820, 7.164000000]: /robot_body_filter(FilterBase::getParamVerboseMap): robot_body_filter_containment: Parameter body_model/inflation/per_link/padding not defined, assigning default: {} m [ INFO] [1652874266.331361251, 7.164000000]: /robot_body_filter(FilterBase::getParamVerboseMap): robot_body_filter_containment: Parameter body_model/inflation/per_link/scale not defined, assigning default: {} [ INFO] [1652874266.331550596, 7.164000000]: /robot_body_filter(FilterBase::getParamVerbose): robot_body_filter_containment: Parameter ignored_links/bounding_sphere not defined, assigning default: [] [ INFO] [1652874266.332191717, 7.164000000]: /robot_body_filter(FilterBase::getParamVerbose): robot_body_filter_containment: Parameter ignored_links/bounding_box not defined, assigning default: [] [ INFO] [1652874266.332314027, 7.164000000]: /robot_body_filter(FilterBase::getParamVerbose): robot_body_filter_containment: Parameter ignored_links/contains_test not defined, assigning default: [] [ INFO] [1652874266.332426775, 7.164000000]: /robot_body_filter(FilterBase::getParamVerbose): robot_body_filter_containment: Parameter ignored_links/shadow_test not defined, assigning default: ["laser"] [ INFO] [1652874266.332543035, 7.164000000]: /robot_body_filter(FilterBase::getParamVerbose): robot_body_filter_containment: Found parameter: ignored_links/everywhere, value: ["torso_cam3d_bottom_link"] [ INFO] [1652874266.332727647, 7.164000000]: /robot_body_filter(FilterBase::getParamVerbose): robot_body_filter_containment: Found parameter: only_links, value: ["test_link"] [ INFO] [1652874266.332963173, 7.164000000]: /robot_body_filter(FilterBase::getParamVerbose): robot_body_filter_containment: Parameter body_model/dynamic_robot_description/field_name not defined, assigning default: robot_model [New Thread 0x7fffe117d700 (LWP 1104874)] [ INFO] [1652874266.379603495, 7.173000000]: /robot_body_filter(TFFramesWatchdog::searchForReachableFrames): TFFramesWatchdog (torso_cam3d_bottom_depth_optical_frame): Frame torso_cam3d_bottom_depth_optical_frame became reachable at 7.171000000 [ INFO] [1652874266.391269303, 7.173000000]: /robot_body_filter(RobotBodyFilter::configure): RobotBodyFilter: Successfully configured. [ INFO] [1652874266.391635744, 7.173000000]: /robot_body_filter(RobotBodyFilter::configure): Filtering data in frame torso_cam3d_bottom_depth_optical_frame [ INFO] [1652874266.391902447, 7.173000000]: /robot_body_filter(RobotBodyFilter::configure): RobotBodyFilter: Filtering into the following categories: [ INFO] [1652874266.392050973, 7.173000000]: /robot_body_filter(RobotBodyFilter::configure): RobotBodyFilter: OUTSIDE [ INFO] [1652874266.392242579, 7.173000000]: /robot_body_filter(RobotBodyFilter::configure): RobotBodyFilter: CLIP [ INFO] [1652874266.392383724, 7.173000000]: /robot_body_filter(RobotBodyFilter::configure): RobotBodyFilter: INSIDE [ INFO] [1652874266.392528936, 7.173000000]: /robot_body_filter(RobotBodyFilter::configure): RobotBodyFilter: Filtering applied to links ["test_link"] with these links excluded: ["torso_cam3d_bottom_link"]. [ INFO] [1652874266.392656495, 7.173000000]: /robot_body_filter(FilterBase::getParamVerbose): robot_body_filter_containment: Found parameter: frames/output, value: torso_cam3d_bottom_depth_optical_frame [ INFO] [1652874266.392801770, 7.173000000]: /robot_body_filter(FilterBase::getParamVerbose): robot_body_filter_containment: Parameter cloud/point_channels not defined, assigning default: ["vp_"] [ INFO] [1652874266.403096473, 7.174000000]: /robot_body_filter(FilterBase::getParamVerbose): robot_body_filter_containment: Parameter cloud/direction_channels not defined, assigning default: ["normal_"] [ INFO] [1652874266.403349974, 7.174000000]: /robot_body_filter(FilterChainBase::initFilters): Configured filter chain of type sensor_msgs/PointCloud2 from namespace /robot_body_filter/cloud_filter_chain [ERROR] [1652874266.948915619, 7.219000000]: /robot_body_filter(RayCastingShapeMask::updateBodyPosesNoLock): Missing transform for shape test_link::0 (box) [ INFO] [1652874266.974278894, 7.221000000]: /robot_body_filter(RobotBodyFilter::computeMask): RobotBodyFilter: Mask computed in 0.00643 secs. [ INFO] [1652874266.974371447, 7.221000000]: /robot_body_filter(RobotBodyFilter::computeMask): RobotBodyFilter: Filtering run time is 0.00658 secs. [ERROR] [1652874267.587398951, 7.281000000]: /robot_body_filter(RayCastingShapeMask::updateBodyPosesNoLock): Missing transform for shape test_link::0 (box) [ INFO] [1652874267.618778450, 7.281000000]: /robot_body_filter(RobotBodyFilter::computeMask): RobotBodyFilter: Mask computed in 0.00626 secs. [ INFO] [1652874267.619368723, 7.281000000]: /robot_body_filter(RobotBodyFilter::computeMask): RobotBodyFilter: Filtering run time is 0.00651 secs. [ERROR] [1652874268.254474563, 7.345000000]: /robot_body_filter(RayCastingShapeMask::updateBodyPosesNoLock): Missing transform for shape test_link::0 (box) [ INFO] [1652874268.262294920, 7.345000000]: /robot_body_filter(RobotBodyFilter::computeMask): RobotBodyFilter: Mask computed in 0.00570 secs. [ INFO] [1652874268.263384428, 7.345000000]: /robot_body_filter(RobotBodyFilter::computeMask): RobotBodyFilter: Filtering run time is 0.00600 secs. [ERROR] [1652874268.804828648, 7.416000000]: /robot_body_filter(RayCastingShapeMask::updateBodyPosesNoLock): Missing transform for shape test_link::0 (box) [ INFO] [1652874268.812696253, 7.417000000]: /robot_body_filter(RobotBodyFilter::computeMask): RobotBodyFilter: Mask computed in 0.00564 secs. [ INFO] [1652874268.812773668, 7.417000000]: /robot_body_filter(RobotBodyFilter::computeMask): RobotBodyFilter: Filtering run time is 0.00575 secs. [ERROR] [1652874269.332022793, 7.472000000]: /robot_body_filter(RayCastingShapeMask::updateBodyPosesNoLock): Missing transform for shape test_link::0 (box) [ INFO] [1652874269.347230021, 7.474000000]: /robot_body_filter(RobotBodyFilter::computeMask): RobotBodyFilter: Mask computed in 0.00576 secs. [ INFO] [1652874269.347289552, 7.474000000]: /robot_body_filter(RobotBodyFilter::computeMask): RobotBodyFilter: Filtering run time is 0.00587 secs. [ERROR] [1652874270.041836650, 7.561000000]: /robot_body_filter(RayCastingShapeMask::updateBodyPosesNoLock): Missing transform for shape test_link::0 (box) [ INFO] [1652874270.074277857, 7.565000000]: /robot_body_filter(RobotBodyFilter::computeMask): RobotBodyFilter: Mask computed in 0.00714 secs. [ INFO] [1652874270.074529526, 7.565000000]: /robot_body_filter(RobotBodyFilter::computeMask): RobotBodyFilter: Filtering run time is 0.00726 secs. [ERROR] [1652874270.567284762, 7.607000000]: /robot_body_filter(RayCastingShapeMask::updateBodyPosesNoLock): Missing transform for shape test_link::0 (box) [ INFO] [1652874270.581640438, 7.607000000]: /robot_body_filter(RobotBodyFilter::computeMask): RobotBodyFilter: Mask computed in 0.00589 secs. [ INFO] [1652874270.581699439, 7.607000000]: /robot_body_filter(RobotBodyFilter::computeMask): RobotBodyFilter: Filtering run time is 0.00598 secs. [ERROR] [1652874271.119675245, 7.682000000]: /robot_body_filter(RayCastingShapeMask::updateBodyPosesNoLock): Missing transform for shape test_link::0 (box) [ INFO] [1652874271.126813706, 7.682000000]: /robot_body_filter(RobotBodyFilter::computeMask): RobotBodyFilter: Mask computed in 0.00577 secs. [ INFO] [1652874271.126874611, 7.682000000]: /robot_body_filter(RobotBodyFilter::computeMask): RobotBodyFilter: Filtering run time is 0.00587 secs. [ERROR] [1652874271.614572948, 7.729000000]: /robot_body_filter(RayCastingShapeMask::updateBodyPosesNoLock): Missing transform for shape test_link::0 (box) [ INFO] [1652874271.625155992, 7.730000000]: /robot_body_filter(RobotBodyFilter::computeMask): RobotBodyFilter: Mask computed in 0.00625 secs. [ INFO] [1652874271.625375549, 7.730000000]: /robot_body_filter(RobotBodyFilter::computeMask): RobotBodyFilter: Filtering run time is 0.00635 secs. [ERROR] [1652874272.386260280, 7.811000000]: /robot_body_filter(RayCastingShapeMask::updateBodyPosesNoLock): Missing transform for shape test_link::0 (box) [ INFO] [1652874272.393699142, 7.811000000]: /robot_body_filter(RobotBodyFilter::computeMask): RobotBodyFilter: Mask computed in 0.00573 secs. [ INFO] [1652874272.393797406, 7.811000000]: /robot_body_filter(RobotBodyFilter::computeMask): RobotBodyFilter: Filtering run time is 0.00588 secs. [ERROR] [1652874273.047674516, 7.891000000]: /robot_body_filter(RayCastingShapeMask::updateBodyPosesNoLock): Missing transform for shape test_link::0 (box) [ INFO] [1652874273.054201521, 7.892000000]: /robot_body_filter(RobotBodyFilter::computeMask): RobotBodyFilter: Mask computed in 0.00609 secs. [ INFO] [1652874273.054305147, 7.892000000]: /robot_body_filter(RobotBodyFilter::computeMask): RobotBodyFilter: Filtering run time is 0.00626 secs. [ERROR] [1652874273.643079076, 7.963000000]: /robot_body_filter(RayCastingShapeMask::updateBodyPosesNoLock): Missing transform for shape test_link::0 (box) [ INFO] [1652874273.650786838, 7.963000000]: /robot_body_filter(RobotBodyFilter::computeMask): RobotBodyFilter: Mask computed in 0.00651 secs. [ INFO] [1652874273.651405320, 7.963000000]: /robot_body_filter(RobotBodyFilter::computeMask): RobotBodyFilter: Filtering run time is 0.00666 secs. [ERROR] [1652874274.104660230, 8.010000000]: /robot_body_filter(RayCastingShapeMask::updateBodyPosesNoLock): Missing transform for shape test_link::0 (box) [ INFO] [1652874274.116530653, 8.012000000]: /robot_body_filter(RobotBodyFilter::computeMask): RobotBodyFilter: Mask computed in 0.00591 secs. [ INFO] [1652874274.117471024, 8.012000000]: /robot_body_filter(RobotBodyFilter::computeMask): RobotBodyFilter: Filtering run time is 0.00615 secs. [ERROR] [1652874274.753310595, 8.086000000]: /robot_body_filter(RayCastingShapeMask::updateBodyPosesNoLock): Missing transform for shape test_link::0 (box) [ INFO] [1652874274.762670958, 8.086000000]: /robot_body_filter(RobotBodyFilter::computeMask): RobotBodyFilter: Mask computed in 0.00611 secs. [ INFO] [1652874274.766801807, 8.090000000]: /robot_body_filter(RobotBodyFilter::computeMask): RobotBodyFilter: Filtering run time is 0.00632 secs. [ERROR] [1652874275.352529500, 8.154000000]: /robot_body_filter(RayCastingShapeMask::updateBodyPosesNoLock): Missing transform for shape test_link::0 (box) [ INFO] [1652874275.361050299, 8.156000000]: /robot_body_filter(RobotBodyFilter::computeMask): RobotBodyFilter: Mask computed in 0.00585 secs. [ INFO] [1652874275.361116807, 8.156000000]: /robot_body_filter(RobotBodyFilter::computeMask): RobotBodyFilter: Filtering run time is 0.00595 secs. [ INFO] [1652874275.464236263, 8.168000000]: /robot_body_filter(TFFramesWatchdog::searchForReachableFrames): TFFramesWatchdog (torso_cam3d_bottom_depth_optical_frame): Frame test_link became reachable at 8.168000000 Thread 1 "pointcloud2_fil" received signal SIGSEGV, Segmentation fault. 0x00007ffff2159f1a in bodies::Box::cloneAt(Eigen::Transform const&, double, double) const () from /home/fxm/projects/default/moveit_ws/devel/lib/libgeometric_shapes.so.0.7.3 (gdb) bt #0 0x00007ffff2159f1a in bodies::Box::cloneAt(Eigen::Transform const&, double, double) const () from /home/fxm/projects/default/moveit_ws/devel/lib/libgeometric_shapes.so.0.7.3 #1 0x00007ffff2280a76 in robot_body_filter::RayCastingShapeMask::updateBodyPosesNoLock() () from /home/fxm/projects/default/robot_ws/devel/lib/libRayCastingShapeMask.so #2 0x00007ffff22828ea in robot_body_filter::RayCastingShapeMask::maskContainmentAndShadows(sensor_msgs::PointCloud2_ > const&, std::vector >&, Eigen::Matrix const&) () from /home/fxm/projects/default/robot_ws/devel/lib/libRayCastingShapeMask.so #3 0x00007ffff221506c in robot_body_filter::RobotBodyFilter > >::computeMask(sensor_msgs::PointCloud2_ > const&, std::vector >&, std::__cxx11::basic_string, std::allocator > const&) () from /home/fxm/projects/default/robot_ws/devel/lib//librobot_body_filter.so #4 0x00007ffff21bc19e in robot_body_filter::RobotBodyFilterPointCloud2::update(sensor_msgs::PointCloud2_ > const&, sensor_msgs::PointCloud2_ >&) () from /home/fxm/projects/default/robot_ws/devel/lib//librobot_body_filter.so #5 0x000055555557fd04 in sensor_filters::FilterChainBase > >::filter(sensor_msgs::PointCloud2_ > const&, sensor_msgs::PointCloud2_ >&) () #6 0x000055555557db84 in sensor_filters::FilterChainBase > >::callbackReference(sensor_msgs::PointCloud2_ > const&) () #7 0x000055555558d434 in ros::SubscriptionCallbackHelperT > const&, void>::call(ros::SubscriptionCallbackHelperCallParams&) () ```
fmessmer commented 2 years ago

the node crashes the first time, a transform becomes available in https://github.com/peci1/robot_body_filter/blob/master/src/RayCastingShapeMask.cpp#L138

but I still don't get what's going on there - as I see the filtered pointcloud before the transform is availalbe....so the robot_body_filter already did what I wanted it to do...and then it crashed because a tranform became available...

is this an issue of some kind of incompatibility with geometric_shapes library? is this possibly related to the PRs there?

@peci1 do you have any further insights here?

peci1 commented 2 years ago

Hi, I'm sorry I ignored this issue when you first reported it, I had some problems with receiving emails from Github. We definitely use this library in sim time, so that should not be a problem.

Could you try to provide a minimal bag file and filter config so that I can try to reproduce the issue locally? And could you provide a stack trace when the library is built in debug mode (it would work super-slow, but for debugging purposes, it would be the best we could have)?

fmessmer commented 2 years ago

thanks for getting back to me!

I guess (part of) the issue was actually on my side I figured out I had a source overlay of geometric_shapes in my workspace chain - as part of my moveit (source) workspace

So once I removed that overlay of geometric_shapes and compiled robot_body_filter against the released version of geometric_shapes (0.7.3), I could use the robot_body_filter in gazebo simulation - as expected!

Adding geometric_shapes overlay again shows that everything still works with geometric_shapes at tag 0.7.3 But I'm getting the same SIGSEGV, Segmentation fault. once I switch geometric_shapes to latest noetic-devel

Seems like there is a potential regression coming up - once the moveit team decides to release a new version of geometric_shapes

Can you confirm my argumentation makes sense?

peci1 commented 2 years ago

Hmm, the noetic-devel branch has added FCL library as a new dependency. Can you check what version of libfcl-dev do you have installed? Is it the system one?

fmessmer commented 2 years ago 
dpkg -l libfcl-dev 
Desired=Unknown/Install/Remove/Purge/Hold
| Status=Not/Inst/Conf-files/Unpacked/halF-conf/Half-inst/trig-aWait/Trig-pend
|/ Err?=(none)/Reinst-required (Status,Err: uppercase=bad)
||/ Name           Version       Architecture Description
+++-==============-=============-============-==============================================
ii  libfcl-dev     0.5.0-5build1 amd64        Flexible Collision Library - development files

I'm (pretty) sure I don't have a source overlay of libfcl....

peci1 commented 2 years ago

And ros-noetic-fcl? Can you check against which is robot_body_filter linked? I.e. ldd devel/lib/librobot_body_filter.so | grep fcl

fmessmer commented 2 years ago 
dpkg -l ros-noetic-fcl
Desired=Unknown/Install/Remove/Purge/Hold
| Status=Not/Inst/Conf-files/Unpacked/halF-conf/Half-inst/trig-aWait/Trig-pend
|/ Err?=(none)/Reinst-required (Status,Err: uppercase=bad)
||/ Name           Version                      Architecture Description
+++-==============-============================-============-===================================
ii  ros-noetic-fcl 0.6.1-3focal.20220515.061114 amd64        FCL: the Flexible Collision Library

using geometric_shapes#0.7.3:

ldd devel/lib/librobot_body_filter.so |grep fcl
    libfcl.so.0.6 => /opt/ros/noetic/lib/x86_64-linux-gnu/libfcl.so.0.6 (0x00007fe410f15000)

using geometric_shapes#noetic-devel now gives me a compilation error: (not sure why I didn't see that before - possibly because my geometric_shapes overlay was downstream of my robot_body_filter overlay....

Starting  >>> robot_body_filter                                                                                                                                                         
________________________________________________________________________________________________________________________________________________________________________________________
Errors     << robot_body_filter:make /home/fxm/projects/default/robot_ws/logs/robot_body_filter/build.make.022.log                                                                      
In file included from /home/fxm/projects/default/robot_ws/src/robot_body_filter/include/robot_body_filter/utils/bodies.h:7,
                 from /home/fxm/projects/default/robot_ws/src/robot_body_filter/src/utils/bodies.cpp:10:
/home/fxm/projects/default/robot_ws/src/robot_body_filter/include/robot_body_filter/utils/obb.h:53:7: error: redefinition of ‘class bodies::OBB’
   53 | class OBB
      |       ^~~
In file included from /home/fxm/projects/default/robot_ws/src/geometric_shapes/include/geometric_shapes/bodies.h:45,
                 from /home/fxm/projects/default/robot_ws/src/robot_body_filter/src/utils/bodies.cpp:6:
/home/fxm/projects/default/robot_ws/src/geometric_shapes/include/geometric_shapes/obb.h:52:7: note: previous definition of ‘class bodies::OBB’
   52 | class OBB
      |       ^~~
In file included from /home/fxm/projects/default/robot_ws/src/robot_body_filter/include/robot_body_filter/utils/bodies.h:7,
                 from /home/fxm/projects/default/robot_ws/src/robot_body_filter/src/utils/shapes.cpp:6:
/home/fxm/projects/default/robot_ws/src/robot_body_filter/include/robot_body_filter/utils/obb.h:53:7: error: redefinition of ‘class bodies::OBB’
   53 | class OBB
      |       ^~~
In file included from /home/fxm/projects/default/robot_ws/src/geometric_shapes/include/geometric_shapes/bodies.h:45,
                 from /home/fxm/projects/default/robot_ws/src/robot_body_filter/include/robot_body_filter/utils/bodies.h:4,
                 from /home/fxm/projects/default/robot_ws/src/robot_body_filter/src/utils/shapes.cpp:6:
/home/fxm/projects/default/robot_ws/src/geometric_shapes/include/geometric_shapes/obb.h:52:7: note: previous definition of ‘class bodies::OBB’
   52 | class OBB
      |       ^~~
make[2]: *** [CMakeFiles/robot_body_filter_utils.dir/build.make:102: CMakeFiles/robot_body_filter_utils.dir/src/utils/shapes.cpp.o] Error 1
make[2]: *** Waiting for unfinished jobs....
make[2]: *** [CMakeFiles/robot_body_filter_utils.dir/build.make:63: CMakeFiles/robot_body_filter_utils.dir/src/utils/bodies.cpp.o] Error 1
make[1]: *** [CMakeFiles/Makefile2:1873: CMakeFiles/robot_body_filter_utils.dir/all] Error 2
make: *** [Makefile:141: all] Error 2
cd /home/fxm/projects/default/robot_ws/build/robot_body_filter; catkin build --get-env robot_body_filter | catkin env -si  /usr/bin/make --jobserver-auth=42,43; cd -

........................................................................................................................................................................................
Failed     << robot_body_filter:make           [ Exited with code 2 ]                                                                                                                   
Failed    <<< robot_body_filter                [ 5.6 seconds ]

as I said: the issue was probably on my end having a messed up workspace and not cleaning/sourcing correctly...

peci1 commented 2 years ago

Ahh, now I see it. There is a conflict between class bodies::OBB defined in this package and the one that was added to geometric_shapes. I've put the local "copy" in the same namespace. This should be easily fixed on my side by moving it to a different namespace. I'll have a look at it.

peci1 commented 2 years ago

Check the latest commit. It should fix the issue you are seeing.

fmessmer commented 2 years ago

thanks @peci1 I can confirm that with https://github.com/peci1/robot_body_filter/commit/7c7466bcbc1774fe0e97b35a57e268ecb34e8e82 I can now use the robot_body_filter with both geometric_shapes#0.7.3 and geometric_shapes#noetic-devel

peci1 commented 2 years ago

Thank you for reporting the issue!