Closed Junjie-Luo closed 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/
Hi ,friend !! I need some help ! I am looking forward to your help.
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?
oak-d.launch.py("namespace"=/r0) ComposableNodeContainer( name="container", namespace="", package="rclcpp_components", executable="component_container", composable_node_descriptions=[
Driver itself
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 }]
[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