rosbagの記録
RealSenseの起動
imuを利用するため，追加の引数を指定する．
ros2 launch realsense2_camera rs_launch.py align_depth.enable:=true enable_gyro:=true enable_accel:=true unite_imu_method:=1 enable_sync:=true rgb_camera.color_profile:='640x480x30' depth_module.depth_profile:='640x480x30'
imu_filter_madwickの起動
RTAB-MAPはorientationも含めたimuの入力が必要である．
RealSenseから出力されるimuデータにはorientationがないので，このパッケージで計算して出力する
これにより/tfが取得でき，RTAB-MAPでimuのデータが利用できるようになる．
ros2 run imu_filter_madgwick imu_filter_madgwick_node --ros-args -p use_mag:=False -p publish_tf:=False -r imu/data_raw:=/camera/camera/imu
RTAB-MAPの起動
ros2 launch rtabmap_launch rtabmap.launch.py frame_id:=camera_link args:="-d" rgb_topic:=/camera/camera/color/image_raw depth_topic:=/camera/camera/aligned_depth_to_color/image_raw camera_info_topic:=/camera/camera/color/camera_info approx_sync:=true rviz:=true wait_imu_to_init:=true
rosbagのrecord
imuのデータはimu_filter_madwickからの出力のみでOK．
ros2 bag record /camera/camera/color/image_raw /camera/camera/color/camera_info /camera/camera/aligned_depth_to_color/image_raw /tf /tf_static /imu/data -s mcap -o <保存先のパス>
rosbagからRTAB-MAPの実行
マップの精度を上げるため，キューサイズの拡大と実行スピードを0.5倍速にした．
rosbagの再生
ros2 bag play <ファイルのパス> --clock --read-ahead-queue-size 2000 --rate 0.5
RTAB-MAPの実行
imuのqosポリシーが合わないとimuデータを利用できないため引数でポリシーを変更
ros2 launch rtabmap_launch rtabmap.launch.py frame_id:=camera_link args:="-d" rgb_topic:=/camera/camera/color/image_raw depth_topic:=/camera/camera/aligned_depth_to_color/image_raw camera_info_topic:=/camera/camera/color/camera_info imu_topic:=/imu/data approx_sync:=true qos:=2 use_sim_time:=true rviz:=true queue_size:=30
結果
rosbagから実行できました
参考