search

rsasaki0109 / lidar_localization_ros2

3D LIDAR Localization using NDT/GICP and pointcloud map in ROS 2 (Not SLAM)
BSD 2-Clause "Simplified" License
281 stars 54 forks source link

ERROR run with Rslidar #49

Open RuPingCen opened 4 days ago

RuPingCen commented 4 days ago

Hi, @rsasaki0109 . I encountered a problem where when I use rslidar for localization, the trajectory is not displayed in RVIZ, and even when I print the topic "/path", there is no output. This point cloud map was built using lidarslam_ros2.

Befor run pcl_localization_ros2 node, I change the yaml file 

/**:
    ros__parameters:
      registration_method: "NDT_OMP"
      score_threshold: 2.0
      ndt_resolution: 1.0
      ndt_step_size: 0.1
      ndt_num_threads: 0
      transform_epsilon: 0.01
      voxel_leaf_size: 0.2
      scan_max_range: 100.0
      scan_min_range: 1.0
      scan_period: 0.1
      use_pcd_map: true
      map_path: "/home/mickrobot/ros2_ws/map.pcd"
      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_odom: false
      use_imu: false
      enable_debug: true
      global_frame_id: map
      odom_frame_id: odom
      base_frame_id: base_link

Also, I hace change the point cloud input topic and lidar_tf for the launch file 

import os

import launch
import launch.actions
import launch.events

import launch_ros
import launch_ros.actions
import launch_ros.events

from launch import LaunchDescription
from launch_ros.actions import LifecycleNode
from launch_ros.actions import Node

import lifecycle_msgs.msg

from ament_index_python.packages import get_package_share_directory

def generate_launch_description():

    ld = launch.LaunchDescription()

    lidar_tf = launch_ros.actions.Node(
        name='lidar_tf',
        package='tf2_ros',
        executable='static_transform_publisher',
        arguments=['0','0','0','0','0','0','1','base_link','rslidar']
        )

    imu_tf = launch_ros.actions.Node(
        name='imu_tf',
        package='tf2_ros',
        executable='static_transform_publisher',
        arguments=['0','0','0','0','0','0','1','base_link','imu_link']
        )

    localization_param_dir = launch.substitutions.LaunchConfiguration(
        'localization_param_dir',
        default=os.path.join(
            get_package_share_directory('pcl_localization_ros2'),
            'param',
            'localization.yaml'))

    pcl_localization = launch_ros.actions.LifecycleNode(
        name='pcl_localization',
        namespace='',
        package='pcl_localization_ros2',
        executable='pcl_localization_node',
        parameters=[localization_param_dir],
        remappings=[('/cloud','/lslidar_point_cloud')],
        output='screen')

    to_inactive = launch.actions.EmitEvent(
        event=launch_ros.events.lifecycle.ChangeState(
            lifecycle_node_matcher=launch.events.matches_action(pcl_localization),
            transition_id=lifecycle_msgs.msg.Transition.TRANSITION_CONFIGURE,
        )
    )

    from_unconfigured_to_inactive = launch.actions.RegisterEventHandler(
        launch_ros.event_handlers.OnStateTransition(
            target_lifecycle_node=pcl_localization,
            goal_state='unconfigured',
            entities=[
                launch.actions.LogInfo(msg="-- Unconfigured --"),
                launch.actions.EmitEvent(event=launch_ros.events.lifecycle.ChangeState(
                    lifecycle_node_matcher=launch.events.matches_action(pcl_localization),
                    transition_id=lifecycle_msgs.msg.Transition.TRANSITION_CONFIGURE,
                )),
            ],
        )
    )

    from_inactive_to_active = launch.actions.RegisterEventHandler(
        launch_ros.event_handlers.OnStateTransition(
            target_lifecycle_node=pcl_localization,
            start_state = 'configuring',
            goal_state='inactive',
            entities=[
                launch.actions.LogInfo(msg="-- Inactive --"),
                launch.actions.EmitEvent(event=launch_ros.events.lifecycle.ChangeState(
                    lifecycle_node_matcher=launch.events.matches_action(pcl_localization),
                    transition_id=lifecycle_msgs.msg.Transition.TRANSITION_ACTIVATE,
                )),
            ],
        )
    )

    ld.add_action(from_unconfigured_to_inactive)
    ld.add_action(from_inactive_to_active)

    ld.add_action(pcl_localization)
    ld.add_action(lidar_tf)
    ld.add_action(to_inactive)

    return ld

Here is the commands for launch pcl_localization_ros2 node

ros2 launch pcl_localization_ros2 pcl_localization.launch.py
rviz2 -d src/lidar_localization_ros2-0.2.0-humble/rviz/localization.rviz

Then, play the bags

ros2 bag play rslidar32_xsens_1

the terminator output information:

mickrobot@HPZ4:~/ros2_ws$ ros2 launch pcl_localization_ros2 pcl_localization.launch.py 
[INFO] [launch]: All log files can be found below /home/mickrobot/.ros/log/2024-11-20-22-23-22-885744-HPZ4-600194
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [pcl_localization_node-1]: process started with pid [600205]
[INFO] [static_transform_publisher-2]: process started with pid [600207]
[static_transform_publisher-2] [WARN] [1732112603.035920488] []: Old-style arguments are deprecated; see --help for new-style arguments
[static_transform_publisher-2] [INFO] [1732112603.061037977] [lidar_tf]: Spinning until stopped - publishing transform
[static_transform_publisher-2] translation: ('0.000000', '0.000000', '0.000000')
[static_transform_publisher-2] rotation: ('0.000000', '0.000000', '0.000000', '1.000000')
[static_transform_publisher-2] from 'base_link' to 'rslidar'
[pcl_localization_node-1] [INFO] [1732112603.266036483] [pcl_localization]: Configuring
[pcl_localization_node-1] [INFO] [1732112603.266331568] [pcl_localization]: initializeParameters
[pcl_localization_node-1] [INFO] [1732112603.266420318] [pcl_localization]: global_frame_id: map
[pcl_localization_node-1] [INFO] [1732112603.266438211] [pcl_localization]: odom_frame_id: odom
[pcl_localization_node-1] [INFO] [1732112603.266450864] [pcl_localization]: base_frame_id: base_link
[pcl_localization_node-1] [INFO] [1732112603.266463447] [pcl_localization]: registration_method: NDT_OMP
[pcl_localization_node-1] [INFO] [1732112603.266475364] [pcl_localization]: ndt_resolution: 1.000000
[pcl_localization_node-1] [INFO] [1732112603.266500436] [pcl_localization]: ndt_step_size: 0.100000
[pcl_localization_node-1] [INFO] [1732112603.266516182] [pcl_localization]: ndt_num_threads: 0
[pcl_localization_node-1] [INFO] [1732112603.266528802] [pcl_localization]: transform_epsilon: 0.010000
[pcl_localization_node-1] [INFO] [1732112603.266543481] [pcl_localization]: voxel_leaf_size: 0.200000
[pcl_localization_node-1] [INFO] [1732112603.266556697] [pcl_localization]: scan_max_range: 100.000000
[pcl_localization_node-1] [INFO] [1732112603.266572565] [pcl_localization]: scan_min_range: 1.000000
[pcl_localization_node-1] [INFO] [1732112603.266585708] [pcl_localization]: scan_period: 0.100000
[pcl_localization_node-1] [INFO] [1732112603.266599375] [pcl_localization]: use_pcd_map: 1
[pcl_localization_node-1] [INFO] [1732112603.266612072] [pcl_localization]: map_path: /home/mickrobot/ros2_ws/map.pcd
[pcl_localization_node-1] [INFO] [1732112603.266625419] [pcl_localization]: set_initial_pose: 1
[pcl_localization_node-1] [INFO] [1732112603.266637294] [pcl_localization]: use_odom: 0
[pcl_localization_node-1] [INFO] [1732112603.266649958] [pcl_localization]: use_imu: 0
[pcl_localization_node-1] [INFO] [1732112603.266661106] [pcl_localization]: enable_debug: 1
[pcl_localization_node-1] [INFO] [1732112603.266675219] [pcl_localization]: initializePubSub
[pcl_localization_node-1] [INFO] [1732112603.279203720] [pcl_localization]: initializePubSub end
[pcl_localization_node-1] [INFO] [1732112603.279278112] [pcl_localization]: initializeRegistration
[INFO] [launch.user]: -- Inactive --
[pcl_localization_node-1] [INFO] [1732112603.279410693] [pcl_localization]: initializeRegistration end
[pcl_localization_node-1] [INFO] [1732112603.279438688] [pcl_localization]: Configuring end
[pcl_localization_node-1] [INFO] [1732112603.285258510] [pcl_localization]: Activating
[pcl_localization_node-1] [INFO] [1732112603.285326801] [pcl_localization]: initialPoseReceived
[pcl_localization_node-1] [INFO] [1732112603.285501413] [pcl_localization]: initialPoseReceived end
[pcl_localization_node-1] [INFO] [1732112606.002641021] [pcl_localization]: Map Size 2366088
[pcl_localization_node-1] [INFO] [1732112606.089621510] [pcl_localization]: Initial Map Published
[pcl_localization_node-1] [INFO] [1732112606.432748191] [pcl_localization]: Activating end

The Rviz can successfully display the map, but cannot display real-time input point clouds and path. image

I wonder why the algorithm does not output a positioning trajectory, and does it not support the use of other lidar?

rsasaki0109 commented 3 days ago

I think it should work with Rslidar as well. Wouldn’t the issue be resolved by adding the --clock option when using ros2 bag play?