Repository: rsasaki0109/lidar_localization_ros2 Branch: main Commit: 7204195a10ad Files: 13 Total size: 63.6 KB Directory structure: gitextract_mandirxt/ ├── .github/ │ └── workflows/ │ └── main.yml ├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── include/ │ └── lidar_localization/ │ ├── lidar_localization_component.hpp │ └── lidar_undistortion.hpp ├── launch/ │ └── lidar_localization.launch.py ├── package.xml ├── param/ │ └── localization.yaml ├── rviz/ │ └── localization.rviz └── src/ ├── lidar_localization_component.cpp └── lidar_localization_node.cpp ================================================ FILE CONTENTS ================================================ ================================================ FILE: .github/workflows/main.yml ================================================ name: build on: pull_request: branches: - main jobs: humble_build: name: Build on Humble runs-on: ubuntu-22.04 container: ros:humble steps: - uses: actions/checkout@v1 with: submodules: true - name: Copy repository run: | mkdir -p ~/ros2_ws/src/lidar_localization_ros2 cp -rf . ~/ros2_ws/src/lidar_localization_ros2 - name: Install dependencies run: | source /opt/ros/humble/setup.bash sudo apt install -y python3-rosdep apt update rosdep update cd ~/ros2_ws/src git clone https://github.com/rsasaki0109/ndt_omp_ros2.git -b humble rosdep install -r -y --from-paths . --ignore-src shell: bash - name: Build packages run: | source /opt/ros/humble/setup.bash sudo apt install python3-colcon-common-extensions cd ~/ros2_ws colcon build source ~/ros2_ws/install/setup.bash shell: bash jazzy_build: name: Build on Jazzy runs-on: ubuntu-24.04 container: ros:jazzy steps: - uses: actions/checkout@v1 with: submodules: true - name: Copy repository run: | mkdir -p ~/ros2_ws/src/lidar_localization_ros2 cp -rf . ~/ros2_ws/src/lidar_localization_ros2 - name: Install dependencies run: | source /opt/ros/jazzy/setup.bash sudo apt install -y python3-rosdep apt update rosdep update cd ~/ros2_ws/src git clone https://github.com/rsasaki0109/ndt_omp_ros2.git -b humble rosdep install -r -y --from-paths . --ignore-src shell: bash - name: Build packages run: | source /opt/ros/jazzy/setup.bash sudo apt install python3-colcon-common-extensions cd ~/ros2_ws colcon build source ~/ros2_ws/install/setup.bash shell: bash ================================================ FILE: .gitignore ================================================ .vscode build map/bin_tc-2017-10-15--ndmap.pcd ================================================ FILE: CMakeLists.txt ================================================ cmake_minimum_required(VERSION 3.5) project(lidar_localization_ros2) if(NOT CMAKE_C_STANDARD) set(CMAKE_C_STANDARD 99) endif() if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 14) endif() SET(CMAKE_CXX_FLAGS "-O2 -g ${CMAKE_CXX_FLAGS}") find_package(ament_cmake REQUIRED) find_package(rclcpp_lifecycle REQUIRED) find_package(lifecycle_msgs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(sensor_msgs REQUIRED) find_package(nav_msgs REQUIRED) find_package(tf2_ros REQUIRED) find_package(tf2_geometry_msgs REQUIRED) find_package(tf2_sensor_msgs REQUIRED) find_package(tf2_eigen REQUIRED) find_package(pcl_conversions REQUIRED) find_package(PCL REQUIRED) find_package(ndt_omp_ros2 REQUIRED) find_package(OpenMP) if (OPENMP_FOUND) set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") endif() add_library(lidar_localization_component SHARED src/lidar_localization_component.cpp ) ament_target_dependencies(lidar_localization_component rclcpp tf2_ros tf2_geometry_msgs tf2_sensor_msgs tf2_eigen geometry_msgs sensor_msgs nav_msgs pcl_conversions ndt_omp_ros2 ) include_directories( include ${PCL_INCLUDE_DIRS} ${lifecycle_msgs_INCLUDE_DIRS} ${rclcpp_lifecycle_INCLUDE_DIRS} ${rclcpp_INCLUDE_DIRS}) add_executable(lidar_localization_node src/lidar_localization_node.cpp) target_link_libraries(lidar_localization_node lidar_localization_component ${PCL_LIBRARIES} ${rclcpp_lifecycle_LIBRARIES} ${std_msgs_LIBRARIES} ) link_directories( ${PCL_LIBRARY_DIRS} ) add_definitions(${PCL_DEFINITIONS}) ament_export_libraries(lidar_localization_component) install(TARGETS lidar_localization_node DESTINATION lib/${PROJECT_NAME}) install(TARGETS lidar_localization_component ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin) install(DIRECTORY launch param DESTINATION share/${PROJECT_NAME}/ ) ament_package() ================================================ FILE: LICENSE ================================================ BSD 2-Clause License Copyright (c) 2020, Ryohei Sasaki All rights reserved. Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ================================================ FILE: README.md ================================================ # lidar_localization_ros2 A ROS2 package of 3D LIDAR-based Localization. Green: path, Red: map (the 5x5 grids in size of 50m × 50m) ## Requirements - [ndt_omp_ros2](https://github.com/rsasaki0109/ndt_omp_ros2.git) ## IO - input /cloud (sensor_msgs/PointCloud2) /map (sensor_msgs/PointCloud2) /initialpose (geometry_msgs/PoseStamed)(when `set_initial_pose` is false) /odom (nav_msgs/Odometry)(optional) /imu (sensor_msgs/Imu)(optional) - output /pcl_pose (geometry_msgs/PoseStamped) /path (nav_msgs/Path) /initial_map (sensor_msgs/PointCloud2)(when `use_pcd_map` is true) ## params |Name|Type|Default value|Description| |---|---|---|---| |registration_method|string|"NDT_OMP"|"NDT" or "GICP" or "NDT_OMP" or "GICP_OMP"| |score_threshold|double|2.0|registration score threshold| |ndt_resolution|double|2.0|resolution size of voxels[m]| |ndt_step_size|double|0.1|step_size maximum step length[m]| |ndt_num_threads|int|4|threads using NDT_OMP(if `0` is set, maximum alloawble threads are used.)| |transform_epsilon|double|0.01|transform epsilon to stop iteration in registration| |voxel_leaf_size|double|0.2|down sample size of input cloud[m]| |scan_max_range|double|100.0|max range of input cloud[m]| |scan_min_range|double|1.0|min range of input cloud[m]| |scan_periad|double|0.1|scan period of input cloud[sec]| |use_pcd_map|bool|false|whether pcd_map is used or not| |map_path|string|"/map/map.pcd"|pcd_map or ply_map file path| |set_initial_pose|bool|false|whether or not to set the default value in the param file| |initial_pose_x|double|0.0|x-coordinate of the initial pose value[m]| |initial_pose_y|double|0.0|y-coordinate of the initial pose value[m]| |initial_pose_z|double|0.0|z-coordinate of the initial pose value[m]| |initial_pose_qx|double|0.0|Quaternion x of the initial pose value| |initial_pose_qy|double|0.0|Quaternion y of the initial pose value| |initial_pose_qz|double|0.0|Quaternion z of the initial pose value| |initial_pose_qw|double|1.0|Quaternion w of the initial pose value| |use_odom|bool|false|whether odom is used or not for initial attitude in point cloud registration| |use_imu|bool|false|whether 9-axis imu is used or not for point cloud distortion correction| |enable_debug|bool|false|whether debug is done or not| |enable_timer_publishing|bool|false|if true, publish tf and pose on a set timer frequency| |pose_publish_frequency|double|10.0|publishing frequency if enable_timer_publishing is true| ## demo demo data(ROS1) by Tier IV（The link has changed and is now broken.) https://data.tier4.jp/rosbag_details/?id=212 To use ros1 rosbag , use [rosbags](https://pypi.org/project/rosbags/). The Velodyne VLP-16 was used in this data. Before running, put `bin_tc-2017-10-15-ndmap.pcd` into your `map` directory and edit the `map_path` parameter of `localization.yaml` in the `param` directory accordingly. ``` rviz2 -d src/lidar_localization_ros2/rviz/localization.rviz ros2 launch lidar_localization_ros2 lidar_localization.launch.py ros2 bag play tc_2017-10-15-15-34-02_free_download/ ``` Green: path, Red: map (the 5x5 grids in size of 50m × 50m) ================================================ FILE: include/lidar_localization/lidar_localization_component.hpp ================================================ #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include "lifecycle_msgs/msg/transition.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" #include "rclcpp_lifecycle/lifecycle_publisher.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "sensor_msgs/msg/point_cloud2.hpp" #include "sensor_msgs/msg/imu.hpp" #include "nav_msgs/msg/odometry.hpp" #include "nav_msgs/msg/path.hpp" #include #include #include #include #include #include #include "lidar_localization/lidar_undistortion.hpp" using namespace std::chrono_literals; class PCLLocalization : public rclcpp_lifecycle::LifecycleNode { public: explicit PCLLocalization(const rclcpp::NodeOptions & options); using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; CallbackReturn on_configure(const rclcpp_lifecycle::State &); CallbackReturn on_activate(const rclcpp_lifecycle::State &); CallbackReturn on_deactivate(const rclcpp_lifecycle::State &); CallbackReturn on_cleanup(const rclcpp_lifecycle::State &); CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state); CallbackReturn on_error(const rclcpp_lifecycle::State & state); void initializeParameters(); void initializePubSub(); void initializeRegistration(); void initialPoseReceived(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); void mapReceived(const sensor_msgs::msg::PointCloud2::SharedPtr msg); void odomReceived(const nav_msgs::msg::Odometry::ConstSharedPtr msg); void imuReceived(const sensor_msgs::msg::Imu::ConstSharedPtr msg); void cloudReceived(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg); // void gnssReceived(); tf2_ros::TransformBroadcaster broadcaster_; rclcpp::Clock clock_; tf2_ros::Buffer tfbuffer_; tf2_ros::TransformListener tflistener_; rclcpp::Subscription::ConstSharedPtr initial_pose_sub_; rclcpp_lifecycle::LifecyclePublisher::SharedPtr pose_pub_; rclcpp_lifecycle::LifecyclePublisher::SharedPtr path_pub_; rclcpp_lifecycle::LifecyclePublisher::SharedPtr initial_map_pub_; rclcpp::Subscription::ConstSharedPtr map_sub_; rclcpp::Subscription::ConstSharedPtr odom_sub_; rclcpp::Subscription::ConstSharedPtr cloud_sub_; rclcpp::Subscription::ConstSharedPtr imu_sub_; boost::shared_ptr> registration_; pcl::VoxelGrid voxel_grid_filter_; geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr corrent_pose_with_cov_stamped_ptr_; nav_msgs::msg::Path::SharedPtr path_ptr_; sensor_msgs::msg::PointCloud2::ConstSharedPtr last_scan_ptr_; bool map_recieved_{false}; bool initialpose_recieved_{false}; // parameters std::string global_frame_id_; std::string odom_frame_id_; std::string base_frame_id_; std::string registration_method_; double scan_max_range_; double scan_min_range_; double scan_period_; double score_threshold_; double ndt_resolution_; double ndt_step_size_; double transform_epsilon_; double voxel_leaf_size_; bool use_pcd_map_{false}; std::string map_path_; bool set_initial_pose_{false}; double initial_pose_x_; double initial_pose_y_; double initial_pose_z_; double initial_pose_qx_; double initial_pose_qy_; double initial_pose_qz_; double initial_pose_qw_; bool use_odom_{false}; double last_odom_received_time_; bool use_imu_{false}; bool enable_debug_{false}; bool enable_map_odom_tf_{false}; int ndt_num_threads_; int ndt_max_iterations_; double pose_publish_frequency_; bool enable_timer_publishing_{false}; // imu LidarUndistortion lidar_undistortion_; rclcpp::TimerBase::SharedPtr pose_publish_timer_; void timerPublishPose(); }; ================================================ FILE: include/lidar_localization/lidar_undistortion.hpp ================================================ #ifndef LIDAR_UNDISTORTION_HPP_ #define LIDAR_UNDISTORTION_HPP_ #include #include #include #include #include #include #include class LidarUndistortion { public: LidarUndistortion() {} // Ref:LeGO-LOAM(BSD-3 LICENSE) // https://github.com/RobustFieldAutonomyLab/LeGO-LOAM/blob/master/LeGO-LOAM/src/featureAssociation.cpp#L431-L459 void getImu( Eigen::Vector3f angular_velo, Eigen::Vector3f acc, const Eigen::Quaternionf quat, const double imu_time /*[sec]*/) { float roll, pitch, yaw; Eigen::Affine3f affine(quat); pcl::getEulerAngles(affine, roll, pitch, yaw); imu_ptr_last_ = (imu_ptr_last_ + 1) % imu_que_length_; if ((imu_ptr_last_ + 1) % imu_que_length_ == imu_ptr_front_) { imu_ptr_front_ = (imu_ptr_front_ + 1) % imu_que_length_; } imu_time_[imu_ptr_last_] = imu_time; imu_roll_[imu_ptr_last_] = roll; imu_pitch_[imu_ptr_last_] = pitch; imu_yaw_[imu_ptr_last_] = yaw; imu_acc_x_[imu_ptr_last_] = acc.x(); imu_acc_y_[imu_ptr_last_] = acc.y(); imu_acc_z_[imu_ptr_last_] = acc.z(); imu_angular_velo_x_[imu_ptr_last_] = angular_velo.x(); imu_angular_velo_y_[imu_ptr_last_] = angular_velo.y(); imu_angular_velo_z_[imu_ptr_last_] = angular_velo.z(); Eigen::Matrix3f rot = quat.toRotationMatrix(); acc = rot * acc; // angular_velo = rot * angular_velo; int imu_ptr_back = (imu_ptr_last_ - 1 + imu_que_length_) % imu_que_length_; double time_diff = imu_time_[imu_ptr_last_] - imu_time_[imu_ptr_back]; if (time_diff < scan_period_) { imu_shift_x_[imu_ptr_last_] = imu_shift_x_[imu_ptr_back] + imu_velo_x_[imu_ptr_back] * time_diff + acc(0) * time_diff * time_diff * 0.5; imu_shift_y_[imu_ptr_last_] = imu_shift_y_[imu_ptr_back] + imu_velo_y_[imu_ptr_back] * time_diff + acc(1) * time_diff * time_diff * 0.5; imu_shift_z_[imu_ptr_last_] = imu_shift_z_[imu_ptr_back] + imu_velo_z_[imu_ptr_back] * time_diff + acc(2) * time_diff * time_diff * 0.5; imu_velo_x_[imu_ptr_last_] = imu_velo_x_[imu_ptr_back] + acc(0) * time_diff; imu_velo_y_[imu_ptr_last_] = imu_velo_y_[imu_ptr_back] + acc(1) * time_diff; imu_velo_z_[imu_ptr_last_] = imu_velo_z_[imu_ptr_back] + acc(2) * time_diff; imu_angular_rot_x_[imu_ptr_last_] = imu_angular_rot_x_[imu_ptr_back] + angular_velo(0) * time_diff; imu_angular_rot_y_[imu_ptr_last_] = imu_angular_rot_y_[imu_ptr_back] + angular_velo(1) * time_diff; imu_angular_rot_z_[imu_ptr_last_] = imu_angular_rot_z_[imu_ptr_back] + angular_velo(2) * time_diff; } } // Ref:LeGO-LOAM(BSD-3 LICENSE) // https://github.com/RobustFieldAutonomyLab/LeGO-LOAM/blob/master/LeGO-LOAM/src/featureAssociation.cpp#L491-L619 void adjustDistortion( pcl::PointCloud::Ptr & cloud, const double scan_time /*[sec]*/) { bool half_passed = false; int cloud_size = cloud->points.size(); float start_ori = -std::atan2(cloud->points[0].y, cloud->points[0].x); float end_ori = -std::atan2(cloud->points[cloud_size - 1].y, cloud->points[cloud_size - 1].x); if (end_ori - start_ori > 3 * M_PI) { end_ori -= 2 * M_PI; } else if (end_ori - start_ori < M_PI) { end_ori += 2 * M_PI; } float ori_diff = end_ori - start_ori; Eigen::Vector3f rpy_start, shift_start, velo_start, rpy_cur, shift_cur, velo_cur; Eigen::Vector3f shift_from_start; Eigen::Matrix3f r_s_i, r_c; Eigen::Vector3f adjusted_p; float ori_h; for (int i = 0; i < cloud_size; ++i) { pcl::PointXYZI & p = cloud->points[i]; ori_h = -std::atan2(p.y, p.x); if (!half_passed) { if (ori_h < start_ori - M_PI * 0.5) { ori_h += 2 * M_PI; } else if (ori_h > start_ori + M_PI * 1.5) { ori_h -= 2 * M_PI; } if (ori_h - start_ori > M_PI) { half_passed = true; } } else { ori_h += 2 * M_PI; if (ori_h < end_ori - 1.5 * M_PI) { ori_h += 2 * M_PI; } else if (ori_h > end_ori + 0.5 * M_PI) { ori_h -= 2 * M_PI; } } float rel_time = (ori_h - start_ori) / ori_diff * scan_period_; if (imu_ptr_last_ > 0) { imu_ptr_front_ = imu_ptr_last_iter_; while (imu_ptr_front_ != imu_ptr_last_) { if (scan_time + rel_time > imu_time_[imu_ptr_front_]) { break; } imu_ptr_front_ = (imu_ptr_front_ + 1) % imu_que_length_; } if (scan_time + rel_time > imu_time_[imu_ptr_front_]) { rpy_cur(0) = imu_roll_[imu_ptr_front_]; rpy_cur(1) = imu_pitch_[imu_ptr_front_]; rpy_cur(2) = imu_yaw_[imu_ptr_front_]; shift_cur(0) = imu_shift_x_[imu_ptr_front_]; shift_cur(1) = imu_shift_y_[imu_ptr_front_]; shift_cur(2) = imu_shift_z_[imu_ptr_front_]; velo_cur(0) = imu_velo_x_[imu_ptr_front_]; velo_cur(1) = imu_velo_y_[imu_ptr_front_]; velo_cur(2) = imu_velo_z_[imu_ptr_front_]; } else { int imu_ptr_back = (imu_ptr_front_ - 1 + imu_que_length_) % imu_que_length_; float ratio_front = (scan_time + rel_time - imu_time_[imu_ptr_back]) / (imu_time_[imu_ptr_front_] - imu_time_[imu_ptr_back]); float ratio_back = 1.0 - ratio_front; rpy_cur(0) = imu_roll_[imu_ptr_front_] * ratio_front + imu_roll_[imu_ptr_back] * ratio_back; rpy_cur(1) = imu_pitch_[imu_ptr_front_] * ratio_front + imu_pitch_[imu_ptr_back] * ratio_back; rpy_cur(2) = imu_yaw_[imu_ptr_front_] * ratio_front + imu_yaw_[imu_ptr_back] * ratio_back; shift_cur(0) = imu_shift_x_[imu_ptr_front_] * ratio_front + imu_shift_x_[imu_ptr_back] * ratio_back; shift_cur(1) = imu_shift_y_[imu_ptr_front_] * ratio_front + imu_shift_y_[imu_ptr_back] * ratio_back; shift_cur(2) = imu_shift_z_[imu_ptr_front_] * ratio_front + imu_shift_z_[imu_ptr_back] * ratio_back; velo_cur(0) = imu_velo_x_[imu_ptr_front_] * ratio_front + imu_velo_x_[imu_ptr_back] * ratio_back; velo_cur(1) = imu_velo_y_[imu_ptr_front_] * ratio_front + imu_velo_y_[imu_ptr_back] * ratio_back; velo_cur(2) = imu_velo_z_[imu_ptr_front_] * ratio_front + imu_velo_z_[imu_ptr_back] * ratio_back; } r_c = ( Eigen::AngleAxisf(rpy_cur(2), Eigen::Vector3f::UnitZ()) * Eigen::AngleAxisf(rpy_cur(1), Eigen::Vector3f::UnitY()) * Eigen::AngleAxisf(rpy_cur(0), Eigen::Vector3f::UnitX()) ).toRotationMatrix(); if (i == 0) { rpy_start = rpy_cur; shift_start = shift_cur; velo_start = velo_cur; r_s_i = r_c.inverse(); } else { shift_from_start = shift_cur - shift_start - velo_start * rel_time; adjusted_p = r_s_i * (r_c * Eigen::Vector3f(p.x, p.y, p.z) + shift_from_start); p.x = adjusted_p.x(); p.y = adjusted_p.y(); p.z = adjusted_p.z(); } } imu_ptr_last_iter_ = imu_ptr_front_; } } void setScanPeriod(const double scan_period /*[sec]*/) { scan_period_ = scan_period; } private: double scan_period_{0.1}; static const int imu_que_length_{200}; int imu_ptr_front_{0}, imu_ptr_last_{-1}, imu_ptr_last_iter_{0}; std::array imu_time_; std::array imu_roll_; std::array imu_pitch_; std::array imu_yaw_; std::array imu_acc_x_; std::array imu_acc_y_; std::array imu_acc_z_; std::array imu_velo_x_; std::array imu_velo_y_; std::array imu_velo_z_; std::array imu_shift_x_; std::array imu_shift_y_; std::array imu_shift_z_; std::array imu_angular_velo_x_; std::array imu_angular_velo_y_; std::array imu_angular_velo_z_; std::array imu_angular_rot_x_; std::array imu_angular_rot_y_; std::array imu_angular_rot_z_; }; #endif // LIDAR_UNDISTORTION_HPP_ ================================================ FILE: launch/lidar_localization.launch.py ================================================ 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','velodyne'] ) 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('lidar_localization_ros2'), 'param', 'localization.yaml')) lidar_localization = launch_ros.actions.LifecycleNode( name='lidar_localization', namespace='', package='lidar_localization_ros2', executable='lidar_localization_node', parameters=[localization_param_dir], remappings=[('/cloud','/velodyne_points')], output='screen') to_inactive = launch.actions.EmitEvent( event=launch_ros.events.lifecycle.ChangeState( lifecycle_node_matcher=launch.events.matches_action(lidar_localization), transition_id=lifecycle_msgs.msg.Transition.TRANSITION_CONFIGURE, ) ) from_unconfigured_to_inactive = launch.actions.RegisterEventHandler( launch_ros.event_handlers.OnStateTransition( target_lifecycle_node=lidar_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(lidar_localization), transition_id=lifecycle_msgs.msg.Transition.TRANSITION_CONFIGURE, )), ], ) ) from_inactive_to_active = launch.actions.RegisterEventHandler( launch_ros.event_handlers.OnStateTransition( target_lifecycle_node=lidar_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(lidar_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(lidar_localization) ld.add_action(lidar_tf) ld.add_action(to_inactive) return ld ================================================ FILE: package.xml ================================================ lidar_localization_ros2 0.0.1 lidar_localization_ros2 ryohei sasaki BSD2.0 ament_cmake rclcpp_lifecycle lifecycle_msgs geometry_msgs sensor_msgs nav_msgs tf2_ros tf2_sensor_msgs tf2_geometry_msgs tf2_eigen pcl_conversions rclcpp_lifecycle lifecycle_msgs geometry_msgs sensor_msgs nav_msgs tf2_ros tf2_sensor_msgs tf2_geometry_msgs tf2_eigen pcl_conversions ament_lint_auto ament_lint_common ros_testing launch ndt_omp_ros2 ament_cmake ================================================ FILE: param/localization.yaml ================================================ /**: ros__parameters: registration_method: "NDT_OMP" score_threshold: 2.0 ndt_resolution: 1.0 ndt_step_size: 0.1 ndt_num_threads: 4 ndt_max_iterations: 35 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: "" 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: 0.0 use_odom: false use_imu: false enable_debug: true enable_map_odom_tf: false global_frame_id: map odom_frame_id: odom base_frame_id: base_link enable_timer_publishing: false pose_publish_frequency: 30.0 ================================================ FILE: rviz/localization.rviz ================================================ Panels: - Class: rviz_common/Displays Help Height: 78 Name: Displays Property Tree Widget: Expanded: - /Global Options1 - /Status1 Splitter Ratio: 0.5 Tree Height: 617 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties Expanded: - /Publish Point1 Name: Tool Properties Splitter Ratio: 0.5886790156364441 - Class: rviz_common/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 Visualization Manager: Class: "" Displays: - Alpha: 0.5 Cell Size: 50 Class: rviz_default_plugins/Grid Color: 160; 160; 164 Enabled: true Line Style: Line Width: 0.029999999329447746 Value: Lines Name: Grid Normal Cell Count: 0 Offset: X: 0 Y: 0 Z: 0 Plane: XY Plane Cell Count: 5 Reference Frame: Value: true - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 10 Min Value: -10 Value: true Axis: Z Channel Name: intensity Class: rviz_default_plugins/PointCloud2 Color: 255; 255; 255 Color Transformer: "" Decay Time: 0 Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 Max Intensity: 4096 Min Color: 0; 0; 0 Min Intensity: 0 Name: PointCloud2 Position Transformer: "" Selectable: true Size (Pixels): 3 Size (m): 0.009999999776482582 Style: Flat Squares Topic: Depth: 5 Durability Policy: Volatile Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /map Use Fixed Frame: true Use rainbow: true Value: true - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 21.498470306396484 Min Value: -1.9757282733917236 Value: true Axis: Z Channel Name: ring Class: rviz_default_plugins/PointCloud2 Color: 255; 255; 255 Color Transformer: Intensity Decay Time: 0 Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 Max Intensity: 31 Min Color: 0; 0; 0 Min Intensity: 0 Name: PointCloud2 Position Transformer: XYZ Selectable: true Size (Pixels): 3 Size (m): 0.009999999776482582 Style: Points Topic: Depth: 5 Durability Policy: Volatile Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /velodyne_points Use Fixed Frame: true Use rainbow: true Value: true - Alpha: 1 Buffer Length: 1 Class: rviz_default_plugins/Path Color: 25; 255; 0 Enabled: true Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 Line Style: Lines Line Width: 0.029999999329447746 Name: Path Offset: X: 0 Y: 0 Z: 0 Pose Color: 255; 85; 255 Pose Style: None Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 Topic: Depth: 5 Durability Policy: Volatile Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /path Value: true - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 29.598661422729492 Min Value: -3.1513378620147705 Value: true Axis: Z Channel Name: intensity Class: rviz_default_plugins/PointCloud2 Color: 255; 255; 255 Color Transformer: AxisColor Decay Time: 0 Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 Max Intensity: 0 Min Color: 0; 0; 0 Min Intensity: 0 Name: PointCloud2 Position Transformer: XYZ Selectable: true Size (Pixels): 1 Size (m): 0.30000001192092896 Style: Points Topic: Depth: 5 Durability Policy: Volatile Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /initial_map Use Fixed Frame: true Use rainbow: true Value: true - Alpha: 1 Axes Length: 1 Axes Radius: 0.10000000149011612 Class: rviz_default_plugins/PoseWithCovariance Color: 255; 25; 0 Covariance: Orientation: Alpha: 0.5 Color: 255; 255; 127 Color Style: Unique Frame: Local Offset: 1 Scale: 1 Value: true Position: Alpha: 0.30000001192092896 Color: 204; 51; 204 Scale: 1 Value: true Value: true Enabled: true Head Length: 0.30000001192092896 Head Radius: 0.10000000149011612 Name: PoseWithCovariance Shaft Length: 1 Shaft Radius: 0.05000000074505806 Shape: Arrow Topic: Depth: 5 Durability Policy: Volatile Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /pcl_pose Value: true Enabled: true Global Options: Background Color: 48; 48; 48 Fixed Frame: map Frame Rate: 30 Name: root Tools: - Class: rviz_default_plugins/MoveCamera - Class: rviz_default_plugins/Select - Class: rviz_default_plugins/FocusCamera - Class: rviz_default_plugins/Measure Line color: 128; 128; 0 - Class: rviz_default_plugins/SetInitialPose Covariance x: 0.25 Covariance y: 0.25 Covariance yaw: 0.06853891909122467 Topic: Depth: 5 Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable Value: /initialpose - Class: rviz_default_plugins/SetGoal Topic: Depth: 5 Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable Value: /move_base_simple/goal - Class: rviz_default_plugins/PublishPoint Single click: true Topic: Depth: 5 Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable Value: /clicked_point Transformation: Current: Class: rviz_default_plugins/TF Value: true Views: Current: Angle: 0 Class: rviz_default_plugins/TopDownOrtho Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 Scale: 16.579959869384766 Target Frame: base_link Value: TopDownOrtho (rviz_default_plugins) X: 0 Y: 0 Saved: ~ Window Geometry: Displays: collapsed: false Height: 846 Hide Left Dock: false Hide Right Dock: false QMainWindow State: 000000ff00000000fd000000040000000000000156000002f4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002f4000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002f4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002f4000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000023f000002f400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Tool Properties: collapsed: false Views: collapsed: false Width: 1200 X: 514 Y: 127 ================================================ FILE: src/lidar_localization_component.cpp ================================================ #include #include PCLLocalization::PCLLocalization(const rclcpp::NodeOptions & options) : rclcpp_lifecycle::LifecycleNode("lidar_localization", options), clock_(RCL_ROS_TIME), tfbuffer_(std::make_shared(clock_)), tflistener_(tfbuffer_), broadcaster_(this) { declare_parameter("global_frame_id", "map"); declare_parameter("odom_frame_id", "odom"); declare_parameter("base_frame_id", "base_link"); declare_parameter("enable_map_odom_tf", false); declare_parameter("registration_method", "NDT"); declare_parameter("score_threshold", 2.0); declare_parameter("ndt_resolution", 1.0); declare_parameter("ndt_step_size", 0.1); declare_parameter("ndt_max_iterations", 35); declare_parameter("ndt_num_threads", 4); declare_parameter("transform_epsilon", 0.01); declare_parameter("voxel_leaf_size", 0.2); declare_parameter("scan_max_range", 100.0); declare_parameter("scan_min_range", 1.0); declare_parameter("scan_period", 0.1); declare_parameter("use_pcd_map", false); declare_parameter("map_path", "/map/map.pcd"); declare_parameter("set_initial_pose", false); declare_parameter("initial_pose_x", 0.0); declare_parameter("initial_pose_y", 0.0); declare_parameter("initial_pose_z", 0.0); declare_parameter("initial_pose_qx", 0.0); declare_parameter("initial_pose_qy", 0.0); declare_parameter("initial_pose_qz", 0.0); declare_parameter("initial_pose_qw", 1.0); declare_parameter("use_odom", false); declare_parameter("use_imu", false); declare_parameter("enable_debug", false); declare_parameter("enable_timer_publishing", false); declare_parameter("pose_publish_frequency", 10.0); } using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; CallbackReturn PCLLocalization::on_configure(const rclcpp_lifecycle::State &) { RCLCPP_INFO(get_logger(), "Configuring"); initializeParameters(); initializePubSub(); initializeRegistration(); path_ptr_ = std::make_shared(); path_ptr_->header.frame_id = global_frame_id_; RCLCPP_INFO(get_logger(), "Configuring end"); return CallbackReturn::SUCCESS; } CallbackReturn PCLLocalization::on_activate(const rclcpp_lifecycle::State &) { RCLCPP_INFO(get_logger(), "Activating"); pose_pub_->on_activate(); path_pub_->on_activate(); initial_map_pub_->on_activate(); if (set_initial_pose_) { auto msg = std::make_shared(); msg->header.stamp = now(); msg->header.frame_id = global_frame_id_; msg->pose.pose.position.x = initial_pose_x_; msg->pose.pose.position.y = initial_pose_y_; msg->pose.pose.position.z = initial_pose_z_; msg->pose.pose.orientation.x = initial_pose_qx_; msg->pose.pose.orientation.y = initial_pose_qy_; msg->pose.pose.orientation.z = initial_pose_qz_; msg->pose.pose.orientation.w = initial_pose_qw_; geometry_msgs::msg::PoseStamped::SharedPtr pose_stamped(new geometry_msgs::msg::PoseStamped); pose_stamped->header.stamp = msg->header.stamp; pose_stamped->header.frame_id = global_frame_id_; pose_stamped->pose = msg->pose.pose; path_ptr_->poses.push_back(*pose_stamped); initialPoseReceived(msg); } if (use_pcd_map_) { pcl::PointCloud::Ptr map_cloud_ptr(new pcl::PointCloud); // load a pcd or ply file if (map_path_.rfind(".pcd") != std::string::npos) { RCLCPP_INFO(get_logger(), "Loading pcd map from: %s", map_path_.c_str()); if (pcl::io::loadPCDFile(map_path_, *map_cloud_ptr) == -1) { RCLCPP_ERROR(get_logger(), "Failed to load pcd file: %s", map_path_.c_str()); return CallbackReturn::FAILURE; } } else if (map_path_.rfind(".ply") != std::string::npos) { RCLCPP_INFO(get_logger(), "Loading ply map from: %s", map_path_.c_str()); if (pcl::io::loadPLYFile(map_path_, *map_cloud_ptr) == -1) { RCLCPP_ERROR(get_logger(), "Failed to load ply file: %s", map_path_.c_str()); return CallbackReturn::FAILURE; } } else { RCLCPP_ERROR( get_logger(), "Unsupported map file format. Please use .pcd or .ply: %s", map_path_.c_str()); return CallbackReturn::FAILURE; } RCLCPP_INFO(get_logger(), "Map Size %ld", map_cloud_ptr->size()); sensor_msgs::msg::PointCloud2::SharedPtr map_msg_ptr(new sensor_msgs::msg::PointCloud2); pcl::toROSMsg(*map_cloud_ptr, *map_msg_ptr); map_msg_ptr->header.frame_id = global_frame_id_; initial_map_pub_->publish(*map_msg_ptr); RCLCPP_INFO(get_logger(), "Initial Map Published"); if (registration_method_ == "GICP" || registration_method_ == "GICP_OMP") { pcl::PointCloud::Ptr filtered_cloud_ptr(new pcl::PointCloud()); voxel_grid_filter_.setInputCloud(map_cloud_ptr); voxel_grid_filter_.filter(*filtered_cloud_ptr); registration_->setInputTarget(filtered_cloud_ptr); } else { registration_->setInputTarget(map_cloud_ptr); } map_recieved_ = true; } RCLCPP_INFO(get_logger(), "Activating end"); return CallbackReturn::SUCCESS; } CallbackReturn PCLLocalization::on_deactivate(const rclcpp_lifecycle::State &) { RCLCPP_INFO(get_logger(), "Deactivating"); pose_pub_->on_deactivate(); path_pub_->on_deactivate(); initial_map_pub_->on_deactivate(); RCLCPP_INFO(get_logger(), "Deactivating end"); return CallbackReturn::SUCCESS; } CallbackReturn PCLLocalization::on_cleanup(const rclcpp_lifecycle::State &) { RCLCPP_INFO(get_logger(), "Cleaning Up"); initial_pose_sub_.reset(); initial_map_pub_.reset(); path_pub_.reset(); pose_pub_.reset(); odom_sub_.reset(); cloud_sub_.reset(); imu_sub_.reset(); if (enable_timer_publishing_){ pose_publish_timer_.reset(); } RCLCPP_INFO(get_logger(), "Cleaning Up end"); return CallbackReturn::SUCCESS; } CallbackReturn PCLLocalization::on_shutdown(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "Shutting Down from %s", state.label().c_str()); return CallbackReturn::SUCCESS; } CallbackReturn PCLLocalization::on_error(const rclcpp_lifecycle::State & state) { RCLCPP_FATAL(get_logger(), "Error Processing from %s", state.label().c_str()); return CallbackReturn::SUCCESS; } void PCLLocalization::initializeParameters() { RCLCPP_INFO(get_logger(), "initializeParameters"); get_parameter("global_frame_id", global_frame_id_); get_parameter("odom_frame_id", odom_frame_id_); get_parameter("base_frame_id", base_frame_id_); get_parameter("enable_map_odom_tf", enable_map_odom_tf_); get_parameter("registration_method", registration_method_); get_parameter("score_threshold", score_threshold_); get_parameter("ndt_resolution", ndt_resolution_); get_parameter("ndt_step_size", ndt_step_size_); get_parameter("ndt_num_threads", ndt_num_threads_); get_parameter("ndt_max_iterations", ndt_max_iterations_); get_parameter("transform_epsilon", transform_epsilon_); get_parameter("voxel_leaf_size", voxel_leaf_size_); get_parameter("scan_max_range", scan_max_range_); get_parameter("scan_min_range", scan_min_range_); get_parameter("scan_period", scan_period_); get_parameter("use_pcd_map", use_pcd_map_); get_parameter("map_path", map_path_); get_parameter("set_initial_pose", set_initial_pose_); get_parameter("initial_pose_x", initial_pose_x_); get_parameter("initial_pose_y", initial_pose_y_); get_parameter("initial_pose_z", initial_pose_z_); get_parameter("initial_pose_qx", initial_pose_qx_); get_parameter("initial_pose_qy", initial_pose_qy_); get_parameter("initial_pose_qz", initial_pose_qz_); get_parameter("initial_pose_qw", initial_pose_qw_); get_parameter("use_odom", use_odom_); get_parameter("use_imu", use_imu_); get_parameter("enable_debug", enable_debug_); get_parameter("enable_timer_publishing", enable_timer_publishing_); get_parameter("pose_publish_frequency", pose_publish_frequency_); RCLCPP_INFO(get_logger(),"global_frame_id: %s", global_frame_id_.c_str()); RCLCPP_INFO(get_logger(),"odom_frame_id: %s", odom_frame_id_.c_str()); RCLCPP_INFO(get_logger(),"base_frame_id: %s", base_frame_id_.c_str()); RCLCPP_INFO(get_logger(),"enable_map_odom_tf: %d", enable_map_odom_tf_); RCLCPP_INFO(get_logger(),"registration_method: %s", registration_method_.c_str()); RCLCPP_INFO(get_logger(),"ndt_resolution: %lf", ndt_resolution_); RCLCPP_INFO(get_logger(),"ndt_step_size: %lf", ndt_step_size_); RCLCPP_INFO(get_logger(),"ndt_num_threads: %d", ndt_num_threads_); RCLCPP_INFO(get_logger(),"transform_epsilon: %lf", transform_epsilon_); RCLCPP_INFO(get_logger(),"voxel_leaf_size: %lf", voxel_leaf_size_); RCLCPP_INFO(get_logger(),"scan_max_range: %lf", scan_max_range_); RCLCPP_INFO(get_logger(),"scan_min_range: %lf", scan_min_range_); RCLCPP_INFO(get_logger(),"scan_period: %lf", scan_period_); RCLCPP_INFO(get_logger(),"use_pcd_map: %d", use_pcd_map_); RCLCPP_INFO(get_logger(),"map_path: %s", map_path_.c_str()); RCLCPP_INFO(get_logger(),"set_initial_pose: %d", set_initial_pose_); RCLCPP_INFO(get_logger(),"use_odom: %d", use_odom_); RCLCPP_INFO(get_logger(),"use_imu: %d", use_imu_); RCLCPP_INFO(get_logger(),"enable_debug: %d", enable_debug_); RCLCPP_INFO(get_logger(),"enable_timer_publishing: %d", enable_timer_publishing_); RCLCPP_INFO(get_logger(),"pose_publish_frequency: %lf", pose_publish_frequency_); } void PCLLocalization::initializePubSub() { RCLCPP_INFO(get_logger(), "initializePubSub"); pose_pub_ = create_publisher( "pcl_pose", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); path_pub_ = create_publisher( "path", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); initial_map_pub_ = create_publisher( "initial_map", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); initial_pose_sub_ = create_subscription( "initialpose", rclcpp::SystemDefaultsQoS(), std::bind(&PCLLocalization::initialPoseReceived, this, std::placeholders::_1)); map_sub_ = create_subscription( "map", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), std::bind(&PCLLocalization::mapReceived, this, std::placeholders::_1)); odom_sub_ = create_subscription( "odom", rclcpp::SensorDataQoS(), std::bind(&PCLLocalization::odomReceived, this, std::placeholders::_1)); cloud_sub_ = create_subscription( "cloud", rclcpp::SensorDataQoS(), std::bind(&PCLLocalization::cloudReceived, this, std::placeholders::_1)); imu_sub_ = create_subscription( "imu", rclcpp::SensorDataQoS(), std::bind(&PCLLocalization::imuReceived, this, std::placeholders::_1)); if (enable_timer_publishing_) { auto period = std::chrono::duration(1.0 / pose_publish_frequency_); pose_publish_timer_ = create_wall_timer( std::chrono::duration_cast(period), std::bind(&PCLLocalization::timerPublishPose, this)); } RCLCPP_INFO(get_logger(), "initializePubSub end"); } void PCLLocalization::initializeRegistration() { RCLCPP_INFO(get_logger(), "initializeRegistration"); if (registration_method_ == "GICP") { boost::shared_ptr> gicp( new pcl::GeneralizedIterativeClosestPoint()); gicp->setTransformationEpsilon(transform_epsilon_); registration_ = gicp; } else if (registration_method_ == "NDT") { boost::shared_ptr> ndt( new pcl::NormalDistributionsTransform()); ndt->setStepSize(ndt_step_size_); ndt->setResolution(ndt_resolution_); ndt->setTransformationEpsilon(transform_epsilon_); registration_ = ndt; } else if (registration_method_ == "NDT_OMP") { pclomp::NormalDistributionsTransform::Ptr ndt_omp( new pclomp::NormalDistributionsTransform()); ndt_omp->setStepSize(ndt_step_size_); ndt_omp->setResolution(ndt_resolution_); ndt_omp->setTransformationEpsilon(transform_epsilon_); if (ndt_num_threads_ > 0) { ndt_omp->setNumThreads(ndt_num_threads_); } else { ndt_omp->setNumThreads(omp_get_max_threads()); } registration_ = ndt_omp; } else if (registration_method_ == "GICP_OMP") { pclomp::GeneralizedIterativeClosestPoint::Ptr gicp_omp( new pclomp::GeneralizedIterativeClosestPoint()); gicp_omp->setTransformationEpsilon(transform_epsilon_); registration_ = gicp_omp; } else { RCLCPP_ERROR(get_logger(), "Invalid registration method."); exit(EXIT_FAILURE); } registration_->setMaximumIterations(ndt_max_iterations_); voxel_grid_filter_.setLeafSize(voxel_leaf_size_, voxel_leaf_size_, voxel_leaf_size_); RCLCPP_INFO(get_logger(), "initializeRegistration end"); } void PCLLocalization::initialPoseReceived(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) { RCLCPP_INFO(get_logger(), "initialPoseReceived"); if (msg->header.frame_id != global_frame_id_) { RCLCPP_WARN(this->get_logger(), "initialpose_frame_id does not match global_frame_id"); return; } initialpose_recieved_ = true; corrent_pose_with_cov_stamped_ptr_ = msg; pose_pub_->publish(*corrent_pose_with_cov_stamped_ptr_); if(last_scan_ptr_) { cloudReceived(last_scan_ptr_); } RCLCPP_INFO(get_logger(), "initialPoseReceived end"); } void PCLLocalization::mapReceived(const sensor_msgs::msg::PointCloud2::SharedPtr msg) { RCLCPP_INFO(get_logger(), "mapReceived"); pcl::PointCloud::Ptr map_cloud_ptr(new pcl::PointCloud); if (msg->header.frame_id != global_frame_id_) { RCLCPP_WARN(this->get_logger(), "map_frame_id does not match　global_frame_id"); return; } pcl::fromROSMsg(*msg, *map_cloud_ptr); if (registration_method_ == "GICP" || registration_method_ == "GICP_OMP") { pcl::PointCloud::Ptr filtered_cloud_ptr(new pcl::PointCloud()); voxel_grid_filter_.setInputCloud(map_cloud_ptr); voxel_grid_filter_.filter(*filtered_cloud_ptr); registration_->setInputTarget(filtered_cloud_ptr); } else { registration_->setInputTarget(map_cloud_ptr); } map_recieved_ = true; RCLCPP_INFO(get_logger(), "mapReceived end"); } void PCLLocalization::odomReceived(const nav_msgs::msg::Odometry::ConstSharedPtr msg) { if (!use_odom_) {return;} RCLCPP_INFO(get_logger(), "odomReceived"); double current_odom_received_time = msg->header.stamp.sec + msg->header.stamp.nanosec * 1e-9; double dt_odom = current_odom_received_time - last_odom_received_time_; last_odom_received_time_ = current_odom_received_time; if (dt_odom > 1.0 /* [sec] */) { RCLCPP_WARN(this->get_logger(), "odom time interval is too large"); return; } if (dt_odom < 0.0 /* [sec] */) { RCLCPP_WARN(this->get_logger(), "odom time interval is negative"); return; } tf2::Quaternion previous_quat_tf; double roll, pitch, yaw; tf2::fromMsg(corrent_pose_with_cov_stamped_ptr_->pose.pose.orientation, previous_quat_tf); tf2::Matrix3x3(previous_quat_tf).getRPY(roll, pitch, yaw); roll += msg->twist.twist.angular.x * dt_odom; pitch += msg->twist.twist.angular.y * dt_odom; yaw += msg->twist.twist.angular.z * dt_odom; Eigen::Quaterniond quat_eig = Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX()) * Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()); geometry_msgs::msg::Quaternion quat_msg = tf2::toMsg(quat_eig); Eigen::Vector3d odom{ msg->twist.twist.linear.x, msg->twist.twist.linear.y, msg->twist.twist.linear.z}; Eigen::Vector3d delta_position = quat_eig.matrix() * dt_odom * odom; corrent_pose_with_cov_stamped_ptr_->pose.pose.position.x += delta_position.x(); corrent_pose_with_cov_stamped_ptr_->pose.pose.position.y += delta_position.y(); corrent_pose_with_cov_stamped_ptr_->pose.pose.position.z += delta_position.z(); corrent_pose_with_cov_stamped_ptr_->pose.pose.orientation = quat_msg; } void PCLLocalization::imuReceived(const sensor_msgs::msg::Imu::ConstSharedPtr msg) { if (!use_imu_) {return;} sensor_msgs::msg::Imu tf_converted_imu; try { const geometry_msgs::msg::TransformStamped transform = tfbuffer_.lookupTransform( base_frame_id_, msg->header.frame_id, tf2::TimePointZero); geometry_msgs::msg::Vector3Stamped angular_velocity, linear_acceleration, transformed_angular_velocity, transformed_linear_acceleration; geometry_msgs::msg::Quaternion transformed_quaternion; angular_velocity.header = msg->header; angular_velocity.vector = msg->angular_velocity; linear_acceleration.header = msg->header; linear_acceleration.vector = msg->linear_acceleration; tf2::doTransform(angular_velocity, transformed_angular_velocity, transform); tf2::doTransform(linear_acceleration, transformed_linear_acceleration, transform); tf_converted_imu.angular_velocity = transformed_angular_velocity.vector; tf_converted_imu.linear_acceleration = transformed_linear_acceleration.vector; tf_converted_imu.orientation = transformed_quaternion; } catch (tf2::TransformException& ex) { std::cout << "Failed to lookup transform" << std::endl; RCLCPP_WARN(this->get_logger(), "Failed to lookup transform."); return; } Eigen::Vector3f angular_velo{tf_converted_imu.angular_velocity.x, tf_converted_imu.angular_velocity.y, tf_converted_imu.angular_velocity.z}; Eigen::Vector3f acc{tf_converted_imu.linear_acceleration.x, tf_converted_imu.linear_acceleration.y, tf_converted_imu.linear_acceleration.z}; Eigen::Quaternionf quat{msg->orientation.w, msg->orientation.x, msg->orientation.y, msg->orientation.z}; double imu_time = msg->header.stamp.sec + msg->header.stamp.nanosec * 1e-9; lidar_undistortion_.getImu(angular_velo, acc, quat, imu_time); } void PCLLocalization::cloudReceived(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) { if (!msg) { RCLCPP_WARN(get_logger(), "Received null point cloud message"); return; } if (!map_recieved_ || !initialpose_recieved_) {return;} RCLCPP_INFO(get_logger(), "cloudReceived"); pcl::PointCloud::Ptr cloud_ptr(new pcl::PointCloud); pcl::fromROSMsg(*msg, *cloud_ptr); // If your cloud is not robot-centric, convert to base_frame. if (msg->header.frame_id != base_frame_id_) { RCLCPP_DEBUG( this->get_logger(), "Transforming point cloud from %s to %s", msg->header.frame_id.c_str(), base_frame_id_.c_str()); geometry_msgs::msg::TransformStamped base_to_lidar_stamped; try { base_to_lidar_stamped = tfbuffer_.lookupTransform( base_frame_id_, msg->header.frame_id, msg->header.stamp, rclcpp::Duration::from_seconds(0.1)); } catch (const tf2::TransformException & ex) { RCLCPP_ERROR( this->get_logger(), "Could not transform %s to %s: %s", msg->header.frame_id.c_str(), base_frame_id_.c_str(), ex.what()); return; } Eigen::Matrix4f initial_transformation = tf2::transformToEigen(base_to_lidar_stamped.transform).matrix().cast(); pcl::PointCloud::Ptr transformed_cloud(new pcl::PointCloud()); pcl::transformPointCloud(*cloud_ptr, *transformed_cloud, initial_transformation); cloud_ptr = transformed_cloud; } if (use_imu_) { double received_time = msg->header.stamp.sec + msg->header.stamp.nanosec * 1e-9; lidar_undistortion_.adjustDistortion(cloud_ptr, received_time); } pcl::PointCloud::Ptr filtered_cloud_ptr(new pcl::PointCloud()); voxel_grid_filter_.setInputCloud(cloud_ptr); voxel_grid_filter_.filter(*filtered_cloud_ptr); double r; pcl::PointCloud tmp; for (const auto & p : filtered_cloud_ptr->points) { r = sqrt(pow(p.x, 2.0) + pow(p.y, 2.0)); if (scan_min_range_ < r && r < scan_max_range_) { tmp.push_back(p); } } pcl::PointCloud::Ptr tmp_ptr(new pcl::PointCloud(tmp)); registration_->setInputSource(tmp_ptr); Eigen::Affine3d affine; tf2::fromMsg(corrent_pose_with_cov_stamped_ptr_->pose.pose, affine); Eigen::Matrix4f init_guess = affine.matrix().cast(); pcl::PointCloud::Ptr output_cloud(new pcl::PointCloud); rclcpp::Clock system_clock; rclcpp::Time time_align_start = system_clock.now(); registration_->align(*output_cloud, init_guess); rclcpp::Time time_align_end = system_clock.now(); bool has_converged = registration_->hasConverged(); double fitness_score = registration_->getFitnessScore(); if (!has_converged) { RCLCPP_WARN(get_logger(), "The registration didn't converge."); return; } if (fitness_score > score_threshold_) { RCLCPP_WARN(get_logger(), "The fitness score is over %lf.", score_threshold_); } Eigen::Matrix4f final_transformation = registration_->getFinalTransformation(); Eigen::Matrix3d rot_mat = final_transformation.block<3, 3>(0, 0).cast(); Eigen::Quaterniond quat_eig(rot_mat); geometry_msgs::msg::Quaternion quat_msg = tf2::toMsg(quat_eig); corrent_pose_with_cov_stamped_ptr_->header.stamp = msg->header.stamp; corrent_pose_with_cov_stamped_ptr_->header.frame_id = global_frame_id_; corrent_pose_with_cov_stamped_ptr_->pose.pose.position.x = static_cast(final_transformation(0, 3)); corrent_pose_with_cov_stamped_ptr_->pose.pose.position.y = static_cast(final_transformation(1, 3)); corrent_pose_with_cov_stamped_ptr_->pose.pose.position.z = static_cast(final_transformation(2, 3)); corrent_pose_with_cov_stamped_ptr_->pose.pose.orientation = quat_msg; // publish here if timer is not enabled if (!enable_timer_publishing_){ pose_pub_->publish(*corrent_pose_with_cov_stamped_ptr_); geometry_msgs::msg::TransformStamped map_to_base_link_stamped; map_to_base_link_stamped.header.stamp = msg->header.stamp; map_to_base_link_stamped.header.frame_id = global_frame_id_; map_to_base_link_stamped.child_frame_id = base_frame_id_; map_to_base_link_stamped.transform.translation.x = static_cast(final_transformation(0, 3)); map_to_base_link_stamped.transform.translation.y = static_cast(final_transformation(1, 3)); map_to_base_link_stamped.transform.translation.z = static_cast(final_transformation(2, 3)); map_to_base_link_stamped.transform.rotation = quat_msg; if (!enable_map_odom_tf_) { broadcaster_.sendTransform(map_to_base_link_stamped); } else { tf2::Transform map_to_base_link_tf; tf2::fromMsg(map_to_base_link_stamped.transform, map_to_base_link_tf); geometry_msgs::msg::TransformStamped odom_to_base_link_msg; try { odom_to_base_link_msg = tfbuffer_.lookupTransform( odom_frame_id_, base_frame_id_, msg->header.stamp, rclcpp::Duration::from_seconds(0.1)); } catch (tf2::TransformException & ex) { RCLCPP_WARN( this->get_logger(), "Could not get transform %s to %s: %s", base_frame_id_.c_str(), odom_frame_id_.c_str(), ex.what()); return; } tf2::Transform odom_to_base_link_tf; tf2::fromMsg(odom_to_base_link_msg.transform, odom_to_base_link_tf); tf2::Transform map_to_odom_tf = map_to_base_link_tf * odom_to_base_link_tf.inverse(); geometry_msgs::msg::TransformStamped map_to_odom_stamped; map_to_odom_stamped.header.stamp = msg->header.stamp; map_to_odom_stamped.header.frame_id = global_frame_id_; map_to_odom_stamped.child_frame_id = odom_frame_id_; map_to_odom_stamped.transform = tf2::toMsg(map_to_odom_tf); broadcaster_.sendTransform(map_to_odom_stamped); } geometry_msgs::msg::PoseStamped::SharedPtr pose_stamped_ptr(new geometry_msgs::msg::PoseStamped); pose_stamped_ptr->header.stamp = msg->header.stamp; pose_stamped_ptr->header.frame_id = global_frame_id_; pose_stamped_ptr->pose = corrent_pose_with_cov_stamped_ptr_->pose.pose; path_ptr_->poses.push_back(*pose_stamped_ptr); path_pub_->publish(*path_ptr_); } last_scan_ptr_ = msg; if (enable_debug_) { std::cout << "number of filtered cloud points: " << filtered_cloud_ptr->size() << std::endl; std::cout << "align time:" << time_align_end.seconds() - time_align_start.seconds() << "[sec]" << std::endl; std::cout << "has converged: " << has_converged << std::endl; std::cout << "fitness score: " << fitness_score << std::endl; std::cout << "final transformation:" << std::endl; std::cout << final_transformation << std::endl; /* delta_angle check * trace(RotationMatrix) = 2(cos(theta) + 1) */ double init_cos_angle = 0.5 * (init_guess.coeff(0, 0) + init_guess.coeff(1, 1) + init_guess.coeff(2, 2) - 1); double cos_angle = 0.5 * (final_transformation.coeff(0, 0) + final_transformation.coeff(1, 1) + final_transformation.coeff(2, 2) - 1); double init_angle = acos(init_cos_angle); double angle = acos(cos_angle); // Ref:https://twitter.com/Atsushi_twi/status/1185868416864808960 double delta_angle = abs(atan2(sin(init_angle - angle), cos(init_angle - angle))); std::cout << "delta_angle:" << delta_angle * 180 / M_PI << "[deg]" << std::endl; std::cout << "-----------------------------------------------------" << std::endl; } } void PCLLocalization::timerPublishPose() { if (!corrent_pose_with_cov_stamped_ptr_) {return;} geometry_msgs::msg::PoseWithCovarianceStamped pose_copy = *corrent_pose_with_cov_stamped_ptr_; pose_copy.header.stamp = now(); geometry_msgs::msg::PoseStamped stamped; stamped.header = pose_copy.header; stamped.header.frame_id = global_frame_id_; stamped.pose = pose_copy.pose.pose; path_ptr_->poses.push_back(stamped); nav_msgs::msg::Path path_copy = *path_ptr_; pose_pub_->publish(pose_copy); path_pub_->publish(path_copy); geometry_msgs::msg::TransformStamped map_to_base_link_stamped; map_to_base_link_stamped.header.stamp = pose_copy.header.stamp; map_to_base_link_stamped.header.frame_id = global_frame_id_; map_to_base_link_stamped.child_frame_id = base_frame_id_; map_to_base_link_stamped.transform.translation.x = pose_copy.pose.pose.position.x; map_to_base_link_stamped.transform.translation.y = pose_copy.pose.pose.position.y; map_to_base_link_stamped.transform.translation.z = pose_copy.pose.pose.position.z; map_to_base_link_stamped.transform.rotation = pose_copy.pose.pose.orientation; if (!enable_map_odom_tf_) { broadcaster_.sendTransform(map_to_base_link_stamped); } else { tf2::Transform map_to_base_link_tf; tf2::fromMsg(map_to_base_link_stamped.transform, map_to_base_link_tf); geometry_msgs::msg::TransformStamped odom_to_base_link_msg; try { odom_to_base_link_msg = tfbuffer_.lookupTransform( odom_frame_id_, base_frame_id_, pose_copy.header.stamp, rclcpp::Duration::from_seconds(0.1)); } catch (tf2::TransformException & ex) { RCLCPP_WARN( this->get_logger(), "Could not get transform %s to %s: %s", base_frame_id_.c_str(), odom_frame_id_.c_str(), ex.what()); return; } tf2::Transform odom_to_base_link_tf; tf2::fromMsg(odom_to_base_link_msg.transform, odom_to_base_link_tf); tf2::Transform map_to_odom_tf = map_to_base_link_tf * odom_to_base_link_tf.inverse(); geometry_msgs::msg::TransformStamped map_to_odom_stamped; map_to_odom_stamped.header.stamp = pose_copy.header.stamp; map_to_odom_stamped.header.frame_id = global_frame_id_; map_to_odom_stamped.child_frame_id = odom_frame_id_; map_to_odom_stamped.transform = tf2::toMsg(map_to_odom_tf); broadcaster_.sendTransform(map_to_odom_stamped); } } ================================================ FILE: src/lidar_localization_node.cpp ================================================ #include int main(int argc, char * argv[]) { rclcpp::init(argc, argv); rclcpp::executors::SingleThreadedExecutor executor; rclcpp::NodeOptions options; std::shared_ptr pcl_l = std::make_shared(options); executor.add_node(pcl_l->get_node_base_interface()); executor.spin(); rclcpp::shutdown(); return 0; }