Re: Hesai QT128 with Zed2i and external fused Odometry

Posted by aninath93 on
URL: http://official-rtab-map-forum.206.s1.nabble.com/Hesai-QT128-with-Zed2i-and-external-fused-Odometry-tp9512p9650.html

Hi Mathieu,

Opened Issue

I have a issue opened at GITHUB and just wanted your feedback on this, as I am just trying to figure out lidar_deskewing with LIDAR on ROS2 side trying to follow your example from ROS1. If I compute ICP odometry as you had mentioned, it works. As soon as I plug EKF frame into guess_frame_id and odom_frame for lidar_deskewing. It gives me below error.


ekf_filter_node:
    ros__parameters:
        frequency: 40.0
        two_d_mode: true
        publish_tf: true
        use_control: false
        print_diagnostics: false
        debug: false
       
        map_frame: map            
        odom_frame: odom  
        base_link_frame: base_link
        world_frame: odom
        sensor_timeout: 0.15

        #x     , y     , z,
        #roll  , pitch , yaw,
        #vx    , vy    , vz,
        #vroll , vpitch, vyaw,
        #ax    , ay    , az
        odom0: /diffbot/odom
        odom0_differential: false
        odom0_relative: false
        odom0_queue_size: 2
        odom0_config: [false, false, false,
                       false, false, false,
                       true, true, false,
                       false, false, true,
                       false, false, false]

        imu0: /zed2i/zed_node/imu/data
        imu0_remove_gravitational_acceleration: true
        imu0_queue_size: 2
        imu0_relative: false
        imu0_config:    [false, false, false,
                        false, false, true,
                        false, false, false,
                        false, false, true,
                        false, false, false]

        Node(
          package='rtabmap_util', executable='lidar_deskewing', output='screen',
          parameters=[{
              'fixed_frame_id':'odom',
              'slerp':False,
              'wait_for_transform':0.01,
            }],
          remappings=[
                ('input_cloud', '/lidar_points3') #subs
                #('/lidar_points3/deskewed','/lidar_points3/deskewed') #publish
            ]),      
       
        Node(
            package='rtabmap_odom', executable='icp_odometry', output='screen',
            parameters=[{
              'ground_truth_base_frame_id':'',
              'frame_id':'base_link',
              'odom_frame_id':'odom_icp',
              'guess_frame_id':'odom',
              'publish_tf':False,
              'approx_sync':False,
              'tf_prefix':'',
              'wait_for_transform':0.2,
              'queue_size':5,
              'publish_null_when_lost':True,
              'expected_update_rate':10.0,
              'deskewing':deskewing,
              'use_sim_time':use_sim_time,
              'deskewing_slerp':False,
              'scan_cloud_max_points':15000,
              'wait_imu_to_init':False,
            }],
            remappings=[
              ('scan_cloud', '/lidar_points3'), #subs
              ('odom','/odom/icp'), #publis
              #('imu','/zed2i/zed_node/imu/data/filtered')
            ],
            arguments=[
              '--udebug',
              #ICP
              'Icp/PointToPlane', 'true',
              'Icp/Iterations', '15',
              'Icp/VoxelSize', '0.05', #0 means all scans used, check computation power to levrage this
              'Icp/Epsilon', '0.001',
              'Icp/DownsamplingStep','1',
              'Icp/PointToPlaneK', '20',
              'Icp/PointToPlaneRadius', '0',
              'Icp/MaxTranslation', '2', #to reject ICP jumps
              'Icp/MaxCorrespondenceDistance', '0.5', #voxel size res*10
              'Icp/CorrespondenceRatio', '0.01',
              'Icp/PointToPlaneGroundNormalsUp','0.8',
              'Icp/Strategy', '1',
              'Icp/ReciprocalCorrespondences','False',
              'Icp/OutlierRatio', '0.7',
              #Odom
              'Odom/ScanKeyFrameThr', '0.4',
              'Odom/Strategy', '0', #(F2F) (1=F2F)
              'OdomF2M/ScanSubtractRadius', '0.1', #resolution
              'OdomF2M/ScanMaxSize', '15000',
              'OdomF2M/BundleAdjustment', 'false',
              'Odom/ResetCountdown','1',
              'Icp/CCFilterOutFarthestPoints','True',
              'Icp/PointToPlaneLowComplexityStrategy','2',
              'Odom/FilteringStrategy','2',
              'Odom/GuessMotion','false',
              'OdomF2M/MaxSize','1000',
              'Optimizer/Robust','true',
              'Reg/Strategy','1',







I tried doing echo between frames, just to make sure my lidar frame is connected to odom and base_link and it is. Any thoughts on this please ?