Timing issue PointCloud and Collision geometry

[marko-zaric]

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.

[peci1]

As you're running in Gazebo, can you try with slowed down simulation? Just to see whether the problem is due to performance or due to some actual time problems...

[marko-zaric]

As you're running in Gazebo, can you try with slowed down simulation? Just to see whether the problem is due to performance or due to some actual time problems...

Thank you for your answer! The real time factor already was below 0.3 in gazebo (30% of real time speed) but if I slow it down even further by a factor 10 it works. So you are right it is probably not a timing but a performance error. Do you have any suggestions on how to increase the performance in such a case?

[peci1]

I'd start with decreasing update rate of the camera and possibly also lowering its resolution...

[marko-zaric]

The point cloud is downsampled by a pcl voxelgrid filter to <5000 points and this is then feed into the self occlusion filter chain. The update rate is 30Hz the same as the physical RealSense D435 camera. I lowered the resolution to <1000 points and still had the same issue so I don't think these two parameters are the problem. Do you have any values to compare to my camera resolution or update rate?

[peci1]

You can see e.g. https://github.com/ctu-vras/rosdevday_cloud_filtering/blob/master/config/spot_filters.yaml for a configuration of filter that processes 1 3D lidar and 5 stereo depth cams. The depth cams were set to 10 FPS each and the filters were able to run realtime on my computer.

One of the big slowdowns is usually shadow filtering, which is, however, not really relevant for depth cameras. Make sure it is turned off in your filter config: filter/do_shadow_test: False.

[peci1]

Another thing - make sure robot_body_filter is built in Release or RelWithDebInfo build types. If you build it in Debug, it will be super slow.

[marko-zaric]

filter/do_shadow_test is set to false and I put a set(CMAKE_BUILD_TYPE Release) in the cmake file of the robot_body_filter to force release build even if I think my default build was release. Sadly there was still no improvement on the issue.

[peci1]

Okay. Time to look at your collision model. Are any of the <collision> elements meshes?

[marko-zaric]

Initially yes. But I read that this can cause performance issues so in order to check if the mesh is a problem I removed all the collision information for every link except for the one which is visible to the camera (see gif in issue beginning) and put a box as collision primitive:

<collision>
         <geometry>
          <box size="0.1 0.07 0.06"/>
        </geometry>
        <origin rpy="0 0 0" xyz="0.04 0.01 0.0"/>
</collision>

[peci1]

Good, so this should work.

Maybe a dumb question, but use_sim_time is set to true, right?

Otherwise, what happens if you step the simulation? Is the filter too late even when stepping and waiting long enough for an update?

Can you enable debug logging for the filter? That should print out how long does the filter spend in each part of the filtering pipeline...

[marko-zaric]

Good point I forget that sometimes but sadly use_sim_time was set to true so that was not the solution.

If I step through the simulation using Gazebo the issue does not occur! What could this mean?

How do I enable the debug logging for the filter? I can't seem to find it in the .yaml file.

[peci1]

I meant the standard ROS logging level. There is a service call set_logger_levels, but I never know how to use it. I usually open rqt_console, click the little geared wheel icon in its upper right corner, and try to find a logger name that should log what I want. For that logger, I set DEBUG level.

[marko-zaric]

Thanks for the clarification! So I think the filter is very fast each steps take less then 2ms sometimes even less then 1ms. So that does not seem to be the problem. I still feel like it could be some buffer timing error. Since stepping through the simulation does not cause any errors but when running the simulation and performing a dynamic movement in normal time the point cloud slightly trails the contains marker which creates the artifacts.

[peci1]

One possible source of problems could be if you had incorrect timestamps on either the joint positions and their TFs, or the camera image... But I guess at least in Gazebo this should not happen, right?