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
433 stars 44 forks source link

Issues for KITTI experiments #5

Closed HeTemp closed 1 year ago

HeTemp commented 1 year ago

Hello! First of all, thank you very much for sharing such excellent work！

I encountered some problems while using the KITTI dataset for experiments.During the runtime,/r0/cslam/keyframe_data topic has no information. I'm not sure if the issue is with the rosbag that I recorded. Do you have any advice on this matter?

The general information about the rosbag that I recorded is as follows:

I selected a small portion of the KITTI odometry color dataset(the first 1000 images from sequence 00) and converted them into a ROS2 bag using a script and ros_bridge.It has the following topic:

/clock
/r0/stereo_camera/left/camera_info
/r0/stereo_camera/left/image_rect_color
/r0/stereo_camera/right/camera_info
/r0/stereo_camera/right/image_rect_color
/rosout_agg

The messages of /r0/stereo_camera/left/image_rect_colorare as follows:

header:
  stamp:
    sec: 13
    nanosec: 479499999
  frame_id: camera
height: 376
width: 1241
encoding: rgb8
is_bigendian: 0
step: 1241
data:
- 119
- 74
- 28
- 127
- 81
- 55
- 131
- 140
- 64
-  '...'

The messages of /r0/stereo_camera/left/camera_infoare as follows:

layout:
  dim: []
  data_offset: 0
data:
- 718.8560180664062
- 0.0
- 607.1928100585938
- 45.38225173950195
- 0.0
- 718.8560180664062
- 185.2156982421875
- -0.1130886971950531
- 0.0
- 0.0
- 1.0
- 0.003779761027544737
---

The main program is running as follows:

$ ros2 launch cslam_experiments kitti_stereo.launch.py
[INFO] [launch]: All log files can be found below /home/zmh/.ros/log/2023-04-11-18-59-38-540360-ubuntu-25084
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [static_transform_publisher-1]: process started with pid [25086]
[INFO] [static_transform_publisher-2]: process started with pid [25088]
[INFO] [loop_closure_detection_node.py-3]: process started with pid [25090]
[INFO] [map_manager-4]: process started with pid [25092]
[INFO] [pose_graph_manager-5]: process started with pid [25094]
[INFO] [stereo_odometry-6]: process started with pid [25096]
[static_transform_publisher-2] [INFO] [1681264780.239612317] [static_transform_publisher_2gpcTcnwQylTtSMf]: Spinning until killed publishing transform from 'base_link' to 'camera_link'
[static_transform_publisher-1] [INFO] [1681264780.239741528] [static_transform_publisher_XE67iaQFvzrAPYET]: Spinning until killed publishing transform from 'camera_gray_left' to 'camera_link'
[pose_graph_manager-5] [INFO] [1681264780.603052455] [r0.cslam_pose_graph_manager]: Visualization disabled.
[pose_graph_manager-5] [INFO] [1681264780.806752656] [r0.cslam_pose_graph_manager]: Simulated rendezvous schedule of robot 0,900,3600
[pose_graph_manager-5] [INFO] [1681264780.806854331] [r0.cslam_pose_graph_manager]: Initialization done.
[map_manager-4] [INFO] [1681264789.704265334] [r0.cslam_map_manager]: Initialization done.
[INFO] [bag-7]: process started with pid [25176]
[bag-7] [INFO] [1681264804.817631644] [rosbag2_storage]: Opened database '/home/zmh/work/slam/data/rosbag2_0/rosbag2_2023_04_11-18_05_09_0.db3' for READ_ONLY.
[loop_closure_detection_node.py-3] [INFO] [1681264809.661177753] [r0.cslam_loop_closure_detection]: Using CosPlace. (default)
[loop_closure_detection_node.py-3] [INFO] [1681264817.205573525] [r0.cslam_loop_closure_detection]: loading checkpoint '/home/zmh/work/slam/swarm_slam/Swarm-SLAM/install/cslam/share/cslam/models/resnet101_512.pth'
[INFO] [loop_closure_detection_node.py-8]: process started with pid [25192]
[INFO] [map_manager-9]: process started with pid [25194]
[INFO] [pose_graph_manager-10]: process started with pid [25196]
[INFO] [stereo_odometry-11]: process started with pid [25198]
[pose_graph_manager-10] [INFO] [1681264819.033584754] [r1.cslam_pose_graph_manager]: Visualization disabled.
[pose_graph_manager-10] [INFO] [1681264819.069134433] [r1.cslam_pose_graph_manager]: Simulated rendezvous schedule of robot 1,500,3600
[pose_graph_manager-10] [INFO] [1681264819.069213363] [r1.cslam_pose_graph_manager]: Initialization done.
[map_manager-9] [INFO] [1681264820.258284618] [r1.cslam_map_manager]: Initialization done.
[loop_closure_detection_node.py-8] [INFO] [1681264823.048000823] [r1.cslam_loop_closure_detection]: Using CosPlace. (default)
[loop_closure_detection_node.py-8] [INFO] [1681264827.404287054] [r1.cslam_loop_closure_detection]: loading checkpoint '/home/zmh/work/slam/swarm_slam/Swarm-SLAM/install/cslam/share/cslam/models/resnet101_512.pth'
[loop_closure_detection_node.py-8] [INFO] [1681264832.548088864] [r1.cslam_loop_closure_detection]: Initialization done.
[loop_closure_detection_node.py-3] [INFO] [1681264832.548280476] [r0.cslam_loop_closure_detection]: Initialization done.
[INFO] [bag-12]: process started with pid [25277]
[bag-12] [INFO] [1681264844.910747659] [rosbag2_storage]: Opened database '/home/zmh/work/slam/data/rosbag2_1/rosbag2_2023_04_11-18_05_09_0.db3' for READ_ONLY.
[map_manager-4] [WARN] [1681264850.529215450] []: Messages of type 0 arrived out of order (will print only once)
[map_manager-4] [WARN] [1681264850.741559370] []: Messages of type 1 arrived out of order (will print only once)
[INFO] [bag-7]: process has finished cleanly [pid 25176]
[INFO] [bag-12]: process has finished cleanly [pid 25277]
lajoiepy commented 1 year ago

