peci1 / robot_body_filter

Filters the robot's body out of point clouds and laser scans.

robot_body_filter

Filters the robot's body out of point clouds and laser scans.

Tutorial

Check out the webinar recording where a lot of options for this filter are explained and demonstrated! https://www.youtube.com/watch?v=j0ljV0uZy3Q

Changes vs PR2/robot_self_filter:

• Now the package is a normal filters::FilterBase filter and not a standalone node.
• Using both containment and ray-tracing tests.
• Using all collision elements for each link instead of only the first one.
• Enabling generic point type, removing PCL dependency and unnecessary params.
• Using bodies.h and shapes.h from geometric_shapes.
• As a by-product, the filter can compute robot's bounding box or sphere.

Build Status

Development versions:

Release jobs Melodic :

Release jobs Noetic :

Basic Operation

filters::FilterBase API

The basic workings of this filter are done via the filters::FilterBase API implemented for sensor_msgs::LaserScan and sensor_msgs::PointCloud2 types. This means you can load this filter into a FilterChain along other filters as usual. Different from the standard filters, this one can also publish several interesting topics and subscribes to TF.

General overview

This filter reads robot model and the filter config, subscribes to TF, waits for data (laserscans or point clouds) and then cleans them from various artifacts (this is called data filtering).

It can perform 3 kinds of data filters: clip the data based on the provided sensor limits (parameter filter/do_clipping), remove points that are inside or on the surface of the robot body (parameter filter/do_contains_test) and remove points that are seen through a part of the robot body (parameter filter/do_shadow_test). These kinds of tests are further referenced as "clipping", "contains test" and "shadow test".

If working with point clouds, the filter automatically recognizes whether it works with organized or non-organized clouds. In organized clouds, it marks the filtered-out points as NaN. In non-organized clouds, it removes the filtered-out points. In laserscans, removal is not an option, so the filtered-out points are marked with NaN (some guides suggest that max_range + 1 should be used for marking invalid points, but this filter uses NaN as a safer option).

Performance tips

In general, the filter will be computationally expensive (clipping is fast, contains test is medium CPU intensive and shadow test is the most expensive part, because it basically performs raytracing). You can limit the required CPU power by limiting the filter only to parts that matter. E.g. if the robot has a link that can never be seen by the sensor, put it in the list of ignored links. The less links are processed, the better performance. If you're only interested in removing a few links, consider using the only_links parameter.

To speed up shadow filtering, you can set filter/max_shadow_distance, which limits the number of points considered for shadow tests just to points close to the sensor. Setting this to e.g. three times the diameter of the robot should remove all of the shadow points caused by refraction by a part of the robot body. But you have to test this with real data.

Performance also strongly depends on representation of the robot model. The filter reads <collision> tags from the robot URDF. You can use boxes, spheres and cylinders (which are fast to process), or you can use convex meshes (these are much worse performance-wise). If you pass a non-convex mesh, its convex hull will be used for the tests. Don't forget that each link can have multiple <collision> tags. If you do not have time to convert your meshes to the basic shapes, try to at least reduce the number of triangles in your meshes. You can use your high-quality meshes in <visual> tags. To simplify your model to primitive shapes, you can either manually edit the URDF, or you can utilize ColliderGen.

Model inflation

