As ROS2 rclcpp Component [#162]

ros/CMakeLists.txt

@@ -65,18 +65,32 @@ elseif("$ENV{ROS_VERSION}" STREQUAL "2")
 
   find_package(ament_cmake REQUIRED)
   find_package(nav_msgs REQUIRED)
+  find_package(rcutils REQUIRED)
   find_package(rclcpp REQUIRED)
+  find_package(rclcpp_components REQUIRED)
   find_package(sensor_msgs REQUIRED)
   find_package(tf2_ros REQUIRED)
 
   # ROS2 node
-  add_executable(odometry_node ros2/OdometryServer.cpp)
-  target_include_directories(odometry_node PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include)
-  target_link_libraries(odometry_node kiss_icp::pipeline)
-  install(TARGETS odometry_node RUNTIME DESTINATION lib/${PROJECT_NAME})
+  add_library(odometry_component SHARED ros2/OdometryServer.cpp)
+  target_include_directories(odometry_component PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include)
+  target_link_libraries(odometry_component kiss_icp::pipeline)
+  ament_target_dependencies(
+    odometry_component
+    rcutils
+    rclcpp
+    rclcpp_components
+    nav_msgs
+    sensor_msgs
+    tf2_ros)
+
+  rclcpp_components_register_node(odometry_component PLUGIN "kiss_icp_ros::OdometryServer" EXECUTABLE odometry_node)
+
+  install(TARGETS odometry_component LIBRARY DESTINATION lib RUNTIME DESTINATION lib/${PROJECT_NAME})
   install(DIRECTORY launch rviz DESTINATION share/${PROJECT_NAME}/)
-  ament_target_dependencies(odometry_node rclcpp nav_msgs sensor_msgs tf2_ros)
+
   ament_package()
+
 else()
   message(FATAL_ERROR "catkin or colcon not found KISS-ICP-ROS disabled")
 endif()

ros/package.xml

@@ -39,6 +39,7 @@
 
   <!-- ROS2 dependencies -->
   <buildtool_depend condition="$ROS_VERSION == 2">ament_cmake</buildtool_depend>
+  <depend condition="$ROS_VERSION == 2">rcutils</depend>
   <depend condition="$ROS_VERSION == 2">rclcpp</depend>
   <depend condition="$ROS_VERSION == 2">rclcpp_components</depend>
   <exec_depend condition="$ROS_VERSION == 2">ros2launch</exec_depend>

ros/ros2/OdometryServer.cpp

@@ -44,7 +44,8 @@
 
 namespace kiss_icp_ros {
 
-OdometryServer::OdometryServer() : rclcpp::Node("odometry_node") {
+OdometryServer::OdometryServer(const rclcpp::NodeOptions &options)
+    : rclcpp::Node("odometry_node", options) {
     // clang-format off
     child_frame_ = declare_parameter<std::string>("child_frame", child_frame_);
     odom_frame_ = declare_parameter<std::string>("odom_frame", odom_frame_);
@@ -104,7 +105,7 @@ OdometryServer::OdometryServer() : rclcpp::Node("odometry_node") {
     RCLCPP_INFO(this->get_logger(), "KISS-ICP ROS2 odometry node initialized");
 }
 
-void OdometryServer::RegisterFrame(const sensor_msgs::msg::PointCloud2::SharedPtr msg_ptr) {
+void OdometryServer::RegisterFrame(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg_ptr) {
     // ROS2::Foxy can't handle a callback to const MessageT&, so we hack it here
     // https://github.com/ros2/rclcpp/pull/1598
     const sensor_msgs::msg::PointCloud2 &msg = *msg_ptr;
@@ -171,10 +172,3 @@ void OdometryServer::RegisterFrame(const sensor_msgs::msg::PointCloud2::SharedPt
     map_publisher_->publish(utils::EigenToPointCloud2(odometry_.LocalMap(), local_map_header));
 }
 }  // namespace kiss_icp_ros
-
-int main(int argc, char **argv) {
-    rclcpp::init(argc, argv);
-    rclcpp::spin(std::make_shared<kiss_icp_ros::OdometryServer>());
-    rclcpp::shutdown();
-    return 0;
-}

ros/ros2/OdometryServer.hpp

@@ -37,11 +37,12 @@ namespace kiss_icp_ros {
 class OdometryServer : public rclcpp::Node {
 public:
     /// OdometryServer constructor
-    OdometryServer();
+    OdometryServer() = delete;
+    explicit OdometryServer(const rclcpp::NodeOptions &options);
 
 private:
     /// Register new frame
-    void RegisterFrame(const sensor_msgs::msg::PointCloud2::SharedPtr msg_ptr);
+    void RegisterFrame(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg_ptr);
 
 private:
     /// Ros node stuff
@@ -73,3 +74,10 @@ class OdometryServer : public rclcpp::Node {
 };
 
 }  // namespace kiss_icp_ros
+
+#include "rclcpp_components/register_node_macro.hpp"
+
+// Register the component with class_loader.
+// This acts as a sort of entry point, allowing the component to be
+// discoverable when its library is being loaded into a running process.
+RCLCPP_COMPONENTS_REGISTER_NODE(kiss_icp_ros::OdometryServer)