Hi! Thanks for your interest in Swarm-SLAM!

The issue might be the rendezvous simulator. Robots don't publish data unless they have neighbors. They also keep track of what data were shared with each robot in order to avoid duplicated messages and make sure that each neighbor receives all the information. As mentionned in the paper, I introduced a rendezvous schedule for dataset evaluation to simulate the worst case in which robots only meet at the end. According to your log: Simulated rendezvous schedule of robot 0,900,3600, robot 0 will be visible to other only between the 900 second and the 3600 second, so you might have to wait for them to see each other. I just pushed an update to cslam_experiments to disable this mechanism since it is only useful for worst-case evaluation. For now on, people who want to replicate the experiments exactly will need to use the tag 1.0.0.

With my latest commit, the robots will see each other right from the beginning and should start publishing on /r[id]/cslam/keyframe_data right away. Let me know if this fixes your isssue. If not, I suggest that you look to see if all the relevant topics are in the correct namespaces and have corresponding publishers and subscribers. You can look for this using ros2 topic list, ros2 node list and ros2 node info node_name. Feel free to post the output of those commands here if you have'nt resolved the problem :)

HeTemp commented 1 year ago

Hello! Thank you very much for your response!

I have updated my cslam experiment and now I am able to get information from /r0/cslam/heartbeat. However, I still couldn't received any messages (nor error messages) from ros2 topic echo /r0/cslam/keyframe_data or ros2 topic echo /r0/odom_info

According to your advice, I have reviewed the message node information by ros2 node list and ros2 node info node_name, and corrected the camera_info format in my dataset, but it still doesn't seem to have any effect.

I am not sure what command outputs you would need to confirm the cause of the problem. Please let me know, and I will post them here immediately. Thanks!

lajoiepy commented 1 year ago

Ok, the problem might be the odometry which Swarm-SLAM takes as input. If there is no odometry published, the system won't do anything.

I suggest this general methodology to debug while running your setup:

If, as I now suspect, the odometry is not published, check into the odometry launch file and its many parameters: https://github.com/lajoiepy/cslam_experiments/blob/main/launch/odometry/rtabmap_kitti_stereo_odometry.launch.py Note that the odometry is a decoupled input from Swarm-SLAM, so if RTAB-Map does not work for you, but you are able to run another odometry software with your bag, you can use the other one instead.

HeTemp commented 1 year ago

Thanks again for you kind help! After inspection, I think there is something wrong with the dataset I recorded. So I switched to graco_ Lidar dataset, and it works. So the problem may be on my KITTI dataset and I will check my KITTI dataset again.

lajoiepy commented 1 year ago

Ok, I am glad to hear that it works with GrAco. I'll close the issue since it is not directly related to Swarm-SLAM, but please post your solution for the KITTI bags here if you can. Also, feel free to open another issue if you face any other problem, or if you have questions :)

rocker12121 commented 1 year ago

Thanks again for you kind help! After inspection, I think there is something wrong with the dataset I recorded. So I switched to graco_ Lidar dataset, and it works. So the problem may be on my KITTI dataset and I will check my KITTI dataset again.

Can you please let me know how did you run the odometry on graco dataset?