search

MISTLab / Swarm-SLAM

Sparse Decentralized Collaborative Simultaneous Localization and Mapping Framework for Multi-Robot Systems
https://lajoiepy.github.io/cslam_documentation/html/index.html
MIT License
391 stars 40 forks source link

experiment_oak-d_rgbd.launch remappings fail #40

Closed Junjie-Luo closed 4 months ago

Junjie-Luo commented 4 months ago

Hi ,friend !! I need some help ! I am looking forward to your help.

  1. This is my config:

Ubuntu20.04 python3.8 cuda12.0 oak-d

Problem: I found that the plugin used in the oak-d.aunch file that was called was wrong for my oak-d Camera, which was depthai_ros_driver::Camera instead of depthai_ros_driver::RGBDCamera. After the changes are made, the plugin is used to configure and start the oak-d camera. remappings under the location where I will call the plugin after the change is also correctly changed, and the remapped topic is not changed. I was able to run the camera driver normally, and the topics posted by the camera were normal in rviz color images and depth maps. However, the new topic that remaps the topic does not receive the information published by the topic. No output is generated when ros2 topic hz is used. What is the reason for this?

  1. remappings part show:

oak-d.launch.py("namespace"=/r0) ComposableNodeContainer( name="container", namespace="", package="rclcpp_components", executable="component_container", composable_node_descriptions=[

Driver itself

                ComposableNode(
                    package="depthai_ros_driver",
                    plugin="depthai_ros_driver::Camera",            #2024.4.17 之前为depthai_ros_driver::RGBDCamera，找不到？
                    name="camera",
                    parameters=[params_file],               #2024.4.18 之前为i_rgb_fps      ##2024.4.20 新增加入camera.yaml
                    remappings=[
                     **('rgb/image_raw', LaunchConfiguration('namespace').perform(context) + '/color/image_raw'),
                    ('rgb/camera_info', LaunchConfiguration('namespace').perform(context) + '/color/camera_info'),
                    ('stereo/image_raw', LaunchConfiguration('namespace').perform(context) + '/aligned_depth_to_color/image_raw'),**
                            ],
                ),
            ],
            output="screen",
        ),
    ]