You can utilize the builtin model inflation mechanism to slightly alter the size of the model. You will probably want to add a bit "margin" to the contains and shadow tests so that points that are millimeters outside the robot body will anyways get removed. You can set a default scale and padding which are used for all collisions. Different inflation can be used for contains tests and for shadow tests. Inflation can also be specified differently for each link. Look at the body_model/inflation/* parameters for details.

Scaling means multiplying the shape dimensions by the given factor (with its center staying in the same place). Padding means adding the specified metric distance to each "dimension" of the shape. Padding a sphere by p just adds p to its radius; padding a cylinder adds p to its radius and 2p to its length (you pad both the top and the bottom of the cylinder); padding a box adds 2p to all its extents (again, you pad both of the opposing sides); padding a mesh pads each vertex of its convex hull by p along the direction from the mesh center to the vertex (mesh center is the center specified in the mesh file, e.g. DAE). Have a good look at the effects of mesh padding, as the results can be non-intuitive.

Data acquisition modes

The filter supports data captured in two modes - all at once (e.g. RGBD cameras), or each point at a different time instant (mostly lidars). This is handled by sensor/point_by_point setting. Each mode supports both laser scans and point cloud input (although all-at-once laserscans aren't that common). Point-by-point pointclouds are e.g. the output of 3D lidars like Ouster (where more points can have the same timestamp, but not all). If you want to use the point-by-point mode with pointclouds, make sure they contain not only the usual x, y and z fields, but also a float32 field stamps (with time difference from the time in the header) and float32 fields vp_x, vp_y and vp_z which contain the viewpoint (position of the sensor in filtering frame) from which the robot saw that point.

When filtering in the point-by-point mode, the robot posture has to be updated several times during processing a single scan (to reflect the motion the robot has performed during acquiring the scan). The frequency of these updates can also have a significant impact on performance. Use parameter filter/model_pose_update_interval to set the interval for which the robot is considered stationary. The positions of the robot at the beginning and at the end of the scan are queried from TF. The intermediate positions are linearly interpolated between these two positions.

TF frames

The filter recognizes four logical TF frames (some of which may be the same physical frames).

Fixed frame is a frame that doesn't change within the duration of the processed scan. This means for all-at-once scans, this frame is not needed because the duration of the scan is zero. For point-by-point scans, it depends on the particular scenario. In static installations (like manipulators with sensors not attached to them), it can be the sensor frame. For stationary robots with the sensor attached to a movable part of their body, base_link will be a good choice. For completely mobile robots, you will need an external frame, e.g. odom or map (beware of cyclic dependencies - if the map is built from the filtered scans, you obviously cannot use map as the fixed frame for filtering the scans...).

Sensor frame is the frame in which the data were captured. Generally, it would be whatever is in header.frame_id field of the processed messages. You can use the filter for data from multiple sensors - in that case, you can leave the sensor frame unfilled and each message will be processed in the frame it has in its header.

Filtering frame is the frame in which the data filtering is done. For point-by-point scans, it has to be a fixed frame (possibly different from the fixed frame set in frames/fixed). For pointcloud scans, it should be the sensor frame (if all data are coming from a single sensor), or any other frame. It is also used as the frame in which all debugging outputs are published.

Output frame can only be used with pointcloud scans, and allows to transform the filtered pointcloud to a different frame before being published. It is just a convenience which can save you launching a transformation nodelet. By default, filtered pointclouds are output in the filtering frame.

Bounding shapes

As a byproduct, the filter can also compute various bounding shapes of the robot model. There are actually four robot models - one for contains test, one for shadow test, one for bounding sphere computation and one for bounding box (these models can differ by inflation and considered links). All bounding shapes are published in the filtering frame. For point-by-point scans, the bounding shapes correspond to the time instant specified in the header of the processed scan. The computation of bounding shapes is off by default, but enabling it is cheap (performance-wise).

The bounding sphere is easy - the smallest sphere that contains the whole collision model for bounding sphere computation (with the specified exclusions removed).

The bounding box is the smallest axis-aligned bounding box aligned to the filtering frame. It is built from the model for bounding box computation.

The local bounding box is the smallest axis-aligned bounding box aligned to the frame specified in local_bounding_box/frame_id. It is especially useful with mobile robots when the desired frame is base_link. It is built from the model for bounding box computation.

The oriented bounding box should be the smallest box containing the collision model. However, its computation is very bad conditioned, so the results can be very unsatisfactory. Currently, the oriented bounding box of each of the basic collision shapes is "tight", but merging the boxes is not optimal. A good algorithm would probably require costly and advanced iterative methods. The current implementation uses FCL in the background and merges the boxes using fcl::OBB::operator+=() without any further optimizations. It is built from the model for bounding box computation.

The filter also supports publishing auxiliary pointclouds which "cut out" each of these bounding shapes. These are the input data converted to pointcloud in filtering frame from which all points belonging to the bounding shape are removed. Please note that the "base" used for cutting out is the input pointcloud, not the filtered one.

First setup/debugging

The filter offers plenty of debugging outputs to make sure it does exactly what you want it to do. All the options are described in the last part of this page. Generally, you should look at the pointclouds visualizing which points got filtered out and you should also check the robot models used for filtering.

Also, have a look in the [examples] folder to get some inspiration.

Usage

This is a standard filters::FilterBase<T>-based filter which implements the configure() and update(const T&, T&) methods. This means it can be loaded e.g. as a part of a filter chain via the laser_filters package or the relatively new sensor_filters. This means the input and output data are not supplied in the form of topics, but they are instead passed to the update() method via the C++ API.

This filter is a bit unusual - it subscribes and publishes several topics. Normally, filters only operate via the update() method and are not expected to publish anything. This filter is different, it requires a working ROS node handle, and can publish a lot of auxiliary or debug data.

Subscribed Topics

• /tf, /tf_static

  Transforms

• dynamic_robot_model_server/parameter_updates (dynamic_reconfigure/Config)

  Dynamic reconfigure topic on which updated robot model can be subscribed. The model is read from a field defined by parameter body_model/dynamic_robot_description/field_name.

Published Topics

• scan_point_cloud_no_bsphere (sensor_msgs/PointCloud2)

  Point cloud with points inside body bounding sphere removed. Turned on by bounding_sphere/publish_cut_out_pointcloud parameter. Published in filtering frame.

• scan_point_cloud_no_bbox (sensor_msgs/PointCloud2)

  Point cloud with points inside filtering frame axis-aligned bounding box removed. Turned on by bounding_box/publish_cut_out_pointcloud parameter. Published in filtering frame.

• scan_point_cloud_no_oriented_bbox (sensor_msgs/PointCloud2)

  Point cloud with points inside oriented bounding box removed. Turned on by oriented_bounding_box/publish_cut_out_pointcloud parameter. Published in filtering frame. Please read the remarks above in the overview - the results can be non-satisfying.

• scan_point_cloud_no_local_bbox (sensor_msgs/PointCloud2)

  Point cloud with points inside local_bounding_box/frame_id axis-aligned bounding box removed. Turned on by local_bounding_box/publish_cut_out_pointcloud parameter. Published in filtering frame.

• robot_bounding_sphere (robot_body_filter/SphereStamped)

  Bounding sphere of the robot body. Turned on by bounding_sphere/compute parameter. Published in filtering frame.

• robot_bounding_box (geometry_msgs/PolygonStamped)

  Axis-aligned bounding box of the robot body aligned to the filtering frame. First point is the minimal point, second one is the maximal point. Turned on by bounding_box/compute parameter. Published in filtering frame.

• robot_oriented_bounding_box (robot_body_filter/OrientedBoundingBoxStamped)

  Oriented bounding box of the robot body. Turned on by oriented_bounding_box/compute parameter. Please read the remarks above in the overview - the results can be non-satisfying. Published in filtering frame.

• robot_local_bounding_box (geometry_msgs/PolygonStamped)

  Axis-aligned bounding box of the robot body aligned to frame local_bounding_box/frame_id. First point is the minimal point, second one is the maximal point. Turned on by local_bounding_box/compute parameter. Published in frame local_bounding_box/frame_id.

Provided Services

• ~reload_model (std_srvs/Trigger)

  Call this service to re-read the URDF model from parameter server.

Filter Parameters

Have a look in the examples folder to get inspiration for configuration of your filter.

• sensor/point_by_point (bool, default: false for PointCloud2 version, true for LaserScan)

  If true, suppose that every point in the scan was captured at a different time instant. Otherwise, the scan is assumed to be taken at once. If this is true, the processing pipeline expects the pointcloud to have fields int32 index, float32 stamps, and float32 vp_x, vp_y and vp_z viewpoint positions. If one of these fields is missing, computeMask() throws runtime exception.

• frames/fixed (string, default: "base_link")

  The fixed frame. Usually base_link for stationary robots (or sensor frame if both robot and sensor are stationary). For mobile robots, it can be e.g. odom or map. Only needed for point-by-point scans.

• frames/sensor (string, default "")

  Frame of the sensor. If set to empty string, it will be read from header.frame_id of each incoming message. In LaserScan version, if nonempty, it has to match the frame_id of the incoming scans. In PointCloud2 version, the data can come in a different frame from frames/sensor.

• frames/filtering (string, default: frames/fixed)

  Frame in which the filter is applied. For point-by-point scans, it has to be a fixed frame, otherwise, it can be the sensor frame or any other frame. Setting to sensor frame will save some computations, but this frame cannot be empty string, so sensor frame can only be used if all data are coming from a single sensor and you know the scan frame in advance.

• frames/output (string, default: frames/filtering)

  Frame into which output data are transformed. Only applicable in PointCloud2 version.

• sensor/min_distance (float, default: 0.0 m)

  The minimum distance of points from the laser to keep them.

• sensor/max_distance (float, default: 0.0 m)

  The maximum distance of points from the laser to keep them. Set to zero to disable this limit.

• filter/keep_clouds_organized (bool, default true)

  Whether to keep pointclouds organized (if they were). Organized cloud has height > 1.

• filter/model_pose_update_interval (float, default 0.0 s)

  The interval between two consecutive model pose updates when processing a pointByPointScan. If set to zero, the model will be updated for each point separately (might be computationally exhaustive). If non-zero, it will only update the model once in this interval, which makes the masking algorithm a little bit less precise but more computationally affordable.

• filter/do_clipping (bool, default true)

  If true, the filter will mark points outside sensor/(min|max)_distance as CLIP. If false, such points will be marked OUTSIDE.

• filter/do_contains_test (bool, default true)

  If true, the filter will mark points inside robot body as INSIDE. If false, such points will be marked OUTSIDE.

• filter/do_shadow_test (bool, default true)

  If true, the filter will mark points shadowed by robot body as SHADOW. If false, such points will be marked OUTSIDE.

• filter/max_shadow_distance (float, default is the value of sensor/max_distance)

  If greater than zero, specifies the maximum distance of a point from the sensor frame within which the point can be considered for shadow testing. All further points are classified as OUTSIDE. Setting this parameter to a low value may greatly improve performance of the shadow filtering.

• body_model/inflation/scale (float, default 1.0)

  A scale that is applied to the collision model for the purposes of collision checking.

• body_model/inflation/padding (float, default 0.0 m)

  Padding to be added to the collision model for the purposes of collision checking.

• body_model/inflation/contains_test/scale (float, default body_model/inflation/scale)

  A scale that is applied to the collision model used for contains tests.

• body_model/inflation/contains_test/padding (float, default body_model/inflation/padding)

  Padding to be added to the collision model used for contains tests.

• body_model/inflation/shadow_test/scale (float, default body_model/inflation/scale)

  A scale that is applied to the collision model used for shadow tests.

• body_model/inflation/shadow_test/padding (float, default body_model/inflation/padding)

  Padding to be added to the collision model used for shadow tests.

• body_model/inflation/bounding_sphere/scale (float, default body_model/inflation/scale)

  A scale that is applied to the collision model used for bounding sphere computation.

• body_model/inflation/bounding_sphere/padding (float, default body_model/inflation/padding)

  Padding to be added to the collision model used for bounding sphere computation.

• body_model/inflation/bounding_box/scale (float, default body_model/inflation/scale)

  A scale that is applied to the collision model used for bounding box computation.

• body_model/inflation/bounding_box/padding (float, default body_model/inflation/padding)

  Padding to be added to the collision model used for bounding box computation.

• body_model/inflation/per_link/scale (dict[str:float], default {})

  A scale that is applied to the specified links for the purposes of collision checking. Links not specified here will use the default scale set in body_model/inflation/contains_test/scale or body_model/inflation/shadow_test/scale. Keys are names, values are scale. Names can be either names of links (link), names of collisions ( *::my_collision), a combination of link name and zero-based collision index (link::1), or link name and collision name, e.g. link::my_collision. Any such name can have suffix ::contains, ::shadow, ::bounding_sphere or ::bounding_box, which will only change the scale for contains or shadow tests or for bounding sphere or box computation. If a collision is matched by multiple entries, they have priority corresponding to the order they were introduced here (the entries do not "add up", but replace each other).

• body_model/inflation/per_link/padding (dict[str:float], default {})

  Padding to be added to the specified links for the purposes of collision checking. Links not specified here will use the default padding set in body_model/inflation/contains_test/padding or body_model/inflation/shadow_test/padding. Keys are names, values are padding. Names can be either names of links (link), names of collisions ( *::my_collision), a combination of link name and zero-based collision index (link::1), or link name and collision name, e.g. link::my_collision. Any such name can have suffix ::contains, ::shadow, ::bounding_sphere or ::bounding_box, which will only change the padding for contains or shadow tests or for bounding sphere or box computation. If a collision is matched by multiple entries, they have priority corresponding to the order they were introduced here (the entries do not "add up", but replace each other).

• body_model/robot_description_param (string, default: "robot_description")

  Name of the parameter where the robot model can be found.

• transforms/buffer_length (float, default 60.0 s)

  Duration for which transforms will be stored in TF buffer.

• transforms/timeout/reachable (float, default 0.1 s)

  How long to wait while getting reachable TF (i.e. transform which was previously available). Please note that this timeout is computed not from the lookup start time, but from the scan timestamp - this allows you to tell how old scans you still want to process.

• transforms/timeout/unreachable (float, default 0.2 s)

  How long to wait while getting unreachable TF.

• transforms/require_all_reachable (bool, default false)

  If true, the filter won't publish anything until all transforms are reachable.

• ignored_links/bounding_sphere (list[string], default [])

  List of links to be ignored in bounding sphere computation. Can be either names of links (link), names of collisions (*::my_collision), a combination of link name and zero-based collision index (link::1), or link name and collision name, e.g. link::my_collision.

• ignored_links/bounding_box (list[string], default [])

  List of links to be ignored in bounding box computation. Same naming rules as above.

• ignored_links/contains_test (list[string], default [])

  List of links to be ignored when testing if a point is inside robot body. Same naming rules as above.

• ignored_links/shadow_test (list[string], default ['laser'])

  List of links to be ignored when testing if a point is shadowed by robot body. Same naming rules as above. It is essential that this list contains the sensor link - otherwise all points would be shadowed by the sensor itself.

• ignored_links/bounding_sphere (list[string], default [])

  List of links to be ignored when computing the bounding sphere. Same naming rules as above.

• ignored_links/bounding_box (list[string], default [])

  List of links to be ignored when computing the bounding box. Same naming rules as above.

• ignored_links/everywhere (list[string], default [])

  List of links to be completely ignored. Same naming rules as above.

• only_links (list[string], default [])

  If non-empty, only the specified links will be processed. The above-mentioned ignored_links/* filters will still be applied.

• bounding_sphere/compute (bool, default false)

  Whether to compute and publish bounding sphere.

• bounding_box/compute (bool, default false)

  Whether to compute and publish axis-aligned bounding box aligned to filtering frame.

• oriented_bounding_box/compute (bool, default false)

  Whether to compute and publish oriented bounding box (see the remarks in the overview above; the result might be pretty bad).

• local_bounding_box/compute (bool, default false)

  Whether to compute and publish axis-aligned bounding box aligned to frame local_bounding_box/frame_id.

• local_bounding_box/frame_id (str, default: frames/fixed)

  The frame to which local bounding box is aligned.

• bounding_sphere/publish_cut_out_pointcloud (bool, default false)

  Whether to compute and publish pointcloud from which points in the bounding sphere are removed. Will be published on scan_point_cloud_no_bsphere. Implies bounding_sphere/compute. The "base" point cloud before cutting out are the input data, not the filtered data.

• bounding_box/publish_cut_out_pointcloud (bool, default false)

  Whether to compute and publish pointcloud from which points in the bounding box are removed. Will be published on scan_point_cloud_no_bbox. Implies bounding_box/compute. The "base" point cloud before cutting out are the input data, not the filtered data.

• oriented_bounding_box/publish_cut_out_pointcloud (bool, default false)

  Whether to compute and publish pointcloud from which points in the oriented bounding box are removed. Will be published on scan_point_cloud_no_oriented_bbox. Implies oriented_bounding_box/compute. The "base" point cloud before cutting out are the input data, not the filtered data.

• local_bounding_box/publish_cut_out_pointcloud (bool, default false)

  Whether to compute and publish pointcloud from which points in the local bounding box are removed. Will be published on scan_point_cloud_no_local_bbox. Implies local_bounding_box/compute. The "base" point cloud before cutting out are the input data, not the filtered data.

• body_model/dynamic_robot_description/field_name (string, default robot_model)

  If robot model is published by dynamic reconfigure, this is the name of the Config message field which holds the robot model.

• cloud/point_channels (list[string], default ["vp_"])

  List of channels of the incoming pointcloud that should be transformed as positional data. The 3D positions channel given by fields x, y and z is always transformed. This list contains prefixes of fields that form another channel(s). E.g. to transform the channel given by fields vp_x, vp_y and vp_z, add item "vp_" to the list. If a channel is not present in the cloud, nothing happens. This parameter is only available in the PointCloud2 version of the filter.

• cloud/direction_channels (list[string], default ["normal_"])

  List of channels of the incoming pointcloud that should be transformed as directional data. This list contains prefixes of fields that form channel(s). E.g. to transform the channel given by fields normal_x, normal_y and normal_z, add item "normal_" to the list. If a channel is not present in the cloud, nothing happens. This parameter is only available in the PointCloud2 version of the filter.

Debug Operation

These options are there to help correctly set up and debug the filter operation and should be turned off in production environments since they can degrade performance of the filter.

Published Topics

• robot_bounding_sphere_marker (visualization_msgs/Marker)

  Marker of the bounding sphere of the robot body. Turned on by bounding_sphere/marker parameter. Published in filtering frame.

• robot_bounding_box_marker (visualization_msgs/Marker)

  Marker of the bounding box of the robot body. Turned on by bounding_box/marker parameter. Published in filtering frame.

• robot_oriented_bounding_box_marker (visualization_msgs/Marker)

  Marker of the oriented bounding box of the robot body. Turned on by oriented_bounding_box/marker parameter. Published in filtering frame.

• robot_local_bounding_box_marker (visualization_msgs/Marker)

  Marker of the local bounding box of the robot body. Turned on by local bounding_box/marker parameter. Published in frame local_bounding_box/frame_id.

• robot_bounding_sphere_debug (visualization_msgs/MarkerArray)

  Marker array containing the bounding sphere for each collision element. Turned on by bounding_sphere/debug parameter. Published in filtering frame.

• robot_bounding_box_debug (visualization_msgs/MarkerArray)

  Marker array containing the bounding box for each collision element. Turned on by bounding_box/debug parameter. Published in filtering frame.

• robot_oriented_bounding_box_debug (visualization_msgs/MarkerArray)

  Marker array containing the oriented bounding box for each collision element. Turned on by oriented_bounding_box/debug parameter. Published in filtering frame.

• robot_local_bounding_box_debug (visualization_msgs/MarkerArray)

  Marker array containing the local bounding box for each collision element. Turned on by local_bounding_box/debug parameter. Published in frame local_bounding_box/frame_id.

• robot_model_for_contains_test (visualization_msgs/MarkerArray)

  Marker array containing the exact robot model used for contains tests. Turned on by debug/marker/contains parameter.

• robot_model_for_shadow_test (visualization_msgs/MarkerArray)

  Marker array containing the exact robot model used for shadow tests. Turned on by debug/marker/shadow parameter.

• robot_model_for_bounding_sphere (visualization_msgs/MarkerArray)

  Marker array containing the exact robot model used for computation of bounding sphere. Turned on by debug/marker/bounding_sphere parameter.

• robot_model_for_bounding_box (visualization_msgs/MarkerArray)

  Marker array containing the exact robot model used for computation of bounding box. Turned on by debug/marker/bounding_box parameter.

• scan_point_cloud_inside (sensor_msgs/PointCloud2)

  Debugging pointcloud with points classified as INSIDE. Turned on by debug/pcl/inside parameter.

• scan_point_cloud_clip (sensor_msgs/PointCloud2)

  Debugging pointcloud with points classified as CLIP. Turned on by debug/pcl/clip parameter.

• scan_point_cloud_shadow (sensor_msgs/PointCloud2)

  Debugging pointcloud with points classified as SHADOW. Turned on by debug/pcl/shadow parameter.

Filter Parameters

• bounding_sphere/marker (bool, default false)

  Whether to publish a marker representing the bounding sphere.

• bounding_box/marker (bool, default false)

  Whether to publish a marker representing the axis-aligned bounding box aligned to the filtering frame.

• oriented_bounding_box/marker (bool, default false)

  Whether to publish a marker representing the oriented bounding box.

• local_bounding_box/marker (bool, default false)

  Whether to publish a marker representing the axis-aligned bounding box aligned to frame local_bounding_box/frame_id.

• bounding_sphere/debug (bool, default false)

  Whether to compute and publish debug bounding spheres (marker array of spheres for each collision).

• bounding_box/debug (bool, default false)

  Whether to compute and publish debug bounding boxes (marker array of axis-aligned boxes for each collision).

• oriented_bounding_box/debug (bool, default false)

  Whether to compute and publish debug oriented bounding boxes (marker array of oriented boxes for each collision).

• local_bounding_box/debug (bool, default false)

  Whether to compute and publish debug axis-aligned bounding boxes aligned to frame local_bounding_box/frame_id (marker array of axis-aligned boxes for each collision).

• debug/pcl/inside (bool, default false)

  Whether to publish debugging pointcloud with points marked as INSIDE.

• debug/pcl/clip (bool, default false)

  Whether to publish debugging pointcloud with points marked as CLIP.

• debug/pcl/shadow (bool, default false)

  Whether to publish debugging pointcloud with points marked as SHADOW.

• debug/marker/contains (bool, default false)

  Whether to publish debugging marker array containing the exact robot body model used for containment test.

• debug/marker/shadow (bool, default false)

  Whether to publish debugging marker array containing the exact robot body model used for shadow test.

• debug/marker/bounding_sphere (bool, default false)

  Whether to publish debugging marker array containing the exact robot body model used for computing the bounding sphere.

• debug/marker/bounding_box (bool, default false)

  Whether to publish debugging marker array containing the exact robot body model used for computing the bounding box.

robot_body_filter's Issues

Per link padding and scaling not working

Hi, I tried applying a per link scaling and padding but does not seem to be working. Do I need to reinstall the library or is something wrong with my yaml file?

# Filter config for segmenting out Robot from pointcloud.
# The field of view of the sensor can only capture a few links, so the `only_links` parameter is set
# to increase performance. Also, the `filter/keep_clouds_organized` parameter is important here to
# retain the 2D structure of the RGBD camera output.
# For more detail on the different parameters of the filter, refer to:
# http://wiki.ros.org/robot_body_filter
robot_filter_chain:
  - name: RobotBodyFilter
    type: robot_body_filter/RobotBodyFilterPointCloud2
    params:
      frames/fixed: 'canonical_smpl_origin'
      frames/sensor: 'canonical_smpl_origin'
      filter/keep_clouds_organized: True
      filter/do_clipping: True
      filter/do_contains_test: True
      filter/do_shadow_test: True
      sensor/point_by_point: False
      sensor/min_distance: 0.01
      sensor/max_distance: 2.5
      only_links: []
      ignored_links/everywhere: []
      body_model/inflation/scale: 1.2
      body_model/inflation/per_link/padding: {left_touchpoint: 0.072, right_touchpoint: 0.072}
      body_model/robot_description_param: '/robot_description'
      transforms/buffer_length: 15.0
      transforms/timeout/reachable: 0.2
      transforms/timeout/unreachable: 0.2
      bounding_sphere/compute: True
      bounding_sphere/debug: False
      bounding_sphere/publish_cut_out_pointcloud: True
      bounding_box/compute: True
      bounding_box/debug: False
      bounding_box/publish_cut_out_pointcloud: True
      debug/pcl/inside: False
      debug/pcl/clip: False
      debug/pcl/shadow: False
      debug/marker/contains: True
      debug/marker/shadow: False

Problem reloading robot model

Hello it is me again,
The model is loading fine when I start the node and the filtering is great. But when I try to reload a similar urdf with a collision box change I get this error :

[ERROR] [/pointcloud_filter]: Unable to parse component [${M_PI/2}] to a double (while parsing a vector value)
[ERROR] [/pointcloud_filter]: Could not parse visual element for Link [camera_tracking_pose_frame]
[ERROR] [/pointcloud_filter]: Unable to parse component [${M_PI/2}] to a double (while parsing a vector value)
[ERROR] [/pointcloud_filter]: Could not parse visual element for Link [laser]
[ERROR] [/pointcloud_filter]: Unable to parse component [${M_PI/2}] to a double (while parsing a vector value)
[ERROR] [/pointcloud_filter]: Could not parse visual element for Link [imu_bottom_center]
[ERROR] [/pointcloud_filter]: Unable to parse component [${M_PI}] to a double (while parsing a vector value)
[ERROR] [/pointcloud_filter]: Malformed parent origin element for joint [camera_tracking_joint]
[ERROR] [/pointcloud_filter]: joint xml is not initialized correctly
[ERROR] [/pointcloud_filter]: RobotBodyFilter: The URDF model given in parameter '/robot_description' cannot be parsed. See urdf::Model::initString for debugging, or try running 'gzsdf my_robot.urdf'

Is the parsing or init method different between the start of the node and the reload of the model ?

Thanks again for the help

TF watchdog for sensor frame breaks filter

Hello,
the commit ee0c87f introduced a reachability check for the sensor frame. However, the sensor frame is not monitored by the TF watch dog. Therefore, the check always fails and every scan is rejected.

Best regards,
Martin

Cmake error

hello,
while performing cmake I am getting below error
CMake Error at robot_body_filter/cmake/DetectOptional.cmake:4 (if):
if given arguments:

"CMAKE_VERSION" "VERSION_GREATER_EQUAL" "3.8"

Unknown arguments specified
Call Stack (most recent call first):
robot_body_filter/CMakeLists.txt:5 (include)
Can you please help me

transformWithChannels should differentiate between point-like and direction-like channels

robot_body_filter/src/utils/tf2_sensor_msgs.cpp

Line 49 in 7f2c032

transformChannel(out, t, "normal_");

noetic support

Will there be noetic support?

Timing issue PointCloud and Collision geometry

Hello!
I have setup your robot_body_filter for a custom project where I am trying to exclude the robot arm from the point cloud of a top mounted RGB-D camera. When I noticed that the timing was of I first suspected that the collision meshes where to complex. So I removed all but one link from the filter using the "only_links" and "ignored_links/everywhere" in the .yaml file. Additionally I changed the collision of the remaining link to a simple box but the issue still persists. Here is a .gif of the issue (you can see the point cloud trailing the "debug/marker/contains" of the link):

and here is my config:

cloud_filter_chain:
  - name: RobotBodyFilter
    type: robot_body_filter/RobotBodyFilterPointCloud2
    params:
      frames/sensor: 'camera_depth_optical_frame'
      frames/filtering: 'base'
      frames/output: 'camera_depth_optical_frame'
      filter/keep_clouds_organized: True
      filter/do_clipping: True
      filter/do_contains_test: True
      filter/do_shadow_test: False
      sensor/point_by_point: False
      sensor/min_distance: 0.2
      sensor/max_distance: 10.0
      only_links: ["panda_link6"]
      ignored_links/everywhere: ["panda_link0", "panda_hand", "panda_leftfinger", "panda_rightfinger", "panda_link1", "panda_link2", "panda_link3", "panda_link4", "panda_link5", "panda_link7"]
      body_model/inflation/scale: 1.1
      body_model/inflation/padding: 0.05
      body_model/robot_description_param: '/robot_description'
      transforms/buffer_length: 15.0
      transforms/timeout/reachable: 0.2
      transforms/timeout/unreachable: 0.2
      bounding_sphere/compute: False
      bounding_box/compute: False
      oriented_bounding_box/compute: False
      local_bounding_box/compute: False
      debug/pcl/inside: True
      debug/pcl/clip: False
      debug/pcl/shadow: False
      debug/marker/contains: True
      debug/marker/shadow: False

I am sure I have overlooked something but I have no idea where the issue comes from.

Cannot build robot_body_filter in ros-melodic ubuntu18.04

Hi,

It seems that robot_body_filter cannot be built via catkin build robot_body_filter in ros-meodic linux ubunt18.04 - something wrong about the const argument, qualifiers and -fpermissive. Please see stack trace below. Thank you.

In file included from src/robot_body_filter/include/robot_body_filter/RobotBodyFilter.h:16:0,
from /src/robot_body_filter/src/RobotBodyFilter.cpp:12:
src/robot_body_filter/include/robot_body_filter/utils/filter_utils.hpp: In instantiation of ‘T robot_body_filter::FilterBase::getParamVerbose(const string&, const T&, const string&, bool*, robot_body_filter::FilterBase::ToStringFn) const [with T = bool; F = sensor_msgs::LaserScan_<std::allocator >; std::__cxx11::string = std::__cxx11::basic_string; robot_body_filter::FilterBase::ToStringFn = std::_cxx11::basic_string (*)(const bool&)]’:
src/robot_body_filter/src/RobotBodyFilter.cpp:389:79: required from here
src/robot_body_filter/include/robot_body_filter/utils/filter_utils.hpp:77:41: error: passing ‘const robot_body_filter::FilterBase<sensor_msgs::LaserScan<std::allocator > >’ as ‘this’ argument discards qualifiers [-fpermissive]
if (filters::FilterBase::getParam(name, value))
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~

Warnings for unset parameters

Hey,
we recently switched back to your upstream version. Thanks for all the improvements regarding per link padding/scaling.

I noticed that each unset parameter is printed as a warning, e.g.

[ WARN] [1629207245.812202996]: robot_body_filter_containment: Cannot find value for parameter: bounding_sphere/publish_cut_out_pointcloud, assigning default: False

Since this is expected behavior and defaults are well documented, I don't see a reason for this to be a warning. This feels more like a debug message to me. We changed this in our fork tu-darmstadt-ros-pkg/robot_body_filter@2ca2a15. What do you think about this?

Best regards,
Martin

Reloading robot model

Hello, first of all thank you for this awesome package !

I had a question about reloading the robot model by publishing on the topic /dynamic_robot_model_server/parameter_updates.

I tried to use it a few times unsuccessfully, but putting into the 'robot_model' field the path to the urdf file but received the erro 'Error document empty.'.

Do I need to to put the path to the urdf file or the content of the urdf file into the field ?

Thank you for your help

Question: Shortest distance from point to robot manipulator?

Hey!
@peci1, first of all, thank you for the cool project! really appreciate it.
I am working towards workspace surveillance for industrial robots and I have a question:
With the robot_body_filter implementation present, is there a way to utilize it for implementing feature/function, which calculates the shortest distance of a point in the workspace to the robot? (already considering the kinematics and current joint angles)

Thank you and best regards,
Peter

Unable to remove shape handle

Hi, I tried to filter the robot from my simulated point cloud.
Here is my setup.
IsaacSim -> Topic -> filter -> RVIZ

Image

Topics from the simulator

bong@Bong:~/catkin_kinnect$ rostopic list
/Isaac/Depth_pcl # Input point cloud
/joint_states
/tf

my_launch.launch

<?xml version="1.0"?>
<launch>
    <include file="$(find robot_body_filter)/launch/franka_description.launch"/>

    <node name = "laser_filter" 
        pkg="sensor_filters" 
        type = "pointcloud2_filter_chain" 
        output = "screen">

        <rosparam command="load" file="$(find robot_body_filter)/launch/my_config.yaml" />
        <remap from="~input" to="/Isaac/Depth_pcl" />
        <remap from="~output" to="/Isaac/Depth_pcl_filtered_bong" />
    </node>
</launch>

panda_description.launch

<launch>
    <param name="franka_description" 
        textfile="$(find robot_body_filter)/Panda/panda.urdf"/> 
</launch>

my_config.yaml

cloud_filter_chain:
-   name: body_filter
    type: robot_body_filter/RobotBodyFilterPointCloud2
    params:
        frames/filtering: 'panda_link0'
        frames/output: 'world'
        sensor/min_distance: 0.05
        sensor/max_distance: 50.0
        body_model/inflation/scale: 1.1
        body_model/inflation/padding: 0.02
        filter/do_shadow_test: False
        body_model/robot_description_param: "franka_description"

command
$ roslaunch robot_body_filter bong_filter.launch

output
'''
SUMMARY

PARAMETERS

• /franka_description: <?xml version="1....
• /laser_filter/cloud_filter_chain: [{'name': 'body_f...
• /rosdistro: noetic
• /rosversion: 1.16.0

NODES
/
laser_filter (sensor_filters/pointcloud2_filter_chain)

ROS_MASTER_URI=http://localhost:11311

process[laser_filter-1]: started with pid [190082]
[ INFO] [1696841160.032105490]: body_filter: Parameter sensor/point_by_point not defined, assigning default: False
[ INFO] [1696841160.032654530]: body_filter: Parameter transforms/buffer_length not defined, assigning default: 60.000000 s
[ INFO] [1696841160.035163346]: body_filter: Parameter frames/fixed not defined, assigning default: base_link
[ INFO] [1696841160.035194406]: body_filter: Parameter frames/sensor not defined, assigning default:
[ INFO] [1696841160.035215213]: body_filter: Found parameter: frames/filtering, value: panda_link0
[ INFO] [1696841160.035235122]: body_filter: Found parameter: sensor/min_distance, value: 0.050000 m
[ INFO] [1696841160.035245575]: body_filter: Found parameter: sensor/max_distance, value: 50.000000 m
[ INFO] [1696841160.035257429]: body_filter: Found parameter: body_model/robot_description_param, value: franka_description
[ INFO] [1696841160.035268249]: body_filter: Parameter filter/keep_clouds_organized not defined, assigning default: True
[ INFO] [1696841160.035281483]: body_filter: Parameter filter/model_pose_update_interval not defined, assigning default: 0.000000 s
[ INFO] [1696841160.035291174]: body_filter: Parameter filter/do_clipping not defined, assigning default: True
[ INFO] [1696841160.035300561]: body_filter: Parameter filter/do_contains_test not defined, assigning default: True
[ INFO] [1696841160.035311135]: body_filter: Found parameter: filter/do_shadow_test, value: False
[ INFO] [1696841160.035320957]: body_filter: Parameter filter/max_shadow_distance not defined, assigning default: 50.000000 m
[ INFO] [1696841160.035330713]: body_filter: Parameter transforms/timeout/reachable not defined, assigning default: 0.100000 s
[ INFO] [1696841160.035340255]: body_filter: Parameter transforms/timeout/unreachable not defined, assigning default: 0.200000 s
[ INFO] [1696841160.035349261]: body_filter: Parameter transforms/require_all_reachable not defined, assigning default: False
[ INFO] [1696841160.035358272]: body_filter: Parameter bounding_sphere/publish_cut_out_pointcloud not defined, assigning default: False
[ INFO] [1696841160.035367396]: body_filter: Parameter bounding_box/publish_cut_out_pointcloud not defined, assigning default: False
[ INFO] [1696841160.035376599]: body_filter: Parameter oriented_bounding_box/publish_cut_out_pointcloud not defined, assigning default: False
[ INFO] [1696841160.035385675]: body_filter: Parameter local_bounding_box/publish_cut_out_pointcloud not defined, assigning default: False
[ INFO] [1696841160.035394693]: body_filter: Parameter bounding_sphere/compute not defined, assigning default: False
[ INFO] [1696841160.035403605]: body_filter: Parameter bounding_box/compute not defined, assigning default: False
[ INFO] [1696841160.035412584]: body_filter: Parameter oriented_bounding_box/compute not defined, assigning default: False
[ INFO] [1696841160.035421650]: body_filter: Parameter local_bounding_box/compute not defined, assigning default: False
[ INFO] [1696841160.035430456]: body_filter: Parameter bounding_sphere/debug not defined, assigning default: False
[ INFO] [1696841160.035439341]: body_filter: Parameter bounding_box/debug not defined, assigning default: False
[ INFO] [1696841160.035448430]: body_filter: Parameter oriented_bounding_box/debug not defined, assigning default: False
[ INFO] [1696841160.035457332]: body_filter: Parameter local_bounding_box/debug not defined, assigning default: False
[ INFO] [1696841160.035466156]: body_filter: Parameter bounding_sphere/marker not defined, assigning default: False
[ INFO] [1696841160.035474798]: body_filter: Parameter bounding_box/marker not defined, assigning default: False
[ INFO] [1696841160.035483629]: body_filter: Parameter oriented_bounding_box/marker not defined, assigning default: False
[ INFO] [1696841160.035492304]: body_filter: Parameter local_bounding_box/marker not defined, assigning default: False
[ INFO] [1696841160.035501465]: body_filter: Parameter local_bounding_box/frame_id not defined, assigning default: base_link
[ INFO] [1696841160.035510483]: body_filter: Parameter debug/pcl/inside not defined, assigning default: False
[ INFO] [1696841160.035519315]: body_filter: Parameter debug/pcl/clip not defined, assigning default: False
[ INFO] [1696841160.035528038]: body_filter: Parameter debug/pcl/shadow not defined, assigning default: False
[ INFO] [1696841160.035536825]: body_filter: Parameter debug/marker/contains not defined, assigning default: False
[ INFO] [1696841160.035545541]: body_filter: Parameter debug/marker/shadow not defined, assigning default: False
[ INFO] [1696841160.035554253]: body_filter: Parameter debug/marker/bounding_sphere not defined, assigning default: False
[ INFO] [1696841160.035562972]: body_filter: Parameter debug/marker/bounding_box not defined, assigning default: False
[ INFO] [1696841160.035572048]: body_filter: Found parameter: body_model/inflation/padding, value: 0.020000 m
[ INFO] [1696841160.035581013]: body_filter: Found parameter: body_model/inflation/scale, value: 1.100000
[ INFO] [1696841160.035590318]: body_filter: Parameter body_model/inflation/contains_test/padding not defined, assigning default: 0.020000 m
[ INFO] [1696841160.035599664]: body_filter: Parameter body_model/inflation/contains_test/scale not defined, assigning default: 1.100000
[ INFO] [1696841160.035609157]: body_filter: Parameter body_model/inflation/shadow_test/padding not defined, assigning default: 0.020000 m
[ INFO] [1696841160.035619039]: body_filter: Parameter body_model/inflation/shadow_test/scale not defined, assigning default: 1.100000
[ INFO] [1696841160.035628316]: body_filter: Parameter body_model/inflation/bounding_sphere/padding not defined, assigning default: 0.020000 m
[ INFO] [1696841160.035637643]: body_filter: Parameter body_model/inflation/bounding_sphere/scale not defined, assigning default: 1.100000
[ INFO] [1696841160.035646865]: body_filter: Parameter body_model/inflation/bounding_box/padding not defined, assigning default: 0.020000 m
[ INFO] [1696841160.035656148]: body_filter: Parameter body_model/inflation/bounding_box/scale not defined, assigning default: 1.100000
[ INFO] [1696841160.035711795]: body_filter: Parameter body_model/inflation/per_link/padding not defined, assigning default: {} m
[ INFO] [1696841160.035734865]: body_filter: Parameter body_model/inflation/per_link/scale not defined, assigning default: {}
[ INFO] [1696841160.035797128]: body_filter: Parameter ignored_links/bounding_sphere not defined, assigning default: []
[ INFO] [1696841160.035848156]: body_filter: Parameter ignored_links/bounding_box not defined, assigning default: []
[ INFO] [1696841160.035859205]: body_filter: Parameter ignored_links/contains_test not defined, assigning default: []
[ INFO] [1696841160.035913096]: body_filter: Parameter ignored_links/shadow_test not defined, assigning default: ["laser"]
[ INFO] [1696841160.035948658]: body_filter: Parameter ignored_links/everywhere not defined, assigning default: []
[ INFO] [1696841160.035959238]: body_filter: Parameter only_links not defined, assigning default: []
[ INFO] [1696841160.035969795]: body_filter: Parameter body_model/dynamic_robot_description/field_name not defined, assigning default: robot_model
[ INFO] [1696841160.424100000]: RobotBodyFilter: Successfully configured.
[ INFO] [1696841160.424144266]: Filtering data in frame panda_link0
[ INFO] [1696841160.424152895]: RobotBodyFilter: Filtering into the following categories:
[ INFO] [1696841160.424160504]: RobotBodyFilter: OUTSIDE
[ INFO] [1696841160.424169895]: RobotBodyFilter: CLIP
[ INFO] [1696841160.424178009]: RobotBodyFilter: INSIDE
[ INFO] [1696841160.424192773]: RobotBodyFilter: Filtering applied to all links.
[ INFO] [1696841160.424212166]: body_filter: Found parameter: frames/output, value: world
[ INFO] [1696841160.424227347]: body_filter: Parameter cloud/point_channels not defined, assigning default: ["vp_"]
[ INFO] [1696841160.424238427]: body_filter: Parameter cloud/direction_channels not defined, assigning default: ["normal_"]
[ INFO] [1696841160.424297291]: Configured filter chain of type sensor_msgs/PointCloud2 from namespace /laser_filter/cloud_filter_chain
[ WARN] [1696841160.576454380]: RobotBodyFilter: Old TF data received. Clearing TF buffer and reconfiguring laser filter. If you're replaying a bag file, make sure rosparam /use_sim_time is set to true
[ INFO] [1696841160.576495627]: body_filter: Parameter sensor/point_by_point not defined, assigning default: False
[ INFO] [1696841160.576516577]: body_filter: Parameter transforms/buffer_length not defined, assigning default: 60.000000 s
[ INFO] [1696841160.576539651]: body_filter: Parameter frames/fixed not defined, assigning default: base_link
[ INFO] [1696841160.576550873]: body_filter: Parameter frames/sensor not defined, assigning default:
[ INFO] [1696841160.576561644]: body_filter: Found parameter: frames/filtering, value: panda_link0
[ INFO] [1696841160.576573145]: body_filter: Found parameter: sensor/min_distance, value: 0.050000 m
[ INFO] [1696841160.576583281]: body_filter: Found parameter: sensor/max_distance, value: 50.000000 m
[ INFO] [1696841160.576593163]: body_filter: Found parameter: body_model/robot_description_param, value: franka_description
[ INFO] [1696841160.576604308]: body_filter: Parameter filter/keep_clouds_organized not defined, assigning default: True
[ INFO] [1696841160.576615768]: body_filter: Parameter filter/model_pose_update_interval not defined, assigning default: 0.000000 s
[ INFO] [1696841160.576626513]: body_filter: Parameter filter/do_clipping not defined, assigning default: True
[ INFO] [1696841160.576638044]: body_filter: Parameter filter/do_contains_test not defined, assigning default: True
[ INFO] [1696841160.576653591]: body_filter: Found parameter: filter/do_shadow_test, value: False
[ INFO] [1696841160.576668059]: body_filter: Parameter filter/max_shadow_distance not defined, assigning default: 50.000000 m
[ INFO] [1696841160.576680794]: body_filter: Parameter transforms/timeout/reachable not defined, assigning default: 0.100000 s
[ INFO] [1696841160.576694144]: body_filter: Parameter transforms/timeout/unreachable not defined, assigning default: 0.200000 s
[ INFO] [1696841160.576706825]: body_filter: Parameter transforms/require_all_reachable not defined, assigning default: False
[ INFO] [1696841160.576719375]: body_filter: Parameter bounding_sphere/publish_cut_out_pointcloud not defined, assigning default: False
[ INFO] [1696841160.576731442]: body_filter: Parameter bounding_box/publish_cut_out_pointcloud not defined, assigning default: False
[ INFO] [1696841160.576744147]: body_filter: Parameter oriented_bounding_box/publish_cut_out_pointcloud not defined, assigning default: False
[ INFO] [1696841160.576757132]: body_filter: Parameter local_bounding_box/publish_cut_out_pointcloud not defined, assigning default: False
[ INFO] [1696841160.576768636]: body_filter: Parameter bounding_sphere/compute not defined, assigning default: False
[ INFO] [1696841160.576780467]: body_filter: Parameter bounding_box/compute not defined, assigning default: False
[ INFO] [1696841160.576793408]: body_filter: Parameter oriented_bounding_box/compute not defined, assigning default: False
[ INFO] [1696841160.576805676]: body_filter: Parameter local_bounding_box/compute not defined, assigning default: False
[ INFO] [1696841160.576817073]: body_filter: Parameter bounding_sphere/debug not defined, assigning default: False
[ INFO] [1696841160.576829061]: body_filter: Parameter bounding_box/debug not defined, assigning default: False
[ INFO] [1696841160.576841898]: body_filter: Parameter oriented_bounding_box/debug not defined, assigning default: False
[ INFO] [1696841160.576853863]: body_filter: Parameter local_bounding_box/debug not defined, assigning default: False
[ INFO] [1696841160.576865707]: body_filter: Parameter bounding_sphere/marker not defined, assigning default: False
[ INFO] [1696841160.576879140]: body_filter: Parameter bounding_box/marker not defined, assigning default: False
[ INFO] [1696841160.576888557]: body_filter: Parameter oriented_bounding_box/marker not defined, assigning default: False
[ INFO] [1696841160.576895451]: body_filter: Parameter local_bounding_box/marker not defined, assigning default: False
[ INFO] [1696841160.576902579]: body_filter: Parameter local_bounding_box/frame_id not defined, assigning default: base_link
[ INFO] [1696841160.576909095]: body_filter: Parameter debug/pcl/inside not defined, assigning default: False
[ INFO] [1696841160.576915352]: body_filter: Parameter debug/pcl/clip not defined, assigning default: False
[ INFO] [1696841160.576921727]: body_filter: Parameter debug/pcl/shadow not defined, assigning default: False
[ INFO] [1696841160.576927592]: body_filter: Parameter debug/marker/contains not defined, assigning default: False
[ INFO] [1696841160.576934275]: body_filter: Parameter debug/marker/shadow not defined, assigning default: False
[ INFO] [1696841160.576940600]: body_filter: Parameter debug/marker/bounding_sphere not defined, assigning default: False
[ INFO] [1696841160.576946780]: body_filter: Parameter debug/marker/bounding_box not defined, assigning default: False
[ INFO] [1696841160.576954525]: body_filter: Found parameter: body_model/inflation/padding, value: 0.020000 m
[ INFO] [1696841160.576961083]: body_filter: Found parameter: body_model/inflation/scale, value: 1.100000
[ INFO] [1696841160.576968340]: body_filter: Parameter body_model/inflation/contains_test/padding not defined, assigning default: 0.020000 m
[ INFO] [1696841160.576976462]: body_filter: Parameter body_model/inflation/contains_test/scale not defined, assigning default: 1.100000
[ INFO] [1696841160.576983717]: body_filter: Parameter body_model/inflation/shadow_test/padding not defined, assigning default: 0.020000 m
[ INFO] [1696841160.576991645]: body_filter: Parameter body_model/inflation/shadow_test/scale not defined, assigning default: 1.100000
[ INFO] [1696841160.576999353]: body_filter: Parameter body_model/inflation/bounding_sphere/padding not defined, assigning default: 0.020000 m
[ INFO] [1696841160.577011893]: body_filter: Parameter body_model/inflation/bounding_sphere/scale not defined, assigning default: 1.100000
[ INFO] [1696841160.577024531]: body_filter: Parameter body_model/inflation/bounding_box/padding not defined, assigning default: 0.020000 m
[ INFO] [1696841160.577038060]: body_filter: Parameter body_model/inflation/bounding_box/scale not defined, assigning default: 1.100000
[ INFO] [1696841160.577058979]: body_filter: Parameter body_model/inflation/per_link/padding not defined, assigning default: {} m
[ INFO] [1696841160.577076487]: body_filter: Parameter body_model/inflation/per_link/scale not defined, assigning default: {}
[ INFO] [1696841160.577097718]: body_filter: Parameter ignored_links/bounding_sphere not defined, assigning default: []
[ INFO] [1696841160.577114336]: body_filter: Parameter ignored_links/bounding_box not defined, assigning default: []
[ INFO] [1696841160.577121999]: body_filter: Parameter ignored_links/contains_test not defined, assigning default: []
[ INFO] [1696841160.577139810]: body_filter: Parameter ignored_links/shadow_test not defined, assigning default: ["laser"]
[ INFO] [1696841160.577154102]: body_filter: Parameter ignored_links/everywhere not defined, assigning default: []
[ INFO] [1696841160.577162741]: body_filter: Parameter only_links not defined, assigning default: []
[ INFO] [1696841160.577170823]: body_filter: Parameter body_model/dynamic_robot_description/field_name not defined, assigning default: robot_model
[ERROR] [1696841160.578391209]: Tried to advertise a service that is already advertised in this node [/laser_filter/reload_model]
[ERROR] [1696841160.579650175]: Unable to remove shape handle 1
[ERROR] [1696841160.579756266]: Unable to remove shape handle 2
[ERROR] [1696841160.579774046]: Unable to remove shape handle 3
[ERROR] [1696841160.579784502]: Unable to remove shape handle 4
[ERROR] [1696841160.579794978]: Unable to remove shape handle 5
[ERROR] [1696841160.579805400]: Unable to remove shape handle 6
[ERROR] [1696841160.579816417]: Unable to remove shape handle 7
[ERROR] [1696841160.579827515]: Unable to remove shape handle 8
[ERROR] [1696841160.579838438]: Unable to remove shape handle 9
[ERROR] [1696841160.579849765]: Unable to remove shape handle 10
[ERROR] [1696841160.579860980]: Unable to remove shape handle 11
[ INFO] [1696841160.998669953]: RobotBodyFilter: Successfully configured.
[ INFO] [1696841160.998690817]: Filtering data in frame panda_link0
[ INFO] [1696841160.998696080]: RobotBodyFilter: Filtering into the following categories:
[ INFO] [1696841160.998700673]: RobotBodyFilter: OUTSIDE
[ INFO] [1696841160.998706212]: RobotBodyFilter: CLIP
[ INFO] [1696841160.998710752]: RobotBodyFilter: INSIDE
[ INFO] [1696841160.998715908]: RobotBodyFilter: Filtering applied to all links.
[ INFO] [1696841160.998729091]: body_filter: Found parameter: frames/output, value: world
[ INFO] [1696841160.998742088]: body_filter: Parameter cloud/point_channels not defined, assigning default: ["vp_"]
[ INFO] [1696841160.998750767]: body_filter: Parameter cloud/direction_channels not defined, assigning default: ["normal_"]
[ERROR] [1696841160.998781178]: Filtering data from time 257.183346746 failed.
[ WARN] [1696841160.999983595]: RobotBodyFilter: Old TF data received. Clearing TF buffer and reconfiguring laser filter. If you're replaying a bag file, make sure rosparam /use_sim_time is set to true
'''

question

1. At the 1st iteration of the process, there is only warning message:
   [ WARN] [1696841160.576454380]: RobotBodyFilter: Old TF data received. Clearing TF buffer and reconfiguring laser filter. If you're replaying a bag file, make sure rosparam /use_sim_time is set to true

2. After the 1st iteration, the filter doesn't work.

[ERROR] [1696841160.578391209]: Tried to advertise a service that is already advertised in this node [/laser_filter/reload_model]
[ERROR] [1696841160.579650175]: Unable to remove shape handle 1
[ERROR] [1696841160.579756266]: Unable to remove shape handle 2
[ERROR] [1696841160.579774046]: Unable to remove shape handle 3
[ERROR] [1696841160.579784502]: Unable to remove shape handle 4
[ERROR] [1696841160.579794978]: Unable to remove shape handle 5
[ERROR] [1696841160.579805400]: Unable to remove shape handle 6
[ERROR] [1696841160.579816417]: Unable to remove shape handle 7
[ERROR] [1696841160.579827515]: Unable to remove shape handle 8
[ERROR] [1696841160.579838438]: Unable to remove shape handle 9
[ERROR] [1696841160.579849765]: Unable to remove shape handle 10
[ERROR] [1696841160.579860980]: Unable to remove shape handle 11
[ INFO] [1696841160.998669953]: RobotBodyFilter: Successfully configured.
[ INFO] [1696841160.998690817]: Filtering data in frame panda_link0
[ INFO] [1696841160.998696080]: RobotBodyFilter: Filtering into the following categories:
[ INFO] [1696841160.998700673]: RobotBodyFilter: 	OUTSIDE
[ INFO] [1696841160.998706212]: RobotBodyFilter: 	CLIP
[ INFO] [1696841160.998710752]: RobotBodyFilter: 	INSIDE
[ INFO] [1696841160.998715908]: RobotBodyFilter: Filtering applied to all links.
[ INFO] [1696841160.998729091]: body_filter: Found parameter: frames/output, value: world
[ INFO] [1696841160.998742088]: body_filter: Parameter cloud/point_channels not defined, assigning default: ["vp_"]
[ INFO] [1696841160.998750767]: body_filter: Parameter cloud/direction_channels not defined, assigning default: ["normal_"]
[ERROR] [1696841160.998781178]: Filtering data from time 257.183346746 failed.

Could you give me some advice? I tried to find the error message "Unable to remove shape handle," but I have no idea how to handle it.

Thanks!

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

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 http://gnu.org/licenses/gpl.html
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:
http://www.gnu.org/software/gdb/bugs/.
Find the GDB manual and other documentation resources online at:
http://www.gnu.org/software/gdb/documentation/.

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<double, 3, 1, 0> 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<double, 3, 1, 0> 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_<std::allocator > const&, std::vector<robot_body_filter::RayCastingShapeMask::MaskValue, std::allocator<robot_body_filter::RayCastingShapeMask::MaskValue> >&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&) ()
from /home/fxm/projects/default/robot_ws/devel/lib/libRayCastingShapeMask.so
#3 0x00007ffff221506c in robot_body_filter::RobotBodyFilter<sensor_msgs::PointCloud2_<std::allocator > >::computeMask(sensor_msgs::PointCloud2_<std::allocator > const&, std::vector<robot_body_filter::RayCastingShapeMask::MaskValue, std::allocator<robot_body_filter::RayCastingShapeMask::MaskValue> >&, std::cxx11::basic_string<char, std::char_traits, 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<std::allocator > const&, sensor_msgs::PointCloud2<std::allocator >&) () from /home/fxm/projects/default/robot_ws/devel/lib//librobot_body_filter.so
#5 0x000055555557fd04 in sensor_filters::FilterChainBase<sensor_msgs::PointCloud2_<std::allocator > >::filter(sensor_msgs::PointCloud2_<std::allocator > const&, sensor_msgs::PointCloud2_<std::allocator >&) ()
#6 0x000055555557db84 in sensor_filters::FilterChainBase<sensor_msgs::PointCloud2_<std::allocator > >::callbackReference(sensor_msgs::PointCloud2_<std::allocator > const&)
()
#7 0x000055555558d434 in ros::SubscriptionCallbackHelperT<sensor_msgs::PointCloud2_<std::allocator > const&, void>::call(ros::SubscriptionCallbackHelperCallParams&) ()

</details>

TF frames watchdog reachability check breaks our self-filter

First of all, thank you for releasing your improved self-filter!
However, the recently introduced reachability check in the latest commit (ee0c87f#diff-6b286a62ec6b7077dda6d9857ec66a37R609) does not work four our setup as the sensor frame is never reachable according to the TF frames watchdog.
This might be caused by our robot setup. We have a continuously rotating tilted VLP16 lidar and the lidar driver transforms all point clouds into a virtual fixed link located above the spinning axle.
Therefore, our self filter config does not need to transform the point clouds during the filtering pipeline as it is already in the preferred frame.
Also, the usage of a virtual fixed link might break the reachability check of the TF frames watchdog causing our issue of rejected scans.
Do you know, how we can fix our filtering pipeline? Or is it possible to check the necessity of a transform before querying the TF frames watchdog?

Why should the user set the sensor_frame

First of all thanks for this filtering component!

I have a filter chain set-up with incoming messages from different sensors with different frames. The filter component requires that I set a frame_id on forehand, why? Why not just use the header.frame_id to transform the incoming message?

ROS2 plans

Any plan to do a ROS2 port of this package ?

Seg fault when calling configure for robot_body_filter/RobotBodyFilterPointCloud2

Hi,

I'm just wondering if anyone else has had issues configuring RobotBodyFilterPointCloud2 with a filter chain. My system seems to be segfault after calling configure(). I am trying to use almost exactly the the setup used in the example https://github.com/peci1/robot_body_filter/blob/master/examples/tradr_realsense_d435.yaml

Cheers.

process has died during runtime,please help

can you help me,please

SUMMARY

PARAMETERS
 * /depth_camera_kinectv2/cloud_filter_chain: [{'name': 'robot_...
 * /rosdistro: noetic
 * /rosversion: 1.16.0

NODES
  /
    depth_camera_kinectv2 (sensor_filters/pointcloud2_filter_chain)

ROS_MASTER_URI=http://localhost:11311

process[depth_camera_kinectv2-1]: started with pid [284937]
[ INFO] [1705838045.059808596]: robot_body_filter_containment: Parameter sensor/point_by_point not defined, assigning default: False
[ INFO] [1705838045.060513597]: robot_body_filter_containment: Parameter transforms/buffer_length not defined, assigning default: 60.000000 s
[ INFO] [1705838045.064103973]: robot_body_filter_containment: Parameter frames/fixed not defined, assigning default: base_link
[ INFO] [1705838045.064126842]: robot_body_filter_containment: Parameter frames/sensor not defined, assigning default: 
[ INFO] [1705838045.064143821]: robot_body_filter_containment: Found parameter: frames/filtering, value: base
[ INFO] [1705838045.064158689]: robot_body_filter_containment: Found parameter: sensor/min_distance, value: 0.150000 m
[ INFO] [1705838045.064169151]: robot_body_filter_containment: Found parameter: sensor/max_distance, value: 0.000000 m
[ INFO] [1705838045.064180200]: robot_body_filter_containment: Found parameter: body_model/robot_description_param, value: /robot_description2
[ INFO] [1705838045.064189932]: robot_body_filter_containment: Parameter filter/keep_clouds_organized not defined, assigning default: True
[ INFO] [1705838045.064203803]: robot_body_filter_containment: Parameter filter/model_pose_update_interval not defined, assigning default: 0.000000 s
[ INFO] [1705838045.064214794]: robot_body_filter_containment: Found parameter: filter/do_clipping, value: True
[ INFO] [1705838045.064222966]: robot_body_filter_containment: Found parameter: filter/do_contains_test, value: True
[ INFO] [1705838045.064230394]: robot_body_filter_containment: Found parameter: filter/do_shadow_test, value: False
[ INFO] [1705838045.064238683]: robot_body_filter_containment: Parameter filter/max_shadow_distance not defined, assigning default: 0.000000 m
[ INFO] [1705838045.064247800]: robot_body_filter_containment: Parameter transforms/timeout/reachable not defined, assigning default: 0.100000 s
[ INFO] [1705838045.064256734]: robot_body_filter_containment: Parameter transforms/timeout/unreachable not defined, assigning default: 0.200000 s
[ INFO] [1705838045.064266187]: robot_body_filter_containment: Parameter transforms/require_all_reachable not defined, assigning default: False
[ INFO] [1705838045.064274673]: robot_body_filter_containment: Parameter bounding_sphere/publish_cut_out_pointcloud not defined, assigning default: False
[ INFO] [1705838045.064282768]: robot_body_filter_containment: Parameter bounding_box/publish_cut_out_pointcloud not defined, assigning default: False
[ INFO] [1705838045.064291187]: robot_body_filter_containment: Parameter oriented_bounding_box/publish_cut_out_pointcloud not defined, assigning default: False
[ INFO] [1705838045.064299364]: robot_body_filter_containment: Parameter local_bounding_box/publish_cut_out_pointcloud not defined, assigning default: False
[ INFO] [1705838045.064307976]: robot_body_filter_containment: Parameter bounding_sphere/compute not defined, assigning default: False
[ INFO] [1705838045.064315853]: robot_body_filter_containment: Parameter bounding_box/compute not defined, assigning default: False
[ INFO] [1705838045.064323880]: robot_body_filter_containment: Parameter oriented_bounding_box/compute not defined, assigning default: False
[ INFO] [1705838045.064331596]: robot_body_filter_containment: Parameter local_bounding_box/compute not defined, assigning default: False
[ INFO] [1705838045.064339478]: robot_body_filter_containment: Parameter bounding_sphere/debug not defined, assigning default: False
[ INFO] [1705838045.064347139]: robot_body_filter_containment: Parameter bounding_box/debug not defined, assigning default: False
[ INFO] [1705838045.064355032]: robot_body_filter_containment: Parameter oriented_bounding_box/debug not defined, assigning default: False
[ INFO] [1705838045.064362842]: robot_body_filter_containment: Parameter local_bounding_box/debug not defined, assigning default: False
[ INFO] [1705838045.064370459]: robot_body_filter_containment: Parameter bounding_sphere/marker not defined, assigning default: False
[ INFO] [1705838045.064380057]: robot_body_filter_containment: Parameter bounding_box/marker not defined, assigning default: False
[ INFO] [1705838045.064392414]: robot_body_filter_containment: Parameter oriented_bounding_box/marker not defined, assigning default: False
[ INFO] [1705838045.064404617]: robot_body_filter_containment: Parameter local_bounding_box/marker not defined, assigning default: False
[ INFO] [1705838045.064413249]: robot_body_filter_containment: Parameter local_bounding_box/frame_id not defined, assigning default: base_link
[ INFO] [1705838045.064421556]: robot_body_filter_containment: Parameter debug/pcl/inside not defined, assigning default: False
[ INFO] [1705838045.064429377]: robot_body_filter_containment: Found parameter: debug/pcl/clip, value: True
[ INFO] [1705838045.064437224]: robot_body_filter_containment: Parameter debug/pcl/shadow not defined, assigning default: False
[ INFO] [1705838045.064445243]: robot_body_filter_containment: Parameter debug/marker/contains not defined, assigning default: False
[ INFO] [1705838045.064453039]: robot_body_filter_containment: Parameter debug/marker/shadow not defined, assigning default: False
[ INFO] [1705838045.064460906]: robot_body_filter_containment: Parameter debug/marker/bounding_sphere not defined, assigning default: False
[ INFO] [1705838045.064468623]: robot_body_filter_containment: Parameter debug/marker/bounding_box not defined, assigning default: False
[ INFO] [1705838045.064477801]: robot_body_filter_containment: Found parameter: body_model/inflation/padding, value: 0.030000 m
[ INFO] [1705838045.064486021]: robot_body_filter_containment: Found parameter: body_model/inflation/scale, value: 1.000000
[ INFO] [1705838045.064494511]: robot_body_filter_containment: Parameter body_model/inflation/contains_test/padding not defined, assigning default: 0.030000 m
[ INFO] [1705838045.064503124]: robot_body_filter_containment: Parameter body_model/inflation/contains_test/scale not defined, assigning default: 1.000000
[ INFO] [1705838045.064511650]: robot_body_filter_containment: Parameter body_model/inflation/shadow_test/padding not defined, assigning default: 0.030000 m
[ INFO] [1705838045.064520103]: robot_body_filter_containment: Parameter body_model/inflation/shadow_test/scale not defined, assigning default: 1.000000
[ INFO] [1705838045.064529779]: robot_body_filter_containment: Parameter body_model/inflation/bounding_sphere/padding not defined, assigning default: 0.030000 m
[ INFO] [1705838045.064538323]: robot_body_filter_containment: Parameter body_model/inflation/bounding_sphere/scale not defined, assigning default: 1.000000
[ INFO] [1705838045.064546783]: robot_body_filter_containment: Parameter body_model/inflation/bounding_box/padding not defined, assigning default: 0.030000 m
[ INFO] [1705838045.064555202]: robot_body_filter_containment: Parameter body_model/inflation/bounding_box/scale not defined, assigning default: 1.000000
[ INFO] [1705838045.064593742]: robot_body_filter_containment: Parameter body_model/inflation/per_link/padding not defined, assigning default: {} m
[ INFO] [1705838045.064611321]: robot_body_filter_containment: Parameter body_model/inflation/per_link/scale not defined, assigning default: {}
[ INFO] [1705838045.064657104]: robot_body_filter_containment: Parameter ignored_links/bounding_sphere not defined, assigning default: []
[ INFO] [1705838045.064710117]: robot_body_filter_containment: Parameter ignored_links/bounding_box not defined, assigning default: []
[ INFO] [1705838045.064721962]: robot_body_filter_containment: Parameter ignored_links/contains_test not defined, assigning default: []
[ INFO] [1705838045.064765326]: robot_body_filter_containment: Parameter ignored_links/shadow_test not defined, assigning default: ["laser"]
[ INFO] [1705838045.064800306]: robot_body_filter_containment: Parameter ignored_links/everywhere not defined, assigning default: []
[ INFO] [1705838045.064810675]: robot_body_filter_containment: Parameter only_links not defined, assigning default: []
[ INFO] [1705838045.064820570]: robot_body_filter_containment: Parameter body_model/dynamic_robot_description/field_name not defined, assigning default: robot_model
[ INFO] [1705838045.891410713]: RobotBodyFilter: Successfully configured.
[ INFO] [1705838045.891445910]: Filtering data in frame base
[ INFO] [1705838045.891469159]: RobotBodyFilter: Filtering into the following categories:
[ INFO] [1705838045.891494905]: RobotBodyFilter: 	OUTSIDE
[ INFO] [1705838045.891511857]: RobotBodyFilter: 	CLIP
[ INFO] [1705838045.891531219]: RobotBodyFilter: 	INSIDE
[ INFO] [1705838045.891549388]: RobotBodyFilter: Filtering applied to all links.
[ INFO] [1705838045.891580658]: robot_body_filter_containment: Found parameter: frames/output, value: base
[ INFO] [1705838045.891608983]: robot_body_filter_containment: Parameter cloud/point_channels not defined, assigning default: ["vp_"]
[ INFO] [1705838045.891638246]: robot_body_filter_containment: Parameter cloud/direction_channels not defined, assigning default: ["normal_"]
[ INFO] [1705838045.891708809]: Configured filter chain of type sensor_msgs/PointCloud2 from namespace /depth_camera_kinectv2/cloud_filter_chain
[ERROR] [1705838046.077211582]: Filtering data from time 1705838045.705044565 failed.
[ WARN] [1705838047.093697189]: RobotBodyFilter: The pointcloud is dense, which usually means it was captured each point at a different time instant. Consider setting 'point_by_point_scan' to true to get a more accurate version.
[ INFO] [1705838047.093762718]: RobotBodyFilter: Transforming cloud from frame kinect2_ir_optical_frame to base
[depth_camera_kinectv2-1] process has died [pid 284937, exit code -11, cmd /opt/ros/noetic/lib/sensor_filters/pointcloud2_filter_chain ~input:=/number_count ~output:=velodyne_points_filtered __name:=depth_camera_kinectv2 __log:=/home/ted/.ros/log/288a1e94-b84c-11ee-a211-2fb15fb040b4/depth_camera_kinectv2-1.log].
log file: /home/ted/.ros/log/288a1e94-b84c-11ee-a211-2fb15fb040b4/depth_camera_kinectv2-1*.log
all processes on machine have died, roslaunch will exit
shutting down processing monitor...
... shutting down processing monitor complete
done

lsb_release -a

No LSB modules are available. Distributor ID: Ubuntu Description: Ubuntu 20.04.6 LTS Release: 20.04 Codename: focal

uname -a

Linux ted 5.15.0-91-generic #101~20.04.1-Ubuntu SMP Thu Nov 16 14:22:28 UTC 2023 x86_64 x86_64 x86_64 GNU/Linux

gdb

(gdb) bt

#0  0x00007ffff20a234e in _mm256_store_pd (__A=..., __P=0x555555ba93d0) at /usr/lib/gcc/x86_64-linux-gnu/7/include/avxintrin.h:868
#1  Eigen::internal::pstore<double, double __vector(4)>(double*, double __vector(4) const&) (to=0x555555ba93d0, from=...) at /usr/include/eigen3/Eigen/src/Core/arch/AVX/PacketMath.h:252
#2  0x00007ffff214582f in Eigen::internal::pstoret<double, double __vector(4), 32>(double*, double __vector(4) const&) (from=..., to=0x555555ba93d0) at /usr/include/eigen3/Eigen/src/Core/GenericPacketMath.h:474
#3  Eigen::internal::assign_op<double, double>::assignPacket<32, double __vector(4)>(double*, double __vector(4) const&) const (this=0x7fffffffb3f7, a=0x555555ba93d0, b=...)
    at /usr/include/eigen3/Eigen/src/Core/functors/AssignmentFunctors.h:28
#4  0x00007ffff214609e in Eigen::internal::generic_dense_assignment_kernel<Eigen::internal::evaluator<Eigen::Matrix<double, 4, 4, 0, 4, 4> >, Eigen::internal::evaluator<Eigen::Matrix<double, 4, 4, 0, 4, 4> >, Eigen::internal::assign_op<double, double>, 0>::assignPacket<32, 32, double __vector(4)>(long, long) (this=0x7fffffffb2f0, row=0, col=0) at /usr/include/eigen3/Eigen/src/Core/AssignEvaluator.h:652
#5  0x00007ffff2141941 in Eigen::internal::generic_dense_assignment_kernel<Eigen::internal::evaluator<Eigen::Matrix<double, 4, 4, 0, 4, 4> >, Eigen::internal::evaluator<Eigen::Matrix<double, 4, 4, 0, 4, 4> >, Eigen::internal::assign_op<double, double>, 0>::assignPacketByOuterInner<32, 32, double __vector(4)>(long, long) (this=0x7fffffffb2f0, outer=0, inner=0) at /usr/include/eigen3/Eigen/src/Core/AssignEvaluator.h:666
#6  0x00007ffff213c3d6 in Eigen::internal::copy_using_evaluator_innervec_CompleteUnrolling<Eigen::internal::generic_dense_assignment_kernel<Eigen::internal::evaluator<Eigen::Matrix<double, 4, 4, 0, 4, 4> >, Eigen::internal::evaluator<Eigen::Matrix<double, 4, 4, 0, 4, 4> >, Eigen::internal::assign_op<double, double>, 0>, 0, 16>::run (kernel=...) at /usr/include/eigen3/Eigen/src/Core/AssignEvaluator.h:274
#7  0x00007ffff21361b4 in Eigen::internal::dense_assignment_loop<Eigen::internal::generic_dense_assignment_kernel<Eigen::internal::evaluator<Eigen::Matrix<double, 4, 4, 0, 4, 4> >, Eigen::internal::evaluator<Eigen::Matrix<double, 4, 4, 0, 4, 4> >, Eigen::internal::assign_op<double, double>, 0>, 2, 2>::run (kernel=...) at /usr/include/eigen3/Eigen/src/Core/AssignEvaluator.h:468
#8  0x00007ffff212f598 in Eigen::internal::call_dense_assignment_loop<Eigen::Matrix<double, 4, 4, 0, 4, 4>, Eigen::Matrix<double, 4, 4, 0, 4, 4>, Eigen::internal::assign_op<double, double> > (dst=..., src=..., 
    func=...) at /usr/include/eigen3/Eigen/src/Core/AssignEvaluator.h:741
#9  0x00007ffff21272b6 in Eigen::internal::Assignment<Eigen::Matrix<double, 4, 4, 0, 4, 4>, Eigen::Matrix<double, 4, 4, 0, 4, 4>, Eigen::internal::assign_op<double, double>, Eigen::internal::Dense2Dense, void>::run
    (dst=..., src=..., func=...) at /usr/include/eigen3/Eigen/src/Core/AssignEvaluator.h:879
#10 0x00007ffff211deb1 in Eigen::internal::call_assignment_no_alias<Eigen::Matrix<double, 4, 4, 0, 4, 4>, Eigen::Matrix<double, 4, 4, 0, 4, 4>, Eigen::internal::assign_op<double, double> > (dst=..., src=..., 
    func=...) at /usr/include/eigen3/Eigen/src/Core/AssignEvaluator.h:836
#11 0x00007ffff2110dc9 in Eigen::internal::call_assignment<Eigen::Matrix<double, 4, 4, 0, 4, 4>, Eigen::Matrix<double, 4, 4, 0, 4, 4>, Eigen::internal::assign_op<double, double> >(Eigen::Matrix<double, 4, 4, 0, 4, 4>&, Eigen::Matrix<double, 4, 4, 0, 4, 4> const&, Eigen::internal::assign_op<double, double> const&, Eigen::internal::enable_if<!Eigen::internal::evaluator_assume_aliasing<Eigen::Matrix<double, 4, 4, 0, 4, 4>, Eigen::internal::evaluator_traits<Eigen::Matrix<double, 4, 4, 0, 4, 4> >::Shape>::value, void*>::type) (dst=..., src=..., func=...) at /usr/include/eigen3/Eigen/src/Core/AssignEvaluator.h:804
#12 0x00007ffff2102e87 in Eigen::internal::call_assignment<Eigen::Matrix<double, 4, 4, 0, 4, 4>, Eigen::Matrix<double, 4, 4, 0, 4, 4> > (dst=..., src=...) at /usr/include/eigen3/Eigen/src/Core/AssignEvaluator.h:782
#13 0x00007ffff20eb475 in Eigen::PlainObjectBase<Eigen::Matrix<double, 4, 4, 0, 4, 4> >::_set<Eigen::Matrix<double, 4, 4, 0, 4, 4> > (this=0x555555ba93d0, other=...)
    at /usr/include/eigen3/Eigen/src/Core/PlainObjectBase.h:714
#14 0x00007ffff20c7b25 in Eigen::Matrix<double, 4, 4, 0, 4, 4>::operator= (this=0x555555ba93d0, other=...) at /usr/include/eigen3/Eigen/src/Core/Matrix.h:208
#15 0x00007ffff20a9a27 in Eigen::Transform<double, 3, 1, 0>::operator= (this=0x555555ba93d0, other=...) at /usr/include/eigen3/Eigen/src/Geometry/Transform.h:286
#16 0x00007ffff1d8990b in bodies::Body::setPoseDirty (this=0x555555ba93b0, pose=...) at /opt/ros/noetic/include/geometric_shapes/bodies.h:169
#17 0x00007ffff1d89931 in bodies::Body::setPose (this=0x555555ba93b0, pose=...) at /opt/ros/noetic/include/geometric_shapes/bodies.h:175
#18 0x00007ffff1d85f92 in robot_body_filter::RayCastingShapeMask::updateBodyPosesNoLock (this=0x555555b84570) at /home/ted/catkin_ws/src/robot_body_filter/src/RayCastingShapeMask.cpp:140
#19 0x00007ffff1d8711a in robot_body_filter::RayCastingShapeMask::maskContainmentAndShadows (this=0x555555b84570, data=..., mask=std::vector of length 1995, capacity 1995 = {...}, sensorPos=...)
    at /home/ted/catkin_ws/src/robot_body_filter/src/RayCastingShapeMask.cpp:232
#20 0x00007ffff20c34f2 in robot_body_filter::RobotBodyFilter<sensor_msgs::PointCloud2_<std::allocator<void> > >::computeMask (this=0x555555b7cee0, projectedPointCloud=..., 
    pointMask=std::vector of length 1995, capacity 1995 = {...}, sensorFrame="kinect2_ir_optical_frame") at /home/ted/catkin_ws/src/robot_body_filter/src/RobotBodyFilter.cpp:445
#21 0x00007ffff209f538 in robot_body_filter::RobotBodyFilterPointCloud2::update (this=0x555555b7cee0, inputCloud=..., filteredCloud=...) at /home/ted/catkin_ws/src/robot_body_filter/src/RobotBodyFilter.cpp:813
#22 0x0000555555575084 in ?? ()
#23 0x0000555555578897 in ?? ()
#24 0x00007ffff1c8c9cf in ?? () from /opt/ros/noetic/lib//libpoint_cloud_transport_plugins.so
#25 0x00007ffff1c80dd2 in ?? () from /opt/ros/noetic/lib//libpoint_cloud_transport_plugins.so
#26 0x00007ffff1c89728 in ?? () from /opt/ros/noetic/lib//libpoint_cloud_transport_plugins.so
#27 0x00007ffff7e5b139 in ros::SubscriptionQueue::call() () from /opt/ros/noetic/lib/libroscpp.so
#28 0x00007ffff7e09172 in ros::CallbackQueue::callOneCB(ros::CallbackQueue::TLS*) () from /opt/ros/noetic/lib/libroscpp.so
#29 0x00007ffff7e0a883 in ros::CallbackQueue::callAvailable(ros::WallDuration) () from /opt/ros/noetic/lib/libroscpp.so
#30 0x00007ffff7e5dfcf in ros::SingleThreadedSpinner::spin(ros::CallbackQueue*) () from /opt/ros/noetic/lib/libroscpp.so
#31 0x00007ffff7e4621f in ros::spin() () from /opt/ros/noetic/lib/libroscpp.so
#32 0x000055555558b5a1 in ?? ()
#33 0x0000555555568c31 in ?? ()
#34 0x00007ffff7834083 in __libc_start_main (main=0x555555568bf0, argc=5, argv=0x7fffffffd518, init=<optimized out>, fini=<optimized out>, rtld_fini=<optimized out>, stack_end=0x7fffffffd508)
    at ../csu/libc-start.c:308
#35 0x0000555555568dae in ?? ()

launch file & yaml

<launch>
    <!-- Prerequisite: sensor_filters -->
    <!-- http://wiki.ros.org/sensor_filters -->
    <node name="depth_camera_kinectv2" pkg="sensor_filters" type="pointcloud2_filter_chain" output="screen">
        <rosparam command="load" file="$(dirname)/ur3_wth_nerian.yaml" />
        <remap from="~input" to="/number_count" />
        <remap from="~output" to="velodyne_points_filtered" />
        
    </node>
</launch>

cloud_filter_chain:
  # Only containment filtering
  - name: robot_body_filter_containment
    type: robot_body_filter/RobotBodyFilterPointCloud2
    params:
      frames/filtering: "base"
      frames/output: "base"
      sensor/min_distance: 0.15
      sensor/max_distance: 0
      body_model/inflation/scale: 1.0
      body_model/inflation/padding: 0.03
      body_model/robot_description_param: /robot_description2
      filter/do_clipping: true
      filter/do_contains_test: true
      filter/do_shadow_test: false
      debug/pcl/clip: true

my tf tree

and i compiled it from source

after rebuild the geometric_shapes from source still corrupt

SUMMARY
========

PARAMETERS
 * /depth_camera_kinectv2/cloud_filter_chain: [{'name': 'robot_...
 * /rosdistro: noetic
 * /rosversion: 1.16.0

NODES
  /
    depth_camera_kinectv2 (sensor_filters/pointcloud2_filter_chain)

ROS_MASTER_URI=http://localhost:11311

process[depth_camera_kinectv2-1]: started with pid [37568]
[ INFO] [1705890713.326696573]: robot_body_filter_containment: Parameter sensor/point_by_point not defined, assigning default: False
[ INFO] [1705890713.327758450]: robot_body_filter_containment: Parameter transforms/buffer_length not defined, assigning default: 60.000000 s
[ INFO] [1705890713.332238416]: robot_body_filter_containment: Parameter frames/fixed not defined, assigning default: base_link
[ INFO] [1705890713.332270276]: robot_body_filter_containment: Parameter frames/sensor not defined, assigning default: 
[ INFO] [1705890713.332299105]: robot_body_filter_containment: Found parameter: frames/filtering, value: base
[ INFO] [1705890713.332327008]: robot_body_filter_containment: Found parameter: sensor/min_distance, value: 0.150000 m
[ INFO] [1705890713.332352033]: robot_body_filter_containment: Found parameter: sensor/max_distance, value: 0.000000 m
[ INFO] [1705890713.332372992]: robot_body_filter_containment: Found parameter: body_model/robot_description_param, value: /robot_description2
[ INFO] [1705890713.332393416]: robot_body_filter_containment: Parameter filter/keep_clouds_organized not defined, assigning default: True
[ INFO] [1705890713.332419454]: robot_body_filter_containment: Parameter filter/model_pose_update_interval not defined, assigning default: 0.000000 s
[ INFO] [1705890713.332444413]: robot_body_filter_containment: Found parameter: filter/do_clipping, value: True
[ INFO] [1705890713.332463977]: robot_body_filter_containment: Found parameter: filter/do_contains_test, value: True
[ INFO] [1705890713.332484225]: robot_body_filter_containment: Found parameter: filter/do_shadow_test, value: False
[ INFO] [1705890713.332506751]: robot_body_filter_containment: Parameter filter/max_shadow_distance not defined, assigning default: 0.000000 m
[ INFO] [1705890713.332529477]: robot_body_filter_containment: Parameter transforms/timeout/reachable not defined, assigning default: 0.100000 s
[ INFO] [1705890713.332553371]: robot_body_filter_containment: Parameter transforms/timeout/unreachable not defined, assigning default: 0.200000 s
[ INFO] [1705890713.332572037]: robot_body_filter_containment: Parameter transforms/require_all_reachable not defined, assigning default: False
[ INFO] [1705890713.332595799]: robot_body_filter_containment: Parameter bounding_sphere/publish_cut_out_pointcloud not defined, assigning default: False
[ INFO] [1705890713.332617685]: robot_body_filter_containment: Parameter bounding_box/publish_cut_out_pointcloud not defined, assigning default: False
[ INFO] [1705890713.332636753]: robot_body_filter_containment: Parameter oriented_bounding_box/publish_cut_out_pointcloud not defined, assigning default: False
[ INFO] [1705890713.332660105]: robot_body_filter_containment: Parameter local_bounding_box/publish_cut_out_pointcloud not defined, assigning default: False
[ INFO] [1705890713.332681932]: robot_body_filter_containment: Parameter bounding_sphere/compute not defined, assigning default: False
[ INFO] [1705890713.332700905]: robot_body_filter_containment: Parameter bounding_box/compute not defined, assigning default: False
[ INFO] [1705890713.332723935]: robot_body_filter_containment: Parameter oriented_bounding_box/compute not defined, assigning default: False
[ INFO] [1705890713.332746536]: robot_body_filter_containment: Parameter local_bounding_box/compute not defined, assigning default: False
[ INFO] [1705890713.332765413]: robot_body_filter_containment: Parameter bounding_sphere/debug not defined, assigning default: False
[ INFO] [1705890713.332793986]: robot_body_filter_containment: Parameter bounding_box/debug not defined, assigning default: False
[ INFO] [1705890713.332815843]: robot_body_filter_containment: Parameter oriented_bounding_box/debug not defined, assigning default: False
[ INFO] [1705890713.332836823]: robot_body_filter_containment: Parameter local_bounding_box/debug not defined, assigning default: False
[ INFO] [1705890713.332858077]: robot_body_filter_containment: Parameter bounding_sphere/marker not defined, assigning default: False
[ INFO] [1705890713.332878557]: robot_body_filter_containment: Parameter bounding_box/marker not defined, assigning default: False
[ INFO] [1705890713.332898175]: robot_body_filter_containment: Parameter oriented_bounding_box/marker not defined, assigning default: False
[ INFO] [1705890713.332920883]: robot_body_filter_containment: Parameter local_bounding_box/marker not defined, assigning default: False
[ INFO] [1705890713.332939873]: robot_body_filter_containment: Parameter local_bounding_box/frame_id not defined, assigning default: base_link
[ INFO] [1705890713.332962529]: robot_body_filter_containment: Parameter debug/pcl/inside not defined, assigning default: False
[ INFO] [1705890713.332980183]: robot_body_filter_containment: Found parameter: debug/pcl/clip, value: True
[ INFO] [1705890713.333001996]: robot_body_filter_containment: Parameter debug/pcl/shadow not defined, assigning default: False
[ INFO] [1705890713.333018783]: robot_body_filter_containment: Parameter debug/marker/contains not defined, assigning default: False
[ INFO] [1705890713.333040822]: robot_body_filter_containment: Parameter debug/marker/shadow not defined, assigning default: False
[ INFO] [1705890713.333057390]: robot_body_filter_containment: Parameter debug/marker/bounding_sphere not defined, assigning default: False
[ INFO] [1705890713.333079492]: robot_body_filter_containment: Parameter debug/marker/bounding_box not defined, assigning default: False
[ INFO] [1705890713.333100985]: robot_body_filter_containment: Found parameter: body_model/inflation/padding, value: 0.030000 m
[ INFO] [1705890713.333122525]: robot_body_filter_containment: Found parameter: body_model/inflation/scale, value: 1.000000
[ INFO] [1705890713.333145238]: robot_body_filter_containment: Parameter body_model/inflation/contains_test/padding not defined, assigning default: 0.030000 m
[ INFO] [1705890713.333166561]: robot_body_filter_containment: Parameter body_model/inflation/contains_test/scale not defined, assigning default: 1.000000
[ INFO] [1705890713.333188896]: robot_body_filter_containment: Parameter body_model/inflation/shadow_test/padding not defined, assigning default: 0.030000 m
[ INFO] [1705890713.333210039]: robot_body_filter_containment: Parameter body_model/inflation/shadow_test/scale not defined, assigning default: 1.000000
[ INFO] [1705890713.333233289]: robot_body_filter_containment: Parameter body_model/inflation/bounding_sphere/padding not defined, assigning default: 0.030000 m
[ INFO] [1705890713.333255277]: robot_body_filter_containment: Parameter body_model/inflation/bounding_sphere/scale not defined, assigning default: 1.000000
[ INFO] [1705890713.333277705]: robot_body_filter_containment: Parameter body_model/inflation/bounding_box/padding not defined, assigning default: 0.030000 m
[ INFO] [1705890713.333296827]: robot_body_filter_containment: Parameter body_model/inflation/bounding_box/scale not defined, assigning default: 1.000000
[ INFO] [1705890713.333357104]: robot_body_filter_containment: Parameter body_model/inflation/per_link/padding not defined, assigning default: {} m
[ INFO] [1705890713.333393660]: robot_body_filter_containment: Parameter body_model/inflation/per_link/scale not defined, assigning default: {}
[ INFO] [1705890713.333450924]: robot_body_filter_containment: Parameter ignored_links/bounding_sphere not defined, assigning default: []
[ INFO] [1705890713.333523811]: robot_body_filter_containment: Parameter ignored_links/bounding_box not defined, assigning default: []
[ INFO] [1705890713.333553116]: robot_body_filter_containment: Parameter ignored_links/contains_test not defined, assigning default: []
[ INFO] [1705890713.333614747]: robot_body_filter_containment: Parameter ignored_links/shadow_test not defined, assigning default: ["laser"]
[ INFO] [1705890713.333671806]: robot_body_filter_containment: Parameter ignored_links/everywhere not defined, assigning default: []
[ INFO] [1705890713.333698626]: robot_body_filter_containment: Parameter only_links not defined, assigning default: []
[ INFO] [1705890713.333720407]: robot_body_filter_containment: Parameter body_model/dynamic_robot_description/field_name not defined, assigning default: robot_model
[depth_camera_kinectv2-1] process has died [pid 37568, exit code -11, cmd /opt/ros/noetic/lib/sensor_filters/pointcloud2_filter_chain ~input:=/number_count ~output:=velodyne_points_filtered __name:=depth_camera_kinectv2 __log:=/home/ted/.ros/log/1f563560-b8ce-11ee-924f-61a77cf05a9a/depth_camera_kinectv2-1.log].
log file: /home/ted/.ros/log/1f563560-b8ce-11ee-924f-61a77cf05a9a/depth_camera_kinectv2-1*.log
all processes on machine have died, roslaunch will exit
shutting down processing monitor...
... shutting down processing monitor complete
done

(gdb) bt

#0  0x00007ffff20a234e in _mm256_store_pd (__A=..., __P=0x7fffffffb4d0) at /usr/lib/gcc/x86_64-linux-gnu/7/include/avxintrin.h:868
#1  Eigen::internal::pstore<double, double __vector(4)>(double*, double __vector(4) const&) (to=0x7fffffffb4d0, from=...) at /usr/include/eigen3/Eigen/src/Core/arch/AVX/PacketMath.h:252
#2  0x00007ffff214582f in Eigen::internal::pstoret<double, double __vector(4), 32>(double*, double __vector(4) const&) (from=..., to=0x7fffffffb4d0) at /usr/include/eigen3/Eigen/src/Core/GenericPacketMath.h:474
#3  Eigen::internal::assign_op<double, double>::assignPacket<32, double __vector(4)>(double*, double __vector(4) const&) const (this=0x7fffffffb43f, a=0x7fffffffb4d0, b=...) at /usr/include/eigen3/Eigen/src/Core/functors/AssignmentFunctors.h:28
#4  0x00007ffff214609e in Eigen::internal::generic_dense_assignment_kernel<Eigen::internal::evaluator<Eigen::Matrix<double, 4, 4, 0, 4, 4> >, Eigen::internal::evaluator<Eigen::Matrix<double, 4, 4, 0, 4, 4> >, Eigen::internal::assign_op<double, double>, 0>::assignPacket<32, 32, double __vector(4)>(long, long) (
    this=0x7fffffffb480, row=0, col=0) at /usr/include/eigen3/Eigen/src/Core/AssignEvaluator.h:652
#5  0x00007ffff2141941 in Eigen::internal::generic_dense_assignment_kernel<Eigen::internal::evaluator<Eigen::Matrix<double, 4, 4, 0, 4, 4> >, Eigen::internal::evaluator<Eigen::Matrix<double, 4, 4, 0, 4, 4> >, Eigen::internal::assign_op<double, double>, 0>::assignPacketByOuterInner<32, 32, double __vector(4)>(long, long) (this=0x7fffffffb480, outer=0, inner=0) at /usr/include/eigen3/Eigen/src/Core/AssignEvaluator.h:666
#6  0x00007ffff213c3d6 in Eigen::internal::copy_using_evaluator_innervec_CompleteUnrolling<Eigen::internal::generic_dense_assignment_kernel<Eigen::internal::evaluator<Eigen::Matrix<double, 4, 4, 0, 4, 4> >, Eigen::internal::evaluator<Eigen::Matrix<double, 4, 4, 0, 4, 4> >, Eigen::internal::assign_op<double, double>, 0>, 0, 16>::run (kernel=...) at /usr/include/eigen3/Eigen/src/Core/AssignEvaluator.h:274
#7  0x00007ffff1d01e3e in bodies::ConvexMesh::updateInternalData() () from /home/ted/catkin_ws/devel/lib/libgeometric_shapes.so.0.7.5
#8  0x00007ffff129d5b9 in point_containment_filter::ShapeMask::addShape (this=0x555555ba93b0, shape=std::shared_ptr<const shapes::Shape> (use count 1, weak count 0) = {...}, scale=1, padding=0.029999999999999999) at ./point_containment_filter/src/shape_mask.cpp:78
#9  0x00007ffff1d87b62 in robot_body_filter::RayCastingShapeMask::addShape (this=0x555555ba93b0, shape=std::shared_ptr<const shapes::Shape> (use count 1, weak count 0) = {...}, containsScale=1, containsPadding=0.029999999999999999, shadowScale=1, shadowPadding=0.029999999999999999, bsphereScale=1, 
    bspherePadding=0.029999999999999999, bboxScale=1, bboxPadding=0.029999999999999999, updateInternalStructures=false, name="base_link_inertia::0") at /home/ted/catkin_ws/src/robot_body_filter/src/RayCastingShapeMask.cpp:356
#10 0x00007ffff20d8bf5 in robot_body_filter::RobotBodyFilter<sensor_msgs::PointCloud2_<std::allocator<void> > >::addRobotMaskFromUrdf (this=0x555555ba1ba0, 
    urdfModel="<?xml version=\"1.0\" ?>\n<!-- ", '=' <repeats 83 times>, " -->\n<!-- |    This document was autogenerated by xacro from ur3.xacro", ' ' <repeats 19 times>...) at /home/ted/catkin_ws/src/robot_body_filter/src/RobotBodyFilter.cpp:1032
#11 0x00007ffff20bc59a in robot_body_filter::RobotBodyFilter<sensor_msgs::PointCloud2_<std::allocator<void> > >::configure (this=0x555555ba1ba0) at /home/ted/catkin_ws/src/robot_body_filter/src/RobotBodyFilter.cpp:353
#12 0x00007ffff209b678 in robot_body_filter::RobotBodyFilterPointCloud2::configure (this=0x555555ba1ba0) at /home/ted/catkin_ws/src/robot_body_filter/src/RobotBodyFilter.cpp:393
#13 0x000055555558200a in ?? ()
#14 0x0000555555582b4a in ?? ()
#15 0x0000555555582ea2 in ?? ()
#16 0x000055555558d217 in ?? ()
#17 0x000055555558b54c in ?? ()
#18 0x0000555555568c31 in ?? ()
#19 0x00007ffff7834083 in __libc_start_main (main=0x555555568bf0, argc=5, argv=0x7fffffffd468, init=<optimized out>, fini=<optimized out>, rtld_fini=<optimized out>, stack_end=0x7fffffffd458) at ../csu/libc-start.c:308
#20 0x0000555555568dae in ?? ()

[Noetic] error: redefinition of ‘class bodies::OBB’

The Noetic build of this package appears to have been broken after the last geometric_shapes release. It looks like the bodies::OBB class was copied upstream, but now that causes an error in this package.

moveit/geometric_shapes@ef7aa6a

https://build.ros.org/job/Nbin_uF64__robot_body_filter__ubuntu_focal_amd64__binary/98/console

23:13:36 [  2%] Building CXX object CMakeFiles/robot_body_filter_utils.dir/src/utils/bodies.cpp.o
23:13:36 /usr/lib/ccache/c++  -DROSCONSOLE_BACKEND_LOG4CXX -DROS_BUILD_SHARED_LIBS=1 -DROS_PACKAGE_NAME=\"robot_body_filter\" -Drobot_body_filter_utils_EXPORTS -I/tmp/binarydeb/ros-noetic-robot-body-filter-1.2.2/obj-x86_64-linux-gnu/devel/include -I/tmp/binarydeb/ros-noetic-robot-body-filter-1.2.2/include -I/opt/ros/noetic/share/xmlrpcpp/cmake/../../../include/xmlrpcpp -I/usr/include/bullet -I/usr/include/vtk-7.1 -I/usr/include/freetype2 -isystem /opt/ros/noetic/include -isystem /usr/include/eigen3 -isystem /usr/include/pcl-1.10  -g -O2 -fdebug-prefix-map=/tmp/binarydeb/ros-noetic-robot-body-filter-1.2.2=. -fstack-protector-strong -Wformat -Werror=format-security -DNDEBUG -Wdate-time -D_FORTIFY_SOURCE=2 -fPIC   -DROBOT_BODY_FILTER_USE_CXX_OPTIONAL=1 -std=c++17 -o CMakeFiles/robot_body_filter_utils.dir/src/utils/bodies.cpp.o -c /tmp/binarydeb/ros-noetic-robot-body-filter-1.2.2/src/utils/bodies.cpp
23:13:36 In file included from /tmp/binarydeb/ros-noetic-robot-body-filter-1.2.2/include/robot_body_filter/utils/bodies.h:7,
23:13:43                  from /tmp/binarydeb/ros-noetic-robot-body-filter-1.2.2/src/utils/bodies.cpp:10:
23:13:43 /tmp/binarydeb/ros-noetic-robot-body-filter-1.2.2/include/robot_body_filter/utils/obb.h:53:7: error: redefinition of ‘class bodies::OBB’
23:13:43    53 | class OBB
23:13:43       |       ^~~
23:13:43 In file included from /opt/ros/noetic/include/geometric_shapes/bodies.h:45,
23:13:43                  from /tmp/binarydeb/ros-noetic-robot-body-filter-1.2.2/src/utils/bodies.cpp:6:
23:13:43 /opt/ros/noetic/include/geometric_shapes/obb.h:52:7: note: previous definition of ‘class bodies::OBB’
23:13:43    52 | class OBB
23:13:43       |       ^~~

One possible fix would be to remove this typedef and use robot_body_filter::OBB everywhere. That would preserve ABI, but may break API for any downstream users of bodies::OBB expecting it to be the robot_body_filter class.