D455+T265 data not shown in ROS2 Foxy

[Yoshihito-M]

・Infos LibRealSense v2.50.0 ROS2 Foxy ROS2 Foxy Wrapper build 4.0.4 Ubuntu 20.04　LTS

・Ploblem Refer to following URL, I tried to make 3D map via D455 and T265. As you know, the following examples can be executed in ROS1. RGB-D Handheld Mapping

The commands I implemented to Terminal are shown as follows: ros2 launch realsense2_camera rs_d400_and_t265_launch.py ros2 launch rtabmap_ros rtabmap_launch.py args:="-d --Mem/UseOdomGravity true --Optimizer/GravitySigma 0.3" odom_topic:=/t265/odom/sample frame_id:=t265_link rgbd_sync:=true depth_topic:=/d400/aligned_depth_to_color/image_raw rgb_topic:=/d400/color/image_raw camera_info_topic:=/d400/color/camera_info approx_rgbd_sync:=false visual_odometry:=false

however, No 3D map is generated on RTABMAP. I would like to know how to fix these problem. Could you give some me advice?

[Dave-van-der-Meer]

I just had a similar issue that I described in #753. Could it be that you miss to enable the align_depth.enable parameter when launching the realsense launch file? Like with ros2 launch realsense2_camera rs_d400_and_t265_launch.py align_depth.enable:=true?

[matlabbe]

I just tried rs_d400_and_t265_launch.py (using ros2-beta branch, which seems the latest updated) and the aligned depth topic is not published by default. As @Dave-van-der-Meer mentionned, we can launch independently the camera nodes to set align_depth.enable:=true, however on my machine (latest realsense2 code), the topic /D400/aligned_depth_to_color/image_raw will flicker between the aligned and non-aligned depth, so the topic is not usable (issue reported here: https://github.com/IntelRealSense/realsense-ros/issues/2564).

To generate our own aligned depth image, we can use some rtabmap_ros nodes to do that for convenience:

ros2 launch realsense2_camera rs_d400_and_t265_launch.py

# point cloud from not aligned depth
ros2 run rtabmap_ros point_cloud_xyz --ros-args \
   --remap depth/image:=/D400/depth/image_rect_raw \
   --remap /depth/camera_info:=/D400/depth/camera_info \
   --remap cloud:=/D400/cloud_from_depth \
   --param approx_sync:=false

# aligned depth to color camera from the point cloud above
ros2 run rtabmap_ros pointcloud_to_depthimage --ros-args \
   --remap camera_info:=/D400/color/camera_info \
   --remap cloud:=/D400/cloud_from_depth \
   --remap image_raw:=/D400/aligned_depth_to_color/image_raw \
   --param decimation:=2 \
   --param fixed_frame_id:=D400_link \
   --param fill_holes_size:=1

# Static transform between the camera (should be calibrated!!!)
ros2 run tf2_ros static_transform_publisher 0 0 0 0 0 0 T265_link D400_link

ros2 launch rtabmap_ros rtabmap.launch.py \
   args:="-d --Mem/UseOdomGravity true --Optimizer/GravitySigma 0.3" \
   odom_topic:=/T265/pose/sample frame_id:=T265_link \
   rgbd_sync:=true \
   depth_topic:=/D400/aligned_depth_to_color/image_raw \
   rgb_topic:=/D400/color/image_raw \
   camera_info_topic:=/D400/color/camera_info \
   approx_rgbd_sync:=false \
   visual_odometry:=false \
   approx_sync:=true \
   queue_size:=20

Update: to avoid to manually align the depth image to color camera, use this patch: https://github.com/IntelRealSense/realsense-ros/issues/2564#issuecomment-1336288238