rtabmap_rgbd_odometry.launch:("namespace"=/r0) parameters=[{ 'frame_id': LaunchConfiguration('namespace').perform(context)[1:] + '_link', 'subscribe_depth':True, 'approx_sync':True, # Set to True for OAK-D }]

remappings=[
      **('rgb/image', LaunchConfiguration('namespace').perform(context) + '/color/image_raw'),
      ('rgb/camera_info', LaunchConfiguration('namespace').perform(context) + '/color/camera_info'),
      ('depth/image', LaunchConfiguration('namespace').perform(context) + '/aligned_depth_to_color/image_raw'),
      ('odom', LaunchConfiguration('namespace').perform(context) + '/odom'),**    ]
  1. output show:

[INFO] [launch]: All log files can be found below /home/ljj/.ros/log/2024-04-20-21-01-24-722189-ljj-40099 [INFO] [launch]: Default logging verbosity is set to INFO [INFO] [loop_closure_detection_node.py-1]: process started with pid [40112] [INFO] [map_manager-2]: process started with pid [40114] [INFO] [pose_graph_manager-3]: process started with pid [40116] [INFO] [rgbd_odometry-4]: process started with pid [40118] [INFO] [static_transform_publisher-5]: process started with pid [40120] [INFO] [static_transform_publisher-6]: process started with pid [40122] [INFO] [static_transform_publisher-7]: process started with pid [40124] [INFO] [static_transform_publisher-8]: process started with pid [40126] [INFO] [component_container-9]: process started with pid [40128] [static_transform_publisher-5] [INFO] [1713618084.853001630] [base_to_color]: Spinning until killed publishing transform from 'r0_link' to 'r0_color_frame' [static_transform_publisher-6] [INFO] [1713618084.855658879] [base_to_color]: Spinning until killed publishing transform from 'r0_color_frame' to 'camera_color_frame' [pose_graph_manager-3] [INFO] [1713618084.858591698] [r0.cslam_pose_graph_manager]: Visualization enabled. [static_transform_publisher-7] [INFO] [1713618084.862502398] [base_to_left]: Spinning until killed publishing transform from 'r0_link' to 'r0_left_frame' [pose_graph_manager-3] [INFO] [1713618084.864046280] [r0.cslam_pose_graph_manager]: Initialization done. [static_transform_publisher-8] [INFO] [1713618084.865937279] [base_to_right]: Spinning until killed publishing transform from 'r0_link' to 'r0_right_frame' [component_container-9] [INFO] [1713618085.081964534] [container]: Load Library: /home/ljj/code/depthai-ros-foxy/install/depthai_ros_driver/lib/libdepthai_ros_driver.so [rgbd_odometry-4] [INFO] [1713618085.168076235] [rgbd_odometry]: Odometry: frame_id = r0_link [rgbd_odometry-4] [INFO] [1713618085.168402200] [rgbd_odometry]: Odometry: odom_frame_id = odom [rgbd_odometry-4] [INFO] [1713618085.168422208] [rgbd_odometry]: Odometry: publish_tf = true [rgbd_odometry-4] [INFO] [1713618085.168433403] [rgbd_odometry]: Odometry: wait_for_transform = 0.100000 [rgbd_odometry-4] [INFO] [1713618085.168455461] [rgbd_odometry]: Odometry: log_to_rosout_level = 4 [rgbd_odometry-4] [INFO] [1713618085.168487388] [rgbd_odometry]: Odometry: initial_pose = xyz=0.000000,0.000000,0.000000 rpy=0.000000,-0.000000,0.000000 [rgbd_odometry-4] [INFO] [1713618085.168502984] [rgbd_odometry]: Odometry: ground_truth_frame_id = [rgbd_odometry-4] [INFO] [1713618085.168513554] [rgbd_odometry]: Odometry: ground_truth_base_frame_id = r0_link [rgbd_odometry-4] [INFO] [1713618085.168523257] [rgbd_odometry]: Odometry: config_path = [rgbd_odometry-4] [INFO] [1713618085.168532919] [rgbd_odometry]: Odometry: publish_null_when_lost = true [rgbd_odometry-4] [INFO] [1713618085.168790428] [rgbd_odometry]: Odometry: guess_frame_id = [rgbd_odometry-4] [INFO] [1713618085.168805153] [rgbd_odometry]: Odometry: guess_min_translation = 0.000000 [rgbd_odometry-4] [INFO] [1713618085.168816978] [rgbd_odometry]: Odometry: guess_min_rotation = 0.000000 [rgbd_odometry-4] [INFO] [1713618085.168827381] [rgbd_odometry]: Odometry: guess_min_time = 0.000000 [rgbd_odometry-4] [INFO] [1713618085.168838000] [rgbd_odometry]: Odometry: expected_update_rate = 0.000000 Hz [rgbd_odometry-4] [INFO] [1713618085.168848435] [rgbd_odometry]: Odometry: max_update_rate = 0.000000 Hz [rgbd_odometry-4] [INFO] [1713618085.168858773] [rgbd_odometry]: Odometry: min_update_rate = 0.000000 Hz [rgbd_odometry-4] [INFO] [1713618085.168869046] [rgbd_odometry]: Odometry: wait_imu_to_init = false [rgbd_odometry-4] [INFO] [1713618085.168898138] [rgbdodometry]: Odometry: stereoParams=0 visParams=1 icpParams=0 [rgbd_odometry-4] [INFO] [1713618085.224323309] [rgbd_odometry]: RGBDOdometry: approx_sync = true [rgbd_odometry-4] [INFO] [1713618085.224370117] [rgbd_odometry]: RGBDOdometry: approx_sync_max_interval = 0.000000 [rgbd_odometry-4] [INFO] [1713618085.224389626] [rgbd_odometry]: RGBDOdometry: queue_size = 5 [rgbd_odometry-4] [INFO] [1713618085.224401553] [rgbd_odometry]: RGBDOdometry: qos = 0 [rgbd_odometry-4] [INFO] [1713618085.224412823] [rgbd_odometry]: RGBDOdometry: qos_camera_info = 0 [rgbd_odometry-4] [INFO] [1713618085.224428497] [rgbd_odometry]: RGBDOdometry: subscribe_rgbd = false [rgbd_odometry-4] [INFO] [1713618085.224439499] [rgbd_odometry]: RGBDOdometry: rgbd_cameras = 1 [rgbd_odometry-4] [INFO] [1713618085.224450030] [rgbd_odometry]: RGBDOdometry: keep_color = false [rgbd_odometry-4] [INFO] [1713618085.231137807] [rgbd_odometry]: [rgbd_odometry-4] rgbd_odometry subscribed to (approx sync): [rgbd_odometry-4] /r0/color/image_raw, [rgbd_odometry-4] /r0/aligned_depth_to_color/image_raw, [rgbd_odometry-4] /r0/color/camera_info [map_manager-2] [INFO] [1713618085.311563371] [r0.cslam_map_manager]: Initialization done. [component_container-9] [INFO] [1713618085.348194205] [container]: Found class: rclcpp_components::NodeFactoryTemplate [component_container-9] [INFO] [1713618085.348241723] [container]: Instantiate class: rclcpp_components::NodeFactoryTemplate [component_container-9] [INFO] [1713618085.356799401] [name]: No ip/mxid specified, connecting to the next available device. [loop_closure_detection_node.py-1] [INFO] [1713618086.779112278] [r0.cslam_loop_closure_detection]: Using CosPlace. (default) [loop_closure_detection_node.py-1] [INFO] [1713618086.945837074] [r0.cslam_loop_closure_detection]: loading checkpoint '/home/ljj/code/Swarm-SLAM/install/cslam/share/cslam/models/resnet18_64.pth' [loop_closure_detection_node.py-1] [INFO] [1713618087.158818133] [r0.cslam_loop_closure_detection]: Initialization done. [component_container-9] [INFO] [1713618087.783246126] [name]: Camera with MXID: 1944301041A3DE1200 and Name: 1.2 connected! [component_container-9] [INFO] [1713618087.784152307] [name]: USB SPEED: SUPER [component_container-9] [INFO] [1713618087.809134056] [name]: Device type: OAK-D [component_container-9] [INFO] [1713618087.813599740] [name]: Pipeline type: RGBD [component_container-9] [INFO] [1713618087.834437563] [name]: NN Family: mobilenet [component_container-9] [INFO] [1713618087.872087145] [name]: NN input size: 300 x 300. Resizing input image in case of different dimensions. [component_container-9] [INFO] [1713618087.893952926] [name]: Finished setting up pipeline. [component_container-9] [INFO] [1713618088.620749253] [name]: Camera ready! [INFO] [launch_ros.actions.load_composable_nodes]: Loaded node '/name' in container '/container' [rgbd_odometry-4] [WARN] [1713618090.240330166] [rgbd_odometry]: rgbd_odometry: Did not receive data since 5 seconds! Make sure the input topics are published ("$ rostopic hz my_topic") and the timestamps in their header are set. [rgbd_odometry-4] rgbd_odometry subscribed to (approx sync): [rgbd_odometry-4] /r0/color/image_raw, [rgbd_odometry-4] /r0/aligned_depth_to_color/image_raw, [rgbd_odometry-4] /r0/color/camera_info [rgbd_odometry-4] [WARN] [1713618091.475333208] [rgbd_odometry]: rgbd_odometry: Did not receive data since 5 seconds! Make sure the input topics are published ("$ rostopic hz my_topic") and the timestamps in their header are set. [rgbd_odometry-4] rgbd_odometry subscribed to (approx sync): [rgbd_odometry-4] /r0/color/image_raw, [rgbd_odometry-4] /r0/aligned_depth_to_color/image_raw, [rgbd_odometry-4] /r0/color/camera_info

lajoiepy commented 4 months ago

Hi! Thanks for your interest in Swarm-SLAM!

So your problem is with the odometry. Swarm-SLAM expects odometry as input, we provide basic RTAB-MAP scripts for convenience, but you can use any other odometry package. I struggled myself to get working/good odometry estimates using OAK-D cameras. That is why, in the end, I didn't do any experiments with those cameras.

TL;DR: you need to compute accurate odometry first with your OAK-D camera before running Swarm-SLAM. I suggest looking in that direction: https://docs.luxonis.com/en/latest/pages/slam_oak/