diff --git a/README.md b/README.md index a4004d2..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) @@ -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 diff --git a/extended_examples/pluginlib_example/example_plugin_manager/CMakeLists.txt b/extended_examples/pluginlib_example/example_plugin_manager/CMakeLists.txt index e86be7d..554fb19 100644 --- a/extended_examples/pluginlib_example/example_plugin_manager/CMakeLists.txt +++ b/extended_examples/pluginlib_example/example_plugin_manager/CMakeLists.txt @@ -1,69 +1,92 @@ 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") 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(Eigen3 REQUIRED) +set(DEPENDENCIES + ament_cmake + ament_cmake_ros + rclcpp + rclcpp_components + pluginlib + Eigen3 + ros2_examples +) + +set(LIBRARIES + ExamplePluginManager_PluginManager + ) -set(Eigen_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIRS}) -set(Eigen_LIBRARIES ${Eigen_LIBRARIES}) +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 - ) +) install( - DIRECTORY include/ - DESTINATION include/${PROJECT_NAME} + DIRECTORY include + DESTINATION . ) -install(DIRECTORY launch +install( + DIRECTORY launch DESTINATION share/${PROJECT_NAME} ) -install(DIRECTORY config +install( + DIRECTORY config DESTINATION share/${PROJECT_NAME} ) -ament_export_include_directories("include/${PROJECT_NAME}") +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/config/example_plugin_manager.yaml b/extended_examples/pluginlib_example/example_plugin_manager/config/example_plugin_manager.yaml index 3ac0fe4..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", @@ -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..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 9ba63bf..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::Node &parent_node, 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 new file mode 100755 index 0000000..b571b8a --- /dev/null +++ b/extended_examples/pluginlib_example/example_plugin_manager/launch/example_plugin_manager.py @@ -0,0 +1,54 @@ +import launch +import os + +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode +from launch.substitutions import 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 = EnvironmentVariable("UAV_TYPE", default_value="dummy") + + ld.add_action(ComposableNodeContainer( + + 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="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}, + ], + + # remappings=[ + # # topics + # ("~/topic_out", "~/topic"), + # ], + + ), + + ], + + output="screen", + )) + + return ld 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 6dc01c6..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 @@ -1,14 +1,12 @@ +#include +#include #include #include #include #include -#include #include - - -// #include -// #include +#include namespace example_plugin_manager { @@ -40,15 +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; + 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 ------- | @@ -63,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_; @@ -84,29 +83,23 @@ 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("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."); rclcpp::shutdown(); return; } - // | --------------- example of a shared object --------------- | example_of_a_shared_object_ = "Hello, this is a shared object"; @@ -126,41 +119,54 @@ 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::load_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"); // 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; - 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); + // nested params are separated by '.' instead of '/' + 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."); + 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; } } @@ -169,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(*this, _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()); @@ -190,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; @@ -201,16 +207,18 @@ 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; } } // | ---------- 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 ------------------------- | @@ -219,12 +227,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"); @@ -241,14 +243,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) { @@ -282,4 +282,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 e6c8a8e..3ba5435 100644 --- a/extended_examples/pluginlib_example/example_plugins/CMakeLists.txt +++ b/extended_examples/pluginlib_example/example_plugins/CMakeLists.txt @@ -1,72 +1,90 @@ 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") 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) +set(DEPENDENCIES + ament_cmake + ament_cmake_ros + rclcpp + rclcpp_components + Eigen3 + pluginlib + ros2_examples + example_plugin_manager +) -pluginlib_export_plugin_description_file(example_plugin_manager plugins.xml) +set(LIBRARIES + ExamplePlugin_Plugin + ) -set(Eigen_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIRS}) -set(Eigen_LIBRARIES ${Eigen_LIBRARIES}) +foreach(DEP IN LISTS DEPENDENCIES) + find_package(${DEP} REQUIRED) +endforeach() include_directories( include + ${rclcpp_INCLUDE_DIRS} + ${example_plugin_manager_INCLUDE_DIRS} ) -# ExamplePlugin +## -------------------------------------------------------------- +## | Compile | +## -------------------------------------------------------------- + +# Example plugin -add_library(example_plugin SHARED +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(example_plugin PUBLIC - rclcpp - rclcpp_components - pluginlib - Eigen3 - ros2_examples - example_plugin_manager +ament_target_dependencies(ExamplePlugin_Plugin + ${DEPENDENCIES} ) -# each component (nodelet) needs to be registered to make it discoverable at runtime -rclcpp_components_register_nodes(example_plugin "example_plugin::ExamplePlugin") +## -------------------------------------------------------------- +## | Export plugins | +## -------------------------------------------------------------- + +# , +pluginlib_export_plugin_description_file(example_plugin_manager plugins.xml) ## -------------------------------------------------------------- ## | Install | ## -------------------------------------------------------------- -install(TARGETS - example_plugin +ament_export_libraries( + ${LIBRARIES} +) + +install( + TARGETS ${LIBRARIES} + 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} +# 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_dependencies( + ${DEPENDENCIES} ) -ament_package() \ No newline at end of file +ament_package() 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 2ac3e83..edca5fb 100644 --- a/extended_examples/pluginlib_example/example_plugins/plugins.xml +++ b/extended_examples/pluginlib_example/example_plugins/plugins.xml @@ -1,5 +1,6 @@ - - + + + 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..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,28 +1,18 @@ -#include -#include -#include #include -#include #include #include #include - -// #include - 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 std::shared_ptr &sub_node, const std::string& name, std::shared_ptr common_handlers); bool activate(const int& some_number); @@ -31,9 +21,9 @@ 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 parent_node_; + std::shared_ptr node_; std::string _name_; private: @@ -49,10 +39,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 std::shared_ptr &sub_node, const std::string& name, std::shared_ptr common_handlers) { - parent_node_ = std::make_shared(parent_node); + node_ = sub_node; _name_ = name; // I can use this to get stuff from the manager interactively @@ -60,32 +50,21 @@ void ExamplePlugin::initialize(const rclcpp::Node &parent_node, const std::strin // | ------------------- loading parameters ------------------- | - bool loaded_successfully = true; + // 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::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_); - - // 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(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(), node_->get_namespace()); is_initialized_ = true; } @@ -96,7 +75,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 +90,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 +103,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 +122,9 @@ 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) +// Register the plugin. +PLUGINLIB_EXPORT_CLASS(example_plugins::ExamplePlugin, example_plugin_manager::Plugin) \ No newline at end of file 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..d18d386 --- /dev/null +++ b/extended_examples/vision_examples/example_image_transport/CMakeLists.txt @@ -0,0 +1,74 @@ +cmake_minimum_required(VERSION 3.5) +project(vision_examples) + +# 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() + +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) + +include_directories( + include + ${rclcpp_INCLUDE_DIRS} + ${ros2_examples_INCLUDE_DIRS} + ) + +add_library(ExampleImageTransport SHARED + src/example_image_transport.cpp +) + +target_link_libraries(ExampleImageTransport + ${OpenCV_LIBRARIES} +) + +ament_target_dependencies(ExampleImageTransport + ${DEPENDENCIES} +) + +# each component (nodelet) needs to be registered to make it discoverable at runtime +rclcpp_components_register_nodes(ExampleImageTransport PLUGIN "vision_examples::ExampleImageTransport" EXECUTABLE ExampleImageTransport) + +## -------------------------------------------------------------- +## | Install | +## -------------------------------------------------------------- + +install(TARGETS + ${LIBRARIES} + 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/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 new file mode 100644 index 0000000..247a26e --- /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_image_transport_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/default.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..b610b4b --- /dev/null +++ b/extended_examples/vision_examples/example_image_transport/package.xml @@ -0,0 +1,30 @@ + + + + + vision_examples + + 1.0.0 + MRS ROS2 vision example + + Afzal Ahmad + klaxalk + + TODO: License declaration + + ament_cmake + + rclcpp + rclcpp_components + sensor_msgs + cv_bridge + image_transport + libopencv-dev + + vision_examples_utils + + + 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..f06f640 --- /dev/null +++ b/extended_examples/vision_examples/example_image_transport/src/example_image_transport.cpp @@ -0,0 +1,119 @@ +#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"); + + 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); + 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 / rate), 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/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 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..5d2ef89 --- /dev/null +++ b/extended_examples/vision_examples/example_image_transport/tmux/session.yml @@ -0,0 +1,26 @@ +# 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; +startup_window: image_transport +windows: + - dummy_publisher: + layout: tiled + panes: + - ros2 run vision_examples_utils dummy_pub + - image_transport: + layout: tiled + panes: + - ros2 launch vision_examples example_image_transport.py + - rviz: + layout: tiled + panes: + - rqt --perspective-file default.perspective + # - 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() 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/CMakeLists.txt b/ros2_examples/CMakeLists.txt index 994ac1b..06abe90 100644 --- a/ros2_examples/CMakeLists.txt +++ b/ros2_examples/CMakeLists.txt @@ -3,26 +3,44 @@ 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) # 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(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} ) ## -------------------------------------------------------------- @@ -33,6 +51,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 +70,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 +86,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 +102,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 +124,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 +145,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 +162,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,19 +179,22 @@ 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) -############################################################ -# adding libraries both Interface (header-only) -############################################################ -# add template header-only library for implementing simulators -add_library(${PROJECT_NAME}_headers INTERFACE) +## -------------------------------------------------------------- +## | clock management | +## -------------------------------------------------------------- -# add the header files that will be referenced when this target is used -target_include_directories(${PROJECT_NAME}_headers INTERFACE - "$" - "$") +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 | @@ -199,41 +212,34 @@ endif() ## | install | ## -------------------------------------------------------------- -install(TARGETS - params_example - subscriber_example - publisher_example - service_server_example - service_client_example - timer_example - tf2_broadcaster_example - tf2_listener_example +install( + TARGETS ${LIBRARIES} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin ) -install(DIRECTORY include/ - DESTINATION include/${PROJECT_NAME} +install( + DIRECTORY include + DESTINATION . ) install( - TARGETS ${PROJECT_NAME}_headers - EXPORT export_${PROJECT_NAME} - LIBRARY DESTINATION lib - ARCHIVE DESTINATION lib - RUNTIME DESTINATION bin - INCLUDES DESTINATION include + DIRECTORY launch + DESTINATION share/${PROJECT_NAME} ) -ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) - -install(DIRECTORY launch +install( + DIRECTORY config DESTINATION share/${PROJECT_NAME} ) -install(DIRECTORY config - DESTINATION share/${PROJECT_NAME} +ament_export_include_directories( + include +) + +ament_export_dependencies( + ${DEPENDENCIES} ) ament_package() 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/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/include/ros2_examples/params.h b/ros2_examples/include/ros2_examples/params.h index 1e35933..c5c851b 100644 --- a/ros2_examples/include/ros2_examples/params.h +++ b/ros2_examples/include/ros2_examples/params.h @@ -1,32 +1,52 @@ +#include #include namespace utils { -/* parse_param() method //{ */ +// 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; +} + +/* 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; @@ -41,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/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/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() diff --git a/ros2_examples/launch/params_example.py b/ros2_examples/launch/params_example.py index 867250c..5e20f6c 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 @@ -18,33 +20,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', "") + env_var=os.getenv('ENV_VAR', "x500") ld.add_action(ComposableNodeContainer( - namespace="nmspc1", + namespace='nmspc1', name='nmspc1_params_example', package='rclcpp_components', executable='component_container_mt', @@ -55,18 +60,18 @@ def generate_launch_description(): package=pkg_name, plugin='ros2_examples::ParamsExample', - namespace="nmspc1", - name='params_example', + namespace='nmspc1', + name='params_node_ns', parameters=[ pkg_share_path + '/config/params_example.yaml', - custom_config, - {'uav_type': uav_type}, + {'custom_config': custom_config, + 'env_var': env_var}, ], # remappings=[ # # topics - # ("~/topic_out", "~/topic"), + # ('~/topic_out', '~/topic"), # ], ), 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_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/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/params_example.cpp b/ros2_examples/src/params_example.cpp index c6ab3f8..02d1e00 100644 --- a/ros2_examples/src/params_example.cpp +++ b/ros2_examples/src/params_example.cpp @@ -1,5 +1,7 @@ #include +#include +#include #include using namespace std::chrono_literals; @@ -7,55 +9,71 @@ 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_; - - OnSetParametersCallbackHandle::SharedPtr param_callback_handle_; - - // | ------------------------- methods ------------------------ | - - rcl_interfaces::msg::SetParametersResult callback_parameters(std::vector parameters); + 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; }; //} /* ParamsExample() constructor //{ */ -ParamsExample::ParamsExample(rclcpp::NodeOptions options) : Node("params_example", options) { +ParamsExample::ParamsExample(const rclcpp::NodeOptions options) : Node("param_example", options) { RCLCPP_INFO(get_logger(), "initializing"); bool loaded_successfully = true; - // | --------------------- load parameters -------------------- | + 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); 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); + loaded_successfully &= utils::load_param("custom_config", custom_config, *this); - const std::string uav_type = parse_param2("uav_type", loaded_successfully, *this); 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 Node. Shutting down."); rclcpp::shutdown(); return; } - // | --------------- bind pararm server callback -------------- | + 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); + loaded_successfully &= utils::load_param("custom_config", custom_config, *this); - param_callback_handle_ = add_on_set_parameters_callback(std::bind(&ParamsExample::callback_parameters, this, std::placeholders::_1)); + if (!loaded_successfully) { + RCLCPP_ERROR_STREAM(get_logger(), "Could not load all non-optional parameters using Sub-Node. Shutting down."); + rclcpp::shutdown(); + return; + } // | --------------------- finish the init -------------------- | @@ -64,45 +82,6 @@ ParamsExample::ParamsExample(rclcpp::NodeOptions options) : Node("params_example //} -// | ------------------------ 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; -} - -//} - } // namespace ros2_examples #include 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 -------------------- | 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"); } diff --git a/ros2_lib/CMakeLists.txt b/ros2_lib/CMakeLists.txt index 63ae195..fd96928 100644 --- a/ros2_lib/CMakeLists.txt +++ b/ros2_lib/CMakeLists.txt @@ -1,33 +1,33 @@ 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) 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 +49,6 @@ ament_target_dependencies(Ros2Lib_Adder if(BUILD_TESTING) - message(WARNING "Testing enabled.") - add_subdirectory(test) endif() @@ -63,8 +61,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/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} diff --git a/sub_pub_torture_test/CMakeLists.txt b/sub_pub_torture_test/CMakeLists.txt new file mode 100644 index 0000000..59a5e70 --- /dev/null +++ b/sub_pub_torture_test/CMakeLists.txt @@ -0,0 +1,89 @@ +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 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() + +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..6f2022e --- /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 + + + + + + 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