diff --git a/.github/workflows/build-ros.yml b/.github/workflows/build-ros.yml
index b1a8336..a042fbd 100644
--- a/.github/workflows/build-ros.yml
+++ b/.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+
\ No newline at end of file
diff --git a/mrpt_pf_localization/CMakeLists.txt b/mrpt_pf_localization/CMakeLists.txt
index 94d64fa..546ac45 100644
--- a/mrpt_pf_localization/CMakeLists.txt
+++ b/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
+ $
+ $
)
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,16 +102,60 @@ 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(
@@ -115,7 +163,6 @@ install(
DESTINATION share/${PROJECT_NAME}
)
-
ament_export_dependencies()
if(BUILD_TESTING)
diff --git a/mrpt_pf_localization/launch/localization.launch.py b/mrpt_pf_localization/launch/localization.launch.py
index 17482a9..3b88d3c 100644
--- a/mrpt_pf_localization/launch/localization.launch.py
+++ b/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,
diff --git a/mrpt_pf_localization/package.xml b/mrpt_pf_localization/package.xml
index ba95ac3..1c088d2 100644
--- a/mrpt_pf_localization/package.xml
+++ b/mrpt_pf_localization/package.xml
@@ -36,6 +36,8 @@
std_msgs
tf2
tf2_geometry_msgs
+ rclcpp
+ rclcpp_components
ament_cmake_lint_cmake
ament_cmake_xmllint
diff --git a/mrpt_pf_localization/src/main.cpp b/mrpt_pf_localization/src/main.cpp
new file mode 100644
index 0000000..9494769
--- /dev/null
+++ b/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();
+ rclcpp::spin(std::dynamic_pointer_cast(node));
+ rclcpp::shutdown();
+ return 0;
+}
\ No newline at end of file
diff --git a/mrpt_pf_localization/src/mrpt_pf_localization_node.cpp b/mrpt_pf_localization/src/mrpt_pf_localization_component.cpp
similarity index 99%
rename from mrpt_pf_localization/src/mrpt_pf_localization_node.cpp
rename to mrpt_pf_localization/src/mrpt_pf_localization_component.cpp
index 1e021ad..c327255 100644
--- a/mrpt_pf_localization/src/mrpt_pf_localization_node.cpp
+++ b/mrpt_pf_localization/src/mrpt_pf_localization_component.cpp
@@ -27,6 +27,8 @@
#include
#include
+#include "rclcpp_components/register_node_macro.hpp"
+
#include
#include
#include
@@ -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();
- rclcpp::spin(std::dynamic_pointer_cast(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)
diff --git a/mrpt_pointcloud_pipeline/CMakeLists.txt b/mrpt_pointcloud_pipeline/CMakeLists.txt
index 2c79bfd..30dda34 100644
--- a/mrpt_pointcloud_pipeline/CMakeLists.txt
+++ b/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
+ $
+ $
+)
+
+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}
)
diff --git a/mrpt_pointcloud_pipeline/launch/pointcloud_pipeline.launch.py b/mrpt_pointcloud_pipeline/launch/pointcloud_pipeline.launch.py
index f154839..8a8e9d4 100644
--- a/mrpt_pointcloud_pipeline/launch/pointcloud_pipeline.launch.py
+++ b/mrpt_pointcloud_pipeline/launch/pointcloud_pipeline.launch.py
@@ -10,8 +10,10 @@
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, Shutdown
+from launch.conditions import IfCondition, UnlessCondition
from ament_index_python import get_package_share_directory
from launch.actions import GroupAction
from launch_ros.actions import PushRosNamespace
@@ -22,6 +24,8 @@ def generate_launch_description():
myPkgDir = get_package_share_directory("mrpt_pointcloud_pipeline")
# print('myPkgDir : ' + myPkgDir)
+ use_composable = LaunchConfiguration('use_composable')
+
lidar_topic_name_arg = DeclareLaunchArgument(
'scan_topic_name',
default_value='' # /scan, /laser1, etc.
@@ -72,9 +76,15 @@ def generate_launch_description():
description="Logging level"
)
+ container_name_arg = DeclareLaunchArgument(
+ 'container_name',
+ default_value='',
+ )
+
emit_shutdown_action = Shutdown(reason='launch is shutting down')
mrpt_pointcloud_pipeline_node = Node(
+ condition=UnlessCondition(use_composable),
package='mrpt_pointcloud_pipeline',
executable='mrpt_pointcloud_pipeline_node',
name='mrpt_pointcloud_pipeline_node',
@@ -101,7 +111,32 @@ def generate_launch_description():
on_exit=[emit_shutdown_action]
)
+ composable_mrpt_pointcloud_pipeline = LoadComposableNodes(
+ condition=IfCondition(use_composable),
+ target_container=LaunchConfiguration('container_name'),
+ composable_node_descriptions=[
+ ComposableNode(
+ package='mrpt_pointcloud_pipeline',
+ name='mrpt_pointcloud_pipeline_component',
+ plugin='LocalObstaclesNode',
+ parameters=[
+ {'source_topics_2d_scans': LaunchConfiguration('scan_topic_name')},
+ {'source_topics_pointclouds': LaunchConfiguration('points_topic_name')},
+ {'show_gui': LaunchConfiguration('show_gui')},
+ {'pipeline_yaml_file': LaunchConfiguration('pipeline_yaml_file')},
+ {'filter_output_layer_name': LaunchConfiguration('filter_output_layer_name')},
+ {'time_window': LaunchConfiguration('time_window')},
+ {'topic_local_map_pointcloud': LaunchConfiguration('filter_output_topic_name')},
+ {'frameid_reference': LaunchConfiguration('frameid_reference')},
+ {'frameid_robot': LaunchConfiguration('frameid_robot')},
+ {'one_observation_per_topic': LaunchConfiguration('one_observation_per_topic')},
+ ],
+ )
+ ]
+ )
+
ld = LaunchDescription([
+ container_name_arg,
lidar_topic_name_arg,
points_topic_name_arg,
show_gui_arg,
@@ -114,6 +149,7 @@ def generate_launch_description():
log_level_launch_arg,
one_observation_per_topic_arg,
mrpt_pointcloud_pipeline_node,
+ composable_mrpt_pointcloud_pipeline,
])
# Namespace to avoid clash launch argument names with the parent scope:
diff --git a/mrpt_pointcloud_pipeline/src/main.cpp b/mrpt_pointcloud_pipeline/src/main.cpp
new file mode 100644
index 0000000..dfb0c3a
--- /dev/null
+++ b/mrpt_pointcloud_pipeline/src/main.cpp
@@ -0,0 +1,23 @@
+/* +------------------------------------------------------------------------+
+ | 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_pointcloud_pipeline_component.cpp"
+
+int main(int argc, char** argv)
+{
+ rclcpp::init(argc, argv);
+
+ auto node = std::make_shared();
+
+ rclcpp::spin(node->get_node_base_interface());
+
+ rclcpp::shutdown();
+
+ return 0;
+}
\ No newline at end of file
diff --git a/mrpt_pointcloud_pipeline/src/mrpt_pointcloud_pipeline_node.cpp b/mrpt_pointcloud_pipeline/src/mrpt_pointcloud_pipeline_component.cpp
similarity index 98%
rename from mrpt_pointcloud_pipeline/src/mrpt_pointcloud_pipeline_node.cpp
rename to mrpt_pointcloud_pipeline/src/mrpt_pointcloud_pipeline_component.cpp
index 6f9ee57..9df226b 100644
--- a/mrpt_pointcloud_pipeline/src/mrpt_pointcloud_pipeline_node.cpp
+++ b/mrpt_pointcloud_pipeline/src/mrpt_pointcloud_pipeline_component.cpp
@@ -12,7 +12,7 @@
#include
// for now, not needed (node=executable)
-// #include "rclcpp_components/register_node_macro.hpp"
+#include "rclcpp_components/register_node_macro.hpp"
using namespace mrpt::system;
using namespace mrpt::config;
@@ -21,7 +21,7 @@ using namespace mrpt::maps;
using namespace mrpt::obs;
LocalObstaclesNode::LocalObstaclesNode(const rclcpp::NodeOptions& options)
- : Node("mrpt_pointcloud_pipeline", options)
+ :Node("mrpt_pointcloud_pipeline_node", options)
{
m_profiler.setName(Node::get_name());
@@ -571,15 +571,4 @@ void LocalObstaclesNode::read_parameters()
}
}
-int main(int argc, char** argv)
-{
- rclcpp::init(argc, argv);
-
- auto node = std::make_shared();
-
- rclcpp::spin(node);
-
- rclcpp::shutdown();
-
- return 0;
-}
+RCLCPP_COMPONENTS_REGISTER_NODE(LocalObstaclesNode);
\ No newline at end of file
diff --git a/mrpt_tutorials/launch/demo_composable_nodes.launch.py b/mrpt_tutorials/launch/demo_composable_nodes.launch.py
new file mode 100644
index 0000000..f6eba5d
--- /dev/null
+++ b/mrpt_tutorials/launch/demo_composable_nodes.launch.py
@@ -0,0 +1,120 @@
+# ROS 2 launch file for example in mrpt_tutorials
+#
+# See repo online: https://github.com/mrpt-ros-pkg/mrpt_navigation
+#
+
+from launch import LaunchDescription
+from launch.substitutions import TextSubstitution
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node, ComposableNodeContainer
+from launch.conditions import IfCondition
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from ament_index_python import get_package_share_directory
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+import os
+
+
+def generate_launch_description():
+ tutsDir = get_package_share_directory("mrpt_tutorials")
+ # print('tutsDir : ' + tutsDir)
+
+ use_composable = LaunchConfiguration('use_composable')
+
+ use_composable_arg = DeclareLaunchArgument(
+ 'use_composable',
+ default_value='false',
+ )
+
+ container = ComposableNodeContainer(
+ condition = IfCondition(use_composable),
+ name='demo_composable_container',
+ package = 'rclcpp_components',
+ executable = 'component_container',
+ namespace = '',
+ output = 'screen',
+ )
+
+ mrpt_map_launch = IncludeLaunchDescription(
+ PythonLaunchDescriptionSource([os.path.join(
+ get_package_share_directory('mrpt_map_server'), 'launch',
+ 'mrpt_map_server.launch.py')]),
+ launch_arguments={
+ 'map_yaml_file': os.path.join(tutsDir, 'maps', 'demo_world2.yaml'),
+ }.items()
+ )
+
+ mvsim_node = Node(
+ package='mvsim',
+ executable='mvsim_node',
+ name='mvsim',
+ output='screen',
+ parameters=[
+ # os.path.join(tutsDir, 'params', 'mvsim_ros2_params.yaml'),
+ {
+ "world_file": os.path.join(tutsDir, 'mvsim', 'demo_world2.world.xml'),
+ "do_fake_localization": True,
+ "headless": True,
+ }]
+ )
+
+ # Launch for mrpt_pointcloud_pipeline:
+ pointcloud_pipeline_launch = IncludeLaunchDescription(
+ PythonLaunchDescriptionSource([os.path.join(
+ get_package_share_directory('mrpt_pointcloud_pipeline'), 'launch',
+ 'pointcloud_pipeline.launch.py')]),
+ launch_arguments={
+ #
+ 'use_composable': LaunchConfiguration('use_composable'),
+ 'container_name': 'demo_composable_container',
+ #
+ 'log_level': 'INFO',
+ 'scan_topic_name': '/laser1, /laser2',
+ 'points_topic_name': '/lidar1_points',
+ 'filter_output_topic_name': '/local_map_pointcloud',
+ 'time_window': '0.20',
+ 'show_gui': 'False',
+ 'frameid_robot': 'base_link',
+ 'frameid_reference': 'odom',
+ }.items()
+ )
+
+ # Launch for pf_localization:
+ pf_localization_launch = IncludeLaunchDescription(
+ PythonLaunchDescriptionSource([os.path.join(
+ get_package_share_directory('mrpt_pf_localization'), 'launch',
+ 'localization.launch.py')]),
+ launch_arguments={
+ #
+ 'use_composable': LaunchConfiguration('use_composable'),
+ 'container_name': 'demo_composable_container',
+ #
+ 'log_level': 'INFO',
+ 'log_level_core': 'INFO',
+ 'topic_sensors_2d_scan': '/laser1',
+ 'topic_sensors_point_clouds': '',
+
+ # For robots with wheels odometry, use: 'base_link'-> 'odom' -> 'map'
+ # For systems without wheels odometry, use: 'base_link'-> 'base_link' -> 'map'
+ 'base_link_frame_id': 'base_link',
+ 'odom_frame_id': 'odom',
+ 'global_frame_id': 'map',
+ }.items()
+ )
+
+ rviz2_node = Node(
+ package='rviz2',
+ executable='rviz2',
+ name='rviz2',
+ arguments=[
+ '-d', [os.path.join(tutsDir, 'rviz2', 'gridmap.rviz')]]
+ )
+
+ return LaunchDescription([
+ use_composable_arg,
+ container,
+ mrpt_map_launch,
+ pf_localization_launch,
+ mvsim_node,
+ pointcloud_pipeline_launch,
+ rviz2_node,
+ ])