From 7093029e651e7f50c6a2218b2019bb2e5d3ec95c Mon Sep 17 00:00:00 2001 From: Afzal Ahmad Date: Mon, 27 Jan 2025 18:48:12 +0100 Subject: [PATCH 01/27] added: Plugin example base --- .../example_plugin_manager/CMakeLists.txt | 6 +- .../example_plugins/CMakeLists.txt | 95 +++++++++---------- .../example_plugins/package.xml | 20 ++-- .../example_plugins/plugins.xml | 6 +- .../example_plugins/src/example_plugin.cpp | 59 +++++++----- 5 files changed, 101 insertions(+), 85 deletions(-) diff --git a/extended_examples/pluginlib_example/example_plugin_manager/CMakeLists.txt b/extended_examples/pluginlib_example/example_plugin_manager/CMakeLists.txt index 7d2ba40..e86be7d 100644 --- a/extended_examples/pluginlib_example/example_plugin_manager/CMakeLists.txt +++ b/extended_examples/pluginlib_example/example_plugin_manager/CMakeLists.txt @@ -52,8 +52,9 @@ install(TARGETS RUNTIME DESTINATION bin ) -install(DIRECTORY include/ - DESTINATION include/ +install( + DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} ) install(DIRECTORY launch @@ -64,4 +65,5 @@ install(DIRECTORY config DESTINATION share/${PROJECT_NAME} ) +ament_export_include_directories("include/${PROJECT_NAME}") ament_package() diff --git a/extended_examples/pluginlib_example/example_plugins/CMakeLists.txt b/extended_examples/pluginlib_example/example_plugins/CMakeLists.txt index 4e50e68..e6c8a8e 100644 --- a/extended_examples/pluginlib_example/example_plugins/CMakeLists.txt +++ b/extended_examples/pluginlib_example/example_plugins/CMakeLists.txt @@ -1,73 +1,72 @@ -cmake_minimum_required(VERSION 3.15.0) +cmake_minimum_required(VERSION 3.5) project(example_plugins) -set(CATKIN_DEPENDENCIES - roscpp - cmake_modules - mrs_lib - example_plugin_manager - pluginlib - ) - -set(LIBRARIES - ExamplePlugin - ) - -find_package(catkin REQUIRED COMPONENTS - ${CATKIN_DEPENDENCIES} - ) - -find_package(Eigen3 REQUIRED) -set(Eigen_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIRS}) -set(Eigen_LIBRARIES ${Eigen_LIBRARIES}) - set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) + +# set the compile options to show code warnings +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(pluginlib REQUIRED) +find_package(ros2_examples REQUIRED) +find_package(example_plugin_manager REQUIRED) +find_package(Eigen3 REQUIRED) -add_definitions(-Wall) -add_definitions(-Wextra) -add_definitions(-Wpedantic) +pluginlib_export_plugin_description_file(example_plugin_manager plugins.xml) -catkin_package( - LIBRARIES ${LIBRARIES} - CATKIN_DEPENDS ${CATKIN_DEPENDENCIES} - DEPENDS Eigen - ) +set(Eigen_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIRS}) +set(Eigen_LIBRARIES ${Eigen_LIBRARIES}) include_directories( - ${catkin_INCLUDE_DIRS} + include ) # ExamplePlugin -add_library(ExamplePlugin +add_library(example_plugin SHARED src/example_plugin.cpp ) -add_dependencies(ExamplePlugin - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} - ) +# ament manages cmake related variables and dependency search (similar to catkin in ROS1) +ament_target_dependencies(example_plugin PUBLIC + rclcpp + rclcpp_components + pluginlib + Eigen3 + ros2_examples + example_plugin_manager +) -target_link_libraries(ExamplePlugin - ${catkin_LIBRARIES} - ${Eigen_LIBRARIES} - ) +# each component (nodelet) needs to be registered to make it discoverable at runtime +rclcpp_components_register_nodes(example_plugin "example_plugin::ExamplePlugin") ## -------------------------------------------------------------- ## | Install | ## -------------------------------------------------------------- -install(TARGETS ${LIBRARIES} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +install(TARGETS + example_plugin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) +install(DIRECTORY include/ + DESTINATION include/ +) + +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME} +) + install(DIRECTORY config - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} - ) + DESTINATION share/${PROJECT_NAME} +) -install(FILES plugins.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} - ) +ament_package() \ No newline at end of file diff --git a/extended_examples/pluginlib_example/example_plugins/package.xml b/extended_examples/pluginlib_example/example_plugins/package.xml index a6d331d..eb54f57 100644 --- a/extended_examples/pluginlib_example/example_plugins/package.xml +++ b/extended_examples/pluginlib_example/example_plugins/package.xml @@ -1,25 +1,27 @@ - + example_plugins 1.0.0 The example_plugins package - Tomas Baca + Afzal Ahmad + Afzal Ahmad Tomas Baca - MIT + TODO: License declaration - catkin + ament_cmake - pluginlib - roscpp - cmake_modules + rclcpp + rclcpp_components + nodelet mrs_lib + ros2_examples example_plugin_manager - + ament_cmake - + diff --git a/extended_examples/pluginlib_example/example_plugins/plugins.xml b/extended_examples/pluginlib_example/example_plugins/plugins.xml index 7158edc..2ac3e83 100644 --- a/extended_examples/pluginlib_example/example_plugins/plugins.xml +++ b/extended_examples/pluginlib_example/example_plugins/plugins.xml @@ -1,5 +1,5 @@ - - + + This is ExamplePlugin. - + \ No newline at end of file diff --git a/extended_examples/pluginlib_example/example_plugins/src/example_plugin.cpp b/extended_examples/pluginlib_example/example_plugins/src/example_plugin.cpp index bae7683..149b1ad 100644 --- a/extended_examples/pluginlib_example/example_plugins/src/example_plugin.cpp +++ b/extended_examples/pluginlib_example/example_plugins/src/example_plugin.cpp @@ -1,9 +1,15 @@ -#include -#include +#include +#include +#include +#include +#include +#include #include +#include -#include + +// #include namespace example_plugins { @@ -16,7 +22,7 @@ namespace example_plugin class ExamplePlugin : public example_plugin_manager::Plugin { public: - void initialize(const ros::NodeHandle& parent_nh, const std::string& name, const std::string& name_space, + void initialize(const rclcpp::Node &parent_node, const std::string& name, const std::string& name_space, std::shared_ptr common_handlers); bool activate(const int& some_number); @@ -27,6 +33,7 @@ class ExamplePlugin : public example_plugin_manager::Plugin { // parameter from a config file double _pi_; + std::shared_ptr parent_node_; std::string _name_; private: @@ -42,38 +49,43 @@ class ExamplePlugin : public example_plugin_manager::Plugin { /* initialize() //{ */ -void ExamplePlugin::initialize(const ros::NodeHandle& parent_nh, const std::string& name, const std::string& name_space, +void ExamplePlugin::initialize(const rclcpp::Node &parent_node, const std::string& name, const std::string& name_space, std::shared_ptr common_handlers) { - // nh_ will behave just like normal NodeHandle - ros::NodeHandle nh_(parent_nh, name_space); - + parent_node_ = std::make_shared(parent_node); _name_ = name; // I can use this to get stuff from the manager interactively common_handlers_ = common_handlers; - ros::Time::waitForValid(); - // | ------------------- loading parameters ------------------- | - mrs_lib::ParamLoader param_loader(nh_, "ExamplePlugin"); + bool loaded_successfully = true; + + loaded_successfully &= utils::parse_param("pi", _pi_, *parent_node_); - param_loader.addYamlFile(ros::package::getPath("example_plugins") + "/config/example_plugin.yaml"); + if (!loaded_successfully) { + RCLCPP_ERROR_STREAM(parent_node_->get_logger(), "Could not load all non-optional parameters. Shutting down."); + rclcpp::shutdown(); + return; + } + // mrs_lib::ParamLoader param_loader(nh_, "ExamplePlugin"); + + // param_loader.addYamlFile(ros::package::getPath("example_plugins") + "/config/example_plugin.yaml"); // can load params like in a ROS node - param_loader.loadParam("pi", _pi_); + // param_loader.loadParam("pi", _pi_); - if (!param_loader.loadedSuccessfully()) { - ROS_ERROR("[%s]: could not load all parameters!", _name_.c_str()); - ros::shutdown(); - } + // if (!param_loader.loadedSuccessfully()) { + // ROS_ERROR("[%s]: could not load all parameters!", _name_.c_str()); + // ros::shutdown(); + // } - ROS_INFO("[%s]: loaded custom parameter: pi=%f", _name_.c_str(), _pi_); + RCLCPP_INFO(parent_node_->get_logger(), "[%s]: loaded custom parameter: pi=%f", _name_.c_str(), _pi_); // | ----------------------- finish init ---------------------- | - ROS_INFO("[%s]: initialized under the name '%s', and namespace '%s'", _name_.c_str(), name.c_str(), name_space.c_str()); + RCLCPP_INFO(parent_node_->get_logger(), "[%s]: initialized under the name '%s', and namespace '%s'", _name_.c_str(), name.c_str(), name_space.c_str()); is_initialized_ = true; } @@ -84,7 +96,7 @@ void ExamplePlugin::initialize(const ros::NodeHandle& parent_nh, const std::stri bool ExamplePlugin::activate(const int& some_number) { - ROS_INFO("[%s]: activated with some_number=%d", _name_.c_str(), some_number); + RCLCPP_INFO(parent_node_->get_logger(), "[%s]: activated with some_number=%d", _name_.c_str(), some_number); is_active_ = true; @@ -99,7 +111,7 @@ void ExamplePlugin::deactivate(void) { is_active_ = false; - ROS_INFO("[%s]: deactivated", _name_.c_str()); + RCLCPP_INFO(parent_node_->get_logger(), "[%s]: deactivated", _name_.c_str()); } //} @@ -112,7 +124,7 @@ const std::optional ExamplePlugin::update(const Eigen::Vector3d& input) return false; } - ROS_INFO_STREAM("[" << _name_ << "]: update() was called, let's find out the size of the vector [" << input.transpose() << "]"); + RCLCPP_ERROR_STREAM(parent_node_->get_logger(), "[" << _name_ << "]: update() was called, let's find out the size of the vector [" << input.transpose() << "]"); // check some property from the "manager" if (common_handlers_->vector_calculator.enabled) { @@ -135,5 +147,6 @@ const std::optional ExamplePlugin::update(const Eigen::Vector3d& input) } // namespace example_plugins -#include +#include + PLUGINLIB_EXPORT_CLASS(example_plugins::example_plugin::ExamplePlugin, example_plugin_manager::Plugin) From 8e5cc18a836f2412e18d64f3628f3c01aa0dbbd8 Mon Sep 17 00:00:00 2001 From: Afzal Ahmad Date: Tue, 28 Jan 2025 17:58:03 +0100 Subject: [PATCH 02/27] fixed: example plugins are discoverable now, cmake target and plugins.xml should have same names :( --- .../example_plugin_manager/CMakeLists.txt | 4 +- .../config/example_plugin_manager.yaml | 2 +- .../config/plugins.yaml | 4 +- .../example_plugin_manager/plugin_interface.h | 2 +- .../launch/example_plugin_manager.py | 63 +++++++++++++++++++ .../src/example_plugin_manager.cpp | 45 ++++++++----- .../example_plugins/CMakeLists.txt | 30 ++++----- .../example_plugins/plugins.xml | 2 +- .../example_plugins/src/example_plugin.cpp | 30 ++++----- ros2_examples/include/ros2_examples/params.h | 14 +++++ 10 files changed, 139 insertions(+), 57 deletions(-) create mode 100755 extended_examples/pluginlib_example/example_plugin_manager/launch/example_plugin_manager.py diff --git a/extended_examples/pluginlib_example/example_plugin_manager/CMakeLists.txt b/extended_examples/pluginlib_example/example_plugin_manager/CMakeLists.txt index e86be7d..3e073e4 100644 --- a/extended_examples/pluginlib_example/example_plugin_manager/CMakeLists.txt +++ b/extended_examples/pluginlib_example/example_plugin_manager/CMakeLists.txt @@ -54,7 +54,7 @@ install(TARGETS install( DIRECTORY include/ - DESTINATION include/${PROJECT_NAME} + DESTINATION include ) install(DIRECTORY launch @@ -65,5 +65,5 @@ install(DIRECTORY config DESTINATION share/${PROJECT_NAME} ) -ament_export_include_directories("include/${PROJECT_NAME}") +ament_export_include_directories("include") ament_package() diff --git a/extended_examples/pluginlib_example/example_plugin_manager/config/example_plugin_manager.yaml b/extended_examples/pluginlib_example/example_plugin_manager/config/example_plugin_manager.yaml index 3ac0fe4..1df728f 100644 --- a/extended_examples/pluginlib_example/example_plugin_manager/config/example_plugin_manager.yaml +++ b/extended_examples/pluginlib_example/example_plugin_manager/config/example_plugin_manager.yaml @@ -9,4 +9,4 @@ plugins : [ # should be within "plugins" initial_plugin: "ExamplePlugin2" -update_timer_rate: 1 # [Hz] +update_timer_rate: 1.0 # [Hz] diff --git a/extended_examples/pluginlib_example/example_plugin_manager/config/plugins.yaml b/extended_examples/pluginlib_example/example_plugin_manager/config/plugins.yaml index 0537828..58e3676 100644 --- a/extended_examples/pluginlib_example/example_plugin_manager/config/plugins.yaml +++ b/extended_examples/pluginlib_example/example_plugin_manager/config/plugins.yaml @@ -1,12 +1,12 @@ # define "running instances" of plugins, there can be more than one for each ExamplePlugin: - address: "example_plugins/ExamplePlugin" # this is taken from the plugin's package.xml + address: "example_plugins::ExamplePlugin" # this is taken from the plugin's package.xml name_space: "example_plugin" # useful for loading parameters into difference instances of the same plugin some_property: 6.28 # this can be anything specific the manager needs to hold for each instance of each plugin # We can run another instance of the same plugin ExamplePlugin2: - address: "example_plugins/ExamplePlugin" # this is taken from the plugin's package.xml + address: "example_plugins::ExamplePlugin" # this is taken from the plugin's package.xml name_space: "example_plugin_2" some_property: 10.1 diff --git a/extended_examples/pluginlib_example/example_plugin_manager/include/example_plugin_manager/plugin_interface.h b/extended_examples/pluginlib_example/example_plugin_manager/include/example_plugin_manager/plugin_interface.h index 9ba63bf..875b090 100644 --- a/extended_examples/pluginlib_example/example_plugin_manager/include/example_plugin_manager/plugin_interface.h +++ b/extended_examples/pluginlib_example/example_plugin_manager/include/example_plugin_manager/plugin_interface.h @@ -9,7 +9,7 @@ namespace example_plugin_manager class Plugin { public: - virtual void initialize(const rclcpp::Node &parent_node, const std::string& name, const std::string& name_space, + virtual void initialize(const rclcpp::NodeOptions &node_options, const std::string& name, const std::string& name_space, std::shared_ptr common_handlers) = 0; virtual bool activate(const int& some_number) = 0; diff --git a/extended_examples/pluginlib_example/example_plugin_manager/launch/example_plugin_manager.py b/extended_examples/pluginlib_example/example_plugin_manager/launch/example_plugin_manager.py new file mode 100755 index 0000000..06320d3 --- /dev/null +++ b/extended_examples/pluginlib_example/example_plugin_manager/launch/example_plugin_manager.py @@ -0,0 +1,63 @@ +import launch +import os + +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode +from launch.actions import DeclareLaunchArgument +from launch.substitutions import ( + LaunchConfiguration, + IfElseSubstitution, + PythonExpression, + PathJoinSubstitution, + EnvironmentVariable, +) + +from ament_index_python.packages import get_package_share_directory + + + +def generate_launch_description(): + + ld = launch.LaunchDescription() + + pkg_name = "example_plugin_manager" + pkg_share_path = get_package_share_directory(pkg_name) + + # param loaded from env variable + uav_type=os.getenv('UAV_TYPE', "") + + ld.add_action(ComposableNodeContainer( + + namespace="nmspc1", + name='nmspc1_example_plugin_manager', + package='rclcpp_components', + executable='component_container_mt', + + composable_node_descriptions=[ + + ComposableNode( + + package=pkg_name, + plugin='example_plugin_manager::ExamplePluginManager', + namespace="nmspc1", + name='example_plugin_manager', + + parameters=[ + pkg_share_path + '/config/example_plugin_manager.yaml', + pkg_share_path + '/config/plugins.yaml', + {'uav_type': uav_type}, + ], + + # remappings=[ + # # topics + # ("~/topic_out", "~/topic"), + # ], + + ), + + ], + + output='screen', + )) + + return ld \ No newline at end of file diff --git a/extended_examples/pluginlib_example/example_plugin_manager/src/example_plugin_manager.cpp b/extended_examples/pluginlib_example/example_plugin_manager/src/example_plugin_manager.cpp index 6dc01c6..fce1fd6 100644 --- a/extended_examples/pluginlib_example/example_plugin_manager/src/example_plugin_manager.cpp +++ b/extended_examples/pluginlib_example/example_plugin_manager/src/example_plugin_manager.cpp @@ -106,7 +106,6 @@ ExamplePluginManager::ExamplePluginManager(const rclcpp::NodeOptions &options) rclcpp::shutdown(); return; } - // | --------------- example of a shared object --------------- | example_of_a_shared_object_ = "Hello, this is a shared object"; @@ -126,12 +125,25 @@ ExamplePluginManager::ExamplePluginManager(const rclcpp::NodeOptions &options) // | load the plugins | // -------------------------------------------------------------- - rclcpp::Parameter param_plugin_names; - loaded_successfully &= this->get_parameter("plugins", param_plugin_names); - _plugin_names_ = param_plugin_names.as_string_array(); + // rclcpp::Parameter param_plugin_names; + // loaded_successfully &= this->get_parameter("plugins", param_plugin_names); + // _plugin_names_ = param_plugin_names.as_string_array(); + loaded_successfully &= utils::parse_param("plugins", _plugin_names_ , *this); + + if (!loaded_successfully) { + RCLCPP_ERROR_STREAM(get_logger(), "Could not load all non-optional parameters. Shutting down."); + rclcpp::shutdown(); + return; + } plugin_loader_ = std::make_unique>("example_plugin_manager", "example_plugin_manager::Plugin"); + std::vector plugins = plugin_loader_->getDeclaredClasses(); // Method name changed + + for (const auto& plugin : plugins) { + RCLCPP_INFO(get_logger(), "Plugin: %s", plugin.c_str()); + } + // for each plugin in the list for (int i = 0; i < int(_plugin_names_.size()); i++) { std::string plugin_name = _plugin_names_[i]; @@ -141,26 +153,34 @@ ExamplePluginManager::ExamplePluginManager(const rclcpp::NodeOptions &options) std::string name_space; double some_property; - loaded_successfully &= utils::parse_param(plugin_name + "/address", address, *this); - loaded_successfully &= utils::parse_param(plugin_name + "/name_space", name_space, *this); - loaded_successfully &= utils::parse_param(plugin_name + "/some_property", some_property, *this); + loaded_successfully &= utils::parse_param(plugin_name + ".address", address, *this); + loaded_successfully &= utils::parse_param(plugin_name + ".name_space", name_space, *this); + loaded_successfully &= utils::parse_param(plugin_name + ".some_property", some_property, *this); + + if (!loaded_successfully) { + RCLCPP_ERROR_STREAM(get_logger(), "Could not load all non-optional parameters. Shutting down."); + rclcpp::shutdown(); + return; + } PluginParams new_plugin(address, name_space, some_property); plugins_.insert(std::pair(plugin_name, new_plugin)); try { RCLCPP_INFO(get_logger(), "[ExamplePluginManager]: loading the plugin '%s'", new_plugin.address.c_str()); - plugin_list_.push_back(plugin_loader_->createSharedInstance(new_plugin.address.c_str())); + plugin_list_.emplace_back(plugin_loader_->createSharedInstance(new_plugin.address)); } catch (pluginlib::CreateClassException& ex1) { RCLCPP_ERROR(get_logger(), "[ExamplePluginManager]: CreateClassException for the plugin '%s'", new_plugin.address.c_str()); RCLCPP_ERROR(get_logger(), "[ExamplePluginManager]: Error: %s", ex1.what()); rclcpp::shutdown(); + return; } catch (pluginlib::PluginlibException& ex) { RCLCPP_ERROR(get_logger(), "[ExamplePluginManager]: PluginlibException for the plugin '%s'", new_plugin.address.c_str()); RCLCPP_ERROR(get_logger(), "[ExamplePluginManager]: Error: %s", ex.what()); rclcpp::shutdown(); + return; } } @@ -172,7 +192,7 @@ ExamplePluginManager::ExamplePluginManager(const rclcpp::NodeOptions &options) it = plugins_.find(_plugin_names_[i]); RCLCPP_INFO(get_logger(), "[ExamplePluginManager]: initializing the plugin '%s'", it->second.address.c_str()); - plugin_list_[i]->initialize(*this, _plugin_names_[i], it->second.name_space, common_handlers_); + plugin_list_[i]->initialize(options, _plugin_names_[i], it->second.name_space, common_handlers_); } catch (std::runtime_error& ex) { RCLCPP_ERROR(get_logger(), "[ExamplePluginManager]: exception caught during plugin initialization: '%s'", ex.what()); @@ -201,6 +221,7 @@ ExamplePluginManager::ExamplePluginManager(const rclcpp::NodeOptions &options) if (!check) { RCLCPP_ERROR(get_logger(), "[ExamplePluginManager]: the initial plugin (%s) is not within the loaded plugins", _initial_plugin_name_.c_str()); rclcpp::shutdown(); + return; } } @@ -219,12 +240,6 @@ ExamplePluginManager::ExamplePluginManager(const rclcpp::NodeOptions &options) // | ----------------------- finish init ---------------------- | - if (!loaded_successfully) { - RCLCPP_ERROR_STREAM(get_logger(), "Could not load all non-optional parameters. Shutting down."); - rclcpp::shutdown(); - return; - } - is_initialized_ = true; RCLCPP_INFO(get_logger(), "[ExamplePluginManager]: initialized"); diff --git a/extended_examples/pluginlib_example/example_plugins/CMakeLists.txt b/extended_examples/pluginlib_example/example_plugins/CMakeLists.txt index e6c8a8e..74f8f33 100644 --- a/extended_examples/pluginlib_example/example_plugins/CMakeLists.txt +++ b/extended_examples/pluginlib_example/example_plugins/CMakeLists.txt @@ -11,6 +11,7 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") endif() find_package(ament_cmake REQUIRED) +find_package(ament_cmake_ros REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) find_package(pluginlib REQUIRED) @@ -23,18 +24,14 @@ pluginlib_export_plugin_description_file(example_plugin_manager plugins.xml) set(Eigen_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIRS}) set(Eigen_LIBRARIES ${Eigen_LIBRARIES}) -include_directories( - include - ) - # ExamplePlugin -add_library(example_plugin SHARED +add_library(${PROJECT_NAME} SHARED src/example_plugin.cpp ) # ament manages cmake related variables and dependency search (similar to catkin in ROS1) -ament_target_dependencies(example_plugin PUBLIC +ament_target_dependencies(${PROJECT_NAME} PUBLIC rclcpp rclcpp_components pluginlib @@ -43,30 +40,27 @@ ament_target_dependencies(example_plugin PUBLIC example_plugin_manager ) -# each component (nodelet) needs to be registered to make it discoverable at runtime -rclcpp_components_register_nodes(example_plugin "example_plugin::ExamplePlugin") - ## -------------------------------------------------------------- ## | Install | ## -------------------------------------------------------------- -install(TARGETS - example_plugin +install( + TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin - ) - -install(DIRECTORY include/ - DESTINATION include/ ) -install(DIRECTORY launch +install(DIRECTORY config DESTINATION share/${PROJECT_NAME} ) -install(DIRECTORY config - DESTINATION share/${PROJECT_NAME} +ament_export_libraries( + ${PROJECT_NAME} +) +ament_export_targets( + export_${PROJECT_NAME} ) ament_package() \ No newline at end of file diff --git a/extended_examples/pluginlib_example/example_plugins/plugins.xml b/extended_examples/pluginlib_example/example_plugins/plugins.xml index 2ac3e83..db70cac 100644 --- a/extended_examples/pluginlib_example/example_plugins/plugins.xml +++ b/extended_examples/pluginlib_example/example_plugins/plugins.xml @@ -1,5 +1,5 @@ - + This is ExamplePlugin. \ No newline at end of file diff --git a/extended_examples/pluginlib_example/example_plugins/src/example_plugin.cpp b/extended_examples/pluginlib_example/example_plugins/src/example_plugin.cpp index 149b1ad..09e36b3 100644 --- a/extended_examples/pluginlib_example/example_plugins/src/example_plugin.cpp +++ b/extended_examples/pluginlib_example/example_plugins/src/example_plugin.cpp @@ -1,5 +1,6 @@ #include #include +#include #include #include #include @@ -14,15 +15,12 @@ namespace example_plugins { -namespace example_plugin -{ - /* class ExamplePlugin //{ */ class ExamplePlugin : public example_plugin_manager::Plugin { public: - void initialize(const rclcpp::Node &parent_node, const std::string& name, const std::string& name_space, + void initialize(const rclcpp::NodeOptions &node_options, const std::string& name, const std::string& name_space, std::shared_ptr common_handlers); bool activate(const int& some_number); @@ -33,7 +31,7 @@ class ExamplePlugin : public example_plugin_manager::Plugin { // parameter from a config file double _pi_; - std::shared_ptr parent_node_; + std::shared_ptr node_; std::string _name_; private: @@ -49,10 +47,10 @@ class ExamplePlugin : public example_plugin_manager::Plugin { /* initialize() //{ */ -void ExamplePlugin::initialize(const rclcpp::Node &parent_node, const std::string& name, const std::string& name_space, +void ExamplePlugin::initialize(const rclcpp::NodeOptions &node_options, const std::string& name, const std::string& name_space, std::shared_ptr common_handlers) { - parent_node_ = std::make_shared(parent_node); + node_ = std::make_shared(name, name_space, node_options); _name_ = name; // I can use this to get stuff from the manager interactively @@ -62,10 +60,10 @@ void ExamplePlugin::initialize(const rclcpp::Node &parent_node, const std::strin bool loaded_successfully = true; - loaded_successfully &= utils::parse_param("pi", _pi_, *parent_node_); + loaded_successfully &= utils::parse_param("pi", _pi_, *node_); if (!loaded_successfully) { - RCLCPP_ERROR_STREAM(parent_node_->get_logger(), "Could not load all non-optional parameters. Shutting down."); + RCLCPP_ERROR_STREAM(node_->get_logger(), "Could not load all non-optional parameters. Shutting down."); rclcpp::shutdown(); return; } @@ -81,11 +79,11 @@ void ExamplePlugin::initialize(const rclcpp::Node &parent_node, const std::strin // ros::shutdown(); // } - RCLCPP_INFO(parent_node_->get_logger(), "[%s]: loaded custom parameter: pi=%f", _name_.c_str(), _pi_); + RCLCPP_INFO(node_->get_logger(), "[%s]: loaded custom parameter: pi=%f", _name_.c_str(), _pi_); // | ----------------------- finish init ---------------------- | - RCLCPP_INFO(parent_node_->get_logger(), "[%s]: initialized under the name '%s', and namespace '%s'", _name_.c_str(), name.c_str(), name_space.c_str()); + RCLCPP_INFO(node_->get_logger(), "[%s]: initialized under the name '%s', and namespace '%s'", _name_.c_str(), name.c_str(), name_space.c_str()); is_initialized_ = true; } @@ -96,7 +94,7 @@ void ExamplePlugin::initialize(const rclcpp::Node &parent_node, const std::strin bool ExamplePlugin::activate(const int& some_number) { - RCLCPP_INFO(parent_node_->get_logger(), "[%s]: activated with some_number=%d", _name_.c_str(), some_number); + RCLCPP_INFO(node_->get_logger(), "[%s]: activated with some_number=%d", _name_.c_str(), some_number); is_active_ = true; @@ -111,7 +109,7 @@ void ExamplePlugin::deactivate(void) { is_active_ = false; - RCLCPP_INFO(parent_node_->get_logger(), "[%s]: deactivated", _name_.c_str()); + RCLCPP_INFO(node_->get_logger(), "[%s]: deactivated", _name_.c_str()); } //} @@ -124,7 +122,7 @@ const std::optional ExamplePlugin::update(const Eigen::Vector3d& input) return false; } - RCLCPP_ERROR_STREAM(parent_node_->get_logger(), "[" << _name_ << "]: update() was called, let's find out the size of the vector [" << input.transpose() << "]"); + RCLCPP_ERROR_STREAM(node_->get_logger(), "[" << _name_ << "]: update() was called, let's find out the size of the vector [" << input.transpose() << "]"); // check some property from the "manager" if (common_handlers_->vector_calculator.enabled) { @@ -143,10 +141,8 @@ const std::optional ExamplePlugin::update(const Eigen::Vector3d& input) //} -} // namespace example_plugin - } // namespace example_plugins #include -PLUGINLIB_EXPORT_CLASS(example_plugins::example_plugin::ExamplePlugin, example_plugin_manager::Plugin) +PLUGINLIB_EXPORT_CLASS(example_plugins::ExamplePlugin, example_plugin_manager::Plugin) diff --git a/ros2_examples/include/ros2_examples/params.h b/ros2_examples/include/ros2_examples/params.h index 1e35933..85b9497 100644 --- a/ros2_examples/include/ros2_examples/params.h +++ b/ros2_examples/include/ros2_examples/params.h @@ -3,6 +3,20 @@ namespace utils { +// Overload << operator for std::vector +template +std::ostream& operator<<(std::ostream& os, const std::vector& vec) { + os << "["; + for (size_t i = 0; i < vec.size(); ++i) { + os << vec[i]; + if (i != vec.size() - 1) { + os << ", "; // Add a comma between elements + } + } + os << "]"; + return os; +} + /* parse_param() method //{ */ // a helper parameter loading function template From 28ce29cee2e903fe8a0c9e12d660ae2f9afcad0d Mon Sep 17 00:00:00 2001 From: Afzal Ahmad Date: Wed, 29 Jan 2025 16:39:05 +0100 Subject: [PATCH 03/27] update: cleaned and made fixes to the plugin load --- .../example_plugin_manager/CMakeLists.txt | 9 ++-- .../config/example_plugin_manager.yaml | 2 +- .../config/plugins.yaml | 8 ++-- .../example_plugin_manager/plugin_interface.h | 2 +- .../launch/example_plugin_manager.launch | 22 --------- .../launch/example_plugin_manager.py | 33 ++++++------- .../example_plugin_manager/package.xml | 6 ++- .../src/example_plugin_manager.cpp | 46 ++++++------------- .../example_plugins/CMakeLists.txt | 22 ++++----- .../example_plugins/package.xml | 5 +- .../example_plugins/plugins.xml | 1 + .../example_plugins/src/example_plugin.cpp | 46 ++++++------------- 12 files changed, 71 insertions(+), 131 deletions(-) delete mode 100644 extended_examples/pluginlib_example/example_plugin_manager/launch/example_plugin_manager.launch diff --git a/extended_examples/pluginlib_example/example_plugin_manager/CMakeLists.txt b/extended_examples/pluginlib_example/example_plugin_manager/CMakeLists.txt index 3e073e4..525ffa8 100644 --- a/extended_examples/pluginlib_example/example_plugin_manager/CMakeLists.txt +++ b/extended_examples/pluginlib_example/example_plugin_manager/CMakeLists.txt @@ -11,11 +11,12 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") endif() find_package(ament_cmake REQUIRED) +find_package(ament_cmake_ros REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) find_package(pluginlib REQUIRED) -find_package(ros2_examples REQUIRED) find_package(Eigen3 REQUIRED) +find_package(ros2_examples REQUIRED) set(Eigen_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIRS}) set(Eigen_LIBRARIES ${Eigen_LIBRARIES}) @@ -25,7 +26,6 @@ include_directories( ) # ExamplePluginManager - add_library(example_plugin_manager SHARED src/example_plugin_manager.cpp ) @@ -52,6 +52,7 @@ install(TARGETS RUNTIME DESTINATION bin ) +# this will copy the header files for use by other libraries install( DIRECTORY include/ DESTINATION include @@ -65,5 +66,7 @@ install(DIRECTORY config DESTINATION share/${PROJECT_NAME} ) +# this will export the include-related variables for use by other libraries +# this is needed if you wish to include the headers from this pkg into downstream libraries ament_export_include_directories("include") -ament_package() +ament_package() \ No newline at end of file diff --git a/extended_examples/pluginlib_example/example_plugin_manager/config/example_plugin_manager.yaml b/extended_examples/pluginlib_example/example_plugin_manager/config/example_plugin_manager.yaml index 1df728f..f32650d 100644 --- a/extended_examples/pluginlib_example/example_plugin_manager/config/example_plugin_manager.yaml +++ b/extended_examples/pluginlib_example/example_plugin_manager/config/example_plugin_manager.yaml @@ -1,5 +1,5 @@ # the list of "names" of the loaded plugins -# each plugin's properties are defined in 'plugins.yaml' +# each plugin's properties are defined in 'plugins.yaml' of the example_plugin package plugins : [ "ExamplePlugin", "ExamplePlugin2", diff --git a/extended_examples/pluginlib_example/example_plugin_manager/config/plugins.yaml b/extended_examples/pluginlib_example/example_plugin_manager/config/plugins.yaml index 58e3676..983cfc2 100644 --- a/extended_examples/pluginlib_example/example_plugin_manager/config/plugins.yaml +++ b/extended_examples/pluginlib_example/example_plugin_manager/config/plugins.yaml @@ -1,12 +1,12 @@ # define "running instances" of plugins, there can be more than one for each ExamplePlugin: - address: "example_plugins::ExamplePlugin" # this is taken from the plugin's package.xml - name_space: "example_plugin" # useful for loading parameters into difference instances of the same plugin + address: "example_plugins::ExamplePlugin" # this is taken from the 'type' arg of plugins.xml which is inside the 'example_plugin' pkg + name_space: "example_plugin" # useful for loading parameters into different instances of the same plugin some_property: 6.28 # this can be anything specific the manager needs to hold for each instance of each plugin # We can run another instance of the same plugin ExamplePlugin2: - address: "example_plugins::ExamplePlugin" # this is taken from the plugin's package.xml - name_space: "example_plugin_2" + address: "example_plugins::ExamplePlugin" + name_space: "example_plugin_2" # notice the different namespace to separate these plugins some_property: 10.1 diff --git a/extended_examples/pluginlib_example/example_plugin_manager/include/example_plugin_manager/plugin_interface.h b/extended_examples/pluginlib_example/example_plugin_manager/include/example_plugin_manager/plugin_interface.h index 875b090..3d3f13a 100644 --- a/extended_examples/pluginlib_example/example_plugin_manager/include/example_plugin_manager/plugin_interface.h +++ b/extended_examples/pluginlib_example/example_plugin_manager/include/example_plugin_manager/plugin_interface.h @@ -9,7 +9,7 @@ namespace example_plugin_manager class Plugin { public: - virtual void initialize(const rclcpp::NodeOptions &node_options, const std::string& name, const std::string& name_space, + virtual void initialize(const std::shared_ptr &sub_node, const std::string& name, std::shared_ptr common_handlers) = 0; virtual bool activate(const int& some_number) = 0; diff --git a/extended_examples/pluginlib_example/example_plugin_manager/launch/example_plugin_manager.launch b/extended_examples/pluginlib_example/example_plugin_manager/launch/example_plugin_manager.launch deleted file mode 100644 index c87ab11..0000000 --- a/extended_examples/pluginlib_example/example_plugin_manager/launch/example_plugin_manager.launch +++ /dev/null @@ -1,22 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - diff --git a/extended_examples/pluginlib_example/example_plugin_manager/launch/example_plugin_manager.py b/extended_examples/pluginlib_example/example_plugin_manager/launch/example_plugin_manager.py index 06320d3..deb4390 100755 --- a/extended_examples/pluginlib_example/example_plugin_manager/launch/example_plugin_manager.py +++ b/extended_examples/pluginlib_example/example_plugin_manager/launch/example_plugin_manager.py @@ -3,14 +3,7 @@ from launch_ros.actions import ComposableNodeContainer from launch_ros.descriptions import ComposableNode -from launch.actions import DeclareLaunchArgument -from launch.substitutions import ( - LaunchConfiguration, - IfElseSubstitution, - PythonExpression, - PathJoinSubstitution, - EnvironmentVariable, -) +from launch.substitutions import EnvironmentVariable from ament_index_python.packages import get_package_share_directory @@ -24,28 +17,28 @@ def generate_launch_description(): pkg_share_path = get_package_share_directory(pkg_name) # param loaded from env variable - uav_type=os.getenv('UAV_TYPE', "") + uav_type = EnvironmentVariable("UAV_TYPE", default_value="dummy") ld.add_action(ComposableNodeContainer( - namespace="nmspc1", - name='nmspc1_example_plugin_manager', - package='rclcpp_components', - executable='component_container_mt', + namespace="container_ns", + name="comp_container", + package="rclcpp_components", + executable="component_container_mt", composable_node_descriptions=[ ComposableNode( package=pkg_name, - plugin='example_plugin_manager::ExamplePluginManager', - namespace="nmspc1", - name='example_plugin_manager', + plugin="example_plugin_manager::ExamplePluginManager", + namespace="node_ns", + name="example_plugin_manager", parameters=[ - pkg_share_path + '/config/example_plugin_manager.yaml', - pkg_share_path + '/config/plugins.yaml', - {'uav_type': uav_type}, + pkg_share_path + "/config/example_plugin_manager.yaml", + pkg_share_path + "/config/plugins.yaml", + {"uav_type": uav_type}, ], # remappings=[ @@ -57,7 +50,7 @@ def generate_launch_description(): ], - output='screen', + output="screen", )) return ld \ No newline at end of file diff --git a/extended_examples/pluginlib_example/example_plugin_manager/package.xml b/extended_examples/pluginlib_example/example_plugin_manager/package.xml index 69c3bf3..c4bee84 100644 --- a/extended_examples/pluginlib_example/example_plugin_manager/package.xml +++ b/extended_examples/pluginlib_example/example_plugin_manager/package.xml @@ -7,16 +7,18 @@ Afzal Ahmad Afzal Ahmad + Tomas Baca TODO: License declaration ament_cmake + ament_cmake_ros rclcpp rclcpp_components - nodelet - mrs_lib + pluginlib ros2_examples + eigen ament_cmake diff --git a/extended_examples/pluginlib_example/example_plugin_manager/src/example_plugin_manager.cpp b/extended_examples/pluginlib_example/example_plugin_manager/src/example_plugin_manager.cpp index fce1fd6..06c6476 100644 --- a/extended_examples/pluginlib_example/example_plugin_manager/src/example_plugin_manager.cpp +++ b/extended_examples/pluginlib_example/example_plugin_manager/src/example_plugin_manager.cpp @@ -1,14 +1,12 @@ +#include +#include #include #include #include #include -#include #include - - -// #include -// #include +#include namespace example_plugin_manager { @@ -44,6 +42,7 @@ class ExamplePluginManager : public rclcpp::Node { private: bool is_initialized_ = false; + std::string _uav_type_ = ""; // | ---------------------- update timer ---------------------- | @@ -84,20 +83,15 @@ class ExamplePluginManager : public rclcpp::Node { /* onInit() //{ */ -ExamplePluginManager::ExamplePluginManager(const rclcpp::NodeOptions &options) : Node("params_example", options) { +ExamplePluginManager::ExamplePluginManager(const rclcpp::NodeOptions &options) : Node("example_plugin_manager", options) { RCLCPP_INFO(get_logger(), "[ExamplePluginManager]: initializing"); - // -------------------------------------------------------------- - // | params | - // -------------------------------------------------------------- - - // mrs_lib::ParamLoader param_loader(nh_, "ExamplePluginManager"); - // | --------------------- load parameters -------------------- | bool loaded_successfully = true; + loaded_successfully &= utils::parse_param("uav_type", _uav_type_, *this); loaded_successfully &= utils::parse_param("update_timer_rate", _rate_timer_update_, *this); loaded_successfully &= utils::parse_param("initial_plugin", _initial_plugin_name_, *this); @@ -125,9 +119,6 @@ ExamplePluginManager::ExamplePluginManager(const rclcpp::NodeOptions &options) // | load the plugins | // -------------------------------------------------------------- - // rclcpp::Parameter param_plugin_names; - // loaded_successfully &= this->get_parameter("plugins", param_plugin_names); - // _plugin_names_ = param_plugin_names.as_string_array(); loaded_successfully &= utils::parse_param("plugins", _plugin_names_ , *this); if (!loaded_successfully) { @@ -138,21 +129,16 @@ ExamplePluginManager::ExamplePluginManager(const rclcpp::NodeOptions &options) plugin_loader_ = std::make_unique>("example_plugin_manager", "example_plugin_manager::Plugin"); - std::vector plugins = plugin_loader_->getDeclaredClasses(); // Method name changed - - for (const auto& plugin : plugins) { - RCLCPP_INFO(get_logger(), "Plugin: %s", plugin.c_str()); - } - // for each plugin in the list for (int i = 0; i < int(_plugin_names_.size()); i++) { - std::string plugin_name = _plugin_names_[i]; + std::string plugin_name = _plugin_names_.at(i); // load the plugin parameters std::string address; std::string name_space; double some_property; + // nested params are separated by '.' instead of '/' loaded_successfully &= utils::parse_param(plugin_name + ".address", address, *this); loaded_successfully &= utils::parse_param(plugin_name + ".name_space", name_space, *this); loaded_successfully &= utils::parse_param(plugin_name + ".some_property", some_property, *this); @@ -189,10 +175,10 @@ ExamplePluginManager::ExamplePluginManager(const rclcpp::NodeOptions &options) for (int i = 0; i < int(plugin_list_.size()); i++) { try { std::map::iterator it; - it = plugins_.find(_plugin_names_[i]); + it = plugins_.find(_plugin_names_.at(i)); - RCLCPP_INFO(get_logger(), "[ExamplePluginManager]: initializing the plugin '%s'", it->second.address.c_str()); - plugin_list_[i]->initialize(options, _plugin_names_[i], it->second.name_space, common_handlers_); + RCLCPP_INFO(get_logger(), "[ExamplePluginManager]: initializing the plugin '%s' of type '%s'", it->second.name_space.c_str(), it->second.address.c_str()); + plugin_list_.at(i)->initialize(this->create_sub_node(it->second.name_space), _plugin_names_.at(i), common_handlers_); } catch (std::runtime_error& ex) { RCLCPP_ERROR(get_logger(), "[ExamplePluginManager]: exception caught during plugin initialization: '%s'", ex.what()); @@ -210,7 +196,7 @@ ExamplePluginManager::ExamplePluginManager(const rclcpp::NodeOptions &options) for (int i = 0; i < int(_plugin_names_.size()); i++) { - std::string plugin_name = _plugin_names_[i]; + std::string plugin_name = _plugin_names_.at(i); if (plugin_name == _initial_plugin_name_) { check = true; @@ -227,11 +213,11 @@ ExamplePluginManager::ExamplePluginManager(const rclcpp::NodeOptions &options) // | ---------- activate the first plugin on the list --------- | - RCLCPP_INFO(get_logger(), "[ExamplePluginManager]: activating plugin with idx %d on the list (named: %s)", _initial_plugin_idx_, _plugin_names_[_initial_plugin_idx_].c_str()); + RCLCPP_INFO(get_logger(), "[ExamplePluginManager]: activating plugin with idx %d on the list (named: %s)", _initial_plugin_idx_, _plugin_names_.at(_initial_plugin_idx_).c_str()); int some_activation_input_to_plugin = 1234; - plugin_list_[_initial_plugin_idx_]->activate(some_activation_input_to_plugin); + plugin_list_.at(_initial_plugin_idx_)->activate(some_activation_input_to_plugin); active_plugin_idx_ = _initial_plugin_idx_; // | ------------------------- timers ------------------------- | @@ -256,14 +242,12 @@ void ExamplePluginManager::timerUpdate() { if (!is_initialized_) return; - // auto active_plugin_idx = mrs_lib::get_mutexed(mutex_plugins_, active_plugin_idx_); - // plugin input Eigen::Vector3d input; input << 0, 1, 2; // call the plugin's update routine - auto result = plugin_list_[active_plugin_idx_]->update(input); + auto result = plugin_list_.at(active_plugin_idx_)->update(input); if (result) { diff --git a/extended_examples/pluginlib_example/example_plugins/CMakeLists.txt b/extended_examples/pluginlib_example/example_plugins/CMakeLists.txt index 74f8f33..3e8ee99 100644 --- a/extended_examples/pluginlib_example/example_plugins/CMakeLists.txt +++ b/extended_examples/pluginlib_example/example_plugins/CMakeLists.txt @@ -14,18 +14,18 @@ find_package(ament_cmake REQUIRED) find_package(ament_cmake_ros REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) +find_package(Eigen3 REQUIRED) find_package(pluginlib REQUIRED) find_package(ros2_examples REQUIRED) find_package(example_plugin_manager REQUIRED) -find_package(Eigen3 REQUIRED) +# export the plugins listed in plugins.xml to be discovered by the manager pluginlib_export_plugin_description_file(example_plugin_manager plugins.xml) set(Eigen_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIRS}) set(Eigen_LIBRARIES ${Eigen_LIBRARIES}) -# ExamplePlugin - +# ${PROJECT_NAME} add_library(${PROJECT_NAME} SHARED src/example_plugin.cpp ) @@ -34,8 +34,8 @@ add_library(${PROJECT_NAME} SHARED ament_target_dependencies(${PROJECT_NAME} PUBLIC rclcpp rclcpp_components - pluginlib Eigen3 + pluginlib ros2_examples example_plugin_manager ) @@ -45,8 +45,8 @@ ament_target_dependencies(${PROJECT_NAME} PUBLIC ## -------------------------------------------------------------- install( - TARGETS ${PROJECT_NAME} - EXPORT export_${PROJECT_NAME} + TARGETS ${PROJECT_NAME} # list all libraries to be installed and exported out from this pkg + EXPORT export_${PROJECT_NAME} # a single export name is enough to export all libraries from this pkg ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin @@ -55,12 +55,8 @@ install( install(DIRECTORY config DESTINATION share/${PROJECT_NAME} ) - -ament_export_libraries( - ${PROJECT_NAME} -) -ament_export_targets( - export_${PROJECT_NAME} -) +# export the installed library and related info to the cmake system +# this is ESSENTIAL for the plugin to be discovered dynamically +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) ament_package() \ No newline at end of file diff --git a/extended_examples/pluginlib_example/example_plugins/package.xml b/extended_examples/pluginlib_example/example_plugins/package.xml index eb54f57..6d5f1d7 100644 --- a/extended_examples/pluginlib_example/example_plugins/package.xml +++ b/extended_examples/pluginlib_example/example_plugins/package.xml @@ -12,11 +12,12 @@ TODO: License declaration ament_cmake + ament_cmake_ros rclcpp rclcpp_components - nodelet - mrs_lib + pluginlib + eigen ros2_examples example_plugin_manager diff --git a/extended_examples/pluginlib_example/example_plugins/plugins.xml b/extended_examples/pluginlib_example/example_plugins/plugins.xml index db70cac..7d0567f 100644 --- a/extended_examples/pluginlib_example/example_plugins/plugins.xml +++ b/extended_examples/pluginlib_example/example_plugins/plugins.xml @@ -1,4 +1,5 @@ + This is ExamplePlugin. diff --git a/extended_examples/pluginlib_example/example_plugins/src/example_plugin.cpp b/extended_examples/pluginlib_example/example_plugins/src/example_plugin.cpp index 09e36b3..e66a329 100644 --- a/extended_examples/pluginlib_example/example_plugins/src/example_plugin.cpp +++ b/extended_examples/pluginlib_example/example_plugins/src/example_plugin.cpp @@ -1,17 +1,9 @@ -#include -#include -#include -#include #include -#include #include #include #include - -// #include - namespace example_plugins { @@ -20,7 +12,7 @@ namespace example_plugins class ExamplePlugin : public example_plugin_manager::Plugin { public: - void initialize(const rclcpp::NodeOptions &node_options, const std::string& name, const std::string& name_space, + void initialize(const std::shared_ptr &sub_node, const std::string& name, std::shared_ptr common_handlers); bool activate(const int& some_number); @@ -29,7 +21,7 @@ class ExamplePlugin : public example_plugin_manager::Plugin { const std::optional update(const Eigen::Vector3d& input); // parameter from a config file - double _pi_; + // double _pi_; std::shared_ptr node_; std::string _name_; @@ -47,10 +39,10 @@ class ExamplePlugin : public example_plugin_manager::Plugin { /* initialize() //{ */ -void ExamplePlugin::initialize(const rclcpp::NodeOptions &node_options, const std::string& name, const std::string& name_space, +void ExamplePlugin::initialize(const std::shared_ptr &sub_node, const std::string& name, std::shared_ptr common_handlers) { - node_ = std::make_shared(name, name_space, node_options); + node_ = sub_node; _name_ = name; // I can use this to get stuff from the manager interactively @@ -58,32 +50,21 @@ void ExamplePlugin::initialize(const rclcpp::NodeOptions &node_options, const st // | ------------------- loading parameters ------------------- | - bool loaded_successfully = true; - - loaded_successfully &= utils::parse_param("pi", _pi_, *node_); - - if (!loaded_successfully) { - RCLCPP_ERROR_STREAM(node_->get_logger(), "Could not load all non-optional parameters. Shutting down."); - rclcpp::shutdown(); - return; - } - // mrs_lib::ParamLoader param_loader(nh_, "ExamplePlugin"); - - // param_loader.addYamlFile(ros::package::getPath("example_plugins") + "/config/example_plugin.yaml"); + // bool loaded_successfully = true; - // can load params like in a ROS node - // param_loader.loadParam("pi", _pi_); + // loaded_successfully &= utils::parse_param("pi", _pi_, *node_); - // if (!param_loader.loadedSuccessfully()) { - // ROS_ERROR("[%s]: could not load all parameters!", _name_.c_str()); - // ros::shutdown(); + // if (!loaded_successfully) { + // RCLCPP_ERROR_STREAM(node_->get_logger(), "Could not load all non-optional parameters. Shutting down."); + // rclcpp::shutdown(); + // return; // } - RCLCPP_INFO(node_->get_logger(), "[%s]: loaded custom parameter: pi=%f", _name_.c_str(), _pi_); + // RCLCPP_INFO(node_->get_logger(), "[%s]: loaded custom parameter: pi=%f", _name_.c_str(), _pi_); // | ----------------------- finish init ---------------------- | - RCLCPP_INFO(node_->get_logger(), "[%s]: initialized under the name '%s', and namespace '%s'", _name_.c_str(), name.c_str(), name_space.c_str()); + RCLCPP_INFO(node_->get_logger(), "[%s]: initialized under the name '%s', and namespace '%s'", _name_.c_str(), name.c_str(), node_->get_namespace()); is_initialized_ = true; } @@ -145,4 +126,5 @@ const std::optional ExamplePlugin::update(const Eigen::Vector3d& input) #include -PLUGINLIB_EXPORT_CLASS(example_plugins::ExamplePlugin, example_plugin_manager::Plugin) +// Register the plugin. +PLUGINLIB_EXPORT_CLASS(example_plugins::ExamplePlugin, example_plugin_manager::Plugin) \ No newline at end of file From 07506f9e44b9d1afd481357a6b4c313f64a4d379 Mon Sep 17 00:00:00 2001 From: Afzal Ahmad Date: Wed, 29 Jan 2025 17:29:26 +0100 Subject: [PATCH 04/27] fix: a cleaner method to export the headers --- ros2_examples/CMakeLists.txt | 27 ++++----------------------- 1 file changed, 4 insertions(+), 23 deletions(-) diff --git a/ros2_examples/CMakeLists.txt b/ros2_examples/CMakeLists.txt index 9bf6332..42a6cfb 100644 --- a/ros2_examples/CMakeLists.txt +++ b/ros2_examples/CMakeLists.txt @@ -170,17 +170,6 @@ ament_target_dependencies(tf2_listener_example # TODO HOW DOES THIS WORK? rclcpp_components_register_nodes(tf2_listener_example PLUGIN "ros2_examples::Tf2ListenerExample" EXECUTABLE tf2_listener_example) -############################################################ -# adding libraries both Interface (header-only) -############################################################ -# add template header-only library for implementing simulators -add_library(${PROJECT_NAME}_headers INTERFACE) - -# add the header files that will be referenced when this target is used -target_include_directories(${PROJECT_NAME}_headers INTERFACE - "$" - "$") - ## -------------------------------------------------------------- ## | install | ## -------------------------------------------------------------- @@ -200,20 +189,9 @@ install(TARGETS ) install(DIRECTORY include/ - DESTINATION include/${PROJECT_NAME} -) - -install( - TARGETS ${PROJECT_NAME}_headers - EXPORT export_${PROJECT_NAME} - LIBRARY DESTINATION lib - ARCHIVE DESTINATION lib - RUNTIME DESTINATION bin - INCLUDES DESTINATION include + DESTINATION include ) -ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) - install(DIRECTORY launch DESTINATION share/${PROJECT_NAME} ) @@ -222,4 +200,7 @@ install(DIRECTORY config DESTINATION share/${PROJECT_NAME} ) +# this will export the include-related variables for use by other libraries +# this is needed if you wish to include the headers from this pkg into downstream libraries +ament_export_include_directories("include") ament_package() From 93550afdd228bcb5b5fbb6abc56a29a35d6b049d Mon Sep 17 00:00:00 2001 From: Afzal Ahmad Date: Tue, 28 Jan 2025 17:58:03 +0100 Subject: [PATCH 05/27] fixed: example plugins are discoverable now, cmake target and plugins.xml should have same names :( --- .../example_plugin_manager/CMakeLists.txt | 4 +- .../config/example_plugin_manager.yaml | 2 +- .../config/plugins.yaml | 4 +- .../example_plugin_manager/plugin_interface.h | 2 +- .../launch/example_plugin_manager.py | 63 +++++++++++++++++++ .../src/example_plugin_manager.cpp | 45 ++++++++----- .../example_plugins/CMakeLists.txt | 30 ++++----- .../example_plugins/plugins.xml | 2 +- .../example_plugins/src/example_plugin.cpp | 30 ++++----- ros2_examples/include/ros2_examples/params.h | 14 +++++ 10 files changed, 139 insertions(+), 57 deletions(-) create mode 100755 extended_examples/pluginlib_example/example_plugin_manager/launch/example_plugin_manager.py diff --git a/extended_examples/pluginlib_example/example_plugin_manager/CMakeLists.txt b/extended_examples/pluginlib_example/example_plugin_manager/CMakeLists.txt index e86be7d..3e073e4 100644 --- a/extended_examples/pluginlib_example/example_plugin_manager/CMakeLists.txt +++ b/extended_examples/pluginlib_example/example_plugin_manager/CMakeLists.txt @@ -54,7 +54,7 @@ install(TARGETS install( DIRECTORY include/ - DESTINATION include/${PROJECT_NAME} + DESTINATION include ) install(DIRECTORY launch @@ -65,5 +65,5 @@ install(DIRECTORY config DESTINATION share/${PROJECT_NAME} ) -ament_export_include_directories("include/${PROJECT_NAME}") +ament_export_include_directories("include") ament_package() diff --git a/extended_examples/pluginlib_example/example_plugin_manager/config/example_plugin_manager.yaml b/extended_examples/pluginlib_example/example_plugin_manager/config/example_plugin_manager.yaml index 3ac0fe4..1df728f 100644 --- a/extended_examples/pluginlib_example/example_plugin_manager/config/example_plugin_manager.yaml +++ b/extended_examples/pluginlib_example/example_plugin_manager/config/example_plugin_manager.yaml @@ -9,4 +9,4 @@ plugins : [ # should be within "plugins" initial_plugin: "ExamplePlugin2" -update_timer_rate: 1 # [Hz] +update_timer_rate: 1.0 # [Hz] diff --git a/extended_examples/pluginlib_example/example_plugin_manager/config/plugins.yaml b/extended_examples/pluginlib_example/example_plugin_manager/config/plugins.yaml index 0537828..58e3676 100644 --- a/extended_examples/pluginlib_example/example_plugin_manager/config/plugins.yaml +++ b/extended_examples/pluginlib_example/example_plugin_manager/config/plugins.yaml @@ -1,12 +1,12 @@ # define "running instances" of plugins, there can be more than one for each ExamplePlugin: - address: "example_plugins/ExamplePlugin" # this is taken from the plugin's package.xml + address: "example_plugins::ExamplePlugin" # this is taken from the plugin's package.xml name_space: "example_plugin" # useful for loading parameters into difference instances of the same plugin some_property: 6.28 # this can be anything specific the manager needs to hold for each instance of each plugin # We can run another instance of the same plugin ExamplePlugin2: - address: "example_plugins/ExamplePlugin" # this is taken from the plugin's package.xml + address: "example_plugins::ExamplePlugin" # this is taken from the plugin's package.xml name_space: "example_plugin_2" some_property: 10.1 diff --git a/extended_examples/pluginlib_example/example_plugin_manager/include/example_plugin_manager/plugin_interface.h b/extended_examples/pluginlib_example/example_plugin_manager/include/example_plugin_manager/plugin_interface.h index 9ba63bf..875b090 100644 --- a/extended_examples/pluginlib_example/example_plugin_manager/include/example_plugin_manager/plugin_interface.h +++ b/extended_examples/pluginlib_example/example_plugin_manager/include/example_plugin_manager/plugin_interface.h @@ -9,7 +9,7 @@ namespace example_plugin_manager class Plugin { public: - virtual void initialize(const rclcpp::Node &parent_node, const std::string& name, const std::string& name_space, + virtual void initialize(const rclcpp::NodeOptions &node_options, const std::string& name, const std::string& name_space, std::shared_ptr common_handlers) = 0; virtual bool activate(const int& some_number) = 0; diff --git a/extended_examples/pluginlib_example/example_plugin_manager/launch/example_plugin_manager.py b/extended_examples/pluginlib_example/example_plugin_manager/launch/example_plugin_manager.py new file mode 100755 index 0000000..06320d3 --- /dev/null +++ b/extended_examples/pluginlib_example/example_plugin_manager/launch/example_plugin_manager.py @@ -0,0 +1,63 @@ +import launch +import os + +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode +from launch.actions import DeclareLaunchArgument +from launch.substitutions import ( + LaunchConfiguration, + IfElseSubstitution, + PythonExpression, + PathJoinSubstitution, + EnvironmentVariable, +) + +from ament_index_python.packages import get_package_share_directory + + + +def generate_launch_description(): + + ld = launch.LaunchDescription() + + pkg_name = "example_plugin_manager" + pkg_share_path = get_package_share_directory(pkg_name) + + # param loaded from env variable + uav_type=os.getenv('UAV_TYPE', "") + + ld.add_action(ComposableNodeContainer( + + namespace="nmspc1", + name='nmspc1_example_plugin_manager', + package='rclcpp_components', + executable='component_container_mt', + + composable_node_descriptions=[ + + ComposableNode( + + package=pkg_name, + plugin='example_plugin_manager::ExamplePluginManager', + namespace="nmspc1", + name='example_plugin_manager', + + parameters=[ + pkg_share_path + '/config/example_plugin_manager.yaml', + pkg_share_path + '/config/plugins.yaml', + {'uav_type': uav_type}, + ], + + # remappings=[ + # # topics + # ("~/topic_out", "~/topic"), + # ], + + ), + + ], + + output='screen', + )) + + return ld \ No newline at end of file diff --git a/extended_examples/pluginlib_example/example_plugin_manager/src/example_plugin_manager.cpp b/extended_examples/pluginlib_example/example_plugin_manager/src/example_plugin_manager.cpp index 6dc01c6..fce1fd6 100644 --- a/extended_examples/pluginlib_example/example_plugin_manager/src/example_plugin_manager.cpp +++ b/extended_examples/pluginlib_example/example_plugin_manager/src/example_plugin_manager.cpp @@ -106,7 +106,6 @@ ExamplePluginManager::ExamplePluginManager(const rclcpp::NodeOptions &options) rclcpp::shutdown(); return; } - // | --------------- example of a shared object --------------- | example_of_a_shared_object_ = "Hello, this is a shared object"; @@ -126,12 +125,25 @@ ExamplePluginManager::ExamplePluginManager(const rclcpp::NodeOptions &options) // | load the plugins | // -------------------------------------------------------------- - rclcpp::Parameter param_plugin_names; - loaded_successfully &= this->get_parameter("plugins", param_plugin_names); - _plugin_names_ = param_plugin_names.as_string_array(); + // rclcpp::Parameter param_plugin_names; + // loaded_successfully &= this->get_parameter("plugins", param_plugin_names); + // _plugin_names_ = param_plugin_names.as_string_array(); + loaded_successfully &= utils::parse_param("plugins", _plugin_names_ , *this); + + if (!loaded_successfully) { + RCLCPP_ERROR_STREAM(get_logger(), "Could not load all non-optional parameters. Shutting down."); + rclcpp::shutdown(); + return; + } plugin_loader_ = std::make_unique>("example_plugin_manager", "example_plugin_manager::Plugin"); + std::vector plugins = plugin_loader_->getDeclaredClasses(); // Method name changed + + for (const auto& plugin : plugins) { + RCLCPP_INFO(get_logger(), "Plugin: %s", plugin.c_str()); + } + // for each plugin in the list for (int i = 0; i < int(_plugin_names_.size()); i++) { std::string plugin_name = _plugin_names_[i]; @@ -141,26 +153,34 @@ ExamplePluginManager::ExamplePluginManager(const rclcpp::NodeOptions &options) std::string name_space; double some_property; - loaded_successfully &= utils::parse_param(plugin_name + "/address", address, *this); - loaded_successfully &= utils::parse_param(plugin_name + "/name_space", name_space, *this); - loaded_successfully &= utils::parse_param(plugin_name + "/some_property", some_property, *this); + loaded_successfully &= utils::parse_param(plugin_name + ".address", address, *this); + loaded_successfully &= utils::parse_param(plugin_name + ".name_space", name_space, *this); + loaded_successfully &= utils::parse_param(plugin_name + ".some_property", some_property, *this); + + if (!loaded_successfully) { + RCLCPP_ERROR_STREAM(get_logger(), "Could not load all non-optional parameters. Shutting down."); + rclcpp::shutdown(); + return; + } PluginParams new_plugin(address, name_space, some_property); plugins_.insert(std::pair(plugin_name, new_plugin)); try { RCLCPP_INFO(get_logger(), "[ExamplePluginManager]: loading the plugin '%s'", new_plugin.address.c_str()); - plugin_list_.push_back(plugin_loader_->createSharedInstance(new_plugin.address.c_str())); + plugin_list_.emplace_back(plugin_loader_->createSharedInstance(new_plugin.address)); } catch (pluginlib::CreateClassException& ex1) { RCLCPP_ERROR(get_logger(), "[ExamplePluginManager]: CreateClassException for the plugin '%s'", new_plugin.address.c_str()); RCLCPP_ERROR(get_logger(), "[ExamplePluginManager]: Error: %s", ex1.what()); rclcpp::shutdown(); + return; } catch (pluginlib::PluginlibException& ex) { RCLCPP_ERROR(get_logger(), "[ExamplePluginManager]: PluginlibException for the plugin '%s'", new_plugin.address.c_str()); RCLCPP_ERROR(get_logger(), "[ExamplePluginManager]: Error: %s", ex.what()); rclcpp::shutdown(); + return; } } @@ -172,7 +192,7 @@ ExamplePluginManager::ExamplePluginManager(const rclcpp::NodeOptions &options) it = plugins_.find(_plugin_names_[i]); RCLCPP_INFO(get_logger(), "[ExamplePluginManager]: initializing the plugin '%s'", it->second.address.c_str()); - plugin_list_[i]->initialize(*this, _plugin_names_[i], it->second.name_space, common_handlers_); + plugin_list_[i]->initialize(options, _plugin_names_[i], it->second.name_space, common_handlers_); } catch (std::runtime_error& ex) { RCLCPP_ERROR(get_logger(), "[ExamplePluginManager]: exception caught during plugin initialization: '%s'", ex.what()); @@ -201,6 +221,7 @@ ExamplePluginManager::ExamplePluginManager(const rclcpp::NodeOptions &options) if (!check) { RCLCPP_ERROR(get_logger(), "[ExamplePluginManager]: the initial plugin (%s) is not within the loaded plugins", _initial_plugin_name_.c_str()); rclcpp::shutdown(); + return; } } @@ -219,12 +240,6 @@ ExamplePluginManager::ExamplePluginManager(const rclcpp::NodeOptions &options) // | ----------------------- finish init ---------------------- | - if (!loaded_successfully) { - RCLCPP_ERROR_STREAM(get_logger(), "Could not load all non-optional parameters. Shutting down."); - rclcpp::shutdown(); - return; - } - is_initialized_ = true; RCLCPP_INFO(get_logger(), "[ExamplePluginManager]: initialized"); diff --git a/extended_examples/pluginlib_example/example_plugins/CMakeLists.txt b/extended_examples/pluginlib_example/example_plugins/CMakeLists.txt index e6c8a8e..74f8f33 100644 --- a/extended_examples/pluginlib_example/example_plugins/CMakeLists.txt +++ b/extended_examples/pluginlib_example/example_plugins/CMakeLists.txt @@ -11,6 +11,7 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") endif() find_package(ament_cmake REQUIRED) +find_package(ament_cmake_ros REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) find_package(pluginlib REQUIRED) @@ -23,18 +24,14 @@ pluginlib_export_plugin_description_file(example_plugin_manager plugins.xml) set(Eigen_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIRS}) set(Eigen_LIBRARIES ${Eigen_LIBRARIES}) -include_directories( - include - ) - # ExamplePlugin -add_library(example_plugin SHARED +add_library(${PROJECT_NAME} SHARED src/example_plugin.cpp ) # ament manages cmake related variables and dependency search (similar to catkin in ROS1) -ament_target_dependencies(example_plugin PUBLIC +ament_target_dependencies(${PROJECT_NAME} PUBLIC rclcpp rclcpp_components pluginlib @@ -43,30 +40,27 @@ ament_target_dependencies(example_plugin PUBLIC example_plugin_manager ) -# each component (nodelet) needs to be registered to make it discoverable at runtime -rclcpp_components_register_nodes(example_plugin "example_plugin::ExamplePlugin") - ## -------------------------------------------------------------- ## | Install | ## -------------------------------------------------------------- -install(TARGETS - example_plugin +install( + TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin - ) - -install(DIRECTORY include/ - DESTINATION include/ ) -install(DIRECTORY launch +install(DIRECTORY config DESTINATION share/${PROJECT_NAME} ) -install(DIRECTORY config - DESTINATION share/${PROJECT_NAME} +ament_export_libraries( + ${PROJECT_NAME} +) +ament_export_targets( + export_${PROJECT_NAME} ) ament_package() \ No newline at end of file diff --git a/extended_examples/pluginlib_example/example_plugins/plugins.xml b/extended_examples/pluginlib_example/example_plugins/plugins.xml index 2ac3e83..db70cac 100644 --- a/extended_examples/pluginlib_example/example_plugins/plugins.xml +++ b/extended_examples/pluginlib_example/example_plugins/plugins.xml @@ -1,5 +1,5 @@ - + This is ExamplePlugin. \ No newline at end of file diff --git a/extended_examples/pluginlib_example/example_plugins/src/example_plugin.cpp b/extended_examples/pluginlib_example/example_plugins/src/example_plugin.cpp index 149b1ad..09e36b3 100644 --- a/extended_examples/pluginlib_example/example_plugins/src/example_plugin.cpp +++ b/extended_examples/pluginlib_example/example_plugins/src/example_plugin.cpp @@ -1,5 +1,6 @@ #include #include +#include #include #include #include @@ -14,15 +15,12 @@ namespace example_plugins { -namespace example_plugin -{ - /* class ExamplePlugin //{ */ class ExamplePlugin : public example_plugin_manager::Plugin { public: - void initialize(const rclcpp::Node &parent_node, const std::string& name, const std::string& name_space, + void initialize(const rclcpp::NodeOptions &node_options, const std::string& name, const std::string& name_space, std::shared_ptr common_handlers); bool activate(const int& some_number); @@ -33,7 +31,7 @@ class ExamplePlugin : public example_plugin_manager::Plugin { // parameter from a config file double _pi_; - std::shared_ptr parent_node_; + std::shared_ptr node_; std::string _name_; private: @@ -49,10 +47,10 @@ class ExamplePlugin : public example_plugin_manager::Plugin { /* initialize() //{ */ -void ExamplePlugin::initialize(const rclcpp::Node &parent_node, const std::string& name, const std::string& name_space, +void ExamplePlugin::initialize(const rclcpp::NodeOptions &node_options, const std::string& name, const std::string& name_space, std::shared_ptr common_handlers) { - parent_node_ = std::make_shared(parent_node); + node_ = std::make_shared(name, name_space, node_options); _name_ = name; // I can use this to get stuff from the manager interactively @@ -62,10 +60,10 @@ void ExamplePlugin::initialize(const rclcpp::Node &parent_node, const std::strin bool loaded_successfully = true; - loaded_successfully &= utils::parse_param("pi", _pi_, *parent_node_); + loaded_successfully &= utils::parse_param("pi", _pi_, *node_); if (!loaded_successfully) { - RCLCPP_ERROR_STREAM(parent_node_->get_logger(), "Could not load all non-optional parameters. Shutting down."); + RCLCPP_ERROR_STREAM(node_->get_logger(), "Could not load all non-optional parameters. Shutting down."); rclcpp::shutdown(); return; } @@ -81,11 +79,11 @@ void ExamplePlugin::initialize(const rclcpp::Node &parent_node, const std::strin // ros::shutdown(); // } - RCLCPP_INFO(parent_node_->get_logger(), "[%s]: loaded custom parameter: pi=%f", _name_.c_str(), _pi_); + RCLCPP_INFO(node_->get_logger(), "[%s]: loaded custom parameter: pi=%f", _name_.c_str(), _pi_); // | ----------------------- finish init ---------------------- | - RCLCPP_INFO(parent_node_->get_logger(), "[%s]: initialized under the name '%s', and namespace '%s'", _name_.c_str(), name.c_str(), name_space.c_str()); + RCLCPP_INFO(node_->get_logger(), "[%s]: initialized under the name '%s', and namespace '%s'", _name_.c_str(), name.c_str(), name_space.c_str()); is_initialized_ = true; } @@ -96,7 +94,7 @@ void ExamplePlugin::initialize(const rclcpp::Node &parent_node, const std::strin bool ExamplePlugin::activate(const int& some_number) { - RCLCPP_INFO(parent_node_->get_logger(), "[%s]: activated with some_number=%d", _name_.c_str(), some_number); + RCLCPP_INFO(node_->get_logger(), "[%s]: activated with some_number=%d", _name_.c_str(), some_number); is_active_ = true; @@ -111,7 +109,7 @@ void ExamplePlugin::deactivate(void) { is_active_ = false; - RCLCPP_INFO(parent_node_->get_logger(), "[%s]: deactivated", _name_.c_str()); + RCLCPP_INFO(node_->get_logger(), "[%s]: deactivated", _name_.c_str()); } //} @@ -124,7 +122,7 @@ const std::optional ExamplePlugin::update(const Eigen::Vector3d& input) return false; } - RCLCPP_ERROR_STREAM(parent_node_->get_logger(), "[" << _name_ << "]: update() was called, let's find out the size of the vector [" << input.transpose() << "]"); + RCLCPP_ERROR_STREAM(node_->get_logger(), "[" << _name_ << "]: update() was called, let's find out the size of the vector [" << input.transpose() << "]"); // check some property from the "manager" if (common_handlers_->vector_calculator.enabled) { @@ -143,10 +141,8 @@ const std::optional ExamplePlugin::update(const Eigen::Vector3d& input) //} -} // namespace example_plugin - } // namespace example_plugins #include -PLUGINLIB_EXPORT_CLASS(example_plugins::example_plugin::ExamplePlugin, example_plugin_manager::Plugin) +PLUGINLIB_EXPORT_CLASS(example_plugins::ExamplePlugin, example_plugin_manager::Plugin) diff --git a/ros2_examples/include/ros2_examples/params.h b/ros2_examples/include/ros2_examples/params.h index 1e35933..85b9497 100644 --- a/ros2_examples/include/ros2_examples/params.h +++ b/ros2_examples/include/ros2_examples/params.h @@ -3,6 +3,20 @@ namespace utils { +// Overload << operator for std::vector +template +std::ostream& operator<<(std::ostream& os, const std::vector& vec) { + os << "["; + for (size_t i = 0; i < vec.size(); ++i) { + os << vec[i]; + if (i != vec.size() - 1) { + os << ", "; // Add a comma between elements + } + } + os << "]"; + return os; +} + /* parse_param() method //{ */ // a helper parameter loading function template From 47d1c63f2bf1e9be1fa66359cf0c6c15ff33ee22 Mon Sep 17 00:00:00 2001 From: Afzal Ahmad Date: Wed, 29 Jan 2025 16:39:05 +0100 Subject: [PATCH 06/27] update: cleaned and made fixes to the plugin load --- .../example_plugin_manager/CMakeLists.txt | 9 ++-- .../config/example_plugin_manager.yaml | 2 +- .../config/plugins.yaml | 8 ++-- .../example_plugin_manager/plugin_interface.h | 2 +- .../launch/example_plugin_manager.launch | 22 --------- .../launch/example_plugin_manager.py | 33 ++++++------- .../example_plugin_manager/package.xml | 6 ++- .../src/example_plugin_manager.cpp | 46 ++++++------------- .../example_plugins/CMakeLists.txt | 22 ++++----- .../example_plugins/package.xml | 5 +- .../example_plugins/plugins.xml | 1 + .../example_plugins/src/example_plugin.cpp | 46 ++++++------------- 12 files changed, 71 insertions(+), 131 deletions(-) delete mode 100644 extended_examples/pluginlib_example/example_plugin_manager/launch/example_plugin_manager.launch diff --git a/extended_examples/pluginlib_example/example_plugin_manager/CMakeLists.txt b/extended_examples/pluginlib_example/example_plugin_manager/CMakeLists.txt index 3e073e4..525ffa8 100644 --- a/extended_examples/pluginlib_example/example_plugin_manager/CMakeLists.txt +++ b/extended_examples/pluginlib_example/example_plugin_manager/CMakeLists.txt @@ -11,11 +11,12 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") endif() find_package(ament_cmake REQUIRED) +find_package(ament_cmake_ros REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) find_package(pluginlib REQUIRED) -find_package(ros2_examples REQUIRED) find_package(Eigen3 REQUIRED) +find_package(ros2_examples REQUIRED) set(Eigen_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIRS}) set(Eigen_LIBRARIES ${Eigen_LIBRARIES}) @@ -25,7 +26,6 @@ include_directories( ) # ExamplePluginManager - add_library(example_plugin_manager SHARED src/example_plugin_manager.cpp ) @@ -52,6 +52,7 @@ install(TARGETS RUNTIME DESTINATION bin ) +# this will copy the header files for use by other libraries install( DIRECTORY include/ DESTINATION include @@ -65,5 +66,7 @@ install(DIRECTORY config DESTINATION share/${PROJECT_NAME} ) +# this will export the include-related variables for use by other libraries +# this is needed if you wish to include the headers from this pkg into downstream libraries ament_export_include_directories("include") -ament_package() +ament_package() \ No newline at end of file diff --git a/extended_examples/pluginlib_example/example_plugin_manager/config/example_plugin_manager.yaml b/extended_examples/pluginlib_example/example_plugin_manager/config/example_plugin_manager.yaml index 1df728f..f32650d 100644 --- a/extended_examples/pluginlib_example/example_plugin_manager/config/example_plugin_manager.yaml +++ b/extended_examples/pluginlib_example/example_plugin_manager/config/example_plugin_manager.yaml @@ -1,5 +1,5 @@ # the list of "names" of the loaded plugins -# each plugin's properties are defined in 'plugins.yaml' +# each plugin's properties are defined in 'plugins.yaml' of the example_plugin package plugins : [ "ExamplePlugin", "ExamplePlugin2", diff --git a/extended_examples/pluginlib_example/example_plugin_manager/config/plugins.yaml b/extended_examples/pluginlib_example/example_plugin_manager/config/plugins.yaml index 58e3676..983cfc2 100644 --- a/extended_examples/pluginlib_example/example_plugin_manager/config/plugins.yaml +++ b/extended_examples/pluginlib_example/example_plugin_manager/config/plugins.yaml @@ -1,12 +1,12 @@ # define "running instances" of plugins, there can be more than one for each ExamplePlugin: - address: "example_plugins::ExamplePlugin" # this is taken from the plugin's package.xml - name_space: "example_plugin" # useful for loading parameters into difference instances of the same plugin + address: "example_plugins::ExamplePlugin" # this is taken from the 'type' arg of plugins.xml which is inside the 'example_plugin' pkg + name_space: "example_plugin" # useful for loading parameters into different instances of the same plugin some_property: 6.28 # this can be anything specific the manager needs to hold for each instance of each plugin # We can run another instance of the same plugin ExamplePlugin2: - address: "example_plugins::ExamplePlugin" # this is taken from the plugin's package.xml - name_space: "example_plugin_2" + address: "example_plugins::ExamplePlugin" + name_space: "example_plugin_2" # notice the different namespace to separate these plugins some_property: 10.1 diff --git a/extended_examples/pluginlib_example/example_plugin_manager/include/example_plugin_manager/plugin_interface.h b/extended_examples/pluginlib_example/example_plugin_manager/include/example_plugin_manager/plugin_interface.h index 875b090..3d3f13a 100644 --- a/extended_examples/pluginlib_example/example_plugin_manager/include/example_plugin_manager/plugin_interface.h +++ b/extended_examples/pluginlib_example/example_plugin_manager/include/example_plugin_manager/plugin_interface.h @@ -9,7 +9,7 @@ namespace example_plugin_manager class Plugin { public: - virtual void initialize(const rclcpp::NodeOptions &node_options, const std::string& name, const std::string& name_space, + virtual void initialize(const std::shared_ptr &sub_node, const std::string& name, std::shared_ptr common_handlers) = 0; virtual bool activate(const int& some_number) = 0; diff --git a/extended_examples/pluginlib_example/example_plugin_manager/launch/example_plugin_manager.launch b/extended_examples/pluginlib_example/example_plugin_manager/launch/example_plugin_manager.launch deleted file mode 100644 index c87ab11..0000000 --- a/extended_examples/pluginlib_example/example_plugin_manager/launch/example_plugin_manager.launch +++ /dev/null @@ -1,22 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - diff --git a/extended_examples/pluginlib_example/example_plugin_manager/launch/example_plugin_manager.py b/extended_examples/pluginlib_example/example_plugin_manager/launch/example_plugin_manager.py index 06320d3..deb4390 100755 --- a/extended_examples/pluginlib_example/example_plugin_manager/launch/example_plugin_manager.py +++ b/extended_examples/pluginlib_example/example_plugin_manager/launch/example_plugin_manager.py @@ -3,14 +3,7 @@ from launch_ros.actions import ComposableNodeContainer from launch_ros.descriptions import ComposableNode -from launch.actions import DeclareLaunchArgument -from launch.substitutions import ( - LaunchConfiguration, - IfElseSubstitution, - PythonExpression, - PathJoinSubstitution, - EnvironmentVariable, -) +from launch.substitutions import EnvironmentVariable from ament_index_python.packages import get_package_share_directory @@ -24,28 +17,28 @@ def generate_launch_description(): pkg_share_path = get_package_share_directory(pkg_name) # param loaded from env variable - uav_type=os.getenv('UAV_TYPE', "") + uav_type = EnvironmentVariable("UAV_TYPE", default_value="dummy") ld.add_action(ComposableNodeContainer( - namespace="nmspc1", - name='nmspc1_example_plugin_manager', - package='rclcpp_components', - executable='component_container_mt', + namespace="container_ns", + name="comp_container", + package="rclcpp_components", + executable="component_container_mt", composable_node_descriptions=[ ComposableNode( package=pkg_name, - plugin='example_plugin_manager::ExamplePluginManager', - namespace="nmspc1", - name='example_plugin_manager', + plugin="example_plugin_manager::ExamplePluginManager", + namespace="node_ns", + name="example_plugin_manager", parameters=[ - pkg_share_path + '/config/example_plugin_manager.yaml', - pkg_share_path + '/config/plugins.yaml', - {'uav_type': uav_type}, + pkg_share_path + "/config/example_plugin_manager.yaml", + pkg_share_path + "/config/plugins.yaml", + {"uav_type": uav_type}, ], # remappings=[ @@ -57,7 +50,7 @@ def generate_launch_description(): ], - output='screen', + output="screen", )) return ld \ No newline at end of file diff --git a/extended_examples/pluginlib_example/example_plugin_manager/package.xml b/extended_examples/pluginlib_example/example_plugin_manager/package.xml index 69c3bf3..c4bee84 100644 --- a/extended_examples/pluginlib_example/example_plugin_manager/package.xml +++ b/extended_examples/pluginlib_example/example_plugin_manager/package.xml @@ -7,16 +7,18 @@ Afzal Ahmad Afzal Ahmad + Tomas Baca TODO: License declaration ament_cmake + ament_cmake_ros rclcpp rclcpp_components - nodelet - mrs_lib + pluginlib ros2_examples + eigen ament_cmake diff --git a/extended_examples/pluginlib_example/example_plugin_manager/src/example_plugin_manager.cpp b/extended_examples/pluginlib_example/example_plugin_manager/src/example_plugin_manager.cpp index fce1fd6..06c6476 100644 --- a/extended_examples/pluginlib_example/example_plugin_manager/src/example_plugin_manager.cpp +++ b/extended_examples/pluginlib_example/example_plugin_manager/src/example_plugin_manager.cpp @@ -1,14 +1,12 @@ +#include +#include #include #include #include #include -#include #include - - -// #include -// #include +#include namespace example_plugin_manager { @@ -44,6 +42,7 @@ class ExamplePluginManager : public rclcpp::Node { private: bool is_initialized_ = false; + std::string _uav_type_ = ""; // | ---------------------- update timer ---------------------- | @@ -84,20 +83,15 @@ class ExamplePluginManager : public rclcpp::Node { /* onInit() //{ */ -ExamplePluginManager::ExamplePluginManager(const rclcpp::NodeOptions &options) : Node("params_example", options) { +ExamplePluginManager::ExamplePluginManager(const rclcpp::NodeOptions &options) : Node("example_plugin_manager", options) { RCLCPP_INFO(get_logger(), "[ExamplePluginManager]: initializing"); - // -------------------------------------------------------------- - // | params | - // -------------------------------------------------------------- - - // mrs_lib::ParamLoader param_loader(nh_, "ExamplePluginManager"); - // | --------------------- load parameters -------------------- | bool loaded_successfully = true; + loaded_successfully &= utils::parse_param("uav_type", _uav_type_, *this); loaded_successfully &= utils::parse_param("update_timer_rate", _rate_timer_update_, *this); loaded_successfully &= utils::parse_param("initial_plugin", _initial_plugin_name_, *this); @@ -125,9 +119,6 @@ ExamplePluginManager::ExamplePluginManager(const rclcpp::NodeOptions &options) // | load the plugins | // -------------------------------------------------------------- - // rclcpp::Parameter param_plugin_names; - // loaded_successfully &= this->get_parameter("plugins", param_plugin_names); - // _plugin_names_ = param_plugin_names.as_string_array(); loaded_successfully &= utils::parse_param("plugins", _plugin_names_ , *this); if (!loaded_successfully) { @@ -138,21 +129,16 @@ ExamplePluginManager::ExamplePluginManager(const rclcpp::NodeOptions &options) plugin_loader_ = std::make_unique>("example_plugin_manager", "example_plugin_manager::Plugin"); - std::vector plugins = plugin_loader_->getDeclaredClasses(); // Method name changed - - for (const auto& plugin : plugins) { - RCLCPP_INFO(get_logger(), "Plugin: %s", plugin.c_str()); - } - // for each plugin in the list for (int i = 0; i < int(_plugin_names_.size()); i++) { - std::string plugin_name = _plugin_names_[i]; + std::string plugin_name = _plugin_names_.at(i); // load the plugin parameters std::string address; std::string name_space; double some_property; + // nested params are separated by '.' instead of '/' loaded_successfully &= utils::parse_param(plugin_name + ".address", address, *this); loaded_successfully &= utils::parse_param(plugin_name + ".name_space", name_space, *this); loaded_successfully &= utils::parse_param(plugin_name + ".some_property", some_property, *this); @@ -189,10 +175,10 @@ ExamplePluginManager::ExamplePluginManager(const rclcpp::NodeOptions &options) for (int i = 0; i < int(plugin_list_.size()); i++) { try { std::map::iterator it; - it = plugins_.find(_plugin_names_[i]); + it = plugins_.find(_plugin_names_.at(i)); - RCLCPP_INFO(get_logger(), "[ExamplePluginManager]: initializing the plugin '%s'", it->second.address.c_str()); - plugin_list_[i]->initialize(options, _plugin_names_[i], it->second.name_space, common_handlers_); + RCLCPP_INFO(get_logger(), "[ExamplePluginManager]: initializing the plugin '%s' of type '%s'", it->second.name_space.c_str(), it->second.address.c_str()); + plugin_list_.at(i)->initialize(this->create_sub_node(it->second.name_space), _plugin_names_.at(i), common_handlers_); } catch (std::runtime_error& ex) { RCLCPP_ERROR(get_logger(), "[ExamplePluginManager]: exception caught during plugin initialization: '%s'", ex.what()); @@ -210,7 +196,7 @@ ExamplePluginManager::ExamplePluginManager(const rclcpp::NodeOptions &options) for (int i = 0; i < int(_plugin_names_.size()); i++) { - std::string plugin_name = _plugin_names_[i]; + std::string plugin_name = _plugin_names_.at(i); if (plugin_name == _initial_plugin_name_) { check = true; @@ -227,11 +213,11 @@ ExamplePluginManager::ExamplePluginManager(const rclcpp::NodeOptions &options) // | ---------- activate the first plugin on the list --------- | - RCLCPP_INFO(get_logger(), "[ExamplePluginManager]: activating plugin with idx %d on the list (named: %s)", _initial_plugin_idx_, _plugin_names_[_initial_plugin_idx_].c_str()); + RCLCPP_INFO(get_logger(), "[ExamplePluginManager]: activating plugin with idx %d on the list (named: %s)", _initial_plugin_idx_, _plugin_names_.at(_initial_plugin_idx_).c_str()); int some_activation_input_to_plugin = 1234; - plugin_list_[_initial_plugin_idx_]->activate(some_activation_input_to_plugin); + plugin_list_.at(_initial_plugin_idx_)->activate(some_activation_input_to_plugin); active_plugin_idx_ = _initial_plugin_idx_; // | ------------------------- timers ------------------------- | @@ -256,14 +242,12 @@ void ExamplePluginManager::timerUpdate() { if (!is_initialized_) return; - // auto active_plugin_idx = mrs_lib::get_mutexed(mutex_plugins_, active_plugin_idx_); - // plugin input Eigen::Vector3d input; input << 0, 1, 2; // call the plugin's update routine - auto result = plugin_list_[active_plugin_idx_]->update(input); + auto result = plugin_list_.at(active_plugin_idx_)->update(input); if (result) { diff --git a/extended_examples/pluginlib_example/example_plugins/CMakeLists.txt b/extended_examples/pluginlib_example/example_plugins/CMakeLists.txt index 74f8f33..3e8ee99 100644 --- a/extended_examples/pluginlib_example/example_plugins/CMakeLists.txt +++ b/extended_examples/pluginlib_example/example_plugins/CMakeLists.txt @@ -14,18 +14,18 @@ find_package(ament_cmake REQUIRED) find_package(ament_cmake_ros REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) +find_package(Eigen3 REQUIRED) find_package(pluginlib REQUIRED) find_package(ros2_examples REQUIRED) find_package(example_plugin_manager REQUIRED) -find_package(Eigen3 REQUIRED) +# export the plugins listed in plugins.xml to be discovered by the manager pluginlib_export_plugin_description_file(example_plugin_manager plugins.xml) set(Eigen_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIRS}) set(Eigen_LIBRARIES ${Eigen_LIBRARIES}) -# ExamplePlugin - +# ${PROJECT_NAME} add_library(${PROJECT_NAME} SHARED src/example_plugin.cpp ) @@ -34,8 +34,8 @@ add_library(${PROJECT_NAME} SHARED ament_target_dependencies(${PROJECT_NAME} PUBLIC rclcpp rclcpp_components - pluginlib Eigen3 + pluginlib ros2_examples example_plugin_manager ) @@ -45,8 +45,8 @@ ament_target_dependencies(${PROJECT_NAME} PUBLIC ## -------------------------------------------------------------- install( - TARGETS ${PROJECT_NAME} - EXPORT export_${PROJECT_NAME} + TARGETS ${PROJECT_NAME} # list all libraries to be installed and exported out from this pkg + EXPORT export_${PROJECT_NAME} # a single export name is enough to export all libraries from this pkg ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin @@ -55,12 +55,8 @@ install( install(DIRECTORY config DESTINATION share/${PROJECT_NAME} ) - -ament_export_libraries( - ${PROJECT_NAME} -) -ament_export_targets( - export_${PROJECT_NAME} -) +# export the installed library and related info to the cmake system +# this is ESSENTIAL for the plugin to be discovered dynamically +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) ament_package() \ No newline at end of file diff --git a/extended_examples/pluginlib_example/example_plugins/package.xml b/extended_examples/pluginlib_example/example_plugins/package.xml index eb54f57..6d5f1d7 100644 --- a/extended_examples/pluginlib_example/example_plugins/package.xml +++ b/extended_examples/pluginlib_example/example_plugins/package.xml @@ -12,11 +12,12 @@ TODO: License declaration ament_cmake + ament_cmake_ros rclcpp rclcpp_components - nodelet - mrs_lib + pluginlib + eigen ros2_examples example_plugin_manager diff --git a/extended_examples/pluginlib_example/example_plugins/plugins.xml b/extended_examples/pluginlib_example/example_plugins/plugins.xml index db70cac..7d0567f 100644 --- a/extended_examples/pluginlib_example/example_plugins/plugins.xml +++ b/extended_examples/pluginlib_example/example_plugins/plugins.xml @@ -1,4 +1,5 @@ + This is ExamplePlugin. diff --git a/extended_examples/pluginlib_example/example_plugins/src/example_plugin.cpp b/extended_examples/pluginlib_example/example_plugins/src/example_plugin.cpp index 09e36b3..e66a329 100644 --- a/extended_examples/pluginlib_example/example_plugins/src/example_plugin.cpp +++ b/extended_examples/pluginlib_example/example_plugins/src/example_plugin.cpp @@ -1,17 +1,9 @@ -#include -#include -#include -#include #include -#include #include #include #include - -// #include - namespace example_plugins { @@ -20,7 +12,7 @@ namespace example_plugins class ExamplePlugin : public example_plugin_manager::Plugin { public: - void initialize(const rclcpp::NodeOptions &node_options, const std::string& name, const std::string& name_space, + void initialize(const std::shared_ptr &sub_node, const std::string& name, std::shared_ptr common_handlers); bool activate(const int& some_number); @@ -29,7 +21,7 @@ class ExamplePlugin : public example_plugin_manager::Plugin { const std::optional update(const Eigen::Vector3d& input); // parameter from a config file - double _pi_; + // double _pi_; std::shared_ptr node_; std::string _name_; @@ -47,10 +39,10 @@ class ExamplePlugin : public example_plugin_manager::Plugin { /* initialize() //{ */ -void ExamplePlugin::initialize(const rclcpp::NodeOptions &node_options, const std::string& name, const std::string& name_space, +void ExamplePlugin::initialize(const std::shared_ptr &sub_node, const std::string& name, std::shared_ptr common_handlers) { - node_ = std::make_shared(name, name_space, node_options); + node_ = sub_node; _name_ = name; // I can use this to get stuff from the manager interactively @@ -58,32 +50,21 @@ void ExamplePlugin::initialize(const rclcpp::NodeOptions &node_options, const st // | ------------------- loading parameters ------------------- | - bool loaded_successfully = true; - - loaded_successfully &= utils::parse_param("pi", _pi_, *node_); - - if (!loaded_successfully) { - RCLCPP_ERROR_STREAM(node_->get_logger(), "Could not load all non-optional parameters. Shutting down."); - rclcpp::shutdown(); - return; - } - // mrs_lib::ParamLoader param_loader(nh_, "ExamplePlugin"); - - // param_loader.addYamlFile(ros::package::getPath("example_plugins") + "/config/example_plugin.yaml"); + // bool loaded_successfully = true; - // can load params like in a ROS node - // param_loader.loadParam("pi", _pi_); + // loaded_successfully &= utils::parse_param("pi", _pi_, *node_); - // if (!param_loader.loadedSuccessfully()) { - // ROS_ERROR("[%s]: could not load all parameters!", _name_.c_str()); - // ros::shutdown(); + // if (!loaded_successfully) { + // RCLCPP_ERROR_STREAM(node_->get_logger(), "Could not load all non-optional parameters. Shutting down."); + // rclcpp::shutdown(); + // return; // } - RCLCPP_INFO(node_->get_logger(), "[%s]: loaded custom parameter: pi=%f", _name_.c_str(), _pi_); + // RCLCPP_INFO(node_->get_logger(), "[%s]: loaded custom parameter: pi=%f", _name_.c_str(), _pi_); // | ----------------------- finish init ---------------------- | - RCLCPP_INFO(node_->get_logger(), "[%s]: initialized under the name '%s', and namespace '%s'", _name_.c_str(), name.c_str(), name_space.c_str()); + RCLCPP_INFO(node_->get_logger(), "[%s]: initialized under the name '%s', and namespace '%s'", _name_.c_str(), name.c_str(), node_->get_namespace()); is_initialized_ = true; } @@ -145,4 +126,5 @@ const std::optional ExamplePlugin::update(const Eigen::Vector3d& input) #include -PLUGINLIB_EXPORT_CLASS(example_plugins::ExamplePlugin, example_plugin_manager::Plugin) +// Register the plugin. +PLUGINLIB_EXPORT_CLASS(example_plugins::ExamplePlugin, example_plugin_manager::Plugin) \ No newline at end of file From d83f4398260bf2c141f7f78fe954912fa04e988b Mon Sep 17 00:00:00 2001 From: Afzal Ahmad Date: Wed, 29 Jan 2025 17:29:26 +0100 Subject: [PATCH 07/27] fix: a cleaner method to export the headers --- ros2_examples/CMakeLists.txt | 27 ++++----------------------- 1 file changed, 4 insertions(+), 23 deletions(-) diff --git a/ros2_examples/CMakeLists.txt b/ros2_examples/CMakeLists.txt index 994ac1b..8cb60e3 100644 --- a/ros2_examples/CMakeLists.txt +++ b/ros2_examples/CMakeLists.txt @@ -172,17 +172,6 @@ ament_target_dependencies(tf2_listener_example # TODO HOW DOES THIS WORK? rclcpp_components_register_nodes(tf2_listener_example PLUGIN "ros2_examples::Tf2ListenerExample" EXECUTABLE tf2_listener_example) -############################################################ -# adding libraries both Interface (header-only) -############################################################ -# add template header-only library for implementing simulators -add_library(${PROJECT_NAME}_headers INTERFACE) - -# add the header files that will be referenced when this target is used -target_include_directories(${PROJECT_NAME}_headers INTERFACE - "$" - "$") - ## -------------------------------------------------------------- ## | Testing | ## -------------------------------------------------------------- @@ -214,20 +203,9 @@ install(TARGETS ) install(DIRECTORY include/ - DESTINATION include/${PROJECT_NAME} -) - -install( - TARGETS ${PROJECT_NAME}_headers - EXPORT export_${PROJECT_NAME} - LIBRARY DESTINATION lib - ARCHIVE DESTINATION lib - RUNTIME DESTINATION bin - INCLUDES DESTINATION include + DESTINATION include ) -ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) - install(DIRECTORY launch DESTINATION share/${PROJECT_NAME} ) @@ -236,4 +214,7 @@ install(DIRECTORY config DESTINATION share/${PROJECT_NAME} ) +# this will export the include-related variables for use by other libraries +# this is needed if you wish to include the headers from this pkg into downstream libraries +ament_export_include_directories("include") ament_package() From ebe2bf4d4c096c9ce3b4451d327d8f2bc2270042 Mon Sep 17 00:00:00 2001 From: Tomas Baca Date: Tue, 4 Feb 2025 12:50:09 +0100 Subject: [PATCH 08/27] minor changes --- ros2_examples/launch/params_example.py | 2 ++ ros2_examples/launch/publisher_example.py | 2 ++ ros2_examples/launch/service_client_example.py | 2 ++ ros2_examples/launch/service_server_example.py | 2 ++ ros2_examples/launch/subscriber_example.py | 2 ++ ros2_examples/launch/tf2_broadcaster_example.py | 2 ++ ros2_examples/launch/tf2_listener_example.py | 2 ++ ros2_examples/launch/timer_example.py | 2 ++ ros2_lib/test/adder/CMakeLists.txt | 1 + 9 files changed, 17 insertions(+) diff --git a/ros2_examples/launch/params_example.py b/ros2_examples/launch/params_example.py index 867250c..3c826ff 100755 --- a/ros2_examples/launch/params_example.py +++ b/ros2_examples/launch/params_example.py @@ -1,3 +1,5 @@ +#!/usr/bin/env python3 + import launch import os diff --git a/ros2_examples/launch/publisher_example.py b/ros2_examples/launch/publisher_example.py index 5a1bb5d..0ec7e15 100755 --- a/ros2_examples/launch/publisher_example.py +++ b/ros2_examples/launch/publisher_example.py @@ -1,3 +1,5 @@ +#!/usr/bin/env python3 + import launch from ament_index_python import get_package_share_directory from launch_ros.actions import ComposableNodeContainer diff --git a/ros2_examples/launch/service_client_example.py b/ros2_examples/launch/service_client_example.py index 3e9703b..121ee38 100755 --- a/ros2_examples/launch/service_client_example.py +++ b/ros2_examples/launch/service_client_example.py @@ -1,3 +1,5 @@ +#!/usr/bin/env python3 + import launch from launch_ros.actions import ComposableNodeContainer diff --git a/ros2_examples/launch/service_server_example.py b/ros2_examples/launch/service_server_example.py index 3cb82de..5aec517 100755 --- a/ros2_examples/launch/service_server_example.py +++ b/ros2_examples/launch/service_server_example.py @@ -1,3 +1,5 @@ +#!/usr/bin/env python3 + import launch from launch_ros.actions import ComposableNodeContainer diff --git a/ros2_examples/launch/subscriber_example.py b/ros2_examples/launch/subscriber_example.py index f782666..ef04666 100755 --- a/ros2_examples/launch/subscriber_example.py +++ b/ros2_examples/launch/subscriber_example.py @@ -1,3 +1,5 @@ +#!/usr/bin/env python3 + import launch from ament_index_python.packages import get_package_share_directory from launch_ros.actions import ComposableNodeContainer diff --git a/ros2_examples/launch/tf2_broadcaster_example.py b/ros2_examples/launch/tf2_broadcaster_example.py index 4a21e12..3ff064e 100755 --- a/ros2_examples/launch/tf2_broadcaster_example.py +++ b/ros2_examples/launch/tf2_broadcaster_example.py @@ -1,3 +1,5 @@ +#!/usr/bin/env python3 + import launch from launch_ros.actions import ComposableNodeContainer diff --git a/ros2_examples/launch/tf2_listener_example.py b/ros2_examples/launch/tf2_listener_example.py index 58cd7c0..7eb849e 100755 --- a/ros2_examples/launch/tf2_listener_example.py +++ b/ros2_examples/launch/tf2_listener_example.py @@ -1,3 +1,5 @@ +#!/usr/bin/env python3 + import launch from launch_ros.actions import ComposableNodeContainer diff --git a/ros2_examples/launch/timer_example.py b/ros2_examples/launch/timer_example.py index b8a2a32..377d13c 100755 --- a/ros2_examples/launch/timer_example.py +++ b/ros2_examples/launch/timer_example.py @@ -1,3 +1,5 @@ +#!/usr/bin/env python3 + import launch from launch_ros.actions import ComposableNodeContainer diff --git a/ros2_lib/test/adder/CMakeLists.txt b/ros2_lib/test/adder/CMakeLists.txt index 19e2a6a..8db6843 100644 --- a/ros2_lib/test/adder/CMakeLists.txt +++ b/ros2_lib/test/adder/CMakeLists.txt @@ -2,6 +2,7 @@ get_filename_component(TEST_NAME "${CMAKE_CURRENT_SOURCE_DIR}" NAME) ament_add_gtest(test_${TEST_NAME} test.cpp + TIMEOUT 60 ) target_link_libraries(test_${TEST_NAME} From 782c1634244a637fa22b801b5b9054d675c60356 Mon Sep 17 00:00:00 2001 From: Tomas Baca Date: Tue, 4 Feb 2025 14:03:27 +0100 Subject: [PATCH 09/27] improving time handling --- ros2_examples/src/timer_example.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/ros2_examples/src/timer_example.cpp b/ros2_examples/src/timer_example.cpp index a8fe33e..5b385c6 100644 --- a/ros2_examples/src/timer_example.cpp +++ b/ros2_examples/src/timer_example.cpp @@ -157,8 +157,10 @@ void TimerExample::callback_timer3() { const double rate = cntr.update_rate(); publisher_timer_3_->publish(std_msgs::msg::Float64().set__data(rate)); + RCLCPP_INFO_STREAM_THROTTLE(get_logger(), *get_clock(), 1000, "[TimerExample]: 0.2Hz timer (5s period) spinning at rate " << rate << "Hz, starting 4s work"); - rclcpp::sleep_for(std::chrono::milliseconds(4000)); + + rclcpp::sleep_for(4s); RCLCPP_INFO(get_logger(), "[TimerExample]: 0.2 Hz timer stopping work"); } @@ -181,7 +183,7 @@ void TimerExample::callback_timer4() { } // this sleep simulates some kind of work done by this thread that can be paralellized - rclcpp::sleep_for(std::chrono::milliseconds(4000)); + rclcpp::sleep_for(4s); RCLCPP_INFO(get_logger(), "[TimerExample]: 0.5 Hz timer stopping work"); } From 26213e5831c5a9be60fa0bdd352991462f11b3df Mon Sep 17 00:00:00 2001 From: Tomas Baca Date: Tue, 4 Feb 2025 14:29:03 +0100 Subject: [PATCH 10/27] first commit --- README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/README.md b/README.md index a4004d2..797d026 100644 --- a/README.md +++ b/README.md @@ -106,3 +106,4 @@ Everything is a component. We happily [nodelet everything](https://www.clearpath This finds the relevant generated C++ code from ``AddressBook.msg`` and allows your target to link against it. You may have noticed that this step was not necessary when the interfaces being used were from a package that was built separately. This CMake code is only required when you want to use interfaces in the same package as the one in which they are used. [Source](https://docs.ros.org/en/foxy/Tutorials/Single-Package-Define-And-Use-Interface.html#link-against-the-interface) +# download_artifact_test From e31cd5dcb8069fa56d1e7cf44cafd74e3bba1f92 Mon Sep 17 00:00:00 2001 From: Tomas Baca Date: Wed, 19 Feb 2025 22:05:57 +0100 Subject: [PATCH 11/27] wip --- ros2_examples/CMakeLists.txt | 27 +++++++++++++++++---------- 1 file changed, 17 insertions(+), 10 deletions(-) diff --git a/ros2_examples/CMakeLists.txt b/ros2_examples/CMakeLists.txt index 8cb60e3..4231591 100644 --- a/ros2_examples/CMakeLists.txt +++ b/ros2_examples/CMakeLists.txt @@ -33,6 +33,7 @@ add_library(subscriber_example SHARED src/subscriber_example.cpp ) +# ament manages cmake related variables and dependency search (similar to catkin in ROS1) ament_target_dependencies(subscriber_example rclcpp rclcpp_components @@ -51,14 +52,12 @@ add_library(publisher_example SHARED src/publisher_example.cpp ) -# ament manages cmake related variables and dependency search (similar to catkin in ROS1) ament_target_dependencies(publisher_example rclcpp rclcpp_components std_msgs ) -# each component (nodelet) needs to be registered to make it discoverable at runtime rclcpp_components_register_nodes(publisher_example PLUGIN "ros2_examples::PublisherExample") ## -------------------------------------------------------------- @@ -69,14 +68,12 @@ add_library(service_server_example SHARED src/service_server_example.cpp ) -# ament manages cmake related variables and dependecy search (similar to catkin in ROS1) ament_target_dependencies(service_server_example rclcpp rclcpp_components std_srvs ) -# each component (nodelet) needs to be registered to make it discoverable at runtime rclcpp_components_register_nodes(service_server_example PLUGIN "ros2_examples::ServiceServerExample") ## -------------------------------------------------------------- @@ -87,14 +84,12 @@ add_library(service_client_example SHARED src/service_client_example.cpp ) -# ament manages cmake related variables and dependecy search (similar to catkin in ROS1) ament_target_dependencies(service_client_example rclcpp rclcpp_components std_srvs ) -# each component (nodelet) needs to be registered to make it discoverable at runtime rclcpp_components_register_nodes(service_client_example PLUGIN "ros2_examples::ServiceClientExample") ## -------------------------------------------------------------- @@ -111,7 +106,6 @@ ament_target_dependencies(timer_example std_msgs ) -# each component (nodelet) needs to be registered to make it discoverable at runtime rclcpp_components_register_nodes(timer_example PLUGIN "ros2_examples::TimerExample" EXECUTABLE timer_example) ## -------------------------------------------------------------- @@ -133,7 +127,6 @@ ament_target_dependencies(params_example std_srvs ) -# each component (nodelet) needs to be registered to make it discoverable at runtime rclcpp_components_register_nodes(params_example PLUGIN "ros2_examples::ParamsExample" EXECUTABLE params_example) ## -------------------------------------------------------------- @@ -151,7 +144,6 @@ ament_target_dependencies(tf2_broadcaster_example geometry_msgs ) -# TODO HOW DOES THIS WORK? rclcpp_components_register_nodes(tf2_broadcaster_example PLUGIN "ros2_examples::Tf2BroadcasterExample" EXECUTABLE tf2_broadcaster_example) ## -------------------------------------------------------------- @@ -169,9 +161,23 @@ ament_target_dependencies(tf2_listener_example geometry_msgs ) -# TODO HOW DOES THIS WORK? rclcpp_components_register_nodes(tf2_listener_example PLUGIN "ros2_examples::Tf2ListenerExample" EXECUTABLE tf2_listener_example) +## -------------------------------------------------------------- +## | clock management | +## -------------------------------------------------------------- + +add_library(clock_consumer SHARED + src/clock_consumer.cpp +) + +ament_target_dependencies(clock_consumer + rclcpp + rclcpp_components +) + +rclcpp_components_register_nodes(clock_consumer PLUGIN "ros2_examples::ClockConsumer" EXECUTABLE clock_consumer) + ## -------------------------------------------------------------- ## | Testing | ## -------------------------------------------------------------- @@ -197,6 +203,7 @@ install(TARGETS timer_example tf2_broadcaster_example tf2_listener_example + clock_consumer ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin From 85b8a7031897ea8bd8566c29cb4c4cd6fba09d80 Mon Sep 17 00:00:00 2001 From: Tomas Baca Date: Wed, 19 Feb 2025 22:06:22 +0100 Subject: [PATCH 12/27] + wip clock management --- README.md | 4 +- ros2_examples/config/clock_consumer.yaml | 3 + ros2_examples/launch/clock_management.py | 52 +++++++++ ros2_examples/src/clock_consumer.cpp | 119 +++++++++++++++++++++ ros2_examples/src/tf2_listener_example.cpp | 4 +- 5 files changed, 178 insertions(+), 4 deletions(-) create mode 100644 ros2_examples/config/clock_consumer.yaml create mode 100755 ros2_examples/launch/clock_management.py create mode 100644 ros2_examples/src/clock_consumer.cpp diff --git a/README.md b/README.md index 797d026..85d664a 100644 --- a/README.md +++ b/README.md @@ -89,8 +89,8 @@ Everything is a component. We happily [nodelet everything](https://www.clearpath * [o] **Tests** * [X] unit testing works * [X] integration testing works almost like in ROS1, more complex now but also more powerfull -* [ ] **Pluginlib** - * [ ] TODO +* [X] **Pluginlib** + * [X] TODO * [ ] **Actionlib** * [ ] TODO * [ ] **Lifecycles** (new feature) diff --git a/ros2_examples/config/clock_consumer.yaml b/ros2_examples/config/clock_consumer.yaml new file mode 100644 index 0000000..833769e --- /dev/null +++ b/ros2_examples/config/clock_consumer.yaml @@ -0,0 +1,3 @@ +# there has to be at least something in the config file +# otherwise it will produce a horrible error +some_entry: 666 diff --git a/ros2_examples/launch/clock_management.py b/ros2_examples/launch/clock_management.py new file mode 100755 index 0000000..d479f3c --- /dev/null +++ b/ros2_examples/launch/clock_management.py @@ -0,0 +1,52 @@ +#!/usr/bin/env python3 + +import launch + +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + +from ament_index_python.packages import get_package_share_directory + +def generate_launch_description(): + + ld = launch.LaunchDescription() + + pkg_name = "ros2_examples" + + pkg_share_path = get_package_share_directory(pkg_name) + namespace='nmspc1' + + ld.add_action(ComposableNodeContainer( + + namespace='', + name=namespace+'_clock_management', + package='rclcpp_components', + + executable='component_container_mt', + # executable='component_container', + + composable_node_descriptions=[ + + ComposableNode( + + package=pkg_name, + plugin='ros2_examples::ClockConsumer', + namespace=namespace, + name='clock_consumer', + + parameters=[ + pkg_share_path + '/config/clock_consumer.yaml', + ], + + # remappings=[ + # ("~/topic", "~/topic"), + # ], + ) + + ], + + output='screen', + + )) + + return ld diff --git a/ros2_examples/src/clock_consumer.cpp b/ros2_examples/src/clock_consumer.cpp new file mode 100644 index 0000000..acdf2be --- /dev/null +++ b/ros2_examples/src/clock_consumer.cpp @@ -0,0 +1,119 @@ +#include + +using namespace std::chrono_literals; + +namespace ros2_examples +{ + +/* class ClockConsumer //{ */ + +class ClockConsumer : public rclcpp::Node { +public: + ClockConsumer(rclcpp::NodeOptions options); + +private: + // | ------------------------- timers ------------------------- | + + rclcpp::CallbackGroup::SharedPtr cb_grp1_; + + rclcpp::TimerBase::SharedPtr timer_1_; + + rclcpp::TimerBase::SharedPtr timer_2_; + + void callback_timer1(); + + void callback_timer2(); +}; + +//} + +/* ClockConsumer() //{ */ + +ClockConsumer::ClockConsumer(rclcpp::NodeOptions options) : Node("timer_example", options) { + + RCLCPP_INFO(get_logger(), "[ClockConsumer]: initializing"); + + // | -------------------------- timer ------------------------- | + + cb_grp1_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + + // 100 Hz ROS timer + timer_1_ = this->create_timer(std::chrono::duration(1.0 / 10.0), std::bind(&ClockConsumer::callback_timer1, this), cb_grp1_); + + timer_1_->cancel(); + + // 1 Hz ROS time + timer_2_ = this->create_timer(std::chrono::duration(1.0 / 100.0), std::bind(&ClockConsumer::callback_timer2, this), cb_grp1_); + + // | --------------------- finish the init -------------------- | + + RCLCPP_INFO(get_logger(), "[ClockConsumer]: initialized"); +} + +//} + +// | ------------------------ callbacks ----------------------- | + +/* callbackTimer1() //{ */ + +void ClockConsumer::callback_timer1() { + + RCLCPP_INFO_ONCE(get_logger(), "[Timer 1]: running..."); + + // | ---------------------- testing sleep --------------------- | + + RCLCPP_INFO(get_logger(), "[Timer 1]: should be running at 10 Hz simtime"); +} + +//} + +/* callbackTimer2() //{ */ + +void ClockConsumer::callback_timer2() { + + RCLCPP_INFO_ONCE(get_logger(), "[Timer 2]: running..."); + + // | ---------------------- testing sleep --------------------- | + + auto clock = this->get_clock(); + + RCLCPP_INFO(get_logger(), "[Timer 2]: now we should sleep for 1.1 seconds"); + + clock->sleep_for(std::chrono::duration(1.1s)); + + RCLCPP_INFO(get_logger(), "[Timer 2]: sleep finished"); + + // | ---------------------- testing rate ---------------------- | + + rclcpp::Rate rate(10.0, clock); + + for (int i = 0; i < 10; i++) { + + RCLCPP_INFO(get_logger(), "[Timer 2]: this info should be comming at 10 Hz simtime"); + + rate.sleep(); + } + + // | ------------------------ duration ------------------------ | + + rclcpp::Duration duration(std::chrono::duration(1.1s)); + + RCLCPP_INFO(get_logger(), "[Timer 2]: now we should sleep for %.3f seconds", duration.seconds()); + + clock->sleep_for(duration); + + RCLCPP_INFO(get_logger(), "[Timer 2]: sleep finished"); + + // | -------------------- duration to rate -------------------- | + + timer_2_->cancel(); + + timer_1_->reset(); +} + +//} + +} // namespace ros2_examples + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(ros2_examples::ClockConsumer) diff --git a/ros2_examples/src/tf2_listener_example.cpp b/ros2_examples/src/tf2_listener_example.cpp index 87f99d7..6dc353b 100644 --- a/ros2_examples/src/tf2_listener_example.cpp +++ b/ros2_examples/src/tf2_listener_example.cpp @@ -42,9 +42,9 @@ Tf2ListenerExample::Tf2ListenerExample(rclcpp::NodeOptions options) // to suppress the error message which doesn't apply to this case tf_buffer_.setUsingDedicatedThread(true); - // | -------------------------- timer ------------------------- | + // | -------------------------- timer ------------------------- | - timer_main_ = create_wall_timer(std::chrono::duration(1.0 / 10.0), std::bind(&Tf2ListenerExample::timer_main, this)); + timer_main_ = create_wall_timer(std::chrono::duration(1.0 / 10.0), std::bind(&Tf2ListenerExample::timer_main, this)); // | --------------------- finish the init -------------------- | From 23c72140d869eeeb746d11dde2eeb70cc211eb8f Mon Sep 17 00:00:00 2001 From: Afzal Ahmad Date: Mon, 3 Mar 2025 18:08:11 +0100 Subject: [PATCH 13/27] added: image-transport example --- .../example_image_transport/CMakeLists.txt | 61 ++++++++ .../example_image_transport/LICENSE | 29 ++++ .../example_image_transport/README.md | 102 +++++++++++++ .../config/edge_detector.yaml | 16 +++ .../launch/example_image_transport.py | 46 ++++++ .../example_image_transport/package.xml | 28 ++++ .../src/example_image_transport.cpp | 124 ++++++++++++++++ .../tmux/config/custom_config.yaml | 25 ++++ .../tmux/config/network_config.yaml | 15 ++ .../tmux/config/world_config.yaml | 34 +++++ .../example_image_transport/tmux/kill.sh | 14 ++ .../example_image_transport/tmux/layout.json | 135 ++++++++++++++++++ .../example_image_transport/tmux/session.yml | 62 ++++++++ .../example_image_transport/tmux/start.sh | 27 ++++ .../vision_examples_utils/package.xml | 18 +++ .../resource/vision_examples_utils | 0 .../vision_examples_utils/setup.cfg | 4 + .../vision_examples_utils/setup.py | 26 ++++ .../test/test_copyright.py | 25 ++++ .../vision_examples_utils/test/test_flake8.py | 25 ++++ .../vision_examples_utils/test/test_pep257.py | 23 +++ .../vision_examples_utils/__init__.py | 0 .../vision_examples_utils/dummy_publisher.py | 53 +++++++ 23 files changed, 892 insertions(+) create mode 100644 extended_examples/vision_examples/example_image_transport/CMakeLists.txt create mode 100644 extended_examples/vision_examples/example_image_transport/LICENSE create mode 100644 extended_examples/vision_examples/example_image_transport/README.md create mode 100644 extended_examples/vision_examples/example_image_transport/config/edge_detector.yaml create mode 100644 extended_examples/vision_examples/example_image_transport/launch/example_image_transport.py create mode 100644 extended_examples/vision_examples/example_image_transport/package.xml create mode 100644 extended_examples/vision_examples/example_image_transport/src/example_image_transport.cpp create mode 100644 extended_examples/vision_examples/example_image_transport/tmux/config/custom_config.yaml create mode 100644 extended_examples/vision_examples/example_image_transport/tmux/config/network_config.yaml create mode 100644 extended_examples/vision_examples/example_image_transport/tmux/config/world_config.yaml create mode 100755 extended_examples/vision_examples/example_image_transport/tmux/kill.sh create mode 100644 extended_examples/vision_examples/example_image_transport/tmux/layout.json create mode 100644 extended_examples/vision_examples/example_image_transport/tmux/session.yml create mode 100755 extended_examples/vision_examples/example_image_transport/tmux/start.sh create mode 100644 extended_examples/vision_examples/vision_examples_utils/package.xml create mode 100644 extended_examples/vision_examples/vision_examples_utils/resource/vision_examples_utils create mode 100644 extended_examples/vision_examples/vision_examples_utils/setup.cfg create mode 100644 extended_examples/vision_examples/vision_examples_utils/setup.py create mode 100644 extended_examples/vision_examples/vision_examples_utils/test/test_copyright.py create mode 100644 extended_examples/vision_examples/vision_examples_utils/test/test_flake8.py create mode 100644 extended_examples/vision_examples/vision_examples_utils/test/test_pep257.py create mode 100644 extended_examples/vision_examples/vision_examples_utils/vision_examples_utils/__init__.py create mode 100644 extended_examples/vision_examples/vision_examples_utils/vision_examples_utils/dummy_publisher.py diff --git a/extended_examples/vision_examples/example_image_transport/CMakeLists.txt b/extended_examples/vision_examples/example_image_transport/CMakeLists.txt new file mode 100644 index 0000000..61d4a19 --- /dev/null +++ b/extended_examples/vision_examples/example_image_transport/CMakeLists.txt @@ -0,0 +1,61 @@ +cmake_minimum_required(VERSION 3.5) +project(vision_examples) + +# set the correct standards +set(CMAKE_C_STANDARD 99) +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) + +# set the compile options to show code warnings +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(cv_bridge REQUIRED) +find_package(image_transport REQUIRED) +find_package(OpenCV REQUIRED COMPONENTS highgui imgcodecs imgproc videoio) + +add_library(example_image_transport SHARED + src/example_image_transport.cpp +) + +target_link_libraries(example_image_transport + ${OpenCV_LIBRARIES} +) + +ament_target_dependencies(example_image_transport + rclcpp + rclcpp_components + sensor_msgs + image_transport + cv_bridge +) + +# each component (nodelet) needs to be registered to make it discoverable at runtime +rclcpp_components_register_nodes(example_image_transport PLUGIN "vision_examples::ExampleImageTransport" EXECUTABLE example_image_transport) + +## -------------------------------------------------------------- +## | Install | +## -------------------------------------------------------------- + +install(TARGETS + example_image_transport + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME} +) + +install(DIRECTORY config + DESTINATION share/${PROJECT_NAME} +) + +ament_package() \ No newline at end of file diff --git a/extended_examples/vision_examples/example_image_transport/LICENSE b/extended_examples/vision_examples/example_image_transport/LICENSE new file mode 100644 index 0000000..bd2e223 --- /dev/null +++ b/extended_examples/vision_examples/example_image_transport/LICENSE @@ -0,0 +1,29 @@ +BSD 3-Clause License + +Copyright (c) 2020, Multi-robot Systems (MRS) group at Czech Technical University in Prague +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +3. Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/extended_examples/vision_examples/example_image_transport/README.md b/extended_examples/vision_examples/example_image_transport/README.md new file mode 100644 index 0000000..0f971aa --- /dev/null +++ b/extended_examples/vision_examples/example_image_transport/README.md @@ -0,0 +1,102 @@ +# MRS ROS C++ Vision example + +This package was created as an example of how to use OpenCV and write packages for image processing or computer vision. +For a more general package example, see the [waypoint_flier](https://github.com/ctu-mrs/mrs_core_examples/tree/master/cpp/waypoint_flier). +You can test the program in simulation (see our [simulation tutorial](https://ctu-mrs.github.io/docs/simulation/howto.html)). + +## Example features + +* Subscribing to camera topic (using ImageTransport, which is agnostic to the image compression etc.) +* Publishing images (again using ImageTransport, enabling automatic compression of the output) +* Basic OpenCV image processing (Canny edge detection, image blurring etc.) +* Basic OpenCV drawing functions +* Using TF2 to transform points between frames +* Backprojection of 3D points to the camera image +* Other features, which overlap with the [waypoint_flier](https://github.com/ctu-mrs/mrs_core_examples/tree/master/cpp/waypoint_flier) template + +## How to start it? + +```bash +./tmux/start.sh +``` + +## Coding practices + +Coding practices, related to packages working with images, are described here. +For a more detailed description of good programming practices in the context of ROS, see the [waypoint_flier](https://github.com/ctu-mrs/mrs_core_examples/tree/master/cpp/waypoint_flier). +Also check out our general [C++ good/bad coding practices tutorial](https://ctu-mrs.github.io/docs/introduction/c_to_cpp.html). + +### Using `cv_bridge::toCvShare()` or `cv_bridge::toCvCopy()` for converting between `sensor_msgs::Image` and `cv::Mat` +*Reference documentation: [C++ API docs](http://docs.ros.org/melodic/api/cv_bridge/html/c++/), [example usage](https://wiki.ros.org/cv_bridge/Tutorials/UsingCvBridgeToConvertBetweenROSImagesAndOpenCVImages)* + +Use `cv_bridge::toCvShare()` to avoid copying input images, if applicable. +In contrast to `cv_bridge::toCvCopy()`, which allows modifying the returned data, `cv_bridge::toCvShare()` returns a `cv_bridge::CvImageConstPtr` pointing to the image data, which may be shared by all nodes/nodelets subscribing the image topic. +This is why you must *avoid modifying the image, returned by `cv_bridge::toCvShare()`*! + +The rule of thumb whether to use `cv_bridge::toCvCopy()` or `cv_bridge::toCvShare()` can be summarized as: + +* If you plan on modifying the image data (such as drawing to the image, blurring it, applying erosion etc.), either use `cv_bridge::toCvShare()` and then `cv::Mat::copyTo()` the returned OpenCV matrix to one you are going to modify, or simply use `cv_bridge::toCvCopy()`. +* If you don't want to modify the image data (for example when you only want to display it or if you want to use e.g. `cv::cvtColor()` to convert a color image to grayscale), use `cv_bridge::toCvShare()` to avoid unnecessary copies. + +When using either `cv_bridge::toCvShare()` or `cv_bridge::toCvCopy()`, it is a good practice to specify the desired encoding to which the data should be converted if it comes in a different encoding. +A typical example why this is a good idea is when you plan on using an image from a color camera, which uses RGB ordering of the data, in OpenCV, which mostly presumes BGR ordering. +If you pass `"bgr8"` to `cv_bridge::toCvShare()` or `cv_bridge::toCvCopy()`, and the image data in the ROS message use RGB ordering, the function will automatically reorder the data to BGR. + +For more information, see https://wiki.ros.org/cv_bridge/Tutorials/UsingCvBridgeToConvertBetweenROSImagesAndOpenCVImages. + +### Using `image_geometry::PinholeCameraModel` for 2D->3D and 3D->2D transformation of points +*Reference documentation: [`image_geometry::PinholeCameraModel`](http://docs.ros.org/api/image_geometry/html/c++/classimage__geometry_1_1PinholeCameraModel.html), [camera_calibration](https://wiki.ros.org/camera_calibration)* + +If you want to project a 2D position in the image to a 3D ray or backproject a 3D point to the image, the most straightforward way is using the pinhole camera model, which is conveniently implemented in the ROS `image_geometry` package as the class `image_geometry::PinholeCameraModel`. +The pinhole model works reasonably well for standard cameras, although if you need to use a more exotic camera type (such as an omnidirectional camera), it might not be sufficient (in that case, refer to the [OCamCalib toolbox](https://sites.google.com/site/scarabotix/ocamcalib-toolbox)). +To use the pinhole model, distortion of the camera has to be compensated for. +This is done using the `image_geometry::PinholeCameraModel::rectifyPoint()` and `image_geometry::PinholeCameraModel::unrectifyPoint()` methods. +Note that the terminology used in the `image_geometry::PinholeCameraModel` class is not precise, since technically image undistortion and rectification are different things and in this case, the methods `rectifyPoint` and `unrectifyPoint` are doing undistortion and distortion of the 2D pixel coordinates, not rectification. +The camera parameters need to be known to initialize the `image_geometry::PinholeCameraModel` object. +These are mostly obtained using offline calibration and the convenient ROS calibration tool can be used for this by running `rosrun camera_calibration cameracalibrator.py`. +Some cameras (such as cameras from the Realsense family) publish already undistorted images. +The undistort/distort steps can then be skipped, but it's usually better practice to leave them in in case a different camera was used. + +**Using `image_geometry::PinholeCameraModel` to project a 2D point to 3D has more or less the following steps:** + +1) Obtain parameters of the camera to be used. Usually, a camera will publish these parameters on the corresponding `camera_info` topic. +2) Initialize the `image_geometry::PinholeCameraModel` using the parameters from step 1. +3) Undistort the point coordinates. Use the `image_geometry::PinholeCameraModel::rectifyPoint()` method. +4) Project the undistorted point coordinates to 3D. Use `image_geometry::PinholeCameraModel::projectPixelTo3dRay()`. + +Note that the result is a 3D vector in the camera optical coordinate frame. +To get a 3D point, you need to somehow estimate the distance of the point from the camera center. +It may also be necessary to transform the point to other coordinate frame (see the next section for how to use the ROS TF2 framework). + +**Using `image_geometry::PinholeCameraModel` to backproject a 3D point to the image (this is demonstrated in the `EdgeDetect` nodelet):** + +1) Obtain parameters of the camera to be used. Usually, a camera will publish these parameters on the corresponding `camera_info` topic. +2) Initialize the `image_geometry::PinholeCameraModel` using the parameters from step 1. +2) Transform the 3D point to the camera optical coordinate frame. Use the ROS TF2 framework as described in the next section. +3) Backproject the point to 2D. Use the `image_geometry::PinholeCameraModel::project3dToPixel()` method. +4) Apply camera distortion. Use the `image_geometry::PinholeCameraModel::unrectifyPoint()` method. + +### Using the ROS TF2 framework for transforming stuff between different coordinate frames + +*Reference documentation: [TF2 docs](http://wiki.ros.org/tf2_ros), [tutorial](http://wiki.ros.org/tf2/Tutorials/Introduction%20to%20tf2)* + +First off, you can use `rosrun rqt_tf_tree rqt_tf_tree` or `rosrun tf view_frames && zathura frames.pdf && rm frames.pdf && rm frames.gv` (the first one is recommended) to inspect frames, which are currently available in `roscore`, and their mutual connections. +To observe a transformation between two specific frames in real-time, use `rosrun tf tf_echo [reference_frame] [target_frame]`. +By restarting `roscore`, the frames will reset. +Sometimes, RViz will be stubborn and refuse to use frames, which exist and are correctly connected (you can check that with the abovementioned command). +The solution is usually clicking the `Reset` button in RViz or restarting it. + +If you need to publish a static transformation between two frames (typically from the UAV frame to the camera optical frame), you can use `rosrun tf2_ros static_transform_publisher dx dy dz yaw pitch roll frame_id child_frame_id`, where `frame_id` would typically be the UAV frame (i.e. `fcu_uavX`) and `child_frame_id` would be e.g. the camera frame. + +To use transformations from your code, you need to have a `tf2_ros::Buffer` object, which buffers the transformations and provides lookups between frames at specific time-stamps. +Aditionally, you need a `tf2_ros::TransformListener`, which listens to the transformation messages and fills the `tf2_ros::Buffer`. +Because the `tf2_ros::TransformListener` class does not implement the assignment operator and it cannot be initialized in the constructor of a nodelet, the usual practice is to have a smart-pointer member variable (i.e. `std::unique_ptr`) and instantiate the object in the `OnInit()` method of the nodelet class using `std::make_unique()`. + +To actually do the transformation, you need to follow these steps: + +1) Try to lookup the transform using `tf2_ros::Buffer::lookupTransform()` and a `try` block. +2) Catch a potential `tf2::TransformException` exception using a `catch` block. If exception is caught, alert the user and abort the transformation process. +3) If the lookup was successful, do the actual transformation using `tf2::doTransform()`. + +It is important to note that to use the `tf2::doTransform()` method, you must add the corresponding lib to the `CMakeLists.txt` and `package.xml` files. +For example if you want to transform messages from the `geometry_msgs` package, you need to include `tf2_geometry_msgs` in the `find_package()` function in the `CMakeLists.txt` file and as a dependency in the `package.xml` file, and include the `` header in the corresponding C++ file. diff --git a/extended_examples/vision_examples/example_image_transport/config/edge_detector.yaml b/extended_examples/vision_examples/example_image_transport/config/edge_detector.yaml new file mode 100644 index 0000000..fa71e10 --- /dev/null +++ b/extended_examples/vision_examples/example_image_transport/config/edge_detector.yaml @@ -0,0 +1,16 @@ +# threshold for the canny edge detector (see https://docs.opencv.org/3.2.0/dd/d1a/group__imgproc__feature.html#ga04723e007ed888ddf11d9ba04e2232de) +canny_threshold: 70 + +# coordinates of the point, which will be backprojected to the camera image from the specified frame +world_frame_id: "world_origin" + +world_point: + x: 0.0 + y: 0.0 + z: 0.0 + +rate: + # how often to publish my topic? + publish: 10 # [Hz] + # how often check whether msgs are arriving? + check_subscribers: 1 # [Hz] diff --git a/extended_examples/vision_examples/example_image_transport/launch/example_image_transport.py b/extended_examples/vision_examples/example_image_transport/launch/example_image_transport.py new file mode 100644 index 0000000..1cfa9a0 --- /dev/null +++ b/extended_examples/vision_examples/example_image_transport/launch/example_image_transport.py @@ -0,0 +1,46 @@ +import launch +from ament_index_python import get_package_share_directory +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + +def generate_launch_description(): + + ld = launch.LaunchDescription() + + pkg_name = 'vision_examples' + pkg_share_path = get_package_share_directory(pkg_name) + namespace='nmspc1' + + ld.add_action(ComposableNodeContainer( + + namespace = namespace, + name = 'component_publisher_example', + package='rclcpp_components', + + executable='component_container_mt', + + composable_node_descriptions=[ + + # possible to add multiple nodes to this container + ComposableNode( + package=pkg_name, + plugin='vision_examples::ExampleImageTransport', + namespace=namespace, + name='example_image_transport', + + # parameters=[ + # pkg_share_path + '/config/publisher_example.yaml', + # ], + + remappings=[ + # topics + ("~/image_in", "/dummy_image"), + ], + ) + ], + + output='screen', + + )) + + return ld \ No newline at end of file diff --git a/extended_examples/vision_examples/example_image_transport/package.xml b/extended_examples/vision_examples/example_image_transport/package.xml new file mode 100644 index 0000000..91e4952 --- /dev/null +++ b/extended_examples/vision_examples/example_image_transport/package.xml @@ -0,0 +1,28 @@ + + + + + vision_examples + + 1.0.0 + MRS ROS2 vision example + + Afzal Ahmad + klaxalk + + TODO: License declaration + + ament_cmake + + rclcpp + rclcpp_components + senssor_msgs + cv_bridge + image_transport + libopencv-dev + + + ament_cmake + + + \ No newline at end of file diff --git a/extended_examples/vision_examples/example_image_transport/src/example_image_transport.cpp b/extended_examples/vision_examples/example_image_transport/src/example_image_transport.cpp new file mode 100644 index 0000000..5bf4dce --- /dev/null +++ b/extended_examples/vision_examples/example_image_transport/src/example_image_transport.cpp @@ -0,0 +1,124 @@ +#include + +/* camera image messages */ +#include + +/* some STL includes */ +#include +#include + +/* some OpenCV includes */ +#include +#include +#include +#include + +/* ROS includes for working with OpenCV and images */ +#include +#include + +using namespace std::chrono_literals; + +namespace vision_examples +{ + +class ExampleImageTransport : public rclcpp::Node { + +public: + ExampleImageTransport (const rclcpp::NodeOptions options); + +private: + std::atomic is_initialized_ = false; + + std::mutex mutex_image_; // to prevent data races when accessing the following variables from multiple threads + sensor_msgs::msg::Image::ConstSharedPtr image_; + rclcpp::Time time_last_image_; // time stamp of the last received image message + + void callback_image(const sensor_msgs::msg::Image::ConstSharedPtr& msg); + image_transport::Subscriber sub_image_; + + void callback_timer(); + rclcpp::TimerBase::SharedPtr timer_publisher_; + + image_transport::Publisher pub_edited_image_; +}; + +ExampleImageTransport::ExampleImageTransport(rclcpp::NodeOptions options) : Node("example_image_transport", options) { + + RCLCPP_INFO(get_logger(), "Initializing"); + + { + std::scoped_lock lck(mutex_image_); + if(image_) { + RCLCPP_ERROR(get_logger(), "Image pointer is assigned before initialization, shutting down"); + rclcpp::shutdown(); + return; + } + } + + auto sub_node = this->create_sub_node("image_transport"); + image_transport::ImageTransport image_transporter(sub_node); + image_transport::TransportHints hints(sub_node.get()); + + rclcpp::SubscriptionOptions sub_options; + sub_options.callback_group = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + + sub_image_ = image_transporter.subscribe("~/image_in", 1, &ExampleImageTransport::callback_image, this, &hints, sub_options); + + pub_edited_image_ = image_transporter.advertise("edited_image_out", 1); + + timer_publisher_ = create_timer(std::chrono::duration(1.0 / 2.0), std::bind(&ExampleImageTransport::callback_timer, this)); + + is_initialized_ = true; + RCLCPP_INFO(get_logger(), "Initialized"); +} + +void ExampleImageTransport::callback_image(const sensor_msgs::msg::Image::ConstSharedPtr& msg) { + + if (!is_initialized_) { + return; + } + + { + std::scoped_lock lock(mutex_image_); + time_last_image_ = this->now(); + image_ = msg; + } + RCLCPP_WARN(get_logger(), "Recieved new image"); +} + +void ExampleImageTransport::callback_timer() { + + if (!is_initialized_) { + return; + } + + sensor_msgs::msg::Image::ConstSharedPtr ptr_image; + { + std::scoped_lock lock(mutex_image_); + + ptr_image = image_; + } + + if (ptr_image.use_count() == 0) { + RCLCPP_WARN(get_logger(), "Waiting for 'image' to publish the edited image"); + return; + } + + // toCvShare avoids copying the image data and instead copies only the (smart) constpointer + // to the data. Then, the data cannot be changed (it is potentially shared between multiple nodes) and + // it is automatically freed when all pointers to it are released. If you want to modify the image data, + // use toCvCopy (see https://wiki.ros.org/cv_bridge/Tutorials/UsingCvBridgeToConvertBetweenROSImagesAndOpenCVImages), + // or copy the image data using cv::Mat::copyTo() method. + // Adittionally, toCvShare and toCvCopy will convert the input image to the specified encoding + // if it differs from the one in the message. Try to be consistent in what encodings you use throughout the code. + cv_bridge::CvImagePtr ptr_cv_image = cv_bridge::toCvCopy(ptr_image, "bgr8"); + + cv::putText(ptr_cv_image->image, std::to_string(this->now().seconds()), cv::Point(ptr_cv_image->image.cols / 2.0, ptr_cv_image->image.rows / 2.0), cv::FONT_HERSHEY_TRIPLEX, 1.0, cv::Scalar(255, 255, 255), 1, cv::LINE_AA); + + pub_edited_image_.publish(ptr_cv_image->toImageMsg()); +} +} // namespace vision_examples + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(vision_examples::ExampleImageTransport) diff --git a/extended_examples/vision_examples/example_image_transport/tmux/config/custom_config.yaml b/extended_examples/vision_examples/example_image_transport/tmux/config/custom_config.yaml new file mode 100644 index 0000000..b77364a --- /dev/null +++ b/extended_examples/vision_examples/example_image_transport/tmux/config/custom_config.yaml @@ -0,0 +1,25 @@ +mrs_uav_managers: + + estimation_manager: + + # loaded state estimator plugins + state_estimators: [ + "gps_garmin", + "gps_baro", + ] + + initial_state_estimator: "gps_garmin" # will be used as the first state estimator + agl_height_estimator: "garmin_agl" # only slightly filtered height for checking min height (not used in control feedback) + + uav_manager: + + takeoff: + + during_takeoff: + controller: "MpcController" + tracker: "LandoffTracker" + + after_takeoff: + controller: "MpcController" + # controller: "Se3Controller" + tracker: "MpcTracker" diff --git a/extended_examples/vision_examples/example_image_transport/tmux/config/network_config.yaml b/extended_examples/vision_examples/example_image_transport/tmux/config/network_config.yaml new file mode 100644 index 0000000..08f370d --- /dev/null +++ b/extended_examples/vision_examples/example_image_transport/tmux/config/network_config.yaml @@ -0,0 +1,15 @@ +# 1. This list is used by NimbroNetwork for mutual communication of the UAVs +# The names of the robots have to match hostnames described in /etc/hosts. +# +# 2. This list is used by MpcTracker for mutual collision avoidance of the UAVs. +# The names should match the true "UAV_NAMES" (the topic prefixes). +# +# network_config:=~/config/network_config.yaml +# +# to the core.launch and nimbro.launch. + +network: + + robot_names: [ + uav1, + ] diff --git a/extended_examples/vision_examples/example_image_transport/tmux/config/world_config.yaml b/extended_examples/vision_examples/example_image_transport/tmux/config/world_config.yaml new file mode 100644 index 0000000..9b55067 --- /dev/null +++ b/extended_examples/vision_examples/example_image_transport/tmux/config/world_config.yaml @@ -0,0 +1,34 @@ +world_origin: + + units: "LATLON" # {"UTM, "LATLON"} + + origin_x: 47.397743 + origin_y: 8.545594 + +safety_area: + + enabled: true + + horizontal: + + # the frame of reference in which the points are expressed + frame_name: "world_origin" + + # polygon + # + # x, y [m] for any frame_name except latlon_origin + # x = latitude, y = longitude [deg] for frame_name=="latlon_origin" + points: [ + -50, -50, + 50, -50, + 50, 50, + -50, 50, + ] + + vertical: + + # the frame of reference in which the max&min z is expressed + frame_name: "world_origin" + + max_z: 15.0 + min_z: 0.5 diff --git a/extended_examples/vision_examples/example_image_transport/tmux/kill.sh b/extended_examples/vision_examples/example_image_transport/tmux/kill.sh new file mode 100755 index 0000000..28d50b0 --- /dev/null +++ b/extended_examples/vision_examples/example_image_transport/tmux/kill.sh @@ -0,0 +1,14 @@ +#!/bin/bash + +# Absolute path to this script. /home/user/bin/foo.sh +SCRIPT=$(readlink -f $0) +# Absolute path this script is in. /home/user/bin +SCRIPTPATH=`dirname $SCRIPT` +cd "$SCRIPTPATH" + +export TMUX_SESSION_NAME=simulation +export TMUX_SOCKET_NAME=mrs + +# just attach to the session +tmux -L $TMUX_SOCKET_NAME split-window -t $TMUX_SESSION_NAME +tmux -L $TMUX_SOCKET_NAME send-keys -t $TMUX_SESSION_NAME "sleep 1; tmux list-panes -s -F \"#{pane_pid} #{pane_current_command}\" | grep -v tmux | cut -d\" \" -f1 | while read in; do killp \$in; done; exit" ENTER diff --git a/extended_examples/vision_examples/example_image_transport/tmux/layout.json b/extended_examples/vision_examples/example_image_transport/tmux/layout.json new file mode 100644 index 0000000..990ff6b --- /dev/null +++ b/extended_examples/vision_examples/example_image_transport/tmux/layout.json @@ -0,0 +1,135 @@ +[ +{ + "border": "normal", + "floating": "auto_off", + "fullscreen_mode": 0, + "layout": "splitv", + "percent": 1, + "type": "con", + "nodes": [ + { + "border": "pixel", + "current_border_width": 3, + "floating": "auto_off", + "geometry": { + "height": 758, + "width": 1280, + "x": 0, + "y": 42 + }, + "name": "default.rviz* - RViz", + "percent": 0.333333333333333, + "swallows": [ + { + "instance": "^rviz$" + } + ], + "type": "con" + }, + { + "border": "normal", + "floating": "auto_off", + "layout": "splith", + "percent": 0.466666666666667, + "type": "con", + "nodes": [ + { + "border": "pixel", + "current_border_width": 3, + "floating": "auto_off", + "geometry": { + "height": 481, + "width": 1280, + "x": 0, + "y": 0 + }, + "name": "Gazebo", + "percent": 0.25, + "swallows": [ + { + "instance": "^gazebo$" + } + ], + "type": "con" + }, + { + "border": "pixel", + "current_border_width": 3, + "floating": "auto_off", + "geometry": { + "height": 240, + "width": 320, + "x": 0, + "y": 0 + }, + "name": "edges", + "percent": 0.25, + "swallows": [ + { + "instance": "^original$" + } + ], + "type": "con" + }, + { + "border": "pixel", + "current_border_width": 3, + "floating": "auto_off", + "geometry": { + "height": 240, + "width": 320, + "x": 0, + "y": 0 + }, + "name": "world_point", + "percent": 0.25, + "swallows": [ + { + "instance": "^original$" + } + ], + "type": "con" + }, + { + "border": "pixel", + "current_border_width": 3, + "floating": "auto_off", + "geometry": { + "height": 240, + "width": 320, + "x": 0, + "y": 0 + }, + "name": "original", + "percent": 0.25, + "swallows": [ + { + "instance": "^original$" + } + ], + "type": "con" + } + ] + }, + { + "border": "pixel", + "current_border_width": 3, + "floating": "auto_off", + "geometry": { + "height": 460, + "width": 724, + "x": 0, + "y": 0 + }, + "name": "./start.sh", + "percent": 0.2, + "swallows": [ + { + "instance": "^urxvt$" + } + ], + "type": "con" + } + ] +} +] diff --git a/extended_examples/vision_examples/example_image_transport/tmux/session.yml b/extended_examples/vision_examples/example_image_transport/tmux/session.yml new file mode 100644 index 0000000..f346e9b --- /dev/null +++ b/extended_examples/vision_examples/example_image_transport/tmux/session.yml @@ -0,0 +1,62 @@ +# do not modify these +root: ./ +name: simulation +socket_name: mrs +attach: false +tmux_options: -f /etc/ctu-mrs/tmux.conf +# you can modify these +pre_window: export UAV_NAME=uav1; export RUN_TYPE=simulation; export UAV_TYPE=f550 +startup_window: edge_detector +windows: + - roscore: + layout: tiled + panes: + - roscore + - gazebo: + layout: tiled + panes: + - waitForRos; roslaunch mrs_uav_gazebo_simulation simulation.launch world_name:=grass_plane gui:=true + - waitForControl; gz camera -c gzclient_camera -f $UAV_NAME; history -s gz camera -c gzclient_camera -f $UAV_NAME + - status: + layout: tiled + panes: + - waitForHw; roslaunch mrs_uav_status status.launch + - spawn: + layout: tiled + panes: + - waitForGazebo; rosservice call /mrs_drone_spawner/spawn "1 $UAV_TYPE --pos -5 0 1 0.0 --enable-rangefinder --enable-mobius-camera-front" + - hw_api: + layout: tiled + panes: + - waitForTime; roslaunch mrs_uav_px4_api api.launch + - core: + layout: tiled + panes: + - waitForHw; roslaunch mrs_uav_core core.launch + platform_config:=`rospack find mrs_uav_gazebo_simulation`/config/mrs_uav_system/$UAV_TYPE.yaml + custom_config:=./config/custom_config.yaml + world_config:=./config/world_config.yaml + network_config:=./config/network_config.yaml + - edge_detector: + layout: tiled + panes: + - waitForHw; roslaunch example_edge_detector edge_detector.launch + - takeoff: + layout: tiled + panes: + - waitForHw; roslaunch mrs_uav_autostart automatic_start.launch + - 'waitForControl; rosservice call /$UAV_NAME/hw_api/arming 1; sleep 2; rosservice call /$UAV_NAME/hw_api/offboard' + - goto: + layout: tiled + panes: + - 'history -s rosservice call /$UAV_NAME/control_manager/goto \"goal: \[0.0, 10.0, 1.5, 0.0\]\"' + - rviz: + layout: tiled + panes: + - waitForControl; roslaunch mrs_uav_core rviz.launch + - waitForControl; roslaunch mrs_rviz_plugins load_robot.launch + - waitForTime; waitForControl; roslaunch mrs_rviz_plugins rviz_interface.launch + - layout: + layout: tiled + panes: + - waitForControl; sleep 5; ~/.i3/layout_manager.sh ./layout.json diff --git a/extended_examples/vision_examples/example_image_transport/tmux/start.sh b/extended_examples/vision_examples/example_image_transport/tmux/start.sh new file mode 100755 index 0000000..5bb2275 --- /dev/null +++ b/extended_examples/vision_examples/example_image_transport/tmux/start.sh @@ -0,0 +1,27 @@ +#!/bin/bash + +# Absolute path to this script. /home/user/bin/foo.sh +SCRIPT=$(readlink -f $0) +# Absolute path this script is in. /home/user/bin +SCRIPTPATH=`dirname $SCRIPT` +cd "$SCRIPTPATH" + +export TMUX_SESSION_NAME=simulation +export TMUX_SOCKET_NAME=mrs + +# start tmuxinator +tmuxinator start -p ./session.yml + +# if we are not in tmux +if [ -z $TMUX ]; then + + # just attach to the session + tmux -L $TMUX_SOCKET_NAME a -t $TMUX_SESSION_NAME + +# if we are in tmux +else + + # switch to the newly-started session + tmux detach-client -E "tmux -L $TMUX_SOCKET_NAME a -t $TMUX_SESSION_NAME" + +fi diff --git a/extended_examples/vision_examples/vision_examples_utils/package.xml b/extended_examples/vision_examples/vision_examples_utils/package.xml new file mode 100644 index 0000000..7d2fc1f --- /dev/null +++ b/extended_examples/vision_examples/vision_examples_utils/package.xml @@ -0,0 +1,18 @@ + + + + vision_examples_utils + 0.0.0 + TODO: Package description + ubuntu + None + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/extended_examples/vision_examples/vision_examples_utils/resource/vision_examples_utils b/extended_examples/vision_examples/vision_examples_utils/resource/vision_examples_utils new file mode 100644 index 0000000..e69de29 diff --git a/extended_examples/vision_examples/vision_examples_utils/setup.cfg b/extended_examples/vision_examples/vision_examples_utils/setup.cfg new file mode 100644 index 0000000..d1994e1 --- /dev/null +++ b/extended_examples/vision_examples/vision_examples_utils/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/vision_examples_utils +[install] +install_scripts=$base/lib/vision_examples_utils diff --git a/extended_examples/vision_examples/vision_examples_utils/setup.py b/extended_examples/vision_examples/vision_examples_utils/setup.py new file mode 100644 index 0000000..60c8d59 --- /dev/null +++ b/extended_examples/vision_examples/vision_examples_utils/setup.py @@ -0,0 +1,26 @@ +from setuptools import find_packages, setup + +package_name = 'vision_examples_utils' + +setup( + name=package_name, + version='1.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='Afzal Ahmad', + maintainer_email='afzalhmd14@gmail.com', + description='TODO: Package description', + license='None', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'dummy_pub = vision_examples_utils.dummy_publisher:main', + ], + }, +) diff --git a/extended_examples/vision_examples/vision_examples_utils/test/test_copyright.py b/extended_examples/vision_examples/vision_examples_utils/test/test_copyright.py new file mode 100644 index 0000000..97a3919 --- /dev/null +++ b/extended_examples/vision_examples/vision_examples_utils/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/extended_examples/vision_examples/vision_examples_utils/test/test_flake8.py b/extended_examples/vision_examples/vision_examples_utils/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/extended_examples/vision_examples/vision_examples_utils/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/extended_examples/vision_examples/vision_examples_utils/test/test_pep257.py b/extended_examples/vision_examples/vision_examples_utils/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/extended_examples/vision_examples/vision_examples_utils/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/extended_examples/vision_examples/vision_examples_utils/vision_examples_utils/__init__.py b/extended_examples/vision_examples/vision_examples_utils/vision_examples_utils/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/extended_examples/vision_examples/vision_examples_utils/vision_examples_utils/dummy_publisher.py b/extended_examples/vision_examples/vision_examples_utils/vision_examples_utils/dummy_publisher.py new file mode 100644 index 0000000..e6d53c7 --- /dev/null +++ b/extended_examples/vision_examples/vision_examples_utils/vision_examples_utils/dummy_publisher.py @@ -0,0 +1,53 @@ +#!/usr/bin/env python3 +import rclpy +from rclpy.node import Node +import sensor_msgs.msg + +import cv2 +import numpy as np +from cv_bridge import CvBridge + + +class DummyPublisher(Node): + + def __init__(self): + super().__init__('dummy_publisher') + self.get_logger().info('Initializing') + self.publisher = self.create_publisher(sensor_msgs.msg.Image, 'dummy_image', 10) + self.timer = self.create_timer(0.1, self.callback_timer) + self.counter = 0 + self.get_logger().info('Initialized') + + def callback_timer(self): + cv_bridge = CvBridge() + width = 1280 + height = 720 + image = np.zeros((height, width, 3), np.uint8) + cv2.putText(image, f'{self.get_clock().now().seconds_nanoseconds()}', (10, 50), cv2.FONT_HERSHEY_SIMPLEX, 1, (51, 153, 255), 2) + self.counter += 1 + + msg_image = cv_bridge.cv2_to_imgmsg(image, encoding='bgr8') + msg_image.header.stamp.sec, msg_image.header.stamp.nanosec = self.get_clock().now().seconds_nanoseconds() + msg_image.height = height + msg_image.width = width + msg_image.encoding = 'bgr8' + + self.publisher.publish(msg_image) + self.get_logger().info('Published new image.') + + +def main(args=None): + rclpy.init(args=args) + + dummy_publisher = DummyPublisher() + + rclpy.spin(dummy_publisher) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + dummy_publisher.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() From 6b4f8803166371830254f237e7cc689203a87261 Mon Sep 17 00:00:00 2001 From: Afzal Ahmad Date: Tue, 4 Mar 2025 19:11:23 +0100 Subject: [PATCH 14/27] wip: playing with the param-loader example --- ros2_examples/CMakeLists.txt | 2 + ros2_examples/launch/params_example.py | 31 +++++---- ros2_examples/src/params_example.cpp | 89 +++++++++++++++----------- 3 files changed, 72 insertions(+), 50 deletions(-) diff --git a/ros2_examples/CMakeLists.txt b/ros2_examples/CMakeLists.txt index 8cb60e3..8118e20 100644 --- a/ros2_examples/CMakeLists.txt +++ b/ros2_examples/CMakeLists.txt @@ -20,6 +20,7 @@ find_package(std_srvs REQUIRED) find_package(tf2_ros REQUIRED) find_package(geometry_msgs REQUIRED) find_package(ros2_lib REQUIRED) +find_package(mrs_lib REQUIRED) include_directories( include @@ -131,6 +132,7 @@ ament_target_dependencies(params_example rclcpp_components std_msgs std_srvs + mrs_lib ) # each component (nodelet) needs to be registered to make it discoverable at runtime diff --git a/ros2_examples/launch/params_example.py b/ros2_examples/launch/params_example.py index 867250c..67a2e66 100755 --- a/ros2_examples/launch/params_example.py +++ b/ros2_examples/launch/params_example.py @@ -18,33 +18,36 @@ def generate_launch_description(): ld = launch.LaunchDescription() + custom_config = LaunchConfiguration('custom_config') + # this adds the args to the list of args available for this launch files # these args can be listed at runtime using -s flag + # default_value is required to if the arg is supposed to be optional at launch time ld.add_action(DeclareLaunchArgument( 'custom_config', default_value="", - description="Path to the custom configuration file. The path can be absolute, starting with '/' or relative to the current working directory" + description="Path to the custom configuration file. The path can be absolute, starting with '/' or relative to the current working directory", )) - custom_config = LaunchConfiguration('custom_config', default="") - + # behaviour: + # custom_config == "" => custom_config: "" + # custom_config == "/" => custom_config: "/" + # custom_config == "" => custom_config: "$(pwd)/" custom_config = IfElseSubstitution( - condition=PythonExpression(["'", custom_config, "' != '' and ", "not '", custom_config, "'.startswith('/')"]), + condition=PythonExpression(['"', custom_config, '" != "" and ', 'not "', custom_config, '".startswith("/")']), if_value=PathJoinSubstitution([EnvironmentVariable('PWD'), custom_config]), else_value=custom_config ) - print("custom_config: {}".format(custom_config)) - - pkg_name = "ros2_examples" + pkg_name = 'ros2_examples' pkg_share_path = get_package_share_directory(pkg_name) # param loaded from env variable - uav_type=os.getenv('UAV_TYPE', "") + uav_type=os.getenv('UAV_TYPE', '') ld.add_action(ComposableNodeContainer( - namespace="nmspc1", + namespace='nmspc1', name='nmspc1_params_example', package='rclcpp_components', executable='component_container_mt', @@ -55,18 +58,18 @@ def generate_launch_description(): package=pkg_name, plugin='ros2_examples::ParamsExample', - namespace="nmspc1", - name='params_example', + namespace='nmspc1', + name='params_', parameters=[ pkg_share_path + '/config/params_example.yaml', - custom_config, - {'uav_type': uav_type}, + {'custom_config': custom_config, + 'params.uav_type': uav_type}, ], # remappings=[ # # topics - # ("~/topic_out", "~/topic"), + # ('~/topic_out', '~/topic"), # ], ), diff --git a/ros2_examples/src/params_example.cpp b/ros2_examples/src/params_example.cpp index c6ab3f8..bdec598 100644 --- a/ros2_examples/src/params_example.cpp +++ b/ros2_examples/src/params_example.cpp @@ -1,61 +1,78 @@ +#include #include -#include +#include using namespace std::chrono_literals; namespace ros2_examples { -using namespace utils; +// using namespace utils; /* class ParamsExample //{ */ class ParamsExample : public rclcpp::Node { public: - ParamsExample(rclcpp::NodeOptions options); + ParamsExample(const rclcpp::NodeOptions options); private: // | ----------------------- parameters ----------------------- | double floating_point_number_; std::string some_string_; + std::string uav_type; - OnSetParametersCallbackHandle::SharedPtr param_callback_handle_; + // OnSetParametersCallbackHandle::SharedPtr param_callback_handle_; // | ------------------------- methods ------------------------ | - rcl_interfaces::msg::SetParametersResult callback_parameters(std::vector parameters); + // rcl_interfaces::msg::SetParametersResult callback_parameters(std::vector parameters); }; //} /* ParamsExample() constructor //{ */ -ParamsExample::ParamsExample(rclcpp::NodeOptions options) : Node("params_example", options) { +ParamsExample::ParamsExample(const rclcpp::NodeOptions options) : Node("params_example", options) { RCLCPP_INFO(get_logger(), "initializing"); - bool loaded_successfully = true; + // bool loaded_successfully = true; + auto m_node = this->create_sub_node("params"); // | --------------------- load parameters -------------------- | + mrs_lib::ParamLoader param_loader{m_node, std::string{"ParamsExample"}}; + std::string custom_config = ""; - loaded_successfully &= parse_param("param_namespace.floating_number", floating_point_number_, *this); - loaded_successfully &= parse_param("some_string", some_string_, *this); - loaded_successfully &= parse_param("custom_config", custom_config, *this); + // param_loader.loadParam("param_namespace/floating_number", floating_point_number_); + param_loader.loadParam("some_string", some_string_); + param_loader.loadParam("some_string", some_string_); + // param_loader.loadParam("custom_config", custom_config); + + // if (custom_config == "") { + // RCLCPP_WARN(get_logger(), "custom_config is empty"); + // } - const std::string uav_type = parse_param2("uav_type", loaded_successfully, *this); + param_loader.loadParam("uav_type", uav_type); - if (!loaded_successfully) { + auto param_list = m_node->list_parameters(std::vector(), 0); + + for (const auto& param: param_list.names) { + std::cout << "params: " << param << std::endl; + + } + + if (!param_loader.loadedSuccessfully()) { RCLCPP_ERROR_STREAM(get_logger(), "Could not load all non-optional parameters. Shutting down."); - rclcpp::shutdown(); + // rclcpp::shutdown(); return; } // | --------------- bind pararm server callback -------------- | - param_callback_handle_ = add_on_set_parameters_callback(std::bind(&ParamsExample::callback_parameters, this, std::placeholders::_1)); + // param_callback_handle_ = add_on_set_parameters_callback(std::bind(&ParamsExample::callback_parameters, this, std::placeholders::_1)); // | --------------------- finish the init -------------------- | @@ -68,38 +85,38 @@ ParamsExample::ParamsExample(rclcpp::NodeOptions options) : Node("params_example /* callback_parameters() //{ */ -rcl_interfaces::msg::SetParametersResult ParamsExample::callback_parameters(std::vector parameters) { - rcl_interfaces::msg::SetParametersResult result; +// rcl_interfaces::msg::SetParametersResult ParamsExample::callback_parameters(std::vector parameters) { +// rcl_interfaces::msg::SetParametersResult result; - // Note that setting a parameter to a nonsensical value (such as setting the `param_namespace.floating_number` parameter to `hello`) - // doesn't have any effect - it doesn't even call this callback. - for (auto& param : parameters) { +// // Note that setting a parameter to a nonsensical value (such as setting the `param_namespace.floating_number` parameter to `hello`) +// // doesn't have any effect - it doesn't even call this callback. +// for (auto& param : parameters) { - RCLCPP_INFO_STREAM(get_logger(), "got parameter: '" << param.get_name() << "' with value '" << param.value_to_string() << "'"); +// RCLCPP_INFO_STREAM(get_logger(), "got parameter: '" << param.get_name() << "' with value '" << param.value_to_string() << "'"); - if (param.get_name() == "param_namespace.floating_number") { +// if (param.get_name() == "param_namespace.floating_number") { - floating_point_number_ = param.as_double(); +// floating_point_number_ = param.as_double(); - } else if (param.get_name() == "some_string") { +// } else if (param.get_name() == "some_string") { - some_string_ = param.as_string(); +// some_string_ = param.as_string(); - } else { +// } else { - RCLCPP_WARN_STREAM(get_logger(), "parameter: '" << param.get_name() << "' is not dynamically reconfigurable!"); - result.successful = false; - result.reason = "Parameter '" + param.get_name() + "' is not dynamically reconfigurable!"; - return result; - } - } +// RCLCPP_WARN_STREAM(get_logger(), "parameter: '" << param.get_name() << "' is not dynamically reconfigurable!"); +// result.successful = false; +// result.reason = "Parameter '" + param.get_name() + "' is not dynamically reconfigurable!"; +// return result; +// } +// } - RCLCPP_INFO(get_logger(), "params updated"); - result.successful = true; - result.reason = "OK"; +// RCLCPP_INFO(get_logger(), "params updated"); +// result.successful = true; +// result.reason = "OK"; - return result; -} +// return result; +// } //} From 954012e4d8abd1902df9fb38422a87f9ea5752e4 Mon Sep 17 00:00:00 2001 From: Tomas Baca Date: Mon, 10 Mar 2025 14:54:02 +0100 Subject: [PATCH 15/27] added sub pub torture test --- sub_pub_torture_test/CMakeLists.txt | 91 +++++++++++++ sub_pub_torture_test/config/publishers.yaml | 3 + sub_pub_torture_test/config/subscribers.yaml | 3 + .../include/sub_pub_torture_test/params.h | 64 +++++++++ .../sub_pub_torture_test/rate_counter.h | 33 +++++ sub_pub_torture_test/launch/publishers.py | 43 ++++++ sub_pub_torture_test/launch/subscribers.py | 45 +++++++ sub_pub_torture_test/package.xml | 32 +++++ sub_pub_torture_test/src/publishers.cpp | 104 +++++++++++++++ sub_pub_torture_test/src/subscribers.cpp | 126 ++++++++++++++++++ sub_pub_torture_test/tmux/session.yml | 12 ++ sub_pub_torture_test/tmux/start.sh | 27 ++++ 12 files changed, 583 insertions(+) create mode 100644 sub_pub_torture_test/CMakeLists.txt create mode 100644 sub_pub_torture_test/config/publishers.yaml create mode 100644 sub_pub_torture_test/config/subscribers.yaml create mode 100644 sub_pub_torture_test/include/sub_pub_torture_test/params.h create mode 100644 sub_pub_torture_test/include/sub_pub_torture_test/rate_counter.h create mode 100755 sub_pub_torture_test/launch/publishers.py create mode 100755 sub_pub_torture_test/launch/subscribers.py create mode 100644 sub_pub_torture_test/package.xml create mode 100644 sub_pub_torture_test/src/publishers.cpp create mode 100644 sub_pub_torture_test/src/subscribers.cpp create mode 100644 sub_pub_torture_test/tmux/session.yml create mode 100755 sub_pub_torture_test/tmux/start.sh diff --git a/sub_pub_torture_test/CMakeLists.txt b/sub_pub_torture_test/CMakeLists.txt new file mode 100644 index 0000000..2ddd13f --- /dev/null +++ b/sub_pub_torture_test/CMakeLists.txt @@ -0,0 +1,91 @@ +cmake_minimum_required(VERSION 3.5) +project(sub_pub_torture_test) + +# set the correct standards +set(CMAKE_C_STANDARD 99) +set(CMAKE_CXX_STANDARD 20) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) + +# set the compile options to show code warnings +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +add_compile_options(-O2) + +set(DEPENDENCIES + ament_cmake + rclcpp + rclcpp_components + std_msgs + ) + +foreach(DEP IN LISTS DEPENDENCIES) + find_package(${DEP} REQUIRED) +endforeach() + +include_directories( + include +) + +set(LIBRARIES + subscribers + publishers + ) + +## -------------------------------------------------------------- +## | subscribers | +## -------------------------------------------------------------- + +add_library(subscribers SHARED + src/subscribers.cpp +) + +ament_target_dependencies(subscribers + ${DEPENDENCIES} +) + +rclcpp_components_register_nodes(subscribers PLUGIN "sub_pub_torture_test::Subscribers" EXECUTABLE subscribers) + +## -------------------------------------------------------------- +## | publishers | +## -------------------------------------------------------------- + +add_library(publishers SHARED + src/publishers.cpp +) + +ament_target_dependencies(publishers + ${DEPENDENCIES} +) + +rclcpp_components_register_nodes(publishers PLUGIN "sub_pub_torture_test::Publishers" EXECUTABLE publishers) + +## -------------------------------------------------------------- +## | install | +## -------------------------------------------------------------- + +install(TARGETS + ${LIBRARIES} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install(DIRECTORY include/ + DESTINATION include +) + +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME} +) + +install(DIRECTORY config + DESTINATION share/${PROJECT_NAME} +) + +# this will export the include-related variables for use by other libraries +# this is needed if you wish to include the headers from this pkg into downstream libraries +ament_export_include_directories("include") +ament_package() diff --git a/sub_pub_torture_test/config/publishers.yaml b/sub_pub_torture_test/config/publishers.yaml new file mode 100644 index 0000000..9c72dba --- /dev/null +++ b/sub_pub_torture_test/config/publishers.yaml @@ -0,0 +1,3 @@ +n_publishers: 1000 # [-] + +timer_rate: 100.0 # [Hz] diff --git a/sub_pub_torture_test/config/subscribers.yaml b/sub_pub_torture_test/config/subscribers.yaml new file mode 100644 index 0000000..2b8b235 --- /dev/null +++ b/sub_pub_torture_test/config/subscribers.yaml @@ -0,0 +1,3 @@ +n_subscribers: 1000 # [-] + +timer_rate: 100.0 # [Hz] diff --git a/sub_pub_torture_test/include/sub_pub_torture_test/params.h b/sub_pub_torture_test/include/sub_pub_torture_test/params.h new file mode 100644 index 0000000..dc0fa42 --- /dev/null +++ b/sub_pub_torture_test/include/sub_pub_torture_test/params.h @@ -0,0 +1,64 @@ +#include + +namespace utils +{ + +// Overload << operator for std::vector +template +std::ostream& operator<<(std::ostream& os, const std::vector& vec) { + os << "["; + for (size_t i = 0; i < vec.size(); ++i) { + os << vec[i]; + if (i != vec.size() - 1) { + os << ", "; // Add a comma between elements + } + } + os << "]"; + return os; +} + +/* parse_param() method //{ */ +// a helper parameter loading function +template +bool parse_param(const std::string& param_name, T& param_dest, rclcpp::Node& node) { + + // firstly, the parameter has to be specified (together with its type), which can throw an exception + try { + node.declare_parameter(param_name); // for Galactic and newer, the type has to be specified here + } + catch (const std::exception& e) { + // this can happen if (see + // http://docs.ros.org/en/humble/p/rclcpp/generated/classrclcpp_1_1Node.html#_CPPv4N6rclcpp4Node17declare_parameterERKNSt6stringERKN6rclcpp14ParameterValueERKN14rcl_interfaces3msg19ParameterDescriptorEb): + // * parameter has already been declared (rclcpp::exceptions::ParameterAlreadyDeclaredException) + // * parameter name is invalid (rclcpp::exceptions::InvalidParametersException) + // * initial value fails to be set (rclcpp::exceptions::InvalidParameterValueException, not sure what exactly this means) + // * type of the default value or override is wrong (rclcpp::exceptions::InvalidParameterTypeException, the most common one) + RCLCPP_ERROR_STREAM(node.get_logger(), "Could not load param '" << param_name << "': " << e.what()); + + return false; + } + + // then we can attempt to load its value from the server + if (node.get_parameter(param_name, param_dest)) { + + RCLCPP_INFO_STREAM(node.get_logger(), "Loaded '" << param_name << "' = '" << param_dest << "'"); + return true; + + } else { + + // this branch should never happen since we *just* declared the parameter + RCLCPP_ERROR_STREAM(node.get_logger(), "Could not load param '" << param_name << "': Not declared!"); + return false; + } +} + +template +T parse_param2(const std::string& param_name, bool& ok_out, rclcpp::Node& node) { + T out; + ok_out = parse_param(param_name, out, node); + return out; +} + +//} + +} // namespace utils diff --git a/sub_pub_torture_test/include/sub_pub_torture_test/rate_counter.h b/sub_pub_torture_test/include/sub_pub_torture_test/rate_counter.h new file mode 100644 index 0000000..3d373c4 --- /dev/null +++ b/sub_pub_torture_test/include/sub_pub_torture_test/rate_counter.h @@ -0,0 +1,33 @@ +#include + +namespace utils +{ +class RateCounter { +public: + RateCounter(const rclcpp::Clock::SharedPtr& clk_ptr) : clk_ptr_(clk_ptr), prev_call_(clk_ptr_->now()), avg_dt_(0.1){}; + + double update_rate() { + + const rclcpp::Time cur_call = clk_ptr_->now(); + const rclcpp::Duration cur_dur = cur_call - prev_call_; + + const double cur_dt = cur_dur.seconds(); + + avg_dt_ = 0.90 * avg_dt_ + 0.1 * cur_dt; + + prev_call_ = cur_call; + + if (avg_dt_ > 0) { + return 1.0 / avg_dt_; + } else { + return 0.0; + } + } + +private: + rclcpp::Clock::SharedPtr clk_ptr_; + rclcpp::Time prev_call_; + double avg_dt_; +}; + +} // namespace utils diff --git a/sub_pub_torture_test/launch/publishers.py b/sub_pub_torture_test/launch/publishers.py new file mode 100755 index 0000000..0896861 --- /dev/null +++ b/sub_pub_torture_test/launch/publishers.py @@ -0,0 +1,43 @@ +#!/usr/bin/env python3 + +import launch +from ament_index_python import get_package_share_directory +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + +def generate_launch_description(): + + ld = launch.LaunchDescription() + + pkg_name = 'sub_pub_torture_test' + pkg_share_path = get_package_share_directory(pkg_name) + namespace='' + + ld.add_action(ComposableNodeContainer( + + namespace = namespace, + name = 'publishers_server', + package='rclcpp_components', + executable='component_container_mt', + + composable_node_descriptions=[ + + # possible to add multiple nodes to this container + ComposableNode( + package=pkg_name, + plugin='sub_pub_torture_test::Publishers', + namespace=namespace, + name='publishers', + + parameters=[ + pkg_share_path + '/config/publishers.yaml', + ], + ) + + ], + + output='screen', + + )) + + return ld diff --git a/sub_pub_torture_test/launch/subscribers.py b/sub_pub_torture_test/launch/subscribers.py new file mode 100755 index 0000000..ab6a471 --- /dev/null +++ b/sub_pub_torture_test/launch/subscribers.py @@ -0,0 +1,45 @@ +#!/usr/bin/env python3 + +import launch +from ament_index_python.packages import get_package_share_directory +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + +def generate_launch_description(): + + ld = launch.LaunchDescription() + + pkg_name = "sub_pub_torture_test" + pkg_share_path = get_package_share_directory(pkg_name) + namespace='' + + ld.add_action(ComposableNodeContainer( + + namespace = namespace, + name = 'subscribers_server', + package='rclcpp_components', + executable='component_container_mt', + + parameters=[ + {'thread_num': 16}, + ], + + composable_node_descriptions=[ + + ComposableNode( + package=pkg_name, + plugin='sub_pub_torture_test::Subscribers', + namespace=namespace, + name='subscribers', + + parameters=[ + pkg_share_path + '/config/subscribers.yaml', + ], + ) + + ], + + output='screen', + )) + + return ld diff --git a/sub_pub_torture_test/package.xml b/sub_pub_torture_test/package.xml new file mode 100644 index 0000000..6bcd341 --- /dev/null +++ b/sub_pub_torture_test/package.xml @@ -0,0 +1,32 @@ + + + + + sub_pub_torture_test + + 1.0.0 + Subscriber Timer Bug + + klaxalk + klaxalk + + TODO: License declaration + + ament_cmake + + rclcpp + rclcpp_components + launch + launch_ros + builtin_interfaces + + std_msgs + + + sub_pub_torture_test_py + + + ament_cmake + + + diff --git a/sub_pub_torture_test/src/publishers.cpp b/sub_pub_torture_test/src/publishers.cpp new file mode 100644 index 0000000..47da986 --- /dev/null +++ b/sub_pub_torture_test/src/publishers.cpp @@ -0,0 +1,104 @@ +#include + +#include + +#include +#include + +using namespace std::chrono_literals; + +namespace sub_pub_torture_test +{ + +/* class Publishers //{ */ + +class Publishers : public rclcpp::Node { +public: + Publishers(const rclcpp::NodeOptions options); + +private: + // | -------------------- member variables -------------------- | + + int _n_publishers_; + double _timer_rate_; + + // | ----------------------- publishers ----------------------- | + + std::vector::SharedPtr> publishers_; + + // | ------------------------- timers ------------------------- | + + rclcpp::CallbackGroup::SharedPtr callback_group_; + rclcpp::TimerBase::SharedPtr timer_main_; + + std::shared_ptr timer_rate_counter_; + + // | ------------------------- methods ------------------------ | + + void callbackTimerMain(); +}; + +//} + +/* Publishers() //{ */ + +Publishers::Publishers(rclcpp::NodeOptions options) : Node("publisher_example", options) { + + RCLCPP_INFO(get_logger(), "initializing"); + + utils::parse_param("n_publishers", _n_publishers_, *this); + utils::parse_param("timer_rate", _timer_rate_, *this); + + // | ------------------------ publisher ----------------------- | + + rclcpp::QoS qos = rclcpp::SystemDefaultsQoS(); + + for (int i = 0; i < _n_publishers_; i++) { + + RCLCPP_INFO(get_logger(), "initializing publisher %d", i); + + std::stringstream ss; + ss << i; + + publishers_.push_back(create_publisher("/topic_" + ss.str(), qos)); + } + + // | -------------------------- timer ------------------------- | + + callback_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + + timer_main_ = create_wall_timer(std::chrono::duration(1.0 / _timer_rate_), std::bind(&Publishers::callbackTimerMain, this), callback_group_); + + timer_rate_counter_ = std::make_shared(this->get_clock()); + + // | --------------------- finish the init -------------------- | + + RCLCPP_INFO(get_logger(), "initialized"); +} + +//} + +// | ------------------------ callbacks ----------------------- | + +/* callbackTimerMain() //{ */ + +void Publishers::callbackTimerMain() { + + double rate = timer_rate_counter_->update_rate(); + + RCLCPP_INFO(get_logger(), "timer rate %d", int(round(rate))); + + RCLCPP_INFO_ONCE(get_logger(), "timer_main_ running"); + + for (int i = 0; i < _n_publishers_; i++) { + + publishers_[i]->publish(std_msgs::msg::String().set__data("some data")); + } +} + +//} + +} // namespace sub_pub_torture_test + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(sub_pub_torture_test::Publishers) diff --git a/sub_pub_torture_test/src/subscribers.cpp b/sub_pub_torture_test/src/subscribers.cpp new file mode 100644 index 0000000..b02dca7 --- /dev/null +++ b/sub_pub_torture_test/src/subscribers.cpp @@ -0,0 +1,126 @@ +#include + +#include + +#include +#include + +using namespace std::chrono_literals; + +namespace sub_pub_torture_test +{ + +/* class Subscribers //{ */ + +class Subscribers : public rclcpp::Node { +private: +public: + Subscribers(rclcpp::NodeOptions options); + +private: + // | ------------------------- params ------------------------- | + + int _n_subscribers_; + double _timer_rate_; + + // | ----------------------- subscribers ---------------------- | + + std::vector::SharedPtr> subscribers_; + + void callbackSubscriber(const std_msgs::msg::String::SharedPtr msg, const int subscriber_id); + + // | ------------------ other member variables ---------------- | + + std::vector callback_group_subs_; + + // | ------------------------- timers ------------------------- | + + rclcpp::CallbackGroup::SharedPtr callback_group_timer_; + rclcpp::TimerBase::SharedPtr timer_main_; + + void callbackTimerMain(); + + std::shared_ptr timer_rate_counter_; + + rclcpp::Time last_time_timer_; +}; + +//} + +/* Subscribers() //{ */ + +Subscribers::Subscribers(rclcpp::NodeOptions options) : Node("subscriber_example", options) { + + RCLCPP_INFO(get_logger(), "initializing"); + + utils::parse_param("n_subscribers", _n_subscribers_, *this); + utils::parse_param("timer_rate", _timer_rate_, *this); + + last_time_timer_ = this->get_clock()->now(); + + // | ----------------------- Subscribers ----------------------- | + + rclcpp::QoS qos = rclcpp::SystemDefaultsQoS(); + + for (int i = 0; i < _n_subscribers_; i++) { + + auto sub_opt = rclcpp::SubscriptionOptions(); + + callback_group_subs_.push_back(create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive)); + + sub_opt.callback_group = callback_group_subs_.at(i); + + RCLCPP_INFO(get_logger(), "initializing subscriber %d", i); + + const std::function cbk_ptr = std::bind(&Subscribers::callbackSubscriber, this, std::placeholders::_1, i); + + std::stringstream ss; + ss << i; + + subscribers_.push_back(create_subscription("/topic_" + ss.str(), qos, cbk_ptr, sub_opt)); + } + + // | -------------------------- timer ------------------------- | + + /* assign each timer callback to a different group so they are run in parallel */ + callback_group_timer_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + + timer_main_ = create_timer(std::chrono::duration(1.0 / _timer_rate_), std::bind(&Subscribers::callbackTimerMain, this), callback_group_timer_); + + timer_rate_counter_ = std::make_shared(this->get_clock()); + + // | --------------------- finish the init -------------------- | + + RCLCPP_INFO(get_logger(), "initialized"); +} + +//} + +// | ------------------------ callbacks ----------------------- | + +/* callbackSubscriber() //{ */ + +void Subscribers::callbackSubscriber([[maybe_unused]] const std_msgs::msg::String::SharedPtr msg, [[maybe_unused]] const int subscriber_id) { +} + +//} + +/* callbackTimerMain() //{ */ + +void Subscribers::callbackTimerMain() { + + double rate = timer_rate_counter_->update_rate(); + + double dt = (this->get_clock()->now() - last_time_timer_).seconds(); + + last_time_timer_ = this->get_clock()->now(); + + RCLCPP_INFO(get_logger(), "timer rate %d, dt %.3f", int(round(rate)), dt); +} + +//} + +} // namespace sub_pub_torture_test + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(sub_pub_torture_test::Subscribers) diff --git a/sub_pub_torture_test/tmux/session.yml b/sub_pub_torture_test/tmux/session.yml new file mode 100644 index 0000000..179ff49 --- /dev/null +++ b/sub_pub_torture_test/tmux/session.yml @@ -0,0 +1,12 @@ +root: ./ +socket_name: mrs +name: simulation +attach: false +tmux_options: -f /etc/ctu-mrs/tmux.conf +startup_window: main +windows: + - main: + layout: horizontal + panes: + - ros2 launch sub_pub_torture_test subscribers.py + - ros2 launch sub_pub_torture_test publishers.py diff --git a/sub_pub_torture_test/tmux/start.sh b/sub_pub_torture_test/tmux/start.sh new file mode 100755 index 0000000..5bb2275 --- /dev/null +++ b/sub_pub_torture_test/tmux/start.sh @@ -0,0 +1,27 @@ +#!/bin/bash + +# Absolute path to this script. /home/user/bin/foo.sh +SCRIPT=$(readlink -f $0) +# Absolute path this script is in. /home/user/bin +SCRIPTPATH=`dirname $SCRIPT` +cd "$SCRIPTPATH" + +export TMUX_SESSION_NAME=simulation +export TMUX_SOCKET_NAME=mrs + +# start tmuxinator +tmuxinator start -p ./session.yml + +# if we are not in tmux +if [ -z $TMUX ]; then + + # just attach to the session + tmux -L $TMUX_SOCKET_NAME a -t $TMUX_SESSION_NAME + +# if we are in tmux +else + + # switch to the newly-started session + tmux detach-client -E "tmux -L $TMUX_SOCKET_NAME a -t $TMUX_SESSION_NAME" + +fi From aaee0aae3720e393fb6493cdb25c56c33ac851f0 Mon Sep 17 00:00:00 2001 From: Afzal Ahmad Date: Mon, 10 Mar 2025 17:23:03 +0100 Subject: [PATCH 16/27] wip: ros2 get_param isseues --- ros2_examples/launch/params_example.py | 6 +++--- ros2_examples/package.xml | 1 + ros2_examples/src/params_example.cpp | 27 ++++++++++---------------- 3 files changed, 14 insertions(+), 20 deletions(-) diff --git a/ros2_examples/launch/params_example.py b/ros2_examples/launch/params_example.py index 67a2e66..558f3f8 100755 --- a/ros2_examples/launch/params_example.py +++ b/ros2_examples/launch/params_example.py @@ -43,7 +43,7 @@ def generate_launch_description(): pkg_share_path = get_package_share_directory(pkg_name) # param loaded from env variable - uav_type=os.getenv('UAV_TYPE', '') + uav_type=os.getenv('UAV_TYPE', "x500") ld.add_action(ComposableNodeContainer( @@ -59,12 +59,12 @@ def generate_launch_description(): package=pkg_name, plugin='ros2_examples::ParamsExample', namespace='nmspc1', - name='params_', + name='params_node_ns', parameters=[ pkg_share_path + '/config/params_example.yaml', {'custom_config': custom_config, - 'params.uav_type': uav_type}, + 'uav_type': uav_type}, ], # remappings=[ diff --git a/ros2_examples/package.xml b/ros2_examples/package.xml index 00f9815..755d16f 100644 --- a/ros2_examples/package.xml +++ b/ros2_examples/package.xml @@ -27,6 +27,7 @@ tf2_ros ros2_lib + mrs_lib ros2_examples_py diff --git a/ros2_examples/src/params_example.cpp b/ros2_examples/src/params_example.cpp index bdec598..117cbf8 100644 --- a/ros2_examples/src/params_example.cpp +++ b/ros2_examples/src/params_example.cpp @@ -1,7 +1,9 @@ +#include #include #include #include +#include using namespace std::chrono_literals; @@ -34,11 +36,11 @@ class ParamsExample : public rclcpp::Node { /* ParamsExample() constructor //{ */ -ParamsExample::ParamsExample(const rclcpp::NodeOptions options) : Node("params_example", options) { +ParamsExample::ParamsExample(const rclcpp::NodeOptions options) : Node("dummy", options) { RCLCPP_INFO(get_logger(), "initializing"); - // bool loaded_successfully = true; + bool loaded_successfully = true; auto m_node = this->create_sub_node("params"); // | --------------------- load parameters -------------------- | @@ -46,27 +48,18 @@ ParamsExample::ParamsExample(const rclcpp::NodeOptions options) : Node("params_e mrs_lib::ParamLoader param_loader{m_node, std::string{"ParamsExample"}}; std::string custom_config = ""; - // param_loader.loadParam("param_namespace/floating_number", floating_point_number_); - param_loader.loadParam("some_string", some_string_); - param_loader.loadParam("some_string", some_string_); - // param_loader.loadParam("custom_config", custom_config); - - // if (custom_config == "") { - // RCLCPP_WARN(get_logger(), "custom_config is empty"); - // } - + param_loader.loadParam2("param_namespace/floating_number", floating_point_number_); param_loader.loadParam("uav_type", uav_type); + param_loader.loadParam("some_string", some_string_); + param_loader.loadParam("custom_config", custom_config); - auto param_list = m_node->list_parameters(std::vector(), 0); - - for (const auto& param: param_list.names) { - std::cout << "params: " << param << std::endl; - + if (custom_config == "") { + RCLCPP_WARN(get_logger(), "custom_config is empty"); } if (!param_loader.loadedSuccessfully()) { RCLCPP_ERROR_STREAM(get_logger(), "Could not load all non-optional parameters. Shutting down."); - // rclcpp::shutdown(); + rclcpp::shutdown(); return; } From 9d81e178052e7b2048f5164f4f1492328df2f902 Mon Sep 17 00:00:00 2001 From: Tomas Baca Date: Wed, 12 Mar 2025 13:33:27 +0100 Subject: [PATCH 17/27] disabled py part of packaage.xml that causes deb compilation issues --- ros2_examples/package.xml | 2 +- sub_pub_torture_test/package.xml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/ros2_examples/package.xml b/ros2_examples/package.xml index 00f9815..2146978 100644 --- a/ros2_examples/package.xml +++ b/ros2_examples/package.xml @@ -29,7 +29,7 @@ ros2_lib - ros2_examples_py + ament_cmake diff --git a/sub_pub_torture_test/package.xml b/sub_pub_torture_test/package.xml index 6bcd341..6f2022e 100644 --- a/sub_pub_torture_test/package.xml +++ b/sub_pub_torture_test/package.xml @@ -23,7 +23,7 @@ std_msgs - sub_pub_torture_test_py + ament_cmake From 0b0f6287e1a2bc7aa5767c957731b5be340fdbe1 Mon Sep 17 00:00:00 2001 From: Afzal Ahmad Date: Thu, 13 Mar 2025 19:02:09 +0100 Subject: [PATCH 18/27] added: param-loader example for mrs_lib version --- ros2_examples/CMakeLists.txt | 2 - ros2_examples/config/params_example.yaml | 11 ++-- ros2_examples/launch/params_example.py | 4 +- ros2_examples/package.xml | 1 - ros2_examples/src/params_example.cpp | 84 ++++++++++++++++++------ 5 files changed, 70 insertions(+), 32 deletions(-) diff --git a/ros2_examples/CMakeLists.txt b/ros2_examples/CMakeLists.txt index 8118e20..8cb60e3 100644 --- a/ros2_examples/CMakeLists.txt +++ b/ros2_examples/CMakeLists.txt @@ -20,7 +20,6 @@ find_package(std_srvs REQUIRED) find_package(tf2_ros REQUIRED) find_package(geometry_msgs REQUIRED) find_package(ros2_lib REQUIRED) -find_package(mrs_lib REQUIRED) include_directories( include @@ -132,7 +131,6 @@ ament_target_dependencies(params_example rclcpp_components std_msgs std_srvs - mrs_lib ) # each component (nodelet) needs to be registered to make it discoverable at runtime diff --git a/ros2_examples/config/params_example.yaml b/ros2_examples/config/params_example.yaml index 2542103..9996b7b 100644 --- a/ros2_examples/config/params_example.yaml +++ b/ros2_examples/config/params_example.yaml @@ -1,7 +1,6 @@ -param_namespace: - - # floating_number: "asd" # this will fail to load due to type mismatch - - floating_number: 5.0 - +floating_point_number: 666.666 some_string: "dog is barking" +vec_double: [1.0, 2.0, 3.0, 4.0] +vec_str: ["Cats", "are", "lovely"] +namespace1: + str: "namespaced str" diff --git a/ros2_examples/launch/params_example.py b/ros2_examples/launch/params_example.py index 558f3f8..91c0fd9 100755 --- a/ros2_examples/launch/params_example.py +++ b/ros2_examples/launch/params_example.py @@ -43,7 +43,7 @@ def generate_launch_description(): pkg_share_path = get_package_share_directory(pkg_name) # param loaded from env variable - uav_type=os.getenv('UAV_TYPE', "x500") + env_var=os.getenv('ENV_VAR', "x500") ld.add_action(ComposableNodeContainer( @@ -64,7 +64,7 @@ def generate_launch_description(): parameters=[ pkg_share_path + '/config/params_example.yaml', {'custom_config': custom_config, - 'uav_type': uav_type}, + 'env_var': env_var}, ], # remappings=[ diff --git a/ros2_examples/package.xml b/ros2_examples/package.xml index 755d16f..00f9815 100644 --- a/ros2_examples/package.xml +++ b/ros2_examples/package.xml @@ -27,7 +27,6 @@ tf2_ros ros2_lib - mrs_lib ros2_examples_py diff --git a/ros2_examples/src/params_example.cpp b/ros2_examples/src/params_example.cpp index 117cbf8..1ddce2b 100644 --- a/ros2_examples/src/params_example.cpp +++ b/ros2_examples/src/params_example.cpp @@ -1,9 +1,8 @@ -#include -#include #include -#include +#include #include +#include using namespace std::chrono_literals; @@ -21,43 +20,41 @@ class ParamsExample : public rclcpp::Node { private: // | ----------------------- parameters ----------------------- | - double floating_point_number_; - std::string some_string_; - std::string uav_type; + std::string env_var = "I should have an env variable"; + struct ConfigYAMLParams { + double floating_point_number = 786.00; + std::string some_string = "I should be changed"; + std::vector vec_str{"I", "must", "be", "changed"}; + std::vector vec_double{7.0, 8.0, 6.0}; + std::string namespaced_str = "I should have a namespaced string"; + }; + ConfigYAMLParams params; - // OnSetParametersCallbackHandle::SharedPtr param_callback_handle_; + OnSetParametersCallbackHandle::SharedPtr param_callback_handle_; // | ------------------------- methods ------------------------ | // rcl_interfaces::msg::SetParametersResult callback_parameters(std::vector parameters); + bool load_parameters(const rclcpp::Node& node_ptr); }; //} /* ParamsExample() constructor //{ */ -ParamsExample::ParamsExample(const rclcpp::NodeOptions options) : Node("dummy", options) { +ParamsExample::ParamsExample(const rclcpp::NodeOptions options) : Node("param_example", options) { RCLCPP_INFO(get_logger(), "initializing"); bool loaded_successfully = true; - auto m_node = this->create_sub_node("params"); + auto sub_node = this->create_sub_node("params"); // | --------------------- load parameters -------------------- | - mrs_lib::ParamLoader param_loader{m_node, std::string{"ParamsExample"}}; + loaded_successfully &= load_parameters(*this); + // loaded_successfully &= load_parameters(sub_node); - std::string custom_config = ""; - param_loader.loadParam2("param_namespace/floating_number", floating_point_number_); - param_loader.loadParam("uav_type", uav_type); - param_loader.loadParam("some_string", some_string_); - param_loader.loadParam("custom_config", custom_config); - - if (custom_config == "") { - RCLCPP_WARN(get_logger(), "custom_config is empty"); - } - - if (!param_loader.loadedSuccessfully()) { + if (!loaded_successfully) { RCLCPP_ERROR_STREAM(get_logger(), "Could not load all non-optional parameters. Shutting down."); rclcpp::shutdown(); return; @@ -111,6 +108,51 @@ ParamsExample::ParamsExample(const rclcpp::NodeOptions options) : Node("dummy", // return result; // } +bool ParamsExample::load_parameters(const rclcpp::Node& node_ptr) { + + bool loaded_successfully = true; + + node_ptr->declare_parameter("env_var"); + loaded_successfully &= node_ptr->get_parameter("env_var", env_var); + RCLCPP_INFO_STREAM(get_logger(), "[Param]: env_var: " << env_var); + + node_ptr->declare_parameter("floating_point_number"); + loaded_successfully &= node_ptr->get_parameter("floating_point_number", params.floating_point_number); + RCLCPP_INFO_STREAM(get_logger(), "[Param]: floating_point_number: " << params.floating_point_number); + + node_ptr->declare_parameter("some_string"); + loaded_successfully &= node_ptr->get_parameter("some_string", params.some_string); + RCLCPP_INFO_STREAM(get_logger(), "[Param]: some_string: " << params.some_string); + + node_ptr->declare_parameter>("vec_double"); + loaded_successfully &= node_ptr->get_parameter("vec_double", params.vec_double); + + std::stringstream tmp_str; + tmp_str << "["; + for (const auto& val: params.vec_double) { + tmp_str << val <<", "; + } + tmp_str << "]"; + + RCLCPP_INFO_STREAM(get_logger(), "[Param]: vec_double: " << tmp_str.str()); + + node_ptr->declare_parameter>("vec_str"); + loaded_successfully &= node_ptr->get_parameter("vec_str", params.vec_str); + std::stringstream tmp_double_str; + tmp_double_str << "["; + for (const auto& val: params.vec_double) { + tmp_double_str << val <<", "; + } + tmp_double_str << "]"; + RCLCPP_INFO_STREAM(get_logger(), "[Param]: vec_str: " << tmp_double_str.str()); + + node_ptr->declare_parameter("namespace1.str"); + loaded_successfully &= node_ptr->get_parameter("namespace1.str", params.namespaced_str); + RCLCPP_INFO_STREAM(get_logger(), "[Param]: namespace1.str: " << params.namespaced_str); + + return loaded_successfully; +} + //} } // namespace ros2_examples From acd787eb54e5798e0bfb93f9efe2b1df1fc2e42b Mon Sep 17 00:00:00 2001 From: Afzal Ahmad Date: Mon, 17 Mar 2025 11:57:31 +0100 Subject: [PATCH 19/27] : --- ros2_examples/CMakeLists.txt | 3 -- ros2_examples/include/ros2_examples/params.h | 40 ++++++++------ ros2_examples/src/params_example.cpp | 57 +++++++++++++------- 3 files changed, 61 insertions(+), 39 deletions(-) diff --git a/ros2_examples/CMakeLists.txt b/ros2_examples/CMakeLists.txt index 14ef074..4231591 100644 --- a/ros2_examples/CMakeLists.txt +++ b/ros2_examples/CMakeLists.txt @@ -163,7 +163,6 @@ ament_target_dependencies(tf2_listener_example rclcpp_components_register_nodes(tf2_listener_example PLUGIN "ros2_examples::Tf2ListenerExample" EXECUTABLE tf2_listener_example) -<<<<<<< HEAD ## -------------------------------------------------------------- ## | clock management | ## -------------------------------------------------------------- @@ -179,8 +178,6 @@ ament_target_dependencies(clock_consumer rclcpp_components_register_nodes(clock_consumer PLUGIN "ros2_examples::ClockConsumer" EXECUTABLE clock_consumer) -======= ->>>>>>> mrs-lib-dep ## -------------------------------------------------------------- ## | Testing | ## -------------------------------------------------------------- diff --git a/ros2_examples/include/ros2_examples/params.h b/ros2_examples/include/ros2_examples/params.h index 85b9497..c5c851b 100644 --- a/ros2_examples/include/ros2_examples/params.h +++ b/ros2_examples/include/ros2_examples/params.h @@ -1,3 +1,4 @@ +#include #include namespace utils @@ -17,30 +18,35 @@ std::ostream& operator<<(std::ostream& os, const std::vector& vec) { return os; } -/* parse_param() method //{ */ +/* load_param() method //{ */ // a helper parameter loading function template -bool parse_param(const std::string& param_name, T& param_dest, rclcpp::Node& node) { +bool load_param(const std::string& param_name, T& param_dest, rclcpp::Node& node) { - // firstly, the parameter has to be specified (together with its type), which can throw an exception - try { - node.declare_parameter(param_name); // for Galactic and newer, the type has to be specified here - } - catch (const std::exception& e) { - // this can happen if (see - // http://docs.ros.org/en/humble/p/rclcpp/generated/classrclcpp_1_1Node.html#_CPPv4N6rclcpp4Node17declare_parameterERKNSt6stringERKN6rclcpp14ParameterValueERKN14rcl_interfaces3msg19ParameterDescriptorEb): - // * parameter has already been declared (rclcpp::exceptions::ParameterAlreadyDeclaredException) - // * parameter name is invalid (rclcpp::exceptions::InvalidParametersException) - // * initial value fails to be set (rclcpp::exceptions::InvalidParameterValueException, not sure what exactly this means) - // * type of the default value or override is wrong (rclcpp::exceptions::InvalidParameterTypeException, the most common one) - RCLCPP_ERROR_STREAM(node.get_logger(), "Could not load param '" << param_name << "': " << e.what()); + if (!node.has_parameter(param_name)) { + // firstly, the parameter has to be specified (together with its type), which can throw an exception + try { + node.declare_parameter(param_name); // for Galactic and newer, the type has to be specified here + } + catch (const std::exception& e) { + // this can happen if (see + // http://docs.ros.org/en/humble/p/rclcpp/generated/classrclcpp_1_1Node.html#_CPPv4N6rclcpp4Node17declare_parameterERKNSt6stringERKN6rclcpp14ParameterValueERKN14rcl_interfaces3msg19ParameterDescriptorEb): + // * parameter has already been declared (rclcpp::exceptions::ParameterAlreadyDeclaredException) + // * parameter name is invalid (rclcpp::exceptions::InvalidParametersException) + // * initial value fails to be set (rclcpp::exceptions::InvalidParameterValueException, not sure what exactly this means) + // * type of the default value or override is wrong (rclcpp::exceptions::InvalidParameterTypeException, the most common one) + RCLCPP_ERROR_STREAM(node.get_logger(), "Could not load param '" << param_name << "': " << e.what()); - return false; + return false; + } } + rclcpp::Parameter tmp_param; + // then we can attempt to load its value from the server - if (node.get_parameter(param_name, param_dest)) { + if (node.get_parameter(param_name, tmp_param)) { + param_dest = tmp_param.get_value(); RCLCPP_INFO_STREAM(node.get_logger(), "Loaded '" << param_name << "' = '" << param_dest << "'"); return true; @@ -55,7 +61,7 @@ bool parse_param(const std::string& param_name, T& param_dest, rclcpp::Node& nod template T parse_param2(const std::string& param_name, bool& ok_out, rclcpp::Node& node) { T out; - ok_out = parse_param(param_name, out, node); + ok_out = load_param(param_name, out, node); return out; } diff --git a/ros2_examples/src/params_example.cpp b/ros2_examples/src/params_example.cpp index 1ddce2b..d07d766 100644 --- a/ros2_examples/src/params_example.cpp +++ b/ros2_examples/src/params_example.cpp @@ -3,6 +3,7 @@ #include #include #include +#include using namespace std::chrono_literals; @@ -35,7 +36,7 @@ class ParamsExample : public rclcpp::Node { // | ------------------------- methods ------------------------ | // rcl_interfaces::msg::SetParametersResult callback_parameters(std::vector parameters); - bool load_parameters(const rclcpp::Node& node_ptr); + bool load_parameters(rclcpp::Node& node); }; //} @@ -47,15 +48,33 @@ ParamsExample::ParamsExample(const rclcpp::NodeOptions options) : Node("param_ex RCLCPP_INFO(get_logger(), "initializing"); bool loaded_successfully = true; - auto sub_node = this->create_sub_node("params"); - // | --------------------- load parameters -------------------- | - loaded_successfully &= load_parameters(*this); - // loaded_successfully &= load_parameters(sub_node); + RCLCPP_INFO(get_logger(), "Loading using Node"); + loaded_successfully &= utils::load_param("env_var", env_var, *this); + loaded_successfully &= utils::load_param("floating_point_number", params.floating_point_number, *this); + loaded_successfully &= utils::load_param("some_string", params.some_string, *this); + loaded_successfully &= utils::load_param("vec_double", params.vec_double, *this); + loaded_successfully &= utils::load_param("vec_str", params.vec_str, *this); + loaded_successfully &= utils::load_param("namespace1.str", params.namespaced_str, *this); + + if (!loaded_successfully) { + RCLCPP_ERROR_STREAM(get_logger(), "Could not load all non-optional parameters using Node. Shutting down."); + rclcpp::shutdown(); + return; + } + + RCLCPP_INFO(get_logger(), "Loading using Sub-Node"); + auto sub_node = this->create_sub_node("params"); + loaded_successfully &= utils::load_param("env_var", env_var, *sub_node); + loaded_successfully &= utils::load_param("floating_point_number", params.floating_point_number, *sub_node); + loaded_successfully &= utils::load_param("some_string", params.some_string, *sub_node); + loaded_successfully &= utils::load_param("vec_double", params.vec_double, *sub_node); + loaded_successfully &= utils::load_param("vec_str", params.vec_str, *sub_node); + loaded_successfully &= utils::load_param("namespace1.str", params.namespaced_str, *sub_node); if (!loaded_successfully) { - RCLCPP_ERROR_STREAM(get_logger(), "Could not load all non-optional parameters. Shutting down."); + RCLCPP_ERROR_STREAM(get_logger(), "Could not load all non-optional parameters using Sub-Node. Shutting down."); rclcpp::shutdown(); return; } @@ -108,24 +127,24 @@ ParamsExample::ParamsExample(const rclcpp::NodeOptions options) : Node("param_ex // return result; // } -bool ParamsExample::load_parameters(const rclcpp::Node& node_ptr) { +bool ParamsExample::load_parameters(rclcpp::Node& node) { bool loaded_successfully = true; - node_ptr->declare_parameter("env_var"); - loaded_successfully &= node_ptr->get_parameter("env_var", env_var); + node.declare_parameter("env_var"); + loaded_successfully &= node.get_parameter("env_var", env_var); RCLCPP_INFO_STREAM(get_logger(), "[Param]: env_var: " << env_var); - node_ptr->declare_parameter("floating_point_number"); - loaded_successfully &= node_ptr->get_parameter("floating_point_number", params.floating_point_number); + node.declare_parameter("floating_point_number"); + loaded_successfully &= node.get_parameter("floating_point_number", params.floating_point_number); RCLCPP_INFO_STREAM(get_logger(), "[Param]: floating_point_number: " << params.floating_point_number); - node_ptr->declare_parameter("some_string"); - loaded_successfully &= node_ptr->get_parameter("some_string", params.some_string); + node.declare_parameter("some_string"); + loaded_successfully &= node.get_parameter("some_string", params.some_string); RCLCPP_INFO_STREAM(get_logger(), "[Param]: some_string: " << params.some_string); - node_ptr->declare_parameter>("vec_double"); - loaded_successfully &= node_ptr->get_parameter("vec_double", params.vec_double); + node.declare_parameter>("vec_double"); + loaded_successfully &= node.get_parameter("vec_double", params.vec_double); std::stringstream tmp_str; tmp_str << "["; @@ -136,8 +155,8 @@ bool ParamsExample::load_parameters(const rclcpp::Node& node_ptr) { RCLCPP_INFO_STREAM(get_logger(), "[Param]: vec_double: " << tmp_str.str()); - node_ptr->declare_parameter>("vec_str"); - loaded_successfully &= node_ptr->get_parameter("vec_str", params.vec_str); + node.declare_parameter>("vec_str"); + loaded_successfully &= node.get_parameter("vec_str", params.vec_str); std::stringstream tmp_double_str; tmp_double_str << "["; for (const auto& val: params.vec_double) { @@ -146,8 +165,8 @@ bool ParamsExample::load_parameters(const rclcpp::Node& node_ptr) { tmp_double_str << "]"; RCLCPP_INFO_STREAM(get_logger(), "[Param]: vec_str: " << tmp_double_str.str()); - node_ptr->declare_parameter("namespace1.str"); - loaded_successfully &= node_ptr->get_parameter("namespace1.str", params.namespaced_str); + node.declare_parameter("namespace1.str"); + loaded_successfully &= node.get_parameter("namespace1.str", params.namespaced_str); RCLCPP_INFO_STREAM(get_logger(), "[Param]: namespace1.str: " << params.namespaced_str); return loaded_successfully; From 62b9042019a99647cb25ec52cd2ce345796381d1 Mon Sep 17 00:00:00 2001 From: Afzal Ahmad Date: Mon, 17 Mar 2025 12:09:54 +0100 Subject: [PATCH 20/27] updated: param-example to use Node ptr and custom-config from the cli --- ros2_examples/src/params_example.cpp | 100 ++------------------------- 1 file changed, 4 insertions(+), 96 deletions(-) diff --git a/ros2_examples/src/params_example.cpp b/ros2_examples/src/params_example.cpp index d07d766..02d1e00 100644 --- a/ros2_examples/src/params_example.cpp +++ b/ros2_examples/src/params_example.cpp @@ -1,6 +1,5 @@ #include -#include #include #include #include @@ -30,13 +29,6 @@ class ParamsExample : public rclcpp::Node { std::string namespaced_str = "I should have a namespaced string"; }; ConfigYAMLParams params; - - OnSetParametersCallbackHandle::SharedPtr param_callback_handle_; - - // | ------------------------- methods ------------------------ | - - // rcl_interfaces::msg::SetParametersResult callback_parameters(std::vector parameters); - bool load_parameters(rclcpp::Node& node); }; //} @@ -57,6 +49,9 @@ ParamsExample::ParamsExample(const rclcpp::NodeOptions options) : Node("param_ex loaded_successfully &= utils::load_param("vec_double", params.vec_double, *this); loaded_successfully &= utils::load_param("vec_str", params.vec_str, *this); loaded_successfully &= utils::load_param("namespace1.str", params.namespaced_str, *this); + std::string custom_config = ""; + loaded_successfully &= utils::load_param("custom_config", custom_config, *this); + if (!loaded_successfully) { RCLCPP_ERROR_STREAM(get_logger(), "Could not load all non-optional parameters using Node. Shutting down."); @@ -72,6 +67,7 @@ ParamsExample::ParamsExample(const rclcpp::NodeOptions options) : Node("param_ex loaded_successfully &= utils::load_param("vec_double", params.vec_double, *sub_node); loaded_successfully &= utils::load_param("vec_str", params.vec_str, *sub_node); loaded_successfully &= utils::load_param("namespace1.str", params.namespaced_str, *sub_node); + loaded_successfully &= utils::load_param("custom_config", custom_config, *this); if (!loaded_successfully) { RCLCPP_ERROR_STREAM(get_logger(), "Could not load all non-optional parameters using Sub-Node. Shutting down."); @@ -79,10 +75,6 @@ ParamsExample::ParamsExample(const rclcpp::NodeOptions options) : Node("param_ex return; } - // | --------------- bind pararm server callback -------------- | - - // param_callback_handle_ = add_on_set_parameters_callback(std::bind(&ParamsExample::callback_parameters, this, std::placeholders::_1)); - // | --------------------- finish the init -------------------- | RCLCPP_INFO(get_logger(), "initialized"); @@ -90,90 +82,6 @@ ParamsExample::ParamsExample(const rclcpp::NodeOptions options) : Node("param_ex //} -// | ------------------------ callbacks ----------------------- | - -/* callback_parameters() //{ */ - -// rcl_interfaces::msg::SetParametersResult ParamsExample::callback_parameters(std::vector parameters) { -// rcl_interfaces::msg::SetParametersResult result; - -// // Note that setting a parameter to a nonsensical value (such as setting the `param_namespace.floating_number` parameter to `hello`) -// // doesn't have any effect - it doesn't even call this callback. -// for (auto& param : parameters) { - -// RCLCPP_INFO_STREAM(get_logger(), "got parameter: '" << param.get_name() << "' with value '" << param.value_to_string() << "'"); - -// if (param.get_name() == "param_namespace.floating_number") { - -// floating_point_number_ = param.as_double(); - -// } else if (param.get_name() == "some_string") { - -// some_string_ = param.as_string(); - -// } else { - -// RCLCPP_WARN_STREAM(get_logger(), "parameter: '" << param.get_name() << "' is not dynamically reconfigurable!"); -// result.successful = false; -// result.reason = "Parameter '" + param.get_name() + "' is not dynamically reconfigurable!"; -// return result; -// } -// } - -// RCLCPP_INFO(get_logger(), "params updated"); -// result.successful = true; -// result.reason = "OK"; - -// return result; -// } - -bool ParamsExample::load_parameters(rclcpp::Node& node) { - - bool loaded_successfully = true; - - node.declare_parameter("env_var"); - loaded_successfully &= node.get_parameter("env_var", env_var); - RCLCPP_INFO_STREAM(get_logger(), "[Param]: env_var: " << env_var); - - node.declare_parameter("floating_point_number"); - loaded_successfully &= node.get_parameter("floating_point_number", params.floating_point_number); - RCLCPP_INFO_STREAM(get_logger(), "[Param]: floating_point_number: " << params.floating_point_number); - - node.declare_parameter("some_string"); - loaded_successfully &= node.get_parameter("some_string", params.some_string); - RCLCPP_INFO_STREAM(get_logger(), "[Param]: some_string: " << params.some_string); - - node.declare_parameter>("vec_double"); - loaded_successfully &= node.get_parameter("vec_double", params.vec_double); - - std::stringstream tmp_str; - tmp_str << "["; - for (const auto& val: params.vec_double) { - tmp_str << val <<", "; - } - tmp_str << "]"; - - RCLCPP_INFO_STREAM(get_logger(), "[Param]: vec_double: " << tmp_str.str()); - - node.declare_parameter>("vec_str"); - loaded_successfully &= node.get_parameter("vec_str", params.vec_str); - std::stringstream tmp_double_str; - tmp_double_str << "["; - for (const auto& val: params.vec_double) { - tmp_double_str << val <<", "; - } - tmp_double_str << "]"; - RCLCPP_INFO_STREAM(get_logger(), "[Param]: vec_str: " << tmp_double_str.str()); - - node.declare_parameter("namespace1.str"); - loaded_successfully &= node.get_parameter("namespace1.str", params.namespaced_str); - RCLCPP_INFO_STREAM(get_logger(), "[Param]: namespace1.str: " << params.namespaced_str); - - return loaded_successfully; -} - -//} - } // namespace ros2_examples #include From efc74a46d9c4090a7ec33addd70312aa0345e44a Mon Sep 17 00:00:00 2001 From: Afzal Ahmad Date: Mon, 17 Mar 2025 15:06:59 +0100 Subject: [PATCH 21/27] udpated: image-transport example with tmux session --- .../config/edge_detector.yaml | 16 ----- .../example_image_transport/package.xml | 2 + .../tmux/config/custom_config.yaml | 25 -------- .../tmux/config/network_config.yaml | 15 ----- .../tmux/config/world_config.yaml | 34 ----------- .../example_image_transport/tmux/session.yml | 60 ++++--------------- 6 files changed, 14 insertions(+), 138 deletions(-) delete mode 100644 extended_examples/vision_examples/example_image_transport/config/edge_detector.yaml delete mode 100644 extended_examples/vision_examples/example_image_transport/tmux/config/custom_config.yaml delete mode 100644 extended_examples/vision_examples/example_image_transport/tmux/config/network_config.yaml delete mode 100644 extended_examples/vision_examples/example_image_transport/tmux/config/world_config.yaml diff --git a/extended_examples/vision_examples/example_image_transport/config/edge_detector.yaml b/extended_examples/vision_examples/example_image_transport/config/edge_detector.yaml deleted file mode 100644 index fa71e10..0000000 --- a/extended_examples/vision_examples/example_image_transport/config/edge_detector.yaml +++ /dev/null @@ -1,16 +0,0 @@ -# threshold for the canny edge detector (see https://docs.opencv.org/3.2.0/dd/d1a/group__imgproc__feature.html#ga04723e007ed888ddf11d9ba04e2232de) -canny_threshold: 70 - -# coordinates of the point, which will be backprojected to the camera image from the specified frame -world_frame_id: "world_origin" - -world_point: - x: 0.0 - y: 0.0 - z: 0.0 - -rate: - # how often to publish my topic? - publish: 10 # [Hz] - # how often check whether msgs are arriving? - check_subscribers: 1 # [Hz] diff --git a/extended_examples/vision_examples/example_image_transport/package.xml b/extended_examples/vision_examples/example_image_transport/package.xml index 91e4952..1ed1842 100644 --- a/extended_examples/vision_examples/example_image_transport/package.xml +++ b/extended_examples/vision_examples/example_image_transport/package.xml @@ -21,6 +21,8 @@ image_transport libopencv-dev + vision_examples_utils + ament_cmake diff --git a/extended_examples/vision_examples/example_image_transport/tmux/config/custom_config.yaml b/extended_examples/vision_examples/example_image_transport/tmux/config/custom_config.yaml deleted file mode 100644 index b77364a..0000000 --- a/extended_examples/vision_examples/example_image_transport/tmux/config/custom_config.yaml +++ /dev/null @@ -1,25 +0,0 @@ -mrs_uav_managers: - - estimation_manager: - - # loaded state estimator plugins - state_estimators: [ - "gps_garmin", - "gps_baro", - ] - - initial_state_estimator: "gps_garmin" # will be used as the first state estimator - agl_height_estimator: "garmin_agl" # only slightly filtered height for checking min height (not used in control feedback) - - uav_manager: - - takeoff: - - during_takeoff: - controller: "MpcController" - tracker: "LandoffTracker" - - after_takeoff: - controller: "MpcController" - # controller: "Se3Controller" - tracker: "MpcTracker" diff --git a/extended_examples/vision_examples/example_image_transport/tmux/config/network_config.yaml b/extended_examples/vision_examples/example_image_transport/tmux/config/network_config.yaml deleted file mode 100644 index 08f370d..0000000 --- a/extended_examples/vision_examples/example_image_transport/tmux/config/network_config.yaml +++ /dev/null @@ -1,15 +0,0 @@ -# 1. This list is used by NimbroNetwork for mutual communication of the UAVs -# The names of the robots have to match hostnames described in /etc/hosts. -# -# 2. This list is used by MpcTracker for mutual collision avoidance of the UAVs. -# The names should match the true "UAV_NAMES" (the topic prefixes). -# -# network_config:=~/config/network_config.yaml -# -# to the core.launch and nimbro.launch. - -network: - - robot_names: [ - uav1, - ] diff --git a/extended_examples/vision_examples/example_image_transport/tmux/config/world_config.yaml b/extended_examples/vision_examples/example_image_transport/tmux/config/world_config.yaml deleted file mode 100644 index 9b55067..0000000 --- a/extended_examples/vision_examples/example_image_transport/tmux/config/world_config.yaml +++ /dev/null @@ -1,34 +0,0 @@ -world_origin: - - units: "LATLON" # {"UTM, "LATLON"} - - origin_x: 47.397743 - origin_y: 8.545594 - -safety_area: - - enabled: true - - horizontal: - - # the frame of reference in which the points are expressed - frame_name: "world_origin" - - # polygon - # - # x, y [m] for any frame_name except latlon_origin - # x = latitude, y = longitude [deg] for frame_name=="latlon_origin" - points: [ - -50, -50, - 50, -50, - 50, 50, - -50, 50, - ] - - vertical: - - # the frame of reference in which the max&min z is expressed - frame_name: "world_origin" - - max_z: 15.0 - min_z: 0.5 diff --git a/extended_examples/vision_examples/example_image_transport/tmux/session.yml b/extended_examples/vision_examples/example_image_transport/tmux/session.yml index f346e9b..5d2ef89 100644 --- a/extended_examples/vision_examples/example_image_transport/tmux/session.yml +++ b/extended_examples/vision_examples/example_image_transport/tmux/session.yml @@ -3,60 +3,24 @@ root: ./ name: simulation socket_name: mrs attach: false -tmux_options: -f /etc/ctu-mrs/tmux.conf +# tmux_options: -f /etc/ctu-mrs/tmux.conf # you can modify these -pre_window: export UAV_NAME=uav1; export RUN_TYPE=simulation; export UAV_TYPE=f550 -startup_window: edge_detector +pre_window: export UAV_NAME=uav1; +startup_window: image_transport windows: - - roscore: + - dummy_publisher: layout: tiled panes: - - roscore - - gazebo: + - ros2 run vision_examples_utils dummy_pub + - image_transport: layout: tiled panes: - - waitForRos; roslaunch mrs_uav_gazebo_simulation simulation.launch world_name:=grass_plane gui:=true - - waitForControl; gz camera -c gzclient_camera -f $UAV_NAME; history -s gz camera -c gzclient_camera -f $UAV_NAME - - status: - layout: tiled - panes: - - waitForHw; roslaunch mrs_uav_status status.launch - - spawn: - layout: tiled - panes: - - waitForGazebo; rosservice call /mrs_drone_spawner/spawn "1 $UAV_TYPE --pos -5 0 1 0.0 --enable-rangefinder --enable-mobius-camera-front" - - hw_api: - layout: tiled - panes: - - waitForTime; roslaunch mrs_uav_px4_api api.launch - - core: - layout: tiled - panes: - - waitForHw; roslaunch mrs_uav_core core.launch - platform_config:=`rospack find mrs_uav_gazebo_simulation`/config/mrs_uav_system/$UAV_TYPE.yaml - custom_config:=./config/custom_config.yaml - world_config:=./config/world_config.yaml - network_config:=./config/network_config.yaml - - edge_detector: - layout: tiled - panes: - - waitForHw; roslaunch example_edge_detector edge_detector.launch - - takeoff: - layout: tiled - panes: - - waitForHw; roslaunch mrs_uav_autostart automatic_start.launch - - 'waitForControl; rosservice call /$UAV_NAME/hw_api/arming 1; sleep 2; rosservice call /$UAV_NAME/hw_api/offboard' - - goto: - layout: tiled - panes: - - 'history -s rosservice call /$UAV_NAME/control_manager/goto \"goal: \[0.0, 10.0, 1.5, 0.0\]\"' + - ros2 launch vision_examples example_image_transport.py - rviz: layout: tiled panes: - - waitForControl; roslaunch mrs_uav_core rviz.launch - - waitForControl; roslaunch mrs_rviz_plugins load_robot.launch - - waitForTime; waitForControl; roslaunch mrs_rviz_plugins rviz_interface.launch - - layout: - layout: tiled - panes: - - waitForControl; sleep 5; ~/.i3/layout_manager.sh ./layout.json + - rqt --perspective-file default.perspective + # - layout: + # layout: tiled + # panes: + # - waitForControl; sleep 5; ~/.i3/layout_manager.sh ./layout.json From 02d831e89f2fef84917438c6da332ed08e914a0f Mon Sep 17 00:00:00 2001 From: Afzal Ahmad Date: Mon, 17 Mar 2025 15:07:39 +0100 Subject: [PATCH 22/27] added: rqt file to tmux of image-tranport-example --- .../tmux/default.perspective | 182 ++++++++++++++++++ 1 file changed, 182 insertions(+) create mode 100644 extended_examples/vision_examples/example_image_transport/tmux/default.perspective diff --git a/extended_examples/vision_examples/example_image_transport/tmux/default.perspective b/extended_examples/vision_examples/example_image_transport/tmux/default.perspective new file mode 100644 index 0000000..bf8d06b --- /dev/null +++ b/extended_examples/vision_examples/example_image_transport/tmux/default.perspective @@ -0,0 +1,182 @@ +{ + "keys": {}, + "groups": { + "mainwindow": { + "keys": { + "geometry": { + "repr(QByteArray.hex)": "QtCore.QByteArray(b'01d9d0cb0003000000000042000000200000077f0000043700000042000000450000077f000004370000000000000000078000000042000000450000077f00000437')", + "type": "repr(QByteArray.hex)", + "pretty-print": " B 7 B E 7 B E 7" + }, + "state": { + "repr(QByteArray.hex)": "QtCore.QByteArray(b'000000ff00000000fd00000001000000030000073e000003c9fc0100000002fb0000005a007200710074005f0069006d006100670065005f0076006900650077005f005f0049006d0061006700650056006900650077005f005f0031005f005f0049006d006100670065005600690065007700570069006400670065007401000000000000039b000000be00fffffffb0000005a007200710074005f0069006d006100670065005f0076006900650077005f005f0049006d0061006700650056006900650077005f005f0032005f005f0049006d006100670065005600690065007700570069006400670065007401000003a10000039d000000d300ffffff0000073e0000000000000004000000040000000800000008fc00000001000000030000000100000036004d0069006e0069006d0069007a006500640044006f0063006b00570069006400670065007400730054006f006f006c0062006100720000000000ffffffff0000000000000000')", + "type": "repr(QByteArray.hex)", + "pretty-print": " Zrqt_image_view__ImageView__1__ImageViewWidget Zrqt_image_view__ImageView__2__ImageViewWidget 6MinimizedDockWidgetsToolbar " + } + }, + "groups": { + "toolbar_areas": { + "keys": { + "MinimizedDockWidgetsToolbar": { + "repr": "8", + "type": "repr" + } + }, + "groups": {} + } + } + }, + "pluginmanager": { + "keys": { + "running-plugins": { + "repr": "{'rqt_image_view/ImageView': [1, 2]}", + "type": "repr" + } + }, + "groups": { + "plugin__rqt_image_view__ImageView__1": { + "keys": {}, + "groups": { + "dock_widget__ImageViewWidget": { + "keys": { + "dock_widget_title": { + "repr": "'Image View'", + "type": "repr" + }, + "dockable": { + "repr": "True", + "type": "repr" + }, + "parent": { + "repr": "None", + "type": "repr" + } + }, + "groups": {} + }, + "plugin": { + "keys": { + "color_scheme": { + "repr": "-1", + "type": "repr" + }, + "dynamic_range": { + "repr": "False", + "type": "repr" + }, + "max_range": { + "repr": "10.0", + "type": "repr" + }, + "mouse_pub_topic": { + "repr": "'/dummy_image_mouse_left'", + "type": "repr" + }, + "num_gridlines": { + "repr": "0", + "type": "repr" + }, + "publish_click_location": { + "repr": "False", + "type": "repr" + }, + "rotate": { + "repr": "0", + "type": "repr" + }, + "smooth_image": { + "repr": "False", + "type": "repr" + }, + "toolbar_hidden": { + "repr": "False", + "type": "repr" + }, + "topic": { + "repr": "'/dummy_image'", + "type": "repr" + }, + "zoom1": { + "repr": "False", + "type": "repr" + } + }, + "groups": {} + } + } + }, + "plugin__rqt_image_view__ImageView__2": { + "keys": {}, + "groups": { + "dock_widget__ImageViewWidget": { + "keys": { + "dock_widget_title": { + "repr": "'Image View (2)'", + "type": "repr" + }, + "dockable": { + "repr": "True", + "type": "repr" + }, + "parent": { + "repr": "None", + "type": "repr" + } + }, + "groups": {} + }, + "plugin": { + "keys": { + "color_scheme": { + "repr": "-1", + "type": "repr" + }, + "dynamic_range": { + "repr": "False", + "type": "repr" + }, + "max_range": { + "repr": "10.0", + "type": "repr" + }, + "mouse_pub_topic": { + "repr": "'/nmspc1/edited_image_out_mouse_left'", + "type": "repr" + }, + "num_gridlines": { + "repr": "0", + "type": "repr" + }, + "publish_click_location": { + "repr": "False", + "type": "repr" + }, + "rotate": { + "repr": "0", + "type": "repr" + }, + "smooth_image": { + "repr": "False", + "type": "repr" + }, + "toolbar_hidden": { + "repr": "False", + "type": "repr" + }, + "topic": { + "repr": "'/nmspc1/edited_image_out'", + "type": "repr" + }, + "zoom1": { + "repr": "False", + "type": "repr" + } + }, + "groups": {} + } + } + } + } + } + } +} \ No newline at end of file From 3f2b3971ca9e2d40b10b2ff233fb8ff56444b495 Mon Sep 17 00:00:00 2001 From: Afzal Ahmad Date: Mon, 17 Mar 2025 18:13:50 +0100 Subject: [PATCH 23/27] added: a python module to help with launch file generation --- launch_helper/launch_helper/__init__.py | 0 launch_helper/launch_helper/dummy_node.py | 22 ++++ launch_helper/launch_helper/helper.py | 135 ++++++++++++++++++++++ launch_helper/package.xml | 16 +++ launch_helper/resource/launch_helper | 0 launch_helper/setup.cfg | 4 + launch_helper/setup.py | 27 +++++ ros2_examples/launch/mrs_launch.py | 14 +++ 8 files changed, 218 insertions(+) create mode 100644 launch_helper/launch_helper/__init__.py create mode 100644 launch_helper/launch_helper/dummy_node.py create mode 100644 launch_helper/launch_helper/helper.py create mode 100644 launch_helper/package.xml create mode 100644 launch_helper/resource/launch_helper create mode 100644 launch_helper/setup.cfg create mode 100644 launch_helper/setup.py create mode 100755 ros2_examples/launch/mrs_launch.py diff --git a/launch_helper/launch_helper/__init__.py b/launch_helper/launch_helper/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/launch_helper/launch_helper/dummy_node.py b/launch_helper/launch_helper/dummy_node.py new file mode 100644 index 0000000..11e8fe0 --- /dev/null +++ b/launch_helper/launch_helper/dummy_node.py @@ -0,0 +1,22 @@ +#!/usr/bin/env python3 +import rclpy +from rclpy.node import Node +import ros2_lib +import ros2_lib.launch_helper + +def main(args=None): + rclpy.init(args=args) + + dummy_node = Node('dummy_node') + ld = ros2_lib.launch_helper.LaunchHelper() + + rclpy.spin(dummy_node) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + dummy_publisher.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/launch_helper/launch_helper/helper.py b/launch_helper/launch_helper/helper.py new file mode 100644 index 0000000..4583c0d --- /dev/null +++ b/launch_helper/launch_helper/helper.py @@ -0,0 +1,135 @@ +import os +from pathlib import Path +import launch +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode +from launch.actions import DeclareLaunchArgument +from launch.substitutions import ( + LaunchConfiguration, + IfElseSubstitution, + PythonExpression, + PathJoinSubstitution, + EnvironmentVariable, +) + +from ament_index_python.packages import get_package_share_directory + +class LaunchHelper: + + def __init__(self, pkg_name, node_name, node_type, namespace=None, thread_count=2): + self.ld = launch.LaunchDescription() + + self.pkg_name = pkg_name + + self.pkg_share_path = get_package_share_directory(pkg_name) + self.namespace = namespace if namespace is not None else pkg_name + self.node_name = node_name + self.plugin_type = node_type + self.thread_count = thread_count + self.parameters = list() + self.remaps = list() + + print(f'\nCreating a launch file') + print(f'Package name: [{self.pkg_name}] and namespace: [{self.namespace}]') + print(f'Node name: [{self.node_name}] of type: [{self.plugin_type}]') + + + + def add_launch_arg(self, name, default, *, description='Default description'): + arg = LaunchConfiguration(name) + + # this adds the args to the list of args available for this launch files + # these args can be listed at runtime using -s flag + # default_value is required to if the arg is supposed to be optional at launch time + self.ld.add_action(DeclareLaunchArgument( + name, + default_value=default, + description=description, + )) + + self.parameters.append({name: arg}) + print(f'Added launch arguement [{name}]') + + def add_custom_config(self, *, name='custom_config', default='Default value', description='Default description'): + custom_config = LaunchConfiguration(name) + + # this adds the args to the list of args available for this launch files + # these args can be listed at runtime using -s flag + # default_value is required to if the arg is supposed to be optional at launch time + self.ld.add_action(DeclareLaunchArgument( + name, + default_value=default, + description=description, + )) + + # behaviour: + # custom_config == "" => custom_config: "" + # custom_config == "/" => custom_config: "/" + # custom_config == "" => custom_config: "$(pwd)/" + custom_config = IfElseSubstitution( + condition=PythonExpression(['"', custom_config, '" != "" and ', 'not "', custom_config, '".startswith("/")']), + if_value=PathJoinSubstitution([EnvironmentVariable('PWD'), custom_config]), + else_value=custom_config + ) + + self.parameters.append({name: custom_config}) + print(f'Added arguement [custom_config]') + + def add_param_file(self, yaml_file): + if not os.path.exists(yaml_file): + raise ValueError(f'[{yaml_file}] file does not exist') + + self.parameters.append(yaml_file) + print(f'Added params from file {yaml_file}') + + def add_param_files_from_directory(self, directory=None): + resolved_directory = directory if directory is not None else Path(self.pkg_share_path + '/config') + if not os.path.exists(resolved_directory): + raise ValueError(f'[{resolved_directory}] does not exist') + + self.parameters.extend(list(resolved_directory.rglob('*.yaml'))) + print(f'Added all the params files found in the path {resolved_directory}') + + def add_remapping(self, *, remap_from, remap_to): + self.remaps.append((remap_from, remap_to)) + print(f'Added remapping from: {remap_from} to: {remap_to}') + + def print_description_properties(self): + print('\n') + print(f'Container name: {self.pkg_name}_container') + print(f'Using threads: {self.thread_count}') + + print(f'Node name: {self.node_name}') + print(f'Parameters: {self.parameters}') + print(f'Remappings: {self.remaps}') + print('\n') + + def get_launch_description(self): + + self.ld.add_action(ComposableNodeContainer( + namespace=self.namespace, + name=self.pkg_name + '_container', + package='rclcpp_components', + executable='component_container_mt', + output="screen", + parameters=[ + {'thread_num': self.thread_count}, + ], + + composable_node_descriptions=[ + + ComposableNode( + + package=self.pkg_name, + plugin=self.plugin_type, + namespace=self.namespace, + name=self.node_name, + parameters=self.parameters, + remappings=self.remaps, + ) + ], + )) + + self.print_description_properties() + + return self.ld \ No newline at end of file diff --git a/launch_helper/package.xml b/launch_helper/package.xml new file mode 100644 index 0000000..9781388 --- /dev/null +++ b/launch_helper/package.xml @@ -0,0 +1,16 @@ + + + + + launch_helper + 1.0.0 + ROS2 library package example + Afzal Ahmad + + BSD 3-Clause + + + ament_python + + + diff --git a/launch_helper/resource/launch_helper b/launch_helper/resource/launch_helper new file mode 100644 index 0000000..e69de29 diff --git a/launch_helper/setup.cfg b/launch_helper/setup.cfg new file mode 100644 index 0000000..0ac7f38 --- /dev/null +++ b/launch_helper/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/launch_helper +[install] +install_scripts=$base/lib/launch_helper diff --git a/launch_helper/setup.py b/launch_helper/setup.py new file mode 100644 index 0000000..57e65d6 --- /dev/null +++ b/launch_helper/setup.py @@ -0,0 +1,27 @@ +from setuptools import find_packages, setup + +package_name = 'launch_helper' + +setup( + name=package_name, + version='1.0.0', + # packages=find_packages(exclude=['test']), + packages=[package_name], + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='Afzal Ahmad', + maintainer_email='afzalhmd14@gmail.com', + description='ROS2 launch-helper library', + license='BSD 3-Clause', + py_modules=['launch_helper.helper'], + entry_points={ + 'console_scripts': [ + 'dummy_node = launch_helper.dummy_node:main', + ], + }, +) diff --git a/ros2_examples/launch/mrs_launch.py b/ros2_examples/launch/mrs_launch.py new file mode 100755 index 0000000..56e2be7 --- /dev/null +++ b/ros2_examples/launch/mrs_launch.py @@ -0,0 +1,14 @@ +#!/usr/bin/env python3 +import os +from launch_helper.helper import LaunchHelper + +def generate_launch_description(): + + launch_desc = LaunchHelper(pkg_name='ros2_examples', node_name='param_node', node_type='ros2_examples::ParamsExample', namespace='nmmspc') + + launch_desc.add_launch_arg('env_var', os.getenv('ENV_VAR', 'dummy')) + launch_desc.add_custom_config(default='') + launch_desc.add_param_files_from_directory() + launch_desc.add_remapping(remap_from='~/dummy_out', remap_to='/dummy_out') + + return launch_desc.get_launch_description() From e263ce57ae871ff4bcfeaf84ea7b060c38b134fd Mon Sep 17 00:00:00 2001 From: Tomas Baca Date: Fri, 21 Mar 2025 09:16:21 +0100 Subject: [PATCH 24/27] minor changes --- .../example_plugin_manager/CMakeLists.txt | 4 +- .../src/example_plugin_manager.cpp | 2 +- .../example_plugins/CMakeLists.txt | 4 +- .../example_plugins/plugins.xml | 4 +- ros2_examples/CMakeLists.txt | 52 ++++++++++++------- ros2_lib/CMakeLists.txt | 28 +++++----- ros2_lib/package.xml | 1 + sub_pub_torture_test/CMakeLists.txt | 5 +- 8 files changed, 59 insertions(+), 41 deletions(-) diff --git a/extended_examples/pluginlib_example/example_plugin_manager/CMakeLists.txt b/extended_examples/pluginlib_example/example_plugin_manager/CMakeLists.txt index 525ffa8..bb15a36 100644 --- a/extended_examples/pluginlib_example/example_plugin_manager/CMakeLists.txt +++ b/extended_examples/pluginlib_example/example_plugin_manager/CMakeLists.txt @@ -29,6 +29,7 @@ include_directories( add_library(example_plugin_manager SHARED src/example_plugin_manager.cpp ) + # ament manages cmake related variables and dependency search (similar to catkin in ROS1) ament_target_dependencies(example_plugin_manager rclcpp @@ -69,4 +70,5 @@ install(DIRECTORY config # this will export the include-related variables for use by other libraries # this is needed if you wish to include the headers from this pkg into downstream libraries ament_export_include_directories("include") -ament_package() \ No newline at end of file + +ament_package() diff --git a/extended_examples/pluginlib_example/example_plugin_manager/src/example_plugin_manager.cpp b/extended_examples/pluginlib_example/example_plugin_manager/src/example_plugin_manager.cpp index 06c6476..0517605 100644 --- a/extended_examples/pluginlib_example/example_plugin_manager/src/example_plugin_manager.cpp +++ b/extended_examples/pluginlib_example/example_plugin_manager/src/example_plugin_manager.cpp @@ -281,4 +281,4 @@ double ExamplePluginManager::vectorNorm(const Eigen::Vector3d& input) { // 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(example_plugin_manager::ExamplePluginManager) \ No newline at end of file +RCLCPP_COMPONENTS_REGISTER_NODE(example_plugin_manager::ExamplePluginManager) diff --git a/extended_examples/pluginlib_example/example_plugins/CMakeLists.txt b/extended_examples/pluginlib_example/example_plugins/CMakeLists.txt index 3e8ee99..e30d15f 100644 --- a/extended_examples/pluginlib_example/example_plugins/CMakeLists.txt +++ b/extended_examples/pluginlib_example/example_plugins/CMakeLists.txt @@ -45,7 +45,7 @@ ament_target_dependencies(${PROJECT_NAME} PUBLIC ## -------------------------------------------------------------- install( - TARGETS ${PROJECT_NAME} # list all libraries to be installed and exported out from this pkg + TARGETS ${PROJECT_NAME} # list all libraries to be installed and exported out from this EXPORT export_${PROJECT_NAME} # a single export name is enough to export all libraries from this pkg ARCHIVE DESTINATION lib LIBRARY DESTINATION lib @@ -59,4 +59,4 @@ install(DIRECTORY config # this is ESSENTIAL for the plugin to be discovered dynamically ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) -ament_package() \ No newline at end of file +ament_package() diff --git a/extended_examples/pluginlib_example/example_plugins/plugins.xml b/extended_examples/pluginlib_example/example_plugins/plugins.xml index 7d0567f..d4cdd7e 100644 --- a/extended_examples/pluginlib_example/example_plugins/plugins.xml +++ b/extended_examples/pluginlib_example/example_plugins/plugins.xml @@ -1,6 +1,6 @@ - + This is ExamplePlugin. - \ No newline at end of file + diff --git a/ros2_examples/CMakeLists.txt b/ros2_examples/CMakeLists.txt index 4231591..d6bf074 100644 --- a/ros2_examples/CMakeLists.txt +++ b/ros2_examples/CMakeLists.txt @@ -3,7 +3,7 @@ project(ros2_examples) # set the correct standards set(CMAKE_C_STANDARD 99) -set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD 20) set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_EXPORT_COMPILE_COMMANDS ON) @@ -12,17 +12,36 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_components REQUIRED) -find_package(std_msgs REQUIRED) -find_package(std_srvs REQUIRED) -find_package(tf2_ros REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(ros2_lib REQUIRED) +set(DEPENDENCIES + ament_cmake + rclcpp + rclcpp_components + std_msgs + std_srvs + tf2_ros + geometry_msgs + ros2_lib +) + +foreach(DEP IN LISTS DEPENDENCIES) + find_package(${DEP} REQUIRED) +endforeach() + +set(LIBRARIES + params_example + subscriber_example + publisher_example + service_server_example + service_client_example + timer_example + tf2_broadcaster_example + tf2_listener_example + clock_consumer + ) include_directories( include + ${colcon_INCLUDE_DIRS} ) ## -------------------------------------------------------------- @@ -194,16 +213,8 @@ endif() ## | install | ## -------------------------------------------------------------- -install(TARGETS - params_example - subscriber_example - publisher_example - service_server_example - service_client_example - timer_example - tf2_broadcaster_example - tf2_listener_example - clock_consumer +install( + TARGETS ${LIBRARIES} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin @@ -224,4 +235,7 @@ install(DIRECTORY config # this will export the include-related variables for use by other libraries # this is needed if you wish to include the headers from this pkg into downstream libraries ament_export_include_directories("include") + +ament_export_dependencies(${DEPENDENCIES}) + ament_package() diff --git a/ros2_lib/CMakeLists.txt b/ros2_lib/CMakeLists.txt index 63ae195..78c8ce4 100644 --- a/ros2_lib/CMakeLists.txt +++ b/ros2_lib/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 3.12) project(ros2_lib) -set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD 20) set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_EXPORT_COMPILE_COMMANDS ON) @@ -9,25 +9,27 @@ add_definitions("-Wall") add_definitions("-Wextra") add_definitions("-Wpedantic") -find_package(ament_cmake REQUIRED) +set(DEPENDENCIES + ament_cmake + rclcpp + rclcpp_components + Eigen3 +) -find_package(rclcpp REQUIRED) -find_package(rclcpp_components REQUIRED) + +foreach(DEP IN LISTS DEPENDENCIES) + find_package(${DEP} REQUIRED) +endforeach() include_directories( include - ${rclcpp_INCLUDE_DIRS} - ${Eigen_INCLUDE_DIRS} + ${colcon_INCLUDE_DIRS} ) ## -------------------------------------------------------------- ## | Compile | ## -------------------------------------------------------------- -set(DEPENDENCIES - rclcpp -) - set(LIBRARIES Ros2Lib_Adder ) @@ -49,8 +51,6 @@ ament_target_dependencies(Ros2Lib_Adder if(BUILD_TESTING) - message(WARNING "Testing enabled.") - add_subdirectory(test) endif() @@ -63,8 +63,8 @@ ament_export_libraries( ${LIBRARIES} ) -install(TARGETS - Ros2Lib_Adder +install( + TARGETS ${LIBRARIES} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin diff --git a/ros2_lib/package.xml b/ros2_lib/package.xml index e3a72a6..b373198 100644 --- a/ros2_lib/package.xml +++ b/ros2_lib/package.xml @@ -15,6 +15,7 @@ launch_ros ament_cmake_ros + eigen launch launch_ros launch_testing diff --git a/sub_pub_torture_test/CMakeLists.txt b/sub_pub_torture_test/CMakeLists.txt index 2ddd13f..05ac610 100644 --- a/sub_pub_torture_test/CMakeLists.txt +++ b/sub_pub_torture_test/CMakeLists.txt @@ -66,8 +66,8 @@ rclcpp_components_register_nodes(publishers PLUGIN "sub_pub_torture_test::Publis ## | install | ## -------------------------------------------------------------- -install(TARGETS - ${LIBRARIES} +install( + TARGETS ${LIBRARIES} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin @@ -88,4 +88,5 @@ install(DIRECTORY config # this will export the include-related variables for use by other libraries # this is needed if you wish to include the headers from this pkg into downstream libraries ament_export_include_directories("include") + ament_package() From 0fb0b6ebe1b0c86548c970be96d013088512793d Mon Sep 17 00:00:00 2001 From: Tomas Baca Date: Fri, 21 Mar 2025 11:32:25 +0100 Subject: [PATCH 25/27] refactored the pluginlib examples --- .../example_plugin_manager/CMakeLists.txt | 75 ++++++++++------- .../launch/example_plugin_manager.py | 4 +- .../src/example_plugin_manager.cpp | 29 +++---- .../example_plugins/CMakeLists.txt | 81 +++++++++++++------ .../example_plugins/plugins.xml | 2 +- ros2_examples/CMakeLists.txt | 21 +++-- 6 files changed, 132 insertions(+), 80 deletions(-) diff --git a/extended_examples/pluginlib_example/example_plugin_manager/CMakeLists.txt b/extended_examples/pluginlib_example/example_plugin_manager/CMakeLists.txt index bb15a36..ba0a488 100644 --- a/extended_examples/pluginlib_example/example_plugin_manager/CMakeLists.txt +++ b/extended_examples/pluginlib_example/example_plugin_manager/CMakeLists.txt @@ -10,65 +10,84 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -find_package(ament_cmake REQUIRED) -find_package(ament_cmake_ros REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_components REQUIRED) -find_package(pluginlib REQUIRED) -find_package(Eigen3 REQUIRED) -find_package(ros2_examples REQUIRED) +set(DEPENDENCIES + ament_cmake + ament_cmake_ros + rclcpp + rclcpp_components + pluginlib + Eigen3 + ros2_examples +) -set(Eigen_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIRS}) -set(Eigen_LIBRARIES ${Eigen_LIBRARIES}) +set(LIBRARIES + ExamplePluginManager_PluginManager + ) + +foreach(DEP IN LISTS DEPENDENCIES) + find_package(${DEP} REQUIRED) +endforeach() include_directories( include + ${rclcpp_INCLUDE_DIRS} + ${ros2_examples_INCLUDE_DIRS} ) # ExamplePluginManager -add_library(example_plugin_manager SHARED +add_library(ExamplePluginManager_PluginManager SHARED src/example_plugin_manager.cpp ) # ament manages cmake related variables and dependency search (similar to catkin in ROS1) -ament_target_dependencies(example_plugin_manager - rclcpp - rclcpp_components - pluginlib - Eigen3 - ros2_examples +ament_target_dependencies(ExamplePluginManager_PluginManager + ${DEPENDENCIES} ) # each component (nodelet) needs to be registered to make it discoverable at runtime -rclcpp_components_register_nodes(example_plugin_manager "example_plugin_manager::ExamplePluginManager") +rclcpp_components_register_nodes(ExamplePluginManager_PluginManager "example_plugin_manager::ExamplePluginManager") ## -------------------------------------------------------------- ## | Install | ## -------------------------------------------------------------- -install(TARGETS - example_plugin_manager +ament_export_libraries( + ${LIBRARIES} +) + +install( + TARGETS ${LIBRARIES} + EXPORT export_${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin - ) +) -# this will copy the header files for use by other libraries install( - DIRECTORY include/ - DESTINATION include + DIRECTORY include + DESTINATION . ) -install(DIRECTORY launch +install( + DIRECTORY launch DESTINATION share/${PROJECT_NAME} ) -install(DIRECTORY config +install( + DIRECTORY config DESTINATION share/${PROJECT_NAME} ) -# this will export the include-related variables for use by other libraries -# this is needed if you wish to include the headers from this pkg into downstream libraries -ament_export_include_directories("include") +ament_export_include_directories( + include +) + +ament_export_targets( + export_${PROJECT_NAME} HAS_LIBRARY_TARGET +) + +ament_export_dependencies( + ${DEPENDENCIES} +) ament_package() diff --git a/extended_examples/pluginlib_example/example_plugin_manager/launch/example_plugin_manager.py b/extended_examples/pluginlib_example/example_plugin_manager/launch/example_plugin_manager.py index deb4390..b571b8a 100755 --- a/extended_examples/pluginlib_example/example_plugin_manager/launch/example_plugin_manager.py +++ b/extended_examples/pluginlib_example/example_plugin_manager/launch/example_plugin_manager.py @@ -7,8 +7,6 @@ from ament_index_python.packages import get_package_share_directory - - def generate_launch_description(): ld = launch.LaunchDescription() @@ -53,4 +51,4 @@ def generate_launch_description(): output="screen", )) - return ld \ No newline at end of file + return ld diff --git a/extended_examples/pluginlib_example/example_plugin_manager/src/example_plugin_manager.cpp b/extended_examples/pluginlib_example/example_plugin_manager/src/example_plugin_manager.cpp index 0517605..1c44044 100644 --- a/extended_examples/pluginlib_example/example_plugin_manager/src/example_plugin_manager.cpp +++ b/extended_examples/pluginlib_example/example_plugin_manager/src/example_plugin_manager.cpp @@ -38,16 +38,16 @@ PluginParams::PluginParams(const std::string& address, const std::string& name_s class ExamplePluginManager : public rclcpp::Node { public: - ExamplePluginManager(const rclcpp::NodeOptions &options); + ExamplePluginManager(const rclcpp::NodeOptions& options); private: - bool is_initialized_ = false; - std::string _uav_type_ = ""; + bool is_initialized_ = false; + std::string _uav_type_ = ""; // | ---------------------- update timer ---------------------- | rclcpp::TimerBase::SharedPtr timer_update_; - double _rate_timer_update_; + double _rate_timer_update_; // | -------- an object we want to share to our plugins ------- | @@ -62,7 +62,7 @@ class ExamplePluginManager : public rclcpp::Node { std::unique_ptr> plugin_loader_; // pluginlib loader std::vector _plugin_names_; std::map plugins_; // map between plugin names and plugin params - std::vector> plugin_list_; // list of plugins, routines are callable from this + std::vector> plugin_list_; // list of plugins, routines are callable from this // std::mutex mutex_plugins_; std::string _initial_plugin_name_; @@ -83,7 +83,7 @@ class ExamplePluginManager : public rclcpp::Node { /* onInit() //{ */ -ExamplePluginManager::ExamplePluginManager(const rclcpp::NodeOptions &options) : Node("example_plugin_manager", options) { +ExamplePluginManager::ExamplePluginManager(const rclcpp::NodeOptions& options) : Node("example_plugin_manager", options) { RCLCPP_INFO(get_logger(), "[ExamplePluginManager]: initializing"); @@ -91,9 +91,9 @@ ExamplePluginManager::ExamplePluginManager(const rclcpp::NodeOptions &options) bool loaded_successfully = true; - loaded_successfully &= utils::parse_param("uav_type", _uav_type_, *this); - loaded_successfully &= utils::parse_param("update_timer_rate", _rate_timer_update_, *this); - loaded_successfully &= utils::parse_param("initial_plugin", _initial_plugin_name_, *this); + loaded_successfully &= utils::load_param("uav_type", _uav_type_, *this); + loaded_successfully &= utils::load_param("update_timer_rate", _rate_timer_update_, *this); + loaded_successfully &= utils::load_param("initial_plugin", _initial_plugin_name_, *this); if (!loaded_successfully) { RCLCPP_ERROR_STREAM(get_logger(), "Could not load all non-optional parameters. Shutting down."); @@ -119,7 +119,7 @@ ExamplePluginManager::ExamplePluginManager(const rclcpp::NodeOptions &options) // | load the plugins | // -------------------------------------------------------------- - loaded_successfully &= utils::parse_param("plugins", _plugin_names_ , *this); + loaded_successfully &= utils::load_param("plugins", _plugin_names_, *this); if (!loaded_successfully) { RCLCPP_ERROR_STREAM(get_logger(), "Could not load all non-optional parameters. Shutting down."); @@ -139,9 +139,9 @@ ExamplePluginManager::ExamplePluginManager(const rclcpp::NodeOptions &options) double some_property; // nested params are separated by '.' instead of '/' - loaded_successfully &= utils::parse_param(plugin_name + ".address", address, *this); - loaded_successfully &= utils::parse_param(plugin_name + ".name_space", name_space, *this); - loaded_successfully &= utils::parse_param(plugin_name + ".some_property", some_property, *this); + loaded_successfully &= utils::load_param(plugin_name + ".address", address, *this); + loaded_successfully &= utils::load_param(plugin_name + ".name_space", name_space, *this); + loaded_successfully &= utils::load_param(plugin_name + ".some_property", some_property, *this); if (!loaded_successfully) { RCLCPP_ERROR_STREAM(get_logger(), "Could not load all non-optional parameters. Shutting down."); @@ -213,7 +213,8 @@ ExamplePluginManager::ExamplePluginManager(const rclcpp::NodeOptions &options) // | ---------- activate the first plugin on the list --------- | - RCLCPP_INFO(get_logger(), "[ExamplePluginManager]: activating plugin with idx %d on the list (named: %s)", _initial_plugin_idx_, _plugin_names_.at(_initial_plugin_idx_).c_str()); + RCLCPP_INFO(get_logger(), "[ExamplePluginManager]: activating plugin with idx %d on the list (named: %s)", _initial_plugin_idx_, + _plugin_names_.at(_initial_plugin_idx_).c_str()); int some_activation_input_to_plugin = 1234; diff --git a/extended_examples/pluginlib_example/example_plugins/CMakeLists.txt b/extended_examples/pluginlib_example/example_plugins/CMakeLists.txt index e30d15f..839115b 100644 --- a/extended_examples/pluginlib_example/example_plugins/CMakeLists.txt +++ b/extended_examples/pluginlib_example/example_plugins/CMakeLists.txt @@ -10,28 +10,9 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -find_package(ament_cmake REQUIRED) -find_package(ament_cmake_ros REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_components REQUIRED) -find_package(Eigen3 REQUIRED) -find_package(pluginlib REQUIRED) -find_package(ros2_examples REQUIRED) -find_package(example_plugin_manager REQUIRED) - -# export the plugins listed in plugins.xml to be discovered by the manager -pluginlib_export_plugin_description_file(example_plugin_manager plugins.xml) - -set(Eigen_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIRS}) -set(Eigen_LIBRARIES ${Eigen_LIBRARIES}) - -# ${PROJECT_NAME} -add_library(${PROJECT_NAME} SHARED - src/example_plugin.cpp - ) - -# ament manages cmake related variables and dependency search (similar to catkin in ROS1) -ament_target_dependencies(${PROJECT_NAME} PUBLIC +set(DEPENDENCIES + ament_cmake + ament_cmake_ros rclcpp rclcpp_components Eigen3 @@ -40,23 +21,71 @@ ament_target_dependencies(${PROJECT_NAME} PUBLIC example_plugin_manager ) +set(LIBRARIES + ExamplePlugin_Plugin + ) + +foreach(DEP IN LISTS DEPENDENCIES) + find_package(${DEP} REQUIRED) +endforeach() + +include_directories( + include + ${rclcpp_INCLUDE_DIRS} + ${example_plugin_manager_INCLUDE_DIRS} + ) + +## -------------------------------------------------------------- +## | Compile | +## -------------------------------------------------------------- + +# Example plugin + +add_library(ExamplePlugin_Plugin SHARED + src/example_plugin.cpp + ) + +# ament manages cmake related variables and dependency search (similar to catkin in ROS1) +ament_target_dependencies(ExamplePlugin_Plugin + ${DEPENDENCIES} +) + +## -------------------------------------------------------------- +## | Export plugins | +## -------------------------------------------------------------- + +# , +pluginlib_export_plugin_description_file(example_plugin_manager plugins.xml) + ## -------------------------------------------------------------- ## | Install | ## -------------------------------------------------------------- +ament_export_libraries( + ${LIBRARIES} +) + install( - TARGETS ${PROJECT_NAME} # list all libraries to be installed and exported out from this - EXPORT export_${PROJECT_NAME} # a single export name is enough to export all libraries from this pkg + TARGETS ${LIBRARIES} + EXPORT export_${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin ) -install(DIRECTORY config +install( + DIRECTORY config DESTINATION share/${PROJECT_NAME} ) + # export the installed library and related info to the cmake system # this is ESSENTIAL for the plugin to be discovered dynamically -ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_targets( + export_${PROJECT_NAME} HAS_LIBRARY_TARGET +) + +ament_export_dependencies( + ${DEPENDENCIES} +) ament_package() diff --git a/extended_examples/pluginlib_example/example_plugins/plugins.xml b/extended_examples/pluginlib_example/example_plugins/plugins.xml index d4cdd7e..edca5fb 100644 --- a/extended_examples/pluginlib_example/example_plugins/plugins.xml +++ b/extended_examples/pluginlib_example/example_plugins/plugins.xml @@ -1,4 +1,4 @@ - + This is ExamplePlugin. diff --git a/ros2_examples/CMakeLists.txt b/ros2_examples/CMakeLists.txt index d6bf074..49cff69 100644 --- a/ros2_examples/CMakeLists.txt +++ b/ros2_examples/CMakeLists.txt @@ -220,22 +220,27 @@ install( RUNTIME DESTINATION bin ) -install(DIRECTORY include/ - DESTINATION include +install( + DIRECTORY include + DESTINATION . ) -install(DIRECTORY launch +install( + DIRECTORY launch DESTINATION share/${PROJECT_NAME} ) -install(DIRECTORY config +install( + DIRECTORY config DESTINATION share/${PROJECT_NAME} ) -# this will export the include-related variables for use by other libraries -# this is needed if you wish to include the headers from this pkg into downstream libraries -ament_export_include_directories("include") +ament_export_include_directories( + include +) -ament_export_dependencies(${DEPENDENCIES}) +ament_export_dependencies( + ${DEPENDENCIES} +) ament_package() From 059b6f3ae7f2de9457798dc305f0c3e3c48be2db Mon Sep 17 00:00:00 2001 From: Tomas Baca Date: Mon, 24 Mar 2025 15:27:13 +0100 Subject: [PATCH 26/27] updated cmakelists, removed optimizaation flags --- .../pluginlib_example/example_plugin_manager/CMakeLists.txt | 3 +-- .../pluginlib_example/example_plugins/CMakeLists.txt | 3 +-- ros2_examples/CMakeLists.txt | 1 - ros2_lib/CMakeLists.txt | 2 -- sub_pub_torture_test/CMakeLists.txt | 3 --- 5 files changed, 2 insertions(+), 10 deletions(-) diff --git a/extended_examples/pluginlib_example/example_plugin_manager/CMakeLists.txt b/extended_examples/pluginlib_example/example_plugin_manager/CMakeLists.txt index ba0a488..554fb19 100644 --- a/extended_examples/pluginlib_example/example_plugin_manager/CMakeLists.txt +++ b/extended_examples/pluginlib_example/example_plugin_manager/CMakeLists.txt @@ -1,9 +1,8 @@ cmake_minimum_required(VERSION 3.5) project(example_plugin_manager) -set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD 20) set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_EXPORT_COMPILE_COMMANDS ON) # set the compile options to show code warnings if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") diff --git a/extended_examples/pluginlib_example/example_plugins/CMakeLists.txt b/extended_examples/pluginlib_example/example_plugins/CMakeLists.txt index 839115b..3ba5435 100644 --- a/extended_examples/pluginlib_example/example_plugins/CMakeLists.txt +++ b/extended_examples/pluginlib_example/example_plugins/CMakeLists.txt @@ -1,9 +1,8 @@ cmake_minimum_required(VERSION 3.5) project(example_plugins) -set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD 20) set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_EXPORT_COMPILE_COMMANDS ON) # set the compile options to show code warnings if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") diff --git a/ros2_examples/CMakeLists.txt b/ros2_examples/CMakeLists.txt index 49cff69..06abe90 100644 --- a/ros2_examples/CMakeLists.txt +++ b/ros2_examples/CMakeLists.txt @@ -5,7 +5,6 @@ project(ros2_examples) set(CMAKE_C_STANDARD 99) set(CMAKE_CXX_STANDARD 20) set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_EXPORT_COMPILE_COMMANDS ON) # set the compile options to show code warnings if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") diff --git a/ros2_lib/CMakeLists.txt b/ros2_lib/CMakeLists.txt index 78c8ce4..fd96928 100644 --- a/ros2_lib/CMakeLists.txt +++ b/ros2_lib/CMakeLists.txt @@ -3,7 +3,6 @@ project(ros2_lib) set(CMAKE_CXX_STANDARD 20) set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_EXPORT_COMPILE_COMMANDS ON) add_definitions("-Wall") add_definitions("-Wextra") @@ -16,7 +15,6 @@ set(DEPENDENCIES Eigen3 ) - foreach(DEP IN LISTS DEPENDENCIES) find_package(${DEP} REQUIRED) endforeach() diff --git a/sub_pub_torture_test/CMakeLists.txt b/sub_pub_torture_test/CMakeLists.txt index 05ac610..59a5e70 100644 --- a/sub_pub_torture_test/CMakeLists.txt +++ b/sub_pub_torture_test/CMakeLists.txt @@ -5,15 +5,12 @@ project(sub_pub_torture_test) set(CMAKE_C_STANDARD 99) set(CMAKE_CXX_STANDARD 20) set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_EXPORT_COMPILE_COMMANDS ON) # set the compile options to show code warnings if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -add_compile_options(-O2) - set(DEPENDENCIES ament_cmake rclcpp From d1aac9612c49db9a4575f2d96045a98eb2e7731a Mon Sep 17 00:00:00 2001 From: Afzal Ahmad Date: Thu, 27 Mar 2025 15:55:09 +0100 Subject: [PATCH 27/27] cleaned: image transport example --- .../example_image_transport/CMakeLists.txt | 47 ++++++++++++------- .../config/default.yaml | 2 + .../launch/example_image_transport.py | 8 ++-- .../example_image_transport/package.xml | 2 +- .../src/example_image_transport.cpp | 13 ++--- 5 files changed, 41 insertions(+), 31 deletions(-) create mode 100644 extended_examples/vision_examples/example_image_transport/config/default.yaml diff --git a/extended_examples/vision_examples/example_image_transport/CMakeLists.txt b/extended_examples/vision_examples/example_image_transport/CMakeLists.txt index 61d4a19..d18d386 100644 --- a/extended_examples/vision_examples/example_image_transport/CMakeLists.txt +++ b/extended_examples/vision_examples/example_image_transport/CMakeLists.txt @@ -3,7 +3,7 @@ project(vision_examples) # set the correct standards set(CMAKE_C_STANDARD 99) -set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD 20) set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_EXPORT_COMPILE_COMMANDS ON) @@ -12,39 +12,52 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_components REQUIRED) -find_package(sensor_msgs REQUIRED) -find_package(cv_bridge REQUIRED) -find_package(image_transport REQUIRED) +set(DEPENDENCIES + ament_cmake + ament_cmake_ros + rclcpp + rclcpp_components + sensor_msgs + cv_bridge + image_transport +) + +set(LIBRARIES + ExampleImageTransport +) + +foreach(DEP IN LISTS DEPENDENCIES) + find_package(${DEP} REQUIRED) +endforeach() find_package(OpenCV REQUIRED COMPONENTS highgui imgcodecs imgproc videoio) -add_library(example_image_transport SHARED +include_directories( + include + ${rclcpp_INCLUDE_DIRS} + ${ros2_examples_INCLUDE_DIRS} + ) + +add_library(ExampleImageTransport SHARED src/example_image_transport.cpp ) -target_link_libraries(example_image_transport +target_link_libraries(ExampleImageTransport ${OpenCV_LIBRARIES} ) -ament_target_dependencies(example_image_transport - rclcpp - rclcpp_components - sensor_msgs - image_transport - cv_bridge +ament_target_dependencies(ExampleImageTransport + ${DEPENDENCIES} ) # each component (nodelet) needs to be registered to make it discoverable at runtime -rclcpp_components_register_nodes(example_image_transport PLUGIN "vision_examples::ExampleImageTransport" EXECUTABLE example_image_transport) +rclcpp_components_register_nodes(ExampleImageTransport PLUGIN "vision_examples::ExampleImageTransport" EXECUTABLE ExampleImageTransport) ## -------------------------------------------------------------- ## | Install | ## -------------------------------------------------------------- install(TARGETS - example_image_transport + ${LIBRARIES} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin diff --git a/extended_examples/vision_examples/example_image_transport/config/default.yaml b/extended_examples/vision_examples/example_image_transport/config/default.yaml new file mode 100644 index 0000000..30c0c93 --- /dev/null +++ b/extended_examples/vision_examples/example_image_transport/config/default.yaml @@ -0,0 +1,2 @@ +timer: + rate: 20.0 # Hz \ No newline at end of file diff --git a/extended_examples/vision_examples/example_image_transport/launch/example_image_transport.py b/extended_examples/vision_examples/example_image_transport/launch/example_image_transport.py index 1cfa9a0..247a26e 100644 --- a/extended_examples/vision_examples/example_image_transport/launch/example_image_transport.py +++ b/extended_examples/vision_examples/example_image_transport/launch/example_image_transport.py @@ -14,7 +14,7 @@ def generate_launch_description(): ld.add_action(ComposableNodeContainer( namespace = namespace, - name = 'component_publisher_example', + name = 'component_image_transport_example', package='rclcpp_components', executable='component_container_mt', @@ -28,9 +28,9 @@ def generate_launch_description(): namespace=namespace, name='example_image_transport', - # parameters=[ - # pkg_share_path + '/config/publisher_example.yaml', - # ], + parameters=[ + pkg_share_path + '/config/default.yaml', + ], remappings=[ # topics diff --git a/extended_examples/vision_examples/example_image_transport/package.xml b/extended_examples/vision_examples/example_image_transport/package.xml index 1ed1842..b610b4b 100644 --- a/extended_examples/vision_examples/example_image_transport/package.xml +++ b/extended_examples/vision_examples/example_image_transport/package.xml @@ -16,7 +16,7 @@ rclcpp rclcpp_components - senssor_msgs + sensor_msgs cv_bridge image_transport libopencv-dev diff --git a/extended_examples/vision_examples/example_image_transport/src/example_image_transport.cpp b/extended_examples/vision_examples/example_image_transport/src/example_image_transport.cpp index 5bf4dce..f06f640 100644 --- a/extended_examples/vision_examples/example_image_transport/src/example_image_transport.cpp +++ b/extended_examples/vision_examples/example_image_transport/src/example_image_transport.cpp @@ -47,14 +47,9 @@ ExampleImageTransport::ExampleImageTransport(rclcpp::NodeOptions options) : Node RCLCPP_INFO(get_logger(), "Initializing"); - { - std::scoped_lock lck(mutex_image_); - if(image_) { - RCLCPP_ERROR(get_logger(), "Image pointer is assigned before initialization, shutting down"); - rclcpp::shutdown(); - return; - } - } + this->declare_parameter("timer.rate", 2.0); + double rate = 0; + this->get_parameter("timer.rate", rate); auto sub_node = this->create_sub_node("image_transport"); image_transport::ImageTransport image_transporter(sub_node); @@ -67,7 +62,7 @@ ExampleImageTransport::ExampleImageTransport(rclcpp::NodeOptions options) : Node pub_edited_image_ = image_transporter.advertise("edited_image_out", 1); - timer_publisher_ = create_timer(std::chrono::duration(1.0 / 2.0), std::bind(&ExampleImageTransport::callback_timer, this)); + timer_publisher_ = create_timer(std::chrono::duration(1.0 / rate), std::bind(&ExampleImageTransport::callback_timer, this)); is_initialized_ = true; RCLCPP_INFO(get_logger(), "Initialized");