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