Support for Composition

CMakeLists.txt

@@ -23,6 +23,7 @@ find_package(Eigen3 REQUIRED)
 find_package(tf2_ros REQUIRED)  #tf_ros
 find_package(tf2_geometry_msgs REQUIRED)
 find_package(rclcpp REQUIRED)   #roscpp
+find_package(rclcpp_components REQUIRED)
 find_package(nav_msgs REQUIRED)
 find_package(sensor_msgs REQUIRED)
 #find_package(tf_conversions REQUIRED)
@@ -40,6 +41,17 @@ find_package(tf2_sensor_msgs REQUIRED)
 
 find_package(iris_lama REQUIRED)
 
+rclcpp_components_register_node(loc2d_ros_component
+        PLUGIN "lama::Loc2DROS"
+        EXECUTABLE loc2d_ros)
+
+rclcpp_components_register_node(slam2d_ros_component
+        PLUGIN "lama::Slam2DROS"
+        EXECUTABLE slam2d_ros)
+
+rclcpp_components_register_node(pf_slam2d_ros_component
+        PLUGIN "lama::PFSlam2DROS"
+        EXECUTABLE pf_slam2d_ros)
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)

README.md

@@ -38,6 +38,26 @@ colcon build
 . install/setup.bash
 ```
 
+If you meet a build error, try to add `set(CMAKE_POSITION_INDEPENDENT_CODE ON)` to the top-level CMakeLists.txt of iris_lama.
+```cmake
+# install location of generated cmake config files
+set(ConfigPackageLocation ${CMAKE_INSTALL_LIBDIR}/${CMAKE_PROJECT_NAME}/cmake)
+
+# dependencies
+find_package(Eigen3 3.3 REQUIRED NO_MODULE)
+
+# ########################## Add this line ! ############################
+set(CMAKE_POSITION_INDEPENDENT_CODE ON)
+# #######################################################################
+
+# the code
+add_subdirectory(src)
+
+# header files, note the "/" after include
+install(DIRECTORY include/
+  DESTINATION ${CMAKE_INSTALL_INCLUDEDIR})
+```
+
 The build was tested on **Ubuntu 24.04** and **ROS2 Jazzy**.
 
 ## SLAM nodes
@@ -67,6 +87,14 @@ or
 ros2 launch iris_lama_ros2 pf_slam2d_offline_launch.py
 ```
 
+### Composition Support
+Each node of iris_lama_ros2 can be invoked as composable. Try to launch *_launch.py with `use_composition:=true` (default: false).
+
+For example,
+```
+ros2 launch iris_lama_ros2 slam2d_live_launch.py use_composition:=true
+```
+
 ### Parameters
 
 * `~global_frame_id`: The frame attached to the map (default: "map").

config/offline_mode.yaml

@@ -4,7 +4,6 @@
 
 /iris_lama_ros2/slam2d_ros:
   ros__parameters:
-    rosbag: True
     transform_tolerance: 0.1 
     use_sim_time: True
 
@@ -19,7 +18,6 @@
 
 /iris_lama_ros2/pf_slam2d_ros:
   ros__parameters:
-    rosbag: True
     transform_tolerance: 0.1
     use_sim_time: True
 

include/lama/ros/loc2d_ros.h

@@ -35,6 +35,7 @@
 
 // ROS includes
 #include "rclcpp/rclcpp.hpp"
+#include "rclcpp_components/register_node_macro.hpp"
 #include <tf2_ros/message_filter.h>
 #include "tf2_ros/create_timer_ros.h"
 #include "tf2_ros/buffer.h"
