Composable Nodes

.github/workflows/build-ros.yml

@@ -57,4 +57,4 @@ jobs:
         run: |
           . /opt/ros/*/setup.sh
           env
-          MAKEFLAGS="-j2" colcon build --symlink-install --parallel-workers 2 --event-handlers console_direct+
+          MAKEFLAGS="-j2" colcon build --symlink-install --parallel-workers 2 --event-handlers console_direct+

mrpt_pf_localization/CMakeLists.txt

@@ -15,6 +15,8 @@ find_package(sensor_msgs REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
 find_package(mp2p_icp_map REQUIRED)
 find_package(mp2p_icp_filters REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_components REQUIRED)
 find_package(mola_relocalization) # OPTIONAL
 
 find_package(mrpt-ros2bridge REQUIRED)
@@ -42,14 +44,15 @@ message(STATUS "mola_relocalization_FOUND: ${mola_relocalization_FOUND}")
 ###########
 
 # non-ROS C++ library:
-add_library(${PROJECT_NAME}_core
+add_library(${PROJECT_NAME}_core SHARED
     src/${PROJECT_NAME}/${PROJECT_NAME}_core.cpp
     include/${PROJECT_NAME}/${PROJECT_NAME}_core.h
 )
 
 target_include_directories(${PROJECT_NAME}_core
     PUBLIC
-    include
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include/>  
+    $<INSTALL_INTERFACE:include/>
 )
 
 target_link_libraries(${PROJECT_NAME}_core
@@ -69,12 +72,13 @@ endif()
 
 # ROS node:
 add_executable(${PROJECT_NAME}_node
-    src/${PROJECT_NAME}_node.cpp
+    src/main.cpp
     include/${PROJECT_NAME}_node.h
 )
 
 ament_target_dependencies(${PROJECT_NAME}_node
     rclcpp
+    rclcpp_components
     geometry_msgs
     mrpt_msgs
     mrpt_msgs_bridge
@@ -98,24 +102,67 @@ target_link_libraries(${PROJECT_NAME}_node
   mrpt::ros2bridge
 )
 
+#######################
+# ROS composable node #
+#######################
+
+add_library(${PROJECT_NAME}_component SHARED
+    src/${PROJECT_NAME}_component.cpp
+    include/${PROJECT_NAME}_node.h
+)
+
+ament_target_dependencies(${PROJECT_NAME}_component
+    rclcpp
+    rclcpp_components
+    geometry_msgs
+    mrpt_msgs
+    mrpt_msgs_bridge
+    nav_msgs
+    pose_cov_ops
+    sensor_msgs
+    tf2
+    tf2_geometry_msgs
+)
+
+target_include_directories(${PROJECT_NAME}_component
+    PRIVATE
+    include
+)
+
+## Specify libraries to link a library or executable target against
+target_link_libraries(${PROJECT_NAME}_component
+  ${PROJECT_NAME}_core
+  mrpt::gui
+  mrpt::slam
+  mrpt::ros2bridge
+)
+
+rclcpp_components_register_node(
+    ${PROJECT_NAME}_component
+    PLUGIN "PFLocalizationNode"
+    EXECUTABLE ${PROJECT_NAME}_composable
+)
+
+ament_export_targets(export_${PROJECT_NAME})
+
 #############
 ## Install ##
 #############
 
-install(
-  TARGETS
-    ${PROJECT_NAME}_core
-    ${PROJECT_NAME}_node
-  DESTINATION
-    lib/${PROJECT_NAME}
+install(TARGETS ${PROJECT_NAME}_component ${PROJECT_NAME}_core ${PROJECT_NAME}_node
+        EXPORT export_${PROJECT_NAME}
+        ARCHIVE DESTINATION lib
+        LIBRARY DESTINATION lib
+        RUNTIME DESTINATION bin
+        DESTINATION
+          lib/${PROJECT_NAME}
 )
 
 install(
   DIRECTORY launch params
   DESTINATION share/${PROJECT_NAME}
 )
 
-
 ament_export_dependencies()
 
 if(BUILD_TESTING)

mrpt_pf_localization/launch/localization.launch.py

@@ -10,9 +10,11 @@
 from launch import LaunchDescription
 from launch.substitutions import TextSubstitution
 from launch.substitutions import LaunchConfiguration
-from launch_ros.actions import Node
+from launch_ros.actions import Node, LoadComposableNodes
+from launch_ros.descriptions import ComposableNode
 from launch.actions import (DeclareLaunchArgument,
                             EmitEvent, LogInfo, RegisterEventHandler)
+from launch.conditions import IfCondition, UnlessCondition
 from launch.event_handlers import OnProcessExit
 from launch.events import Shutdown
 from ament_index_python import get_package_share_directory
@@ -25,6 +27,8 @@ def generate_launch_description():
     pfLocDir = get_package_share_directory("mrpt_pf_localization")
     # print('pfLocDir       : ' + pfLocDir)
 
+    use_composable = LaunchConfiguration('use_composable')
+
     pf_params_file_launch_arg = DeclareLaunchArgument(
         "pf_params_file", default_value=TextSubstitution(
             text=os.path.join(pfLocDir, 'params', 'default.config.yaml')))
@@ -84,7 +88,13 @@ def generate_launch_description():
         description="Whether to show a custom UI with details on the PF status"
     )
 
+    container_name_arg = DeclareLaunchArgument(
+        'container_name',
+        default_value='',
+    )
+
     pf_localization_node = Node(
+        condition=UnlessCondition(use_composable),
         package='mrpt_pf_localization',
         executable='mrpt_pf_localization_node',
         name='mrpt_pf_localization_node',
@@ -106,7 +116,33 @@ def generate_launch_description():
                    LaunchConfiguration('log_level')]
     )
 
+    composable_pf_localization_node = LoadComposableNodes(
+        condition=IfCondition(use_composable),
+        target_container=LaunchConfiguration('container_name'),
+        composable_node_descriptions=[
+            ComposableNode(
+                package='mrpt_pf_localization',
+                name='mrpt_pf_localization_component',
+                plugin='PFLocalizationNode',
+                parameters=[
+                    LaunchConfiguration('pf_params_file'),
+                    {
+                        "topic_sensors_2d_scan": LaunchConfiguration('topic_sensors_2d_scan'),
+                        "topic_sensors_point_clouds": LaunchConfiguration('topic_sensors_point_clouds'),
+                        "topic_gnss": LaunchConfiguration('topic_gnss'),
+                        "relocalization_params_file": LaunchConfiguration('relocalization_params_file'),
+                        "gui_enable": LaunchConfiguration('gui_enable'),
+                        "log_level_core": LaunchConfiguration('log_level_core'),
+                        "base_link_frame_id": LaunchConfiguration('base_link_frame_id'),
+                        "odom_frame_id": LaunchConfiguration('odom_frame_id'),
+                        "global_frame_id": LaunchConfiguration('global_frame_id'),
+                    }]
+            )
+        ]
+    )
+
     ld = LaunchDescription([
+        container_name_arg,
         pf_log_level_launch_arg,
         pf_log_level_core_launch_arg,
         relocalization_params_file_launch_arg,
@@ -119,6 +155,7 @@ def generate_launch_description():
         odom_frame_id_arg,
         global_frame_id_arg,
         pf_localization_node,
+        composable_pf_localization_node,
         RegisterEventHandler(
             OnProcessExit(
                 target_action=pf_localization_node,

mrpt_pf_localization/package.xml

@@ -36,6 +36,8 @@
   <depend>std_msgs</depend>
   <depend>tf2</depend>
   <depend>tf2_geometry_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
 
   <depend>ament_cmake_lint_cmake</depend>
   <depend>ament_cmake_xmllint</depend>

mrpt_pf_localization/src/main.cpp

@@ -0,0 +1,19 @@
+/* +------------------------------------------------------------------------+
+   |                             mrpt_navigation                            |
+   |                                                                        |
+   | Copyright (c) 2014-2024, Individual contributors, see commit authors   |
+   | See: https://github.com/mrpt-ros-pkg/mrpt_navigation                   |
+   | All rights reserved. Released under BSD 3-Clause license. See LICENSE  |
+   +------------------------------------------------------------------------+ */
+
+#include "rclcpp/rclcpp.hpp"
+#include "mrpt_pf_localization_component.cpp"
+
+int main(int argc, char** argv)
+{
+	rclcpp::init(argc, argv);
+	auto node = std::make_shared<PFLocalizationNode>();
+	rclcpp::spin(std::dynamic_pointer_cast<rclcpp::Node>(node));
+	rclcpp::shutdown();
+	return 0;
+}

