Migrate to ROS 2

.github/workflows/ci.yml

@@ -10,16 +10,15 @@ on:
 
 jobs:
   build:
-    runs-on: ubuntu-20.04
+    runs-on: ubuntu-latest
     steps:
       - name: Checkout
-        uses: actions/checkout@v3
+        uses: actions/checkout@v4
       - name: Setup ROS
-        uses: ros-tooling/setup-ros@v0.6
+        uses: ros-tooling/setup-ros@v0.7
         with:
           required-ros-distributions: noetic
       - name: Build
         uses: ros-tooling/action-ros-ci@v0.3
         with:
-          target-ros1-distro: noetic
-          vcs-repo-file-url: .rosinstall
+          target-ros1-distro: humble

CMakeLists.txt

@@ -1,71 +1,41 @@
 cmake_minimum_required(VERSION 3.5)
 project(ndt_localizer)
 
-add_compile_options(-std=c++14 -Wall)
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+endif()
 
-find_package(catkin REQUIRED COMPONENTS
-  geometry_msgs
-  nav_msgs
-  roscpp
-  rospy
-  sensor_msgs
-  std_msgs
-  tf2
-  tf2_ros
-  tf2_geometry_msgs
-  pcl_conversions
-  ndt_omp
-)
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
 
-find_package(PCL 1.8.0 REQUIRED)
-find_package(Eigen 3 REQUIRED)
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
 
-find_package(OpenMP)
+find_package(OpenMP REQUIRED)
 if(OPENMP_FOUND)
-    set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
-    set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
+    set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
+    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
 endif()
 
-catkin_package(
-  INCLUDE_DIRS include
-#  LIBRARIES ndt_localizer
-#  CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy sensor_msgs std_msgs tf
-#  DEPENDS system_lib
-)
-
-include_directories(
-  include
-  ${catkin_INCLUDE_DIRS}
-  ${PCL_INCLUDE_DIRS}
-  ${Eigen_INCLUDE_DIRS}
-)
-
-link_directories(
-  ${PCL_LIBRARY_DIRS}
-)
-
-add_definitions(
-  ${PCL_DEFINITIONS}
-)
-
-add_executable(map_matcher src/map_matcher_node.cpp src/map_matcher.cpp)
-add_executable(ndt_odom_integrator src/ndt_odom_integrator_node.cpp src/ndt_odom_integrator.cpp)
+ament_auto_add_executable(map_matcher
+  src/map_matcher_node.cpp
+  src/map_matcher.cpp)
+target_link_libraries(map_matcher OpenMP::OpenMP_CXX)
 
-add_dependencies(map_matcher ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-add_dependencies(ndt_odom_integrator ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+ament_auto_add_executable(ndt_odom_integrator
+  src/ndt_odom_integrator_node.cpp
+  src/ndt_odom_integrator.cpp)
 
-target_link_libraries(map_matcher
-    ${catkin_LIBRARIES}
-    ${PCL_LIBRARIES}
+install(DIRECTORY
+  launch
+  DESTINATION share/${PROJECT_NAME}
 )
 
-target_link_libraries(ndt_odom_integrator
-    ${catkin_LIBRARIES}
-)
-
-if(CATKIN_ENABLE_TESTING)
-  find_package(roslint REQUIRED)
-  set(ROSLINT_CPP_OPTS "--filter=-build/c++11,-runtime/references")
-  roslint_cpp()
-  roslint_add_test()
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  set(ament_cmake_copyright_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
 endif()
+
+ament_auto_package()

README.md

@@ -1,9 +1,7 @@
 # localizer
 
 ## Requirement
-- ros (melodic or noetic)
-- PCL 1.8
-- [ndt_omp](https://github.com/koide3/ndt_omp)
+- ros (humble or later)
 
 ## Runtime requirements
 - tf from robot base frame (e.g. base_link) to sensor frame (e.g. velodyne)
@@ -23,7 +21,7 @@
 - /map_cloud (sensor_msgs/PointCloud2)
   - map data
 - /estimated_pose (geometry_msgs/PoseWithCovarianceStamped)
-  - used to set `init_guess` for NDT 
+  - used to set `init_guess` for NDT
 
 ### ndt_odom_integrator
 #### Published topics

include/ndt_localizer/map_matcher.hpp

@@ -0,0 +1,72 @@
+// Copyright 2023 amsl
+
+#ifndef NDT_LOCALIZER__MAP_MATCHER_HPP_
+#define NDT_LOCALIZER__MAP_MATCHER_HPP_
+
+#include <memory>
+#include <thread>
+
+#include "Eigen/Dense"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
+#include "nav_msgs/msg/odometry.hpp"
+#include "pcl/filters/passthrough.h"
+#include "pcl/filters/voxel_grid.h"
+#include "pcl/point_types.h"
+#include "pcl/registration/ndt.h"
+#include "pcl_conversions/pcl_conversions.h"
+#include "pclomp/ndt_omp.h"
+#include "rclcpp/rclcpp.hpp"
+#include "sensor_msgs/msg/point_cloud2.hpp"
+#include "tf2_eigen/tf2_eigen.hpp"
+#include "tf2_ros/buffer.h"
+#include "tf2_ros/transform_listener.h"
+
+namespace ndt_localizer
+{
+class MapMatcher : public rclcpp::Node
+{
+public:
+  typedef pcl::PointXYZ PointType;
+  typedef pcl::PointCloud<PointType> CloudType;
+  typedef pcl::PointCloud<PointType>::Ptr CloudTypePtr;
+
+  MapMatcher(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
+  virtual ~MapMatcher(void) = default;
+
+  void pose_callback(const nav_msgs::msg::Odometry::ConstSharedPtr & msg);
+  void map_callback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr & msg);
+  void cloud_callback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr & msg);
+  Eigen::Matrix4f get_ndt_transform(const CloudTypePtr & cloud_ptr);
+  void apply_voxel_grid_filter(double leaf_size, CloudTypePtr & cloud_ptr);
+  void apply_passthrough_filter(
+    double range, const CloudTypePtr & cloud_ptr, CloudTypePtr & output_cloud_ptr,
+    const Eigen::Vector3f & center = Eigen::Vector3f::Zero());
+
+private:
+  rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pose_pub_;
+  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr cloud_pub_;
+  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr pose_sub_;
+  rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr map_sub_;
+  rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr cloud_sub_;
+  std::shared_ptr<tf2_ros::TransformListener> tfl_;
+  std::shared_ptr<tf2_ros::Buffer> tf_;
+
+  CloudTypePtr map_cloud_ptr_;
+  bool is_map_received_;
+  nav_msgs::msg::Odometry received_pose_;
+  bool is_pose_updated_;
+
+  // ndt params
+  double epsilon_;
+  double leaf_size_;
+  double step_size_;
+  double resolution_;
+  int max_iterations_;
+
+  double range_;
+};
+
+}  // namespace ndt_localizer
+
+#endif  // NDT_LOCALIZER__MAP_MATCHER_HPP_