PointCloud Data from Ouster OS1 not getting processed

[masx14]

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

[rsasaki0109]

Is it possible to have rosbag provided to me?

[masx14]

Yes, I have uploaded it to Google Drive. It is only a few seconds long, so it is not big in size.

Bag File in Google Drive

[rsasaki0109]

Sorry for the slow response, I've been busy.
I am currently working on addressing the scanning issues related to QoS topics.

ouster driver command

ros2 launch ouster_ros replay.launch.xml bag_file:='/home/sasaki/Downloads/record.bag/record.bag/record.bag_0.db3'

li_slam terminal output

[image_projection-4] [WARN] [1687303247.610952734] [image_projection]: New publisher discovered on topic '/ouster/points', offering incompatible QoS. No messages will be sent to it. Last incompatible policy: RELIABILITY_QOS_POLICY