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, + ])