mrpt_pf_localization/src/mrpt_pf_localization_node.cpp → mrpt_pf_localization/src/mrpt_pf_localization_component.cpp

@@ -27,6 +27,8 @@
 #include <mrpt/version.h>
 #include <pose_cov_ops/pose_cov_ops.h>
 
+#include "rclcpp_components/register_node_macro.hpp"
+
 #include <geometry_msgs/msg/pose_array.hpp>
 #include <mrpt_msgs_bridge/beacon.hpp>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
@@ -45,14 +47,7 @@ std::string hyperlink(
 }  // namespace mrpt::system
 #endif
 
-int main(int argc, char** argv)
-{
-	rclcpp::init(argc, argv);
-	auto node = std::make_shared<PFLocalizationNode>();
-	rclcpp::spin(std::dynamic_pointer_cast<rclcpp::Node>(node));
-	rclcpp::shutdown();
-	return 0;
-}
+RCLCPP_COMPONENTS_REGISTER_NODE(PFLocalizationNode); //Register node to make it composable
 
 PFLocalizationNode::PFLocalizationNode(const rclcpp::NodeOptions& options)
 	: rclcpp::Node("mrpt_pf_localization_node", options)

mrpt_pointcloud_pipeline/CMakeLists.txt

@@ -42,8 +42,8 @@ ENDIF()
 find_package(mp2p_icp_filters REQUIRED)
 
 add_executable(${PROJECT_NAME}_node
-              src/mrpt_pointcloud_pipeline_node.cpp
-              include/mrpt_pointcloud_pipeline/mrpt_pointcloud_pipeline_node.h)
+              src/main.cpp
+              include/${PROJECT_NAME}/mrpt_pointcloud_pipeline_node.h)
 
 target_include_directories(${PROJECT_NAME}_node
                            PUBLIC
@@ -70,8 +70,55 @@ ament_target_dependencies(
   tf2_geometry_msgs
 )
 
+###################
+# Composable node #
+###################
 
-install(TARGETS ${PROJECT_NAME}_node
+add_library(${PROJECT_NAME}_component SHARED
+              src/${PROJECT_NAME}_component.cpp
+              include/${PROJECT_NAME}/${PROJECT_NAME}_node.h)
+
+target_include_directories(${PROJECT_NAME}_component
+                           PUBLIC
+                            $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+                            $<INSTALL_INTERFACE:include>
+)
+
+target_link_libraries(
+  ${PROJECT_NAME}_component
+  mrpt::maps
+  mrpt::obs
+  mrpt::gui
+  mrpt::ros2bridge
+  mola::mp2p_icp_filters
+)
+
+ament_target_dependencies(
+  ${PROJECT_NAME}_component
+  rclcpp
+  rclcpp_components
+  nav_msgs
+  sensor_msgs
+  tf2
+  tf2_geometry_msgs
+)
+
+rclcpp_components_register_node(
+    ${PROJECT_NAME}_component
+    PLUGIN "LocalObstaclesNode"
+    EXECUTABLE ${PROJECT_NAME}_composable
+)
+
+###########
+# INSTALL #
+###########
+
+install(TARGETS ${PROJECT_NAME}_component 
+                ${PROJECT_NAME}_node
+  EXPORT export_${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
   DESTINATION lib/${PROJECT_NAME}
 )