@@ -68,10 +69,12 @@ namespace lama {
     class Loc2DROS {
     public:
 
-        Loc2DROS(const std::string &);
+        Loc2DROS(const rclcpp::NodeOptions& node_options);
 
         ~Loc2DROS();
 
+        rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface() const;
+
         void topic_callback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr) const;
 
         void onInitialPose(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr initial_pose);
@@ -128,3 +131,4 @@ namespace lama {
 
 } /* lama */
 
+RCLCPP_COMPONENTS_REGISTER_NODE(lama::Loc2DROS)

include/lama/ros/pf_slam2d_ros.h

@@ -35,6 +35,7 @@
 
 // ROS includes
 #include "rclcpp/rclcpp.hpp"
+#include "rclcpp_components/register_node_macro.hpp"
 
 // Transform include
 #include <tf2_ros/transform_broadcaster.h>
@@ -66,9 +67,11 @@ namespace lama {
 class PFSlam2DROS {
 public:
 
-    PFSlam2DROS(std::string name);
+    PFSlam2DROS(const rclcpp::NodeOptions& node_options);
     ~PFSlam2DROS();
 
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface() const;
+
     void onLaserScan(sensor_msgs::msg::LaserScan::ConstSharedPtr laser_scan);
     void onGetMap(const std::shared_ptr<nav_msgs::srv::GetMap::Request> req,
                   std::shared_ptr<nav_msgs::srv::GetMap::Response> res);
@@ -141,3 +144,4 @@ class PFSlam2DROS {
 
 } /* lama */
 
+RCLCPP_COMPONENTS_REGISTER_NODE(lama::PFSlam2DROS)

include/lama/ros/slam2d_ros.h

@@ -35,6 +35,7 @@
 
 // ROS includes
 #include "rclcpp/rclcpp.hpp"
+#include "rclcpp_components/register_node_macro.hpp"
 
 // Transform include
 #include <tf2_ros/transform_broadcaster.h>
@@ -66,9 +67,11 @@ namespace lama {
 class Slam2DROS {
 public:
 
-    Slam2DROS(std::string name);
+    Slam2DROS(const rclcpp::NodeOptions& node_options);
     ~Slam2DROS();
 
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface() const;
+
     void onLaserScan(sensor_msgs::msg::LaserScan::ConstSharedPtr laser_scan);
     void onGetMap(const std::shared_ptr<nav_msgs::srv::GetMap::Request> req,
                   std::shared_ptr<nav_msgs::srv::GetMap::Response> res);
@@ -134,3 +137,4 @@ class Slam2DROS {
 
 } /* lama */
 
+RCLCPP_COMPONENTS_REGISTER_NODE(lama::Slam2DROS)

launch/loc2d_launch.py

@@ -1,7 +1,11 @@
 from launch import LaunchDescription
-from launch_ros.actions import Node
+from launch_ros.actions import Node, ComposableNodeContainer
 import launch
 import launch.actions
+from launch.conditions import IfCondition, UnlessCondition
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.descriptions import ComposableNode
 import pathlib
 
 def generate_launch_description():
@@ -11,10 +15,14 @@ def generate_launch_description():
     parameters_file_path = str(pathlib.Path(__file__).parents[1]) + '/config/live.yaml'
     print(parameters_file_path)
 
+    declare_use_composition_cmd = DeclareLaunchArgument('use_composition', default_value='false')
+    use_composition = LaunchConfiguration('use_composition')
+
     return LaunchDescription([
         #launch.actions.DeclareLaunchArgument('particles',      default_value="30", description=''),
         #launch.actions.DeclareLaunchArgument('threads',        default_value="4", description=''),
         #launch.actions.DeclareLaunchArgument('/use_sim_time',  default_value="True", description=''),
+        declare_use_composition_cmd,
         Node(
             package='iris_lama_ros2',
             namespace='iris_lama_ros2',
@@ -26,5 +34,23 @@ def generate_launch_description():
             #],
             output='screen',
             parameters=[parameters_file_path],
-        )
+            condition=UnlessCondition(use_composition)
+        ),
+        ComposableNodeContainer(
+            name='iris_lama_container', 
+            package='rclcpp_components',
+            executable='component_container',
+            namespace='',
+            composable_node_descriptions=[
+                ComposableNode(
+                    package='iris_lama_ros2',
+                    plugin='lama::Loc2DROS',
+                    name='loc2d_ros',
+                    extra_arguments=[{'use_intra_process_comms': True}],
+                ),
+            ],
+            output='screen',
+            parameters=[parameters_file_path],
+            condition=IfCondition(use_composition)
+        ),
     ])

launch/pf_slam2d_live_launch.py

@@ -1,7 +1,11 @@
 from launch import LaunchDescription
-from launch_ros.actions import Node
+from launch_ros.actions import Node, ComposableNodeContainer
 import launch
 import launch.actions
+from launch.conditions import IfCondition, UnlessCondition
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.descriptions import ComposableNode
 import pathlib
 
 def generate_launch_description():
@@ -11,10 +15,14 @@ def generate_launch_description():
     parameters_file_path = str(pathlib.Path(__file__).parents[1]) + '/config/live.yaml'
     print(parameters_file_path)
 
+    declare_use_composition_cmd = DeclareLaunchArgument('use_composition', default_value='false')
+    use_composition = LaunchConfiguration('use_composition')
+
     return LaunchDescription([
         #launch.actions.DeclareLaunchArgument('particles',      default_value="30", description=''),
         #launch.actions.DeclareLaunchArgument('threads',        default_value="4", description=''),
         #launch.actions.DeclareLaunchArgument('/use_sim_time',  default_value="True", description=''),
+        declare_use_composition_cmd,
         Node(
             package='iris_lama_ros2',
             namespace='iris_lama_ros2',
@@ -26,5 +34,23 @@ def generate_launch_description():
             #],
             output='screen',
             parameters=[parameters_file_path],
-        )
+            condition=UnlessCondition(use_composition)
+        ),
+        ComposableNodeContainer(
+            name='iris_lama_container', 
+            package='rclcpp_components',
+            executable='component_container',
+            namespace='',
+            composable_node_descriptions=[
+                ComposableNode(
+                    package='iris_lama_ros2',
+                    plugin='lama::PFSlam2DROS',
+                    name='pf_slam2d_ros',
+                    extra_arguments=[{'use_intra_process_comms': True}],
+                ),
+            ],
+            output='screen',
+            parameters=[parameters_file_path],
+            condition=IfCondition(use_composition)
+        ),
     ])

launch/pf_slam2d_offline_launch.py

@@ -1,7 +1,11 @@
 from launch import LaunchDescription
-from launch_ros.actions import Node
+from launch_ros.actions import Node, ComposableNodeContainer
 import launch
 import launch.actions
+from launch.conditions import IfCondition, UnlessCondition
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.descriptions import ComposableNode
 import pathlib
 
 def generate_launch_description():
@@ -12,10 +16,14 @@ def generate_launch_description():
     parameters_file_path = str(pathlib.Path(__file__).parents[1]) + '/config/offline_mode.yaml'
     print(parameters_file_path)
 
+    declare_use_composition_cmd = DeclareLaunchArgument('use_composition', default_value='false')
+    use_composition = LaunchConfiguration('use_composition')
+
     return LaunchDescription([
         #launch.actions.DeclareLaunchArgument('particles',      default_value="30", description=''),
         #launch.actions.DeclareLaunchArgument('threads',        default_value="4", description=''),
         #launch.actions.DeclareLaunchArgument('/use_sim_time',  default_value="True", description=''),
+        declare_use_composition_cmd,
         Node(
             package='iris_lama_ros2',
             namespace='iris_lama_ros2',
@@ -27,7 +35,25 @@ def generate_launch_description():
             #],
             output='screen',
             parameters=[parameters_file_path],
+            condition=UnlessCondition(use_composition)
         ), 
+        ComposableNodeContainer(
+            name='iris_lama_container', 
+            package='rclcpp_components',
+            executable='component_container',
+            namespace='',
+            composable_node_descriptions=[
+                ComposableNode(
+                    package='iris_lama_ros2',
+                    plugin='lama::PFSlam2DROS',
+                    name='pf_slam2d_ros',
+                    extra_arguments=[{'use_intra_process_comms': True}],
+                ),
+            ],
+            output='screen',
+            parameters=[parameters_file_path],
+            condition=IfCondition(use_composition)
+        ),
         launch.actions.ExecuteProcess(
             cmd=['ros2', 'bag', 'info', bag_file],
             output='screen'

launch/slam2d_live_launch.py

@@ -1,7 +1,11 @@
 from launch import LaunchDescription
-from launch_ros.actions import Node
+from launch_ros.actions import Node, ComposableNodeContainer
 import launch
 import launch.actions
+from launch.conditions import IfCondition, UnlessCondition
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.descriptions import ComposableNode
 import pathlib
 
 def generate_launch_description():
@@ -11,10 +15,14 @@ def generate_launch_description():
     parameters_file_path = str(pathlib.Path(__file__).parents[1]) + '/config/live.yaml'
     print(parameters_file_path)
 
+    declare_use_composition_cmd = DeclareLaunchArgument('use_composition', default_value='false')
+    use_composition = LaunchConfiguration('use_composition')
+
     return LaunchDescription([
         #launch.actions.DeclareLaunchArgument('particles',      default_value="30", description=''),
         #launch.actions.DeclareLaunchArgument('threads',        default_value="4", description=''),
         #launch.actions.DeclareLaunchArgument('/use_sim_time',  default_value="True", description=''),
+        declare_use_composition_cmd,
         Node(
             package='iris_lama_ros2',
             namespace='iris_lama_ros2',
@@ -26,5 +34,23 @@ def generate_launch_description():
             #],
             output='screen',
             parameters=[parameters_file_path],
+            condition=UnlessCondition(use_composition)
+        ),
+        ComposableNodeContainer(
+            name='iris_lama_container', 
+            package='rclcpp_components',
+            executable='component_container',
+            namespace='',
+            composable_node_descriptions=[
+                ComposableNode(
+                    package='iris_lama_ros2',
+                    plugin='lama::Slam2DROS',
+                    name='slam2d_ros',
+                    extra_arguments=[{'use_intra_process_comms': True}],
+                ),
+            ],
+            output='screen',
+            parameters=[parameters_file_path],
+            condition=IfCondition(use_composition)
         )
     ])