Perfomance in real time drone systems

[moralessP]

First of all, I want to thank you for your great work. I am currently developing a drone for a specific task, and I need to be able to localize it using data from my Velodyne 16 Puck Lite LiDAR. I am currently in simulation using Gazebo version Harmonic and ROS 2 Humble. I have tried this repository on my system simulation, and it seems to have errors in the x and y position estimation. I'm not sure why z is working fine but not the other two dimensions. Should the data from my 3D LiDAR be without NaN values? How should the frame of my LiDAR be related to my base_link?

[rsasaki0109]

Could you capture the screen when localization is not working well?
I'm interested in seeing the terminal output, the scan point clouds and what the SLAM estimated trajectory looks like in rviz.

[moralessP]

Hi, sorry the long waiting time. Your code works well when the drone up or forward but it doesn't give me a good estimation when I turn left or right. I don't know why and what parameters I need to change in order to get a good estimation.

[moralessP]

This is the terminal output, everything looks work well.
[INFO] [launch]: All log files can be found below /home/localadm/.ros/log/2024-05-13-09-36-23-105927-lFRS31509-12800 [INFO] [launch]: Default logging verbosity is set to INFO [INFO] [scanmatcher_node-1]: process started with pid [12807] [INFO] [graph_based_slam_node-2]: process started with pid [12809] [scanmatcher_node-1] [INFO] [1715585783.255104294] [scan_matcher]: initialization start [scanmatcher_node-1] registration_method:NDT [scanmatcher_node-1] ndt_resolution[m]:2 [scanmatcher_node-1] ndt_num_threads:2 [scanmatcher_node-1] gicp_corr_dist_threshold[m]:5 [scanmatcher_node-1] trans_for_mapupdate[m]:1.5 [scanmatcher_node-1] vg_size_for_input[m]:0.5 [scanmatcher_node-1] vg_size_for_map[m]:0.1 [scanmatcher_node-1] use_min_max_filter:true [scanmatcher_node-1] scan_min_range[m]:1 [scanmatcher_node-1] scan_max_range[m]:100 [scanmatcher_node-1] set_initial_pose:true [scanmatcher_node-1] publish_tf:false [scanmatcher_node-1] use_odom:false [scanmatcher_node-1] use_imu:false [scanmatcher_node-1] scan_period[sec]:0.1 [scanmatcher_node-1] debug_flag:false [scanmatcher_node-1] map_publish_period[sec]:15 [scanmatcher_node-1] num_targeted_cloud:20 [scanmatcher_node-1] ------------------ [scanmatcher_node-1] [INFO] [1715585783.255741037] [scan_matcher]: initialize Publishers and Subscribers [scanmatcher_node-1] [INFO] [1715585783.259079612] [scan_matcher]: set initial pose [scanmatcher_node-1] [INFO] [1715585783.259120218] [scan_matcher]: initialization end [graph_based_slam_node-2] [INFO] [1715585783.304962446] [graph_based_slam]: initialization start [graph_based_slam_node-2] registration_method:NDT [graph_based_slam_node-2] voxel_leaf_size[m]:0.1 [graph_based_slam_node-2] ndt_resolution[m]:1 [graph_based_slam_node-2] ndt_num_threads:2 [graph_based_slam_node-2] loop_detection_period[Hz]:3000 [graph_based_slam_node-2] threshold_loop_closure_score:0.7 [graph_based_slam_node-2] distance_loop_closure[m]:100 [graph_based_slam_node-2] range_of_searching_loop_closure[m]:20 [graph_based_slam_node-2] search_submap_num:2 [graph_based_slam_node-2] num_adjacent_pose_cnstraints:5 [graph_based_slam_node-2] use_save_map_in_loop:true [graph_based_slam_node-2] debug_flag:true [graph_based_slam_node-2] ------------------ [graph_based_slam_node-2] [INFO] [1715585783.305794386] [graph_based_slam]: initialize Publishers and Subscribers [graph_based_slam_node-2] [INFO] [1715585783.311001802] [graph_based_slam]: initialization end [scanmatcher_node-1] [INFO] [1715585783.505627123] [scan_matcher]: initial_cloud is received [scanmatcher_node-1] [INFO] [1715585783.505683250] [scan_matcher]: create a first map [graph_based_slam_node-2] [INFO] [1715585795.308566704] [graph_based_slam]: searching Loop, num_submaps:2 [scanmatcher_node-1] [INFO] [1715585801.606700657] [scan_matcher]: publish a map, number of points in the map : 16931 [graph_based_slam_node-2] [INFO] [1715585804.308521267] [graph_based_slam]: searching Loop, num_submaps:5 [graph_based_slam_node-2] [INFO] [1715585810.308691856] [graph_based_slam]: searching Loop, num_submaps:6

[rsasaki0109]

