rsasaki0109 / li_slam_ros2 Goto Github PK

@rsasaki0109 Hi Sir. I am Working on this repo and it is working perfectly. I have a doubt, does this repo supports ouster internal IMU. Can you please clarify me. Thanks in advance.

Hi ,
I was trying out this package but there is some issue I guess.
Inn readme you never mention a git command to clone this repo but instead another repo(again your repo), is this intentional ?

Could you please check and confirm ?
Thanks for the awesome work ,

Hi, So I was wondering if there was any particular reason to the header frame id being set to base_link at the line 670 of image_projection.cpp, because I wanted to reuse this deskewed point cloud elsewhere in my code 🤔 ?

@rsasaki0109 Hi. I have tried rooftop ouster dataset from LIO-SAM. The thing is I am getting mapped in rviz but it is showing Failed to match 'ring' and 'time'. Make me if I am wrong, when this happens pcd does affect. Because I saved .pcd format and executed with online tool but still I am not satified with the image. Could not find this issue. Am I missing dependencies?

Hello @rsasaki0109
Thanks for your soon reply. You have help me clean one bug in #23 . And I have add the check delta_t code in imu_preintegration.cpp.

However, when I keep play my bag data, I hope it could finish a loop.
But it will happen a lot of problem randomly.
Here I record some result here.
And I knew you are busy. But if you are free, please help me check my data or how could I avoid this randomly problem.
Thank you very much.

bag file:
https://drive.google.com/drive/folders/1Ov5nnFHmqO3qDI_GLPtWwOS9HZtOQiP0?usp=sharing

Result1:
First half everything is perfact. But other will jump out. And I will get Rviz2 info:
[rviz2]: Message Filter dropping message: frame 'base_link' at time 1686970685.207 for reason 'Unknown'

Result2:
Same. But this time it jump early than result1. And show more info like:
[imu_preintegration]: Large velocity, reset IMU-preintegration!

If I have other new found, I will update here. Thank you very much.

Hello,
just to inform.
The gtsam package is not available for ubuntu 22.04. So it it not running in humble.

Thank you for your work.

Greetings

@rsasaki0109 Hi Sir. Since you have working with mapping related field, I am curious to know about the mapping accuracy. Could you guide me how to check mapping accuracy sir. Thanks in advance.

I want to run Li_SLAm with a Ouster-OS1 with the official Ouster Driver.

I only changed the default parameter file to the settings I used with the Ouster when recording.

The problem I have is that I don't get any output from the SLAM. I think the function cloudHandler doesn't get called, because I did add a debug line in cloudHandler but the line never gets printed. I checked, that PointCloud2 Data is coming from the Ouster Driver Package and by uncommenting "debug IMU data", that the imuHandler is called.

Changed Parameters:

/**:
    ros__parameters:
      # LiDAR setting
      pointCloudTopic: "ouster/points"
      sensor: ouster    # lidar sensor type, 'velodyne' or 'ouster' or 'livox'
      N_SCAN: 64          # number of lidar channel (i.e., Velodyne/Ouster: 16, 32, 64, 128, Livox Horizon: 6)
      Horizon_SCAN: 1024  # lidar horizontal resolution (Velodyne:1800, Ouster:512,1024,2048, Livox Horizon: 4000)
      # IMU setting
      imuTopic: "ouster/imu"
      imuAccNoise: 3.9939570888238808e-03
      imuGyrNoise: 1.5636343949698187e-03
      imuAccBiasN: 6.4356659353532566e-05
      imuGyrBiasN: 3.5640318696367613e-05
      imuGravity: 9.80511
      extrinsicTrans: [0.0, 0.0, 0.0]
      extrinsicRot: [-1.0, 0.0, 0.0,
                  0.0, 1.0, 0.0,
                  0.0, 0.0, -1.0]
      extrinsicRPY: [0.0,  1.0, 0.0,
                 -1.0, 0.0, 0.0,
                  0.0, 0.0, 1.0]

scan_matcher:
    ros__parameters:
      global_frame_id: "map"
      robot_frame_id: "base_link"
      registration_method: "NDT"
      ndt_resolution: 2.0
      ndt_num_threads: 0
      trans_for_mapupdate: 1.5
      vg_size_for_input: 0.1
      vg_size_for_map: 0.1
      use_min_max_filter: false
      map_publish_period: 100.0
      num_targeted_cloud: 20
      set_initial_pose: true
      initial_pose_x: 0.0
      initial_pose_y: 0.0
      initial_pose_z: 0.0
      initial_pose_qx: 0.0
      initial_pose_qy: 0.0
      initial_pose_qz: 0.0
      initial_pose_qw: 1.0
      use_imu: true
      use_odom: false
      debug_flag: true

Log of running li_slam

[INFO] [launch]: All log files can be found below /home/ros2/.ros/log/2023-06-09-11-37-47-396025-ros2-24447
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [scanmatcher_node-1]: process started with pid [24453]
[INFO] [static_transform_publisher-2]: process started with pid [24455]
[INFO] [imu_preintegration-3]: process started with pid [24457]
[INFO] [image_projection-4]: process started with pid [24459]
[INFO] [graph_based_slam_node-5]: process started with pid [24479]
[static_transform_publisher-2] [INFO] [1686303467.512973231] [static_transform_publisher_6DDBrecxhAy7IaLt]: Spinning until killed publishing transform from 'base_link' to 'os_sensor'
[imu_preintegration-3] ----> IMU Preintegration Started.
[image_projection-4] ----> ImageProjection Started.
[scanmatcher_node-1] registration_method:NDT
[scanmatcher_node-1] ndt_resolution[m]:2
[scanmatcher_node-1] ndt_num_threads:0
[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.1
[scanmatcher_node-1] vg_size_for_map[m]:0.1
[scanmatcher_node-1] use_min_max_filter:false
[scanmatcher_node-1] scan_min_range[m]:0.1
[scanmatcher_node-1] scan_max_range[m]:100
[scanmatcher_node-1] set_initial_pose:true
[scanmatcher_node-1] use_odom:false
[scanmatcher_node-1] use_imu:true
[scanmatcher_node-1] debug_flag:true
[scanmatcher_node-1] map_publish_period[sec]:100
[scanmatcher_node-1] num_targeted_cloud:20
[scanmatcher_node-1] ------------------
[scanmatcher_node-1] [INFO] [1686303467.619912710] [scan_matcher]: initialize Publishers and Subscribers
[scanmatcher_node-1] [INFO] [1686303467.634094827] [scan_matcher]: initialization end
[graph_based_slam_node-5] [INFO] [1686303467.658369901] [graph_based_slam]: initialization start
[graph_based_slam_node-5] registration_method:GICP
[graph_based_slam_node-5] voxel_leaf_size[m]:0.1
[graph_based_slam_node-5] ndt_resolution[m]:5
[graph_based_slam_node-5] ndt_num_threads:1
[graph_based_slam_node-5] loop_detection_period[Hz]:3000
[graph_based_slam_node-5] threshold_loop_closure_score:1
[graph_based_slam_node-5] distance_loop_closure[m]:25
[graph_based_slam_node-5] range_of_searching_loop_closure[m]:20
[graph_based_slam_node-5] search_submap_num:4
[graph_based_slam_node-5] num_adjacent_pose_cnstraints:5
[graph_based_slam_node-5] use_save_map_in_loop:false
[graph_based_slam_node-5] ------------------
[graph_based_slam_node-5] [INFO] [1686303467.663264957] [graph_based_slam]: initialize Publishers and Subscribers
[graph_based_slam_node-5] [INFO] [1686303467.666547017] [graph_based_slam]: initialization end

The datasets mentioned in your README (casual_walk.bag and big_loop.bag ) are no longer available here. Can they be downloaded elsewhere?

HI, I'm facing with a fatal error when compiling on humble.

graph_based_slam/include/graph_based_slam/graph_based_slam_component.h:47:10: fatal error: tf2_sensor_msgs/tf2_sensor_msgs.h: No such file or directory 47 | #include <tf2_sensor_msgs/tf2_sensor_msgs.h>

But all deps are installed, including geometry2 package.

From running the li_slam_ros2 in my project, I can see that the covariance matrix for the odometry is set to 0 is their anyway to get calculated values like the original LIO-SAM did ? I would like to add li_slam_ros2 as an odometry source for the robot_localization stack :D

I chosen Livox type, but I cannot find how aim to the livox pointcloud topic, only the imu one
cheers

Hi, using an external 9DoF IMU with an Ouster1-32,
I am playing from a rosbag but im getting such PCL errors.

[image_projection-3] [WARN] [1657123061.814967364] [image_projection]: Point cloud timestamp not available, deskew function disabled, system will drift significantly!
[image_projection-3] Failed to find match for field 'ring'.
[image_projection-3] Failed to find match for field 'time'.

@rsasaki0109 I used li_sam_ros2 repository and tried to run the casual walk bag but it shows deviation and the map doesnot smooth when it processed. It contains lots of noise. can help me out how to solve this issue cuz I am new to this! Thanks in advance.

Hello Sasaki San,
Thank you for integrating the IMU Integration from LIO-SAM to Lidar_slam_ros2. Using this repo, I created the best looking map with large scale dataset by far. The method is very reliable in ROS2 system and easy to use.However, I still have a couple of questions:
Some times weird drift happens. And eventually a small drift results in accumulative error that leads to failure of loop closure, especially for large scale dataset. For me the mapping was all perfect until the mapping hit a small drift.


Loop closure failing
Also another drift happens at this U-turn


Eventually resulted in misalignment in the lower left area, as well as aligning the map into a tilted plane.

The pointcloud plane were not even, and different sections are slightly tiled. Although I set imu input to be true and I see no error returned by IMUPreintegration, I still wonder if IMU input really comes into place.

I am using the default lio_bigloop.yaml set up here and any help will be greatly appreciated!!

@rsasaki0109 Hi sir. I tried os1-128 bag data but it fails to reproduce map. When I tried os1-32 it worked. what is this issue means sir. could you help me out. Thanks in advance.

[scanmatcher_node-1] [INFO] [1692692868.561236163] [scan_matcher]: create a first map
[imu_preintegration-3] terminate called after throwing an instance of 'gtsam::IndeterminantLinearSystemException'
[imu_preintegration-3] what():
[imu_preintegration-3] Indeterminant linear system detected while working near variable
[imu_preintegration-3] 8646911284551352379 (Symbol: x59).
[imu_preintegration-3]
[imu_preintegration-3] Thrown when a linear system is ill-posed. The most common cause for this
[imu_preintegration-3] error is having underconstrained variables. Mathematically, the system is
[imu_preintegration-3] underdetermined. See the GTSAM Doxygen documentation at
[imu_preintegration-3] http://borg.cc.gatech.edu/ on gtsam::IndeterminantLinearSystemException for
[imu_preintegration-3] more information.
[ERROR] [imu_preintegration-3]: process has died [pid 7624, exit code -6, cmd '/home/praveen/ros2_ws/install/scanmatcher/lib/scanmatcher/imu_preintegration --ros-args --params-file /home/praveen/ros2_ws/install/scanmatcher/share/scanmatcher/param/lio_ouster.yaml -r /odometry:=/odom'].

Hello, I have been trying to get ros2 to work with this solution for a few days. I have gotten the rviz2 and ros2 launch scanmatcher commands to run correctly in separate windows.

I have not been able to get the "ros2 bag play -s rosbag_v2 casual_walk.bag" to run correctly in its own window. It cannot read the Velodyne topics, because those topics seem to have been recorded in with ROS 1. Please see below screenshot.

Any ideas on what I am doing incorrectly?

Needed some guidance on the used libraries. It mentions g2o, how do i install it? I have Ubuntu 22 and ros2 humble installed.

Hello. Bother you again.
I build the li_slam_ros2 successfully but there is an error when I use the command "ros2 launch scanmatcher lio.launch.py". The detailed info is shown below. Can you help me solve it? Thank you!
[INFO] [launch]: All log files can be found below /home/cakgod/.ros/log/2020-11-09-03-25-29-689650-cakgod-ThinkCentre-M910x-N000-75592 [INFO] [launch]: Default logging verbosity is set to INFO Task exception was never retrieved future: <Task finished name='Task-2' coro=<LaunchService._process_one_event() done, defined at /opt/ros/foxy/lib/python3.8/site-packages/launch/launch_service.py:271> exception=SubstitutionFailure("executable 'imu_preintegration' not found on the libexec directory '/home/cakgod/ros2_ws/install/scanmatcher/lib/scanmatcher' ")> Traceback (most recent call last): File "/opt/ros/foxy/lib/python3.8/site-packages/launch/launch_service.py", line 273, in _process_one_event await self.__process_event(next_event) File "/opt/ros/foxy/lib/python3.8/site-packages/launch/launch_service.py", line 293, in __process_event visit_all_entities_and_collect_futures(entity, self.__context)) File "/opt/ros/foxy/lib/python3.8/site-packages/launch/utilities/visit_all_entities_and_collect_futures_impl.py", line 45, in visit_all_entities_and_collect_futures futures_to_return += visit_all_entities_and_collect_futures(sub_entity, context) File "/opt/ros/foxy/lib/python3.8/site-packages/launch/utilities/visit_all_entities_and_collect_futures_impl.py", line 45, in visit_all_entities_and_collect_futures futures_to_return += visit_all_entities_and_collect_futures(sub_entity, context) File "/opt/ros/foxy/lib/python3.8/site-packages/launch/utilities/visit_all_entities_and_collect_futures_impl.py", line 45, in visit_all_entities_and_collect_futures futures_to_return += visit_all_entities_and_collect_futures(sub_entity, context) [Previous line repeated 1 more time] File "/opt/ros/foxy/lib/python3.8/site-packages/launch/utilities/visit_all_entities_and_collect_futures_impl.py", line 38, in visit_all_entities_and_collect_futures sub_entities = entity.visit(context) File "/opt/ros/foxy/lib/python3.8/site-packages/launch/action.py", line 108, in visit return self.execute(context) File "/opt/ros/foxy/lib/python3.8/site-packages/launch_ros/actions/node.py", line 442, in execute ret = super().execute(context) File "/opt/ros/foxy/lib/python3.8/site-packages/launch/actions/execute_process.py", line 775, in execute self.__expand_substitutions(context) File "/opt/ros/foxy/lib/python3.8/site-packages/launch/actions/execute_process.py", line 635, in __expand_substitutions cmd = [perform_substitutions(context, x) for x in self.__cmd] File "/opt/ros/foxy/lib/python3.8/site-packages/launch/actions/execute_process.py", line 635, in <listcomp> cmd = [perform_substitutions(context, x) for x in self.__cmd] File "/opt/ros/foxy/lib/python3.8/site-packages/launch/utilities/perform_substitutions_impl.py", line 26, in perform_substitutions return ''.join([context.perform_substitution(sub) for sub in subs]) File "/opt/ros/foxy/lib/python3.8/site-packages/launch/utilities/perform_substitutions_impl.py", line 26, in <listcomp> return ''.join([context.perform_substitution(sub) for sub in subs]) File "/opt/ros/foxy/lib/python3.8/site-packages/launch/launch_context.py", line 184, in perform_substitution return substitution.perform(self) File "/opt/ros/foxy/lib/python3.8/site-packages/launch_ros/substitutions/executable_in_package.py", line 84, in perform raise SubstitutionFailure( launch.substitutions.substitution_failure.SubstitutionFailure: executable 'imu_preintegration' not found on the libexec directory '/home/cakgod/ros2_ws/install/scanmatcher/lib/scanmatcher' Task exception was never retrieved future: <Task finished name='Task-12' coro=<LaunchService._process_one_event() done, defined at /opt/ros/foxy/lib/python3.8/site-packages/launch/launch_service.py:271> exception=RuntimeError('Signal event received before subprocess transport available.')> Traceback (most recent call last): File "/opt/ros/foxy/lib/python3.8/site-packages/launch/launch_service.py", line 273, in _process_one_event await self.__process_event(next_event) File "/opt/ros/foxy/lib/python3.8/site-packages/launch/launch_service.py", line 293, in __process_event visit_all_entities_and_collect_futures(entity, self.__context)) File "/opt/ros/foxy/lib/python3.8/site-packages/launch/utilities/visit_all_entities_and_collect_futures_impl.py", line 38, in visit_all_entities_and_collect_futures sub_entities = entity.visit(context) File "/opt/ros/foxy/lib/python3.8/site-packages/launch/action.py", line 108, in visit return self.execute(context) File "/opt/ros/foxy/lib/python3.8/site-packages/launch/actions/opaque_function.py", line 75, in execute return self.__function(context, *self.__args, **self.__kwargs) File "/opt/ros/foxy/lib/python3.8/site-packages/launch/actions/execute_process.py", line 421, in __on_signal_process_event raise RuntimeError('Signal event received before subprocess transport available.') RuntimeError: Signal event received before subprocess transport available. [INFO] [scanmatcher_node-1]: process started with pid [75594] [INFO] [static_transform_publisher-2]: process started with pid [75596] [INFO] [scanmatcher_node-1]: sending signal 'SIGINT' to process[scanmatcher_node-1] [ERROR] [scanmatcher_node-1]: process has died [pid 75594, exit code -2, cmd '/home/cakgod/ros2_ws/install/scanmatcher/lib/scanmatcher/scanmatcher_node --ros-args -r /input_cloud:=/cloud_deskewed --params-file /home/cakgod/ros2_ws/install/scanmatcher/share/scanmatcher/param/lio.yaml']. [static_transform_publisher-2] [INFO] [1604910329.767936466] [static_transform_publisher_I3XGArIQqPVe3muV]: Spinning until killed publishing transform from 'base_link' to 'velodyne' [ERROR] [static_transform_publisher-2]: process[static_transform_publisher-2] failed to terminate '5' seconds after receiving 'SIGINT', escalating to 'SIGTERM' [INFO] [static_transform_publisher-2]: sending signal 'SIGTERM' to process[static_transform_publisher-2] [ERROR] [static_transform_publisher-2]: process has died [pid 75596, exit code -15, cmd '/opt/ros/foxy/lib/tf2_ros/static_transform_publisher 0 0 0 0 0 0 1 base_link velodyne --ros-args'].

Hello, I am currently trying to run this package. As shown in the picture below, there is a time difference between imu and ouster. So, in the picture below
if (imuQueue.empty() || t_f > timeScanCur || t_b < timeScanNext)
Problem occurs in conditional statement. Is there a way to correct this?
imu time = 1689176983.31
ouster time = 2946.97

Hi Mr. Sasaki.

I tried to launch li_slam. But it won't start to create map. Are there any steps that I missed?

scanmatcher prompt:
:0:1: error: macro names must be identifiers

Found in image_projection.cpp
add_executable(image_projection src/image_projection.cpp) ament_target_dependencies(image_projection rclcpp rclcpp_components tf2_ros tf2_geometry_msgs tf2_sensor_msgs tf2_eigen geometry_msgs sensor_msgs nav_msgs #liosam_msgs PCL OpenCV )
I do not compile and generate image_projection, but there is no problem

Hello, I want to run li_slam with a V16s velodyne lidar and a 9 axis imu. I never change any default parameters in lio.yaml.

My data is a indoor corridor loop. However, when li_slam run just a little time it occur below problem:

And this is my bag file.
https://drive.google.com/drive/folders/1Ov5nnFHmqO3qDI_GLPtWwOS9HZtOQiP0?usp=sharing

Could you help me figure it out? Thank you.

hey there i am new here . Can you help me in running this repo .
i have cloned the repo and built .
i would like to know the commands needed to run this.
the commands that i used are :

I have setup turtlebot3 with velodyne lidar in gazebo and I can view the pointcloud in rviz2, and also checked the topics being published where imu_raw is also present. When i start scanmatcher I get the following error:

[scanmatcher_node-1] [INFO] [1718576201.142020542] [scan_matcher]: create a first map
[imu_preintegration-3] terminate called after throwing an instance of 'gtsam::IndeterminantLinearSystemException'
[imu_preintegration-3]   what():  
[imu_preintegration-3] Indeterminant linear system detected while working near variable
[imu_preintegration-3] 8646911284551352324 (Symbol: x4).
[imu_preintegration-3] 
[imu_preintegration-3] Thrown when a linear system is ill-posed.  The most common cause for this
[imu_preintegration-3] error is having underconstrained variables.  Mathematically, the system is
[imu_preintegration-3] underdetermined.  See the GTSAM Doxygen documentation at
[imu_preintegration-3] http://borg.cc.gatech.edu/ on gtsam::IndeterminantLinearSystemException for
[imu_preintegration-3] more information.
[ERROR] [imu_preintegration-3]: process has died [pid 26718, exit code -6, cmd '/home/rvxfahim/ros2_ws/install/scanmatcher/lib/scanmatcher/imu_preintegration --ros-args --params-file /home/rvxfahim/ros2_ws/install/scanmatcher/share/scanmatcher/param/lio.yaml -r /odometry:=/odom'].
[graph_based_slam_node-5] ----------------------------
[graph_based_slam_node-5] searching Loop, num_submaps:2
[graph_based_slam_node-5] ----------------------------
[graph_based_slam_node-5] searching Loop, num_submaps:3
^C[WARNING] [launch]: user interrupted with ctrl-c (SIGINT)
[scanmatcher_node-1] [INFO] [1718576208.570184806] [rclcpp]: signal_handler(signum=2)
[static_transform_publisher-2] [INFO] [1718576208.570192306] [rclcpp]: signal_handler(signum=2)
[image_projection-4] [INFO] [1718576208.570188036] [rclcpp]: signal_handler(signum=2)
[graph_based_slam_node-5] [INFO] [1718576208.570228275] [rclcpp]: signal_handler(signum=2)
[INFO] [static_transform_publisher-2]: process has finished cleanly [pid 26716]
[INFO] [image_projection-4]: process has finished cleanly [pid 26720]
[INFO] [graph_based_slam_node-5]: process has finished cleanly [pid 26722]
[INFO] [scanmatcher_node-1]: process has finished cleanly [pid 26714]