costmap_depth_camera

This is a costmap plugin for costmap_2d pkg

Considering ray casting method can not satisfy sparse 3D space problem of clearing. This plugin is based on kd-tree search to clear the markings.

This plugin comprises two parts:

1. Marking pointcloud and store it using std::map.
2. Clearing marked pointcloud using kd-tree search which is from Point Cloud Library (PCL).

Required package

navigation2_humble

Demo: Use realsense with the point cloud published

Launch Example

mkdir -p ~/ws/src
cd ~/ws/src
git clone -b humble git@github.com:tsengapola/costmap_depth_camera.git
cd ~/ws && source /opt/ros/humble/setup.bash
colcon build --symlink-install --cmake-arg -DCMAKE_BUILD_TYPE=Release
source install/setup.bash
ros2 launch costmap_depth_camera standalone_test_launch.py

Parameters Description (many of them are supported in dynamic reconfigure)

• use_global_frame_to_mark: true

  This is the new feature in ver 2.0.

See below left video (use_global_frame_to_mark: false), you can find that when robot is moving, the marking is moving as the local frame. The evidence shows that when the realsense turns from right to left, the right marking is moving with robot.

While in the right video (use_global_frame_to_mark: true). The evidence shows that when the realsense turns from right to left, the right marking is kept in its marked location.

I would suggest that to set use_global_frame_to_mark as true if you are good localization system.

• ec_seg_distance: 0.2
• ec_cluster_min_size: 5
• size_of_cluster_rejection: 5

Implement Euclidean Cluster Extraction. For ec_seg_distance, it is useful when some object is black and small, if it is connect to a bigger object, then size_of_cluster_rejection will not reject that small piece of black object.

For size_of_cluster_rejection, this is used to ignore small object in the space. It seems even though ec_cluster_min_size is set to 5, the Euclidean Cluster Extraction will still output the cluster small than 5 points. Therefore, we can use this function to ignore them (will not add in markings).

• voxel_resolution: 0.01

Usually, 1 cm voxel height is enough.

• number_clearing_threshold: 2
• check_radius: 0.1

A kd-tree radius search of a marked point will be used to find the neighbors, if number of neighbors is higher than number_clearing_threshold. The marked point will be cleared. For example, a marked point only has one neighbor within 0.1 meter. So the marked point will be cleared.

• forced_clearing_distance: 0.1

If a point appears within this distance, it will be cleared. The distance is calculated from the point to robot_base_frame. For example, a noise point appear in the robot footprint will be cleared.

• number_points_considered_as_blocked: 5
• enable_near_blocked_protection: true

If number of points in point cloud is less than ‘number_points_considered_as_blocked’, clearing mechanism is skip. For example, my robot can always see floor and thus the point cloud will always have points. If a new message arrive with few points, we simply assume the depth camera is blocked by something (mostly people’s hand, they like to wave their hands in front of robot). When the depth camera detects nothing, it will clear all markings in the frustum, by enabling this protect function, the false clearing can be prevented.

TF requirement

Make sure the sensor_frame in yaml file follows the setting (in general case of realsense is camera_link), i.e.: z axes points to sky, x axes points to moving direction. The orange points show the frustum of my camera.

Acknowledge

Many thanks to Uniring MAX - The leader of robotic floor scrubber