It seems that the issue with unsuccessful matching in the XY direction is due to insufficient feature points. Is it possible to place some objects with geometric features in the simulation to prevent degeneration?

[moralessP]

Yes you are right. I placed my LiDAR horizontally, which resulted in more features and improved performance on the XY axis. It's a shame that it doesn't work with the vertical positioning of my LiDAR. Thank you anyway.

[moralessP]

Hi @rsasaki0109, I have one question. I would like to use this repository (scanmatcher package) only for localization for my drone, do you recommend me this repository or lidar_localization_ros2?

[rsasaki0109]

If you already have a map, you should use lidar_localization_ros2. If you don't have a map, then you should use lidarslam_ros2.

[moralessP]

Okay thank you, and what is the difference between scanmatcher package and lidar_localization_ros2 package?

[moralessP]

I tried the lidar_localization_ros2 package, but it doesn't work. That's why I want to know the difference with scanmatcher since it works with my system.

[rsasaki0109]

Scanmatcher assumes that there is no pre-existing map and generates a map incrementally from the input scans.
On the other hand, lidar_localization_ros2 requires a pre-made map to be input. The input scans are used for localization and not for mapping, making the process lighter.

If lidar_localization_ros2 is not working, I would like to know the details. Did you properly input the map?

[moralessP]

Okay right, now I understand more clearly thank you. And I didn't give the map as input, so it's normal it's didn't work properly.
Last question, when I use lidar_localization_ros2 do you know how should I adapt the scan_period parameter when I activate use_imu? Because when I activate use_imu, I get an error message.

[rsasaki0109]

If the LiDAR frequency is 10 Hz, then the scan_period is 0.1 sec. What error are you encountering when you activate use_imu?

[moralessP]

This error : [INFO] [launch]: All log files can be found below /home/localadm/.ros/log/2024-05-27-10-03-06-958154-lFRS31509-9335 [INFO] [launch]: Default logging verbosity is set to INFO [INFO] [scanmatcher_node-1]: process started with pid [9336] [scanmatcher_node-1] [INFO] [1716796987.172534837] [localization_node]: initialization start [scanmatcher_node-1] registration_method:NDT [scanmatcher_node-1] ndt_resolution[m]:2.5 [scanmatcher_node-1] ndt_num_threads:4 [scanmatcher_node-1] gicp_corr_dist_threshold[m]:5 [scanmatcher_node-1] trans_for_mapupdate[m]:1.5 [scanmatcher_node-1] vg_size_for_input[m]:0.5 [scanmatcher_node-1] vg_size_for_map[m]:0.075 [scanmatcher_node-1] use_min_max_filter:true [scanmatcher_node-1] scan_min_range[m]:0.5 [scanmatcher_node-1] scan_max_range[m]:100 [scanmatcher_node-1] set_initial_pose:true [scanmatcher_node-1] publish_tf:false [scanmatcher_node-1] use_odom:false [scanmatcher_node-1] use_imu:true [scanmatcher_node-1] scan_period[sec]:0.1 [scanmatcher_node-1] debug_flag:false [scanmatcher_node-1] map_publish_period[sec]:15 [scanmatcher_node-1] num_targeted_cloud:50 [scanmatcher_node-1] ------------------ [scanmatcher_node-1] [INFO] [1716796987.174524734] [localization_node]: initialize Publishers and Subscribers [scanmatcher_node-1] [INFO] [1716796987.179819020] [localization_node]: set initial pose [scanmatcher_node-1] [INFO] [1716796987.179868576] [localization_node]: initialization end [scanmatcher_node-1] [INFO] [1716796987.365891293] [localization_node]: initial_cloud is received [scanmatcher_node-1] [INFO] [1716796987.365931419] [localization_node]: create a first map [ERROR] [scanmatcher_node-1]: process has died [pid 9336, exit code -8, cmd '/home/localadm/ros2_ws/install/scanmatcher/lib/scanmatcher/scanmatcher_node --ros-args -r __node:=localization_node --params-file /home/localadm/ros2_ws/install/drone_bringup/share/drone_bringup/config/slam.yaml -r /input_cloud:=/lidar'].

[rsasaki0109]

It looks like I need to debug to understand this. I'll add it to the task list.

If you're going to use an IMU, please use the improved version, li_slam_ros2.

[moralessP]

Hi, it's me again. Do you think this package can be run on Raspberry Pi 5 with 8 GB RAM ?

[rsasaki0109]

I remember it was just barely manageable when I tested it with 16GB RAM, so with only 8GB, it could be challenging.

[rsasaki0109]

It's possible to do just localization as shown below, but since SLAM accumulates point cloud maps over time, some ingenuity is required.

https://x.com/naokiakai/status/1806542391631110299

[moralessP]

I remember it was just barely manageable when I tested it with 16GB RAM, so with only 8GB, it could be challenging.

Owh Okay I see, thanks for your quick answer! :)