From 7f41c4bedee4a89acf32fee0652be5fd743be42f Mon Sep 17 00:00:00 2001 From: Manuel M Date: Mon, 6 Feb 2023 11:17:43 +0100 Subject: [PATCH 01/26] allow an empty robot description file. * Deprecate passing of robot description file to controller manager * subscribe to robot_state_publisher to get robot_description_file --- .../controller_manager/controller_manager.hpp | 8 ++++ controller_manager/src/controller_manager.cpp | 38 +++++++++++++++++-- 2 files changed, 43 insertions(+), 3 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 537f0447be..d9283e7907 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -40,6 +40,8 @@ #include "controller_manager_msgs/srv/switch_controller.hpp" #include "controller_manager_msgs/srv/unload_controller.hpp" +#include "std_msgs/msg/string.hpp" + #include "diagnostic_updater/diagnostic_updater.hpp" #include "hardware_interface/handle.hpp" #include "hardware_interface/resource_manager.hpp" @@ -51,6 +53,7 @@ #include "rclcpp/node_interfaces/node_logging_interface.hpp" #include "rclcpp/node_interfaces/node_parameters_interface.hpp" #include "rclcpp/parameter.hpp" +#include "rclcpp/rclcpp.hpp" namespace controller_manager { @@ -85,6 +88,9 @@ class ControllerManager : public rclcpp::Node CONTROLLER_MANAGER_PUBLIC void init_resource_manager(const std::string & robot_description); + CONTROLLER_MANAGER_PUBLIC + void init_resource_manager_cb(const std_msgs::msg::String & msg); + CONTROLLER_MANAGER_PUBLIC controller_interface::ControllerInterfaceBaseSharedPtr load_controller( const std::string & controller_name, const std::string & controller_type); @@ -501,6 +507,8 @@ class ControllerManager : public rclcpp::Node std::vector activate_command_interface_request_, deactivate_command_interface_request_; + rclcpp::Subscription::SharedPtr robot_description_subscription_; + struct SwitchParams { bool do_switch = {false}; diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index b912b622e2..30c63247c2 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -150,17 +150,29 @@ ControllerManager::ControllerManager( } std::string robot_description = ""; + // TODO(Manuel): robot_description parameter is deprecated and should be removed. + // robot_description should be obtained via robot_state_publisher get_parameter("robot_description", robot_description); if (robot_description.empty()) { - throw std::runtime_error("Unable to initialize resource manager, no robot description found."); + RCLCPP_INFO(get_logger(), "Subscribing to robot_state_publisher for robot description file."); + robot_description_subscription_ = create_subscription( + "/robot_description", 10, + std::bind(&ControllerManager::init_resource_manager_cb, this, std::placeholders::_1)); + } + else + { + RCLCPP_WARN( + get_logger(), + "[Deprecated] Passing the robot description file directly to the control_manager node is " + "deprecated. Use robot_state_publisher instead."); + init_resource_manager(robot_description); } - - init_resource_manager(robot_description); diagnostics_updater_.setHardwareID("ros2_control"); diagnostics_updater_.add( "Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback); + init_services(); } @@ -188,6 +200,26 @@ ControllerManager::ControllerManager( init_services(); } +void ControllerManager::init_resource_manager_cb(const std_msgs::msg::String & robot_description) +{ + RCLCPP_DEBUG( + get_logger(), "'init_resource_manager_cb called with %s", robot_description.data.c_str()); + // TODO(Manuel)errors should probably be caught since we don't want controller_manager node to die if + // a non valid urdf is passed. However, this should be tested and fine tuned. + try + { + init_resource_manager(robot_description.data.c_str()); + } + catch (std::runtime_error & e) + { + RCLCPP_ERROR_STREAM( + get_logger(), + "The published robot description file (urdf) seems not to be genuine. Following error was " + "caught:" + << e.what()); + } +} + void ControllerManager::init_resource_manager(const std::string & robot_description) { // TODO(destogl): manage this when there is an error - CM should not die because URDF is wrong... From d1a0da7f31924b003ac4b14e03d61b84e5892289 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Thu, 16 Feb 2023 08:08:57 +0100 Subject: [PATCH 02/26] on startup wait for robot description, however allow to reveice later --- .../controller_manager/controller_manager.hpp | 12 +++- controller_manager/src/controller_manager.cpp | 65 +++++++++++++++++-- 2 files changed, 67 insertions(+), 10 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index d9283e7907..b4e1beddba 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -15,6 +15,7 @@ #ifndef CONTROLLER_MANAGER__CONTROLLER_MANAGER_HPP_ #define CONTROLLER_MANAGER__CONTROLLER_MANAGER_HPP_ +#include #include #include #include @@ -88,9 +89,6 @@ class ControllerManager : public rclcpp::Node CONTROLLER_MANAGER_PUBLIC void init_resource_manager(const std::string & robot_description); - CONTROLLER_MANAGER_PUBLIC - void init_resource_manager_cb(const std_msgs::msg::String & msg); - CONTROLLER_MANAGER_PUBLIC controller_interface::ControllerInterfaceBaseSharedPtr load_controller( const std::string & controller_name, const std::string & controller_type); @@ -200,6 +198,9 @@ class ControllerManager : public rclcpp::Node CONTROLLER_MANAGER_PUBLIC void init_services(); + CONTROLLER_MANAGER_PUBLIC + void wait_for_robot_description(); + CONTROLLER_MANAGER_PUBLIC controller_interface::ControllerInterfaceBaseSharedPtr add_controller_impl( const ControllerSpec & controller); @@ -311,6 +312,9 @@ class ControllerManager : public rclcpp::Node const std::shared_ptr request, std::shared_ptr response); + CONTROLLER_MANAGER_PUBLIC + void init_resource_manager_cb(const std_msgs::msg::String & msg); + // Per controller update rate support unsigned int update_loop_counter_ = 0; unsigned int update_rate_ = 100; @@ -318,6 +322,8 @@ class ControllerManager : public rclcpp::Node std::unique_ptr resource_manager_; + int64_t wait_for_robot_description_ = 10; + private: std::vector get_controller_names(); std::pair split_command_interface( diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 30c63247c2..bb61bba5c9 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -151,14 +151,10 @@ ControllerManager::ControllerManager( std::string robot_description = ""; // TODO(Manuel): robot_description parameter is deprecated and should be removed. - // robot_description should be obtained via robot_state_publisher get_parameter("robot_description", robot_description); if (robot_description.empty()) { - RCLCPP_INFO(get_logger(), "Subscribing to robot_state_publisher for robot description file."); - robot_description_subscription_ = create_subscription( - "/robot_description", 10, - std::bind(&ControllerManager::init_resource_manager_cb, this, std::placeholders::_1)); + wait_for_robot_description(); } else { @@ -200,12 +196,67 @@ ControllerManager::ControllerManager( init_services(); } +void ControllerManager::wait_for_robot_description() +{ + // TODO(Manuel) Do we want to keep all? This way we eventually could receive an "old" + // robot description + rclcpp::QoS robot_description_qos = + rclcpp::QoS(rclcpp::QoSInitialization(RMW_QOS_POLICY_HISTORY_KEEP_ALL, 1)).reliable(); + + RCLCPP_INFO(get_logger(), "Subscribing to robot_state_publisher for robot description file."); + robot_description_subscription_ = create_subscription( + "/robot_description", robot_description_qos, + std::bind(&ControllerManager::init_resource_manager_cb, this, std::placeholders::_1)); + + if (!get_parameter("wait_for_robot_description", wait_for_robot_description_)) + { + RCLCPP_WARN_STREAM( + get_logger(), + "No \"wait_for_robot_description\" parameter given, which determines how long to wait for " + "receiving the robot description file via robot_state_publisher topic. Using default:" + << wait_for_robot_description_); + } + rclcpp::WaitSet wait_set; + wait_set.add_subscription(robot_description_subscription_); + auto wait_result = wait_set.wait(std::chrono::seconds(wait_for_robot_description_)); + if (wait_result.kind() == rclcpp::WaitResultKind::Ready) + { + std_msgs::msg::String robot_description; + rclcpp::MessageInfo info; + auto take_result = robot_description_subscription_->take(robot_description, info); + if (take_result) + { + init_resource_manager_cb(robot_description); + } + else + { + RCLCPP_WARN( + get_logger(), + "No robot description file received. Continue without robot description file."); + } + } + else if (wait_result.kind() == rclcpp::WaitResultKind::Timeout) + { + RCLCPP_WARN( + get_logger(), + "Waiting for receiving of robot description timed out. Continue without robot description " + "file."); + } + else if (wait_result.kind() == rclcpp::WaitResultKind::Empty) + { + RCLCPP_WARN( + get_logger(), + "Waiting for robot description failed because wait-set is empty. Continue without robot " + "description file."); + } +} + void ControllerManager::init_resource_manager_cb(const std_msgs::msg::String & robot_description) { RCLCPP_DEBUG( get_logger(), "'init_resource_manager_cb called with %s", robot_description.data.c_str()); - // TODO(Manuel)errors should probably be caught since we don't want controller_manager node to die if - // a non valid urdf is passed. However, this should be tested and fine tuned. + // TODO(Manuel) errors should probably be caught since we don't want controller_manager node + // to die if a non valid urdf is passed. However, should maybe be fine tuned. try { init_resource_manager(robot_description.data.c_str()); From f781d1a625857e88be3b732e44cd3aea90c9df92 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Thu, 16 Feb 2023 09:58:35 +0100 Subject: [PATCH 03/26] undo waiting for robot description file, change QoS instead --- .../controller_manager/controller_manager.hpp | 6 -- controller_manager/src/controller_manager.cpp | 64 ++----------------- 2 files changed, 7 insertions(+), 63 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index b4e1beddba..c5f7fe124d 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -15,7 +15,6 @@ #ifndef CONTROLLER_MANAGER__CONTROLLER_MANAGER_HPP_ #define CONTROLLER_MANAGER__CONTROLLER_MANAGER_HPP_ -#include #include #include #include @@ -198,9 +197,6 @@ class ControllerManager : public rclcpp::Node CONTROLLER_MANAGER_PUBLIC void init_services(); - CONTROLLER_MANAGER_PUBLIC - void wait_for_robot_description(); - CONTROLLER_MANAGER_PUBLIC controller_interface::ControllerInterfaceBaseSharedPtr add_controller_impl( const ControllerSpec & controller); @@ -322,8 +318,6 @@ class ControllerManager : public rclcpp::Node std::unique_ptr resource_manager_; - int64_t wait_for_robot_description_ = 10; - private: std::vector get_controller_names(); std::pair split_command_interface( diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index bb61bba5c9..4dbcde87f5 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -154,7 +154,12 @@ ControllerManager::ControllerManager( get_parameter("robot_description", robot_description); if (robot_description.empty()) { - wait_for_robot_description(); + // set QoS to transient local to get messages that have already been published + // (if robot state publisher starts before controller manager) + RCLCPP_INFO(get_logger(), "Subscribing to robot_state_publisher for robot description file."); + robot_description_subscription_ = create_subscription( + "/robot_description", rclcpp::QoS(1).transient_local(), + std::bind(&ControllerManager::init_resource_manager_cb, this, std::placeholders::_1)); } else { @@ -196,64 +201,9 @@ ControllerManager::ControllerManager( init_services(); } -void ControllerManager::wait_for_robot_description() -{ - // TODO(Manuel) Do we want to keep all? This way we eventually could receive an "old" - // robot description - rclcpp::QoS robot_description_qos = - rclcpp::QoS(rclcpp::QoSInitialization(RMW_QOS_POLICY_HISTORY_KEEP_ALL, 1)).reliable(); - - RCLCPP_INFO(get_logger(), "Subscribing to robot_state_publisher for robot description file."); - robot_description_subscription_ = create_subscription( - "/robot_description", robot_description_qos, - std::bind(&ControllerManager::init_resource_manager_cb, this, std::placeholders::_1)); - - if (!get_parameter("wait_for_robot_description", wait_for_robot_description_)) - { - RCLCPP_WARN_STREAM( - get_logger(), - "No \"wait_for_robot_description\" parameter given, which determines how long to wait for " - "receiving the robot description file via robot_state_publisher topic. Using default:" - << wait_for_robot_description_); - } - rclcpp::WaitSet wait_set; - wait_set.add_subscription(robot_description_subscription_); - auto wait_result = wait_set.wait(std::chrono::seconds(wait_for_robot_description_)); - if (wait_result.kind() == rclcpp::WaitResultKind::Ready) - { - std_msgs::msg::String robot_description; - rclcpp::MessageInfo info; - auto take_result = robot_description_subscription_->take(robot_description, info); - if (take_result) - { - init_resource_manager_cb(robot_description); - } - else - { - RCLCPP_WARN( - get_logger(), - "No robot description file received. Continue without robot description file."); - } - } - else if (wait_result.kind() == rclcpp::WaitResultKind::Timeout) - { - RCLCPP_WARN( - get_logger(), - "Waiting for receiving of robot description timed out. Continue without robot description " - "file."); - } - else if (wait_result.kind() == rclcpp::WaitResultKind::Empty) - { - RCLCPP_WARN( - get_logger(), - "Waiting for robot description failed because wait-set is empty. Continue without robot " - "description file."); - } -} - void ControllerManager::init_resource_manager_cb(const std_msgs::msg::String & robot_description) { - RCLCPP_DEBUG( + RCLCPP_ERROR( get_logger(), "'init_resource_manager_cb called with %s", robot_description.data.c_str()); // TODO(Manuel) errors should probably be caught since we don't want controller_manager node // to die if a non valid urdf is passed. However, should maybe be fine tuned. From bf6564abf4ebd15cf27e0deeda2a38da188ab0e0 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Tue, 21 Feb 2023 08:40:29 +0100 Subject: [PATCH 04/26] apply suggestions from review --- .../controller_manager/controller_manager.hpp | 5 ++--- controller_manager/src/controller_manager.cpp | 14 ++++++++------ 2 files changed, 10 insertions(+), 9 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index c5f7fe124d..afb77df8e8 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -40,8 +40,6 @@ #include "controller_manager_msgs/srv/switch_controller.hpp" #include "controller_manager_msgs/srv/unload_controller.hpp" -#include "std_msgs/msg/string.hpp" - #include "diagnostic_updater/diagnostic_updater.hpp" #include "hardware_interface/handle.hpp" #include "hardware_interface/resource_manager.hpp" @@ -54,6 +52,7 @@ #include "rclcpp/node_interfaces/node_parameters_interface.hpp" #include "rclcpp/parameter.hpp" #include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/string.hpp" namespace controller_manager { @@ -309,7 +308,7 @@ class ControllerManager : public rclcpp::Node std::shared_ptr response); CONTROLLER_MANAGER_PUBLIC - void init_resource_manager_cb(const std_msgs::msg::String & msg); + void robot_description_callback(const std_msgs::msg::String & msg); // Per controller update rate support unsigned int update_loop_counter_ = 0; diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 4dbcde87f5..7a04bd2f9f 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -156,10 +156,11 @@ ControllerManager::ControllerManager( { // set QoS to transient local to get messages that have already been published // (if robot state publisher starts before controller manager) - RCLCPP_INFO(get_logger(), "Subscribing to robot_state_publisher for robot description file."); + RCLCPP_INFO( + get_logger(), "Subscribing to ~/robot_description topic for robot description file."); robot_description_subscription_ = create_subscription( - "/robot_description", rclcpp::QoS(1).transient_local(), - std::bind(&ControllerManager::init_resource_manager_cb, this, std::placeholders::_1)); + namespace_ + "/robot_description", rclcpp::QoS(1).transient_local(), + std::bind(&ControllerManager::robot_description_callback, this, std::placeholders::_1)); } else { @@ -201,10 +202,11 @@ ControllerManager::ControllerManager( init_services(); } -void ControllerManager::init_resource_manager_cb(const std_msgs::msg::String & robot_description) +void ControllerManager::robot_description_callback(const std_msgs::msg::String & robot_description) { - RCLCPP_ERROR( - get_logger(), "'init_resource_manager_cb called with %s", robot_description.data.c_str()); + RCLCPP_INFO(get_logger(), "Received robot description file."); + RCLCPP_DEBUG( + get_logger(), "'Content of robot description file: %s", robot_description.data.c_str()); // TODO(Manuel) errors should probably be caught since we don't want controller_manager node // to die if a non valid urdf is passed. However, should maybe be fine tuned. try From 32c2f38d502ce962c6e39f6f620fc02c2b6faefe Mon Sep 17 00:00:00 2001 From: Manuel M Date: Fri, 31 Mar 2023 15:47:25 +0200 Subject: [PATCH 05/26] check if resource manager has been initialized --- controller_manager/src/controller_manager.cpp | 7 +++++++ .../include/hardware_interface/resource_manager.hpp | 13 ++++++++++++- hardware_interface/src/resource_manager.cpp | 8 ++++++-- 3 files changed, 25 insertions(+), 3 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 7a04bd2f9f..a661aadc8d 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -211,6 +211,13 @@ void ControllerManager::robot_description_callback(const std_msgs::msg::String & // to die if a non valid urdf is passed. However, should maybe be fine tuned. try { + if (resource_manager_->is_initialized()) + { + RCLCPP_WARN( + get_logger(), + "ResourceManager has already been initialized. Ignoring received robot description file."); + return; + } init_resource_manager(robot_description.data.c_str()); } catch (std::runtime_error & e) diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index 1693e85574..0d529e2795 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -65,7 +65,8 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager * "autostart_components" and "autoconfigure_components" instead. */ explicit ResourceManager( - const std::string & urdf, bool validate_interfaces = true, bool activate_all = false); + const std::string & urdf, bool validate_interfaces = true, bool activate_all = false, + bool initialized = false); ResourceManager(const ResourceManager &) = delete; @@ -83,6 +84,14 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager */ void load_urdf(const std::string & urdf, bool validate_interfaces = true); + /** + * @brief if the resource manager has been initialized with a valid urdf file this returns true. + * + * @return true if resource manager has been initialized with a valid urdf file + * @return false if resource manager has not been initialized + */ + bool is_initialized() const; + /// Claim a state interface given its key. /** * The resource is claimed as long as being in scope. @@ -405,6 +414,8 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager // Structure to store read and write status so it is not initialized in the real-time loop HardwareReadWriteStatus read_write_status; + + bool initialized_ = false; }; } // namespace hardware_interface diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 33236afd1c..9bb0bd2664 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -574,8 +574,8 @@ ResourceManager::ResourceManager() : resource_storage_(std::make_unique()) + const std::string & urdf, bool validate_interfaces, bool activate_all, bool initialized) +: resource_storage_(std::make_unique()), initialized_(initialized) { load_urdf(urdf, validate_interfaces); @@ -629,8 +629,12 @@ void ResourceManager::load_urdf(const std::string & urdf, bool validate_interfac read_write_status.failed_hardware_names.reserve( resource_storage_->actuators_.size() + resource_storage_->sensors_.size() + resource_storage_->systems_.size()); + + initialized_ = true; } +bool ResourceManager::is_initialized() const { return initialized_; } + // CM API: Called in "update"-thread LoanedStateInterface ResourceManager::claim_state_interface(const std::string & key) { From ce32ebec7d4f2285528109ed472e84717f60b186 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Mon, 3 Apr 2023 11:56:50 +0200 Subject: [PATCH 06/26] rename flag, add some ttests in resource manager --- controller_manager/src/controller_manager.cpp | 5 ++-- .../hardware_interface/resource_manager.hpp | 14 ++++++---- hardware_interface/src/resource_manager.cpp | 9 +++--- .../test/test_resource_manager.cpp | 28 +++++++++++++++++++ 4 files changed, 43 insertions(+), 13 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index a661aadc8d..a55b30993a 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -211,11 +211,12 @@ void ControllerManager::robot_description_callback(const std_msgs::msg::String & // to die if a non valid urdf is passed. However, should maybe be fine tuned. try { - if (resource_manager_->is_initialized()) + if (resource_manager_->load_urdf_called()) { RCLCPP_WARN( get_logger(), - "ResourceManager has already been initialized. Ignoring received robot description file."); + "ResourceManager has already loaded an urdf file. Ignoring attempt to reload a robot " + "description file."); return; } init_resource_manager(robot_description.data.c_str()); diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index 0d529e2795..4a6567a7ea 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -66,7 +66,7 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager */ explicit ResourceManager( const std::string & urdf, bool validate_interfaces = true, bool activate_all = false, - bool initialized = false); + bool urdf_loaded = false); ResourceManager(const ResourceManager &) = delete; @@ -85,12 +85,14 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager void load_urdf(const std::string & urdf, bool validate_interfaces = true); /** - * @brief if the resource manager has been initialized with a valid urdf file this returns true. + * @brief if the resource manager load_urdf(...) function has been called this returns true. + * We want to permit to load the urdf later on but we currently don't want to permit multiple + * calls to load_urdf (reloading/loading different urdf) * - * @return true if resource manager has been initialized with a valid urdf file - * @return false if resource manager has not been initialized + * @return true if resource manager's load_urdf() has been called + * @return false if resource manager's load_urdf() has not been called */ - bool is_initialized() const; + bool load_urdf_called() const; /// Claim a state interface given its key. /** @@ -415,7 +417,7 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager // Structure to store read and write status so it is not initialized in the real-time loop HardwareReadWriteStatus read_write_status; - bool initialized_ = false; + bool load_urdf_called_ = false; }; } // namespace hardware_interface diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 9bb0bd2664..9d438dfad0 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -574,8 +574,8 @@ ResourceManager::ResourceManager() : resource_storage_(std::make_unique()), initialized_(initialized) + const std::string & urdf, bool validate_interfaces, bool activate_all, bool load_urdf_called) +: resource_storage_(std::make_unique()), load_urdf_called_(load_urdf_called) { load_urdf(urdf, validate_interfaces); @@ -593,6 +593,7 @@ ResourceManager::ResourceManager( // CM API: Called in "callback/slow"-thread void ResourceManager::load_urdf(const std::string & urdf, bool validate_interfaces) { + load_urdf_called_ = true; const std::string system_type = "system"; const std::string sensor_type = "sensor"; const std::string actuator_type = "actuator"; @@ -629,11 +630,9 @@ void ResourceManager::load_urdf(const std::string & urdf, bool validate_interfac read_write_status.failed_hardware_names.reserve( resource_storage_->actuators_.size() + resource_storage_->sensors_.size() + resource_storage_->systems_.size()); - - initialized_ = true; } -bool ResourceManager::is_initialized() const { return initialized_; } +bool ResourceManager::load_urdf_called() const { return load_urdf_called_; } // CM API: Called in "update"-thread LoanedStateInterface ResourceManager::claim_state_interface(const std::string & key) diff --git a/hardware_interface/test/test_resource_manager.cpp b/hardware_interface/test/test_resource_manager.cpp index 59257f4edc..f7d1962316 100644 --- a/hardware_interface/test/test_resource_manager.cpp +++ b/hardware_interface/test/test_resource_manager.cpp @@ -212,6 +212,34 @@ TEST_F(ResourceManagerTest, initialization_with_urdf_unclaimed) } } +TEST_F(ResourceManagerTest, no_load_urdf_function_called) +{ + TestableResourceManager rm; + ASSERT_FALSE(rm.load_urdf_called()); +} + +TEST_F(ResourceManagerTest, load_urdf_called_if_urdf_is_invalid) +{ + TestableResourceManager rm; + EXPECT_THROW( + rm.load_urdf(ros2_control_test_assets::minimal_robot_missing_state_keys_urdf), std::exception); + ASSERT_TRUE(rm.load_urdf_called()); +} + +TEST_F(ResourceManagerTest, load_urdf_called_if_urdf_is_valid) +{ + TestableResourceManager rm(ros2_control_test_assets::minimal_robot_urdf); + ASSERT_TRUE(rm.load_urdf_called()); +} + +TEST_F(ResourceManagerTest, can_load_urdf_later) +{ + TestableResourceManager rm; + ASSERT_FALSE(rm.load_urdf_called()); + rm.load_urdf(ros2_control_test_assets::minimal_robot_urdf); + ASSERT_TRUE(rm.load_urdf_called()); +} + TEST_F(ResourceManagerTest, resource_claiming) { TestableResourceManager rm(ros2_control_test_assets::minimal_robot_urdf); From 33ae1ce590e9b8a9613b7fdd77af4dfe9965c044 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Tue, 4 Apr 2023 13:24:23 +0200 Subject: [PATCH 07/26] Apply suggestions from code review --- controller_manager/src/controller_manager.cpp | 9 ++++----- .../include/hardware_interface/resource_manager.hpp | 6 +++--- hardware_interface/src/resource_manager.cpp | 4 ++-- 3 files changed, 9 insertions(+), 10 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index a55b30993a..a46f8ff7ae 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -157,7 +157,7 @@ ControllerManager::ControllerManager( // set QoS to transient local to get messages that have already been published // (if robot state publisher starts before controller manager) RCLCPP_INFO( - get_logger(), "Subscribing to ~/robot_description topic for robot description file."); + get_logger(), "Subscribing to '~/robot_description' topic for robot description file."); robot_description_subscription_ = create_subscription( namespace_ + "/robot_description", rclcpp::QoS(1).transient_local(), std::bind(&ControllerManager::robot_description_callback, this, std::placeholders::_1)); @@ -174,7 +174,6 @@ ControllerManager::ControllerManager( diagnostics_updater_.setHardwareID("ros2_control"); diagnostics_updater_.add( "Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback); - init_services(); } @@ -207,7 +206,7 @@ void ControllerManager::robot_description_callback(const std_msgs::msg::String & RCLCPP_INFO(get_logger(), "Received robot description file."); RCLCPP_DEBUG( get_logger(), "'Content of robot description file: %s", robot_description.data.c_str()); - // TODO(Manuel) errors should probably be caught since we don't want controller_manager node + // TODO(Manuel): errors should probably be caught since we don't want controller_manager node // to die if a non valid urdf is passed. However, should maybe be fine tuned. try { @@ -225,8 +224,8 @@ void ControllerManager::robot_description_callback(const std_msgs::msg::String & { RCLCPP_ERROR_STREAM( get_logger(), - "The published robot description file (urdf) seems not to be genuine. Following error was " - "caught:" + "The published robot description file (urdf) seems not to be genuine. The following error " + "was caught:" << e.what()); } } diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index 4a6567a7ea..3d78cf7b44 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -87,10 +87,10 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager /** * @brief if the resource manager load_urdf(...) function has been called this returns true. * We want to permit to load the urdf later on but we currently don't want to permit multiple - * calls to load_urdf (reloading/loading different urdf) + * calls to load_urdf (reloading/loading different urdf). * - * @return true if resource manager's load_urdf() has been called - * @return false if resource manager's load_urdf() has not been called + * @return true if resource manager's load_urdf() has been already called. + * @return false if resource manager's load_urdf() has not been yet called. */ bool load_urdf_called() const; diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 9d438dfad0..42285a2839 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -574,8 +574,8 @@ ResourceManager::ResourceManager() : resource_storage_(std::make_unique()), load_urdf_called_(load_urdf_called) + const std::string & urdf, bool validate_interfaces, bool activate_all) +: resource_storage_(std::make_unique()) { load_urdf(urdf, validate_interfaces); From f32d36c015c1b0d16675eeeb21cb086c3981e483 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Fri, 7 Apr 2023 11:50:52 +0200 Subject: [PATCH 08/26] update test fixture to use publishing, some tests are failing --- controller_manager/CMakeLists.txt | 1 + controller_manager/package.xml | 1 + controller_manager/src/controller_manager.cpp | 8 +++ .../test/controller_manager_test_common.hpp | 68 ++++++++++++++++--- 4 files changed, 68 insertions(+), 10 deletions(-) diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index 1d9b062efa..ef2f2e3af9 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -14,6 +14,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS pluginlib rclcpp realtime_tools + std_msgs ) find_package(ament_cmake REQUIRED) diff --git a/controller_manager/package.xml b/controller_manager/package.xml index 65291cd864..d8d3a88c7c 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -26,6 +26,7 @@ ros2_control_test_assets ros2param ros2run + std_msgs ament_cmake_gmock ros2_control_test_assets diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index a46f8ff7ae..e10e37c536 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -195,6 +195,14 @@ ControllerManager::ControllerManager( { RCLCPP_WARN(get_logger(), "'update_rate' parameter not set, using default value."); } + + // set QoS to transient local to get messages that have already been published + // (if robot state publisher starts before controller manager) + RCLCPP_INFO(get_logger(), "Subscribing to ~/robot_description topic for robot description file."); + robot_description_subscription_ = create_subscription( + namespace_ + "/robot_description", rclcpp::QoS(1).transient_local(), + std::bind(&ControllerManager::robot_description_callback, this, std::placeholders::_1)); + diagnostics_updater_.setHardwareID("ros2_control"); diagnostics_updater_.add( "Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback); diff --git a/controller_manager/test/controller_manager_test_common.hpp b/controller_manager/test/controller_manager_test_common.hpp index b6e6acac0e..6d1b72698f 100644 --- a/controller_manager/test/controller_manager_test_common.hpp +++ b/controller_manager/test/controller_manager_test_common.hpp @@ -29,8 +29,11 @@ #include "controller_manager/controller_manager.hpp" #include "controller_manager_msgs/srv/switch_controller.hpp" +#include "rclcpp/rclcpp.hpp" #include "rclcpp/utilities.hpp" +#include "std_msgs/msg/string.hpp" + #include "ros2_control_test_assets/descriptions.hpp" #include "test_controller_failed_init/test_controller_failed_init.hpp" @@ -60,21 +63,54 @@ template class ControllerManagerFixture : public ::testing::Test { public: + //TODO parameter hinzufügen, welche hw erwartet wird + explicit ControllerManagerFixture( + const std::string & robot_description = ros2_control_test_assets::minimal_robot_urdf, + const std::string ns = "/", const bool & pass_urdf_as_parameter = false) + : robot_description_(robot_description), ns_(ns), pass_urdf_as_parameter_(pass_urdf_as_parameter) + { + executor_ = std::make_shared(); + // We want ot be able to create a ResourceManager where no urdf file has been passed to + if (robot_description_.empty()) + { + cm_ = std::make_shared( + std::make_unique(), executor_, TEST_CM_NAME); + } + else + { + // can be removed later, needed if we want to have the deprecated way of passing the robot + // description file to the controller manager covered by tests + if (pass_urdf_as_parameter_) + { + cm_ = std::make_shared( + std::make_unique(robot_description_, true, true), + executor_, TEST_CM_NAME); + } + else + { + urdf_publisher_node_ = std::make_shared("robot_description_publisher", ns_); + description_pub_ = urdf_publisher_node_->create_publisher( + "robot_description", rclcpp::QoS(1).transient_local()); + executor_->add_node(urdf_publisher_node_); + publish_robot_description_file(robot_description_); + + cm_ = std::make_shared( + std::make_unique(), executor_, TEST_CM_NAME); + + //TODO warten bis hw da ist und initialisiert + // von urdf wissen wir was da sein soll + // list hw + } + } + } + static void SetUpTestCase() { rclcpp::init(0, nullptr); } static void TearDownTestCase() { rclcpp::shutdown(); } - void SetUp() - { - executor_ = std::make_shared(); - cm_ = std::make_shared( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf, true, true), - executor_, TEST_CM_NAME); - run_updater_ = false; - } + void SetUp() override { run_updater_ = false; } - void TearDown() { stopCmUpdater(); } + void TearDown() override { stopCmUpdater(); } void startCmUpdater() { @@ -115,11 +151,23 @@ class ControllerManagerFixture : public ::testing::Test EXPECT_EQ(expected_return, switch_future.get()); } + void publish_robot_description_file(const std::string & robot_description_file) + { + auto msg = std::make_unique(); + msg->data = robot_description_file; + description_pub_->publish(std::move(msg)); + } + std::shared_ptr executor_; std::shared_ptr cm_; std::thread updater_; bool run_updater_; + const std::string robot_description_; + const std::string ns_; + const bool pass_urdf_as_parameter_; + std::shared_ptr urdf_publisher_node_; + rclcpp::Publisher::SharedPtr description_pub_; }; class TestControllerManagerSrvs From 7fd2e50cc437c1ed911d04c4fa3168b892e23ebc Mon Sep 17 00:00:00 2001 From: Manuel M Date: Fri, 14 Apr 2023 09:03:41 +0200 Subject: [PATCH 09/26] Not working! tried waiting for urdf to arrive before starting controllers in test In tests we have to be sure, that our urdf arrived before continuing with the actuell tests otherwise they will naturally fail... --- controller_manager/CMakeLists.txt | 10 +++ .../test/controller_manager_test_common.hpp | 36 ++++++++--- .../test_controller_manager_urdf_passing.cpp | 63 +++++++++++++++++++ 3 files changed, 102 insertions(+), 7 deletions(-) create mode 100644 controller_manager/test/test_controller_manager_urdf_passing.cpp diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index ef2f2e3af9..3d38403332 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -144,6 +144,16 @@ if(BUILD_TESTING) ament_target_dependencies(test_controller_manager_srvs controller_manager_msgs ) + ament_add_gmock(test_controller_manager_urdf_passing + test/test_controller_manager_urdf_passing.cpp + ) + target_link_libraries(test_controller_manager_urdf_passing + controller_manager + ros2_control_test_assets::ros2_control_test_assets + ) + ament_target_dependencies(test_controller_manager_urdf_passing + controller_manager_msgs + ) add_library(test_controller_with_interfaces SHARED test/test_controller_with_interfaces/test_controller_with_interfaces.cpp diff --git a/controller_manager/test/controller_manager_test_common.hpp b/controller_manager/test/controller_manager_test_common.hpp index 6d1b72698f..7514f6db30 100644 --- a/controller_manager/test/controller_manager_test_common.hpp +++ b/controller_manager/test/controller_manager_test_common.hpp @@ -22,11 +22,13 @@ #include #include #include +#include #include #include "controller_interface/controller_interface.hpp" #include "controller_manager/controller_manager.hpp" +#include "controller_manager_msgs/srv/list_hardware_interfaces.hpp" #include "controller_manager_msgs/srv/switch_controller.hpp" #include "rclcpp/rclcpp.hpp" @@ -63,14 +65,14 @@ template class ControllerManagerFixture : public ::testing::Test { public: - //TODO parameter hinzufügen, welche hw erwartet wird + // TODO(Manuel): Maybe add parameter of which hardware is to be expected explicit ControllerManagerFixture( const std::string & robot_description = ros2_control_test_assets::minimal_robot_urdf, const std::string ns = "/", const bool & pass_urdf_as_parameter = false) : robot_description_(robot_description), ns_(ns), pass_urdf_as_parameter_(pass_urdf_as_parameter) { executor_ = std::make_shared(); - // We want ot be able to create a ResourceManager where no urdf file has been passed to + // We want to be able to create a ResourceManager where no urdf file has been passed to if (robot_description_.empty()) { cm_ = std::make_shared( @@ -88,18 +90,37 @@ class ControllerManagerFixture : public ::testing::Test } else { + // First wie create a node and a publisher for publishing the passed robot description file urdf_publisher_node_ = std::make_shared("robot_description_publisher", ns_); description_pub_ = urdf_publisher_node_->create_publisher( "robot_description", rclcpp::QoS(1).transient_local()); executor_->add_node(urdf_publisher_node_); publish_robot_description_file(robot_description_); - + // Then we create controller manager which subscribes to topic and receive + // published robot description file. Publishing is transient_local so starting cm + // later should not pose problem and is closer to real world applications cm_ = std::make_shared( std::make_unique(), executor_, TEST_CM_NAME); - - //TODO warten bis hw da ist und initialisiert - // von urdf wissen wir was da sein soll - // list hw + executor_->add_node(cm_); + // Now we have to wait for cm to process callback and initialize everything. + // We have to wait here, otherwise controllers can not be initialized since + // no hardware has been received. + service_caller_node_ = std::make_shared("service_caller_node", ns_); + executor_->add_node(service_caller_node_); + auto client = + service_caller_node_->create_client( + "get_hw_interfaces"); + auto request = + std::make_shared(); + EXPECT_TRUE(client->wait_for_service(std::chrono::milliseconds(500))); + auto future = client->async_send_request(request); + EXPECT_EQ( + executor_->spin_until_future_complete(future, std::chrono::milliseconds(1000)), + rclcpp::FutureReturnCode::SUCCESS); + auto res = future.get(); + auto command_interfaces = res->command_interfaces; + auto state_interfaces = res->state_interfaces; + // check for command-/stateinterfaces but spin_until_future_complete times out... } } } @@ -168,6 +189,7 @@ class ControllerManagerFixture : public ::testing::Test const bool pass_urdf_as_parameter_; std::shared_ptr urdf_publisher_node_; rclcpp::Publisher::SharedPtr description_pub_; + std::shared_ptr service_caller_node_; }; class TestControllerManagerSrvs diff --git a/controller_manager/test/test_controller_manager_urdf_passing.cpp b/controller_manager/test/test_controller_manager_urdf_passing.cpp new file mode 100644 index 0000000000..a4327e4c8a --- /dev/null +++ b/controller_manager/test/test_controller_manager_urdf_passing.cpp @@ -0,0 +1,63 @@ +// Copyright 2020 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. + +#include +#include +#include +#include +#include + +#include "controller_manager/controller_manager.hpp" +#include "controller_manager_test_common.hpp" + +class TestControllerManagerWithTestableCM; + +class TestableControllerManager : public controller_manager::ControllerManager +{ + friend TestControllerManagerWithTestableCM; + + FRIEND_TEST(TestControllerManagerWithTestableCM, callback_gets_passed); + FRIEND_TEST(TestControllerManagerWithTestableCM, initial_failing); + +public: + TestableControllerManager( + std::unique_ptr resource_manager, + std::shared_ptr executor, + const std::string & manager_node_name = "controller_manager", + const std::string & namespace_ = "") + : controller_manager::ControllerManager( + std::move(resource_manager), executor, manager_node_name, namespace_) + { + } +}; + +class TestControllerManagerWithTestableCM +: public ControllerManagerFixture, + public testing::WithParamInterface +{ +}; + +// only exemplary to test if working not a useful test yet +TEST_P(TestControllerManagerWithTestableCM, callback_gets_passed) +{ + ASSERT_FALSE(cm_->resource_manager_->load_urdf_called()); +} + +TEST_P(TestControllerManagerWithTestableCM, initial_failing) +{ + ASSERT_TRUE(cm_->resource_manager_->load_urdf_called()); +} + +INSTANTIATE_TEST_SUITE_P( + test_best_effort, TestControllerManagerWithTestableCM, testing::Values(best_effort)); From 6dcd4cdc990501fc519354f6474c41e14f3fc1e7 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Tue, 25 Apr 2023 11:56:37 +0200 Subject: [PATCH 10/26] workaround for topic issues while testing --- .../controller_manager/controller_manager.hpp | 9 ++-- controller_manager/src/controller_manager.cpp | 26 ++++----- .../test/controller_manager_test_common.hpp | 53 ++++--------------- .../test_controller_manager_urdf_passing.cpp | 35 ++++++++++-- .../hardware_interface/resource_manager.hpp | 3 +- 5 files changed, 60 insertions(+), 66 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index afb77df8e8..2c0d1d8fab 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -84,6 +84,12 @@ class ControllerManager : public rclcpp::Node CONTROLLER_MANAGER_PUBLIC virtual ~ControllerManager() = default; + CONTROLLER_MANAGER_PUBLIC + void subscribe_to_robot_description_topic(); + + CONTROLLER_MANAGER_PUBLIC + void robot_description_callback(const std_msgs::msg::String & msg); + CONTROLLER_MANAGER_PUBLIC void init_resource_manager(const std::string & robot_description); @@ -307,9 +313,6 @@ class ControllerManager : public rclcpp::Node const std::shared_ptr request, std::shared_ptr response); - CONTROLLER_MANAGER_PUBLIC - void robot_description_callback(const std_msgs::msg::String & msg); - // Per controller update rate support unsigned int update_loop_counter_ = 0; unsigned int update_rate_ = 100; diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index e10e37c536..c2132cd2d0 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -154,13 +154,7 @@ ControllerManager::ControllerManager( get_parameter("robot_description", robot_description); if (robot_description.empty()) { - // set QoS to transient local to get messages that have already been published - // (if robot state publisher starts before controller manager) - RCLCPP_INFO( - get_logger(), "Subscribing to '~/robot_description' topic for robot description file."); - robot_description_subscription_ = create_subscription( - namespace_ + "/robot_description", rclcpp::QoS(1).transient_local(), - std::bind(&ControllerManager::robot_description_callback, this, std::placeholders::_1)); + subscribe_to_robot_description_topic(); } else { @@ -196,12 +190,7 @@ ControllerManager::ControllerManager( RCLCPP_WARN(get_logger(), "'update_rate' parameter not set, using default value."); } - // set QoS to transient local to get messages that have already been published - // (if robot state publisher starts before controller manager) - RCLCPP_INFO(get_logger(), "Subscribing to ~/robot_description topic for robot description file."); - robot_description_subscription_ = create_subscription( - namespace_ + "/robot_description", rclcpp::QoS(1).transient_local(), - std::bind(&ControllerManager::robot_description_callback, this, std::placeholders::_1)); + subscribe_to_robot_description_topic(); diagnostics_updater_.setHardwareID("ros2_control"); diagnostics_updater_.add( @@ -209,6 +198,17 @@ ControllerManager::ControllerManager( init_services(); } +void ControllerManager::subscribe_to_robot_description_topic() +{ + // set QoS to transient local to get messages that have already been published + // (if robot state publisher starts before controller manager) + RCLCPP_INFO_STREAM( + get_logger(), "Subscribing to '~/robot_description' topic for robot description file."); + robot_description_subscription_ = create_subscription( + "~/robot_description", rclcpp::QoS(1).transient_local(), + std::bind(&ControllerManager::robot_description_callback, this, std::placeholders::_1)); +} + void ControllerManager::robot_description_callback(const std_msgs::msg::String & robot_description) { RCLCPP_INFO(get_logger(), "Received robot description file."); diff --git a/controller_manager/test/controller_manager_test_common.hpp b/controller_manager/test/controller_manager_test_common.hpp index 7514f6db30..78c3fcb06b 100644 --- a/controller_manager/test/controller_manager_test_common.hpp +++ b/controller_manager/test/controller_manager_test_common.hpp @@ -65,11 +65,10 @@ template class ControllerManagerFixture : public ::testing::Test { public: - // TODO(Manuel): Maybe add parameter of which hardware is to be expected explicit ControllerManagerFixture( const std::string & robot_description = ros2_control_test_assets::minimal_robot_urdf, - const std::string ns = "/", const bool & pass_urdf_as_parameter = false) - : robot_description_(robot_description), ns_(ns), pass_urdf_as_parameter_(pass_urdf_as_parameter) + const bool & pass_urdf_as_parameter = false) + : robot_description_(robot_description), pass_urdf_as_parameter_(pass_urdf_as_parameter) { executor_ = std::make_shared(); // We want to be able to create a ResourceManager where no urdf file has been passed to @@ -90,37 +89,16 @@ class ControllerManagerFixture : public ::testing::Test } else { - // First wie create a node and a publisher for publishing the passed robot description file - urdf_publisher_node_ = std::make_shared("robot_description_publisher", ns_); - description_pub_ = urdf_publisher_node_->create_publisher( - "robot_description", rclcpp::QoS(1).transient_local()); - executor_->add_node(urdf_publisher_node_); - publish_robot_description_file(robot_description_); - // Then we create controller manager which subscribes to topic and receive - // published robot description file. Publishing is transient_local so starting cm - // later should not pose problem and is closer to real world applications + // TODO(Manuel) : passing via topic not working in test setup, tested cm does + // not receive msg. Have to check this... + + // this is just a workaround to skip passing cm_ = std::make_shared( std::make_unique(), executor_, TEST_CM_NAME); - executor_->add_node(cm_); - // Now we have to wait for cm to process callback and initialize everything. - // We have to wait here, otherwise controllers can not be initialized since - // no hardware has been received. - service_caller_node_ = std::make_shared("service_caller_node", ns_); - executor_->add_node(service_caller_node_); - auto client = - service_caller_node_->create_client( - "get_hw_interfaces"); - auto request = - std::make_shared(); - EXPECT_TRUE(client->wait_for_service(std::chrono::milliseconds(500))); - auto future = client->async_send_request(request); - EXPECT_EQ( - executor_->spin_until_future_complete(future, std::chrono::milliseconds(1000)), - rclcpp::FutureReturnCode::SUCCESS); - auto res = future.get(); - auto command_interfaces = res->command_interfaces; - auto state_interfaces = res->state_interfaces; - // check for command-/stateinterfaces but spin_until_future_complete times out... + // mimic topic call + auto msg = std_msgs::msg::String(); + msg.data = robot_description_; + cm_->robot_description_callback(msg); } } } @@ -172,24 +150,13 @@ class ControllerManagerFixture : public ::testing::Test EXPECT_EQ(expected_return, switch_future.get()); } - void publish_robot_description_file(const std::string & robot_description_file) - { - auto msg = std::make_unique(); - msg->data = robot_description_file; - description_pub_->publish(std::move(msg)); - } - std::shared_ptr executor_; std::shared_ptr cm_; std::thread updater_; bool run_updater_; const std::string robot_description_; - const std::string ns_; const bool pass_urdf_as_parameter_; - std::shared_ptr urdf_publisher_node_; - rclcpp::Publisher::SharedPtr description_pub_; - std::shared_ptr service_caller_node_; }; class TestControllerManagerSrvs diff --git a/controller_manager/test/test_controller_manager_urdf_passing.cpp b/controller_manager/test/test_controller_manager_urdf_passing.cpp index a4327e4c8a..c73a45e291 100644 --- a/controller_manager/test/test_controller_manager_urdf_passing.cpp +++ b/controller_manager/test/test_controller_manager_urdf_passing.cpp @@ -21,14 +21,19 @@ #include "controller_manager/controller_manager.hpp" #include "controller_manager_test_common.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + class TestControllerManagerWithTestableCM; class TestableControllerManager : public controller_manager::ControllerManager { friend TestControllerManagerWithTestableCM; - FRIEND_TEST(TestControllerManagerWithTestableCM, callback_gets_passed); - FRIEND_TEST(TestControllerManagerWithTestableCM, initial_failing); + FRIEND_TEST(TestControllerManagerWithTestableCM, initial_no_load_urdf_called); + FRIEND_TEST(TestControllerManagerWithTestableCM, load_urdf_called_after_callback); + FRIEND_TEST(TestControllerManagerWithTestableCM, load_urdf_called_after_invalid_urdf_passed); + FRIEND_TEST(TestControllerManagerWithTestableCM, load_urdf_called_after_callback); + FRIEND_TEST(TestControllerManagerWithTestableCM, load_urdf_called_after_callback); public: TestableControllerManager( @@ -46,16 +51,36 @@ class TestControllerManagerWithTestableCM : public ControllerManagerFixture, public testing::WithParamInterface { +public: + // create cm with no urdf + TestControllerManagerWithTestableCM() + : ControllerManagerFixture("", false) + { + } }; -// only exemplary to test if working not a useful test yet -TEST_P(TestControllerManagerWithTestableCM, callback_gets_passed) +TEST_P(TestControllerManagerWithTestableCM, initial_no_load_urdf_called) +{ + ASSERT_FALSE(cm_->resource_manager_->load_urdf_called()); +} + +TEST_P(TestControllerManagerWithTestableCM, load_urdf_called_after_callback) { ASSERT_FALSE(cm_->resource_manager_->load_urdf_called()); + // mimic callback + auto msg = std_msgs::msg::String(); + msg.data = ros2_control_test_assets::minimal_robot_urdf; + cm_->robot_description_callback(msg); + ASSERT_TRUE(cm_->resource_manager_->load_urdf_called()); } -TEST_P(TestControllerManagerWithTestableCM, initial_failing) +TEST_P(TestControllerManagerWithTestableCM, load_urdf_called_after_invalid_urdf_passed) { + ASSERT_FALSE(cm_->resource_manager_->load_urdf_called()); + // mimic callback + auto msg = std_msgs::msg::String(); + msg.data = ros2_control_test_assets::minimal_robot_missing_command_keys_urdf; + cm_->robot_description_callback(msg); ASSERT_TRUE(cm_->resource_manager_->load_urdf_called()); } diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index 3d78cf7b44..836f12eb81 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -65,8 +65,7 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager * "autostart_components" and "autoconfigure_components" instead. */ explicit ResourceManager( - const std::string & urdf, bool validate_interfaces = true, bool activate_all = false, - bool urdf_loaded = false); + const std::string & urdf, bool validate_interfaces = true, bool activate_all = false); ResourceManager(const ResourceManager &) = delete; From 300e9856e23960fb9a26b4f96d5713e62cb0da0e Mon Sep 17 00:00:00 2001 From: Manuel M Date: Thu, 10 Nov 2022 08:42:43 +0100 Subject: [PATCH 11/26] Create poc for distributed control --- .../test/test_force_torque_sensor.cpp | 64 +-- controller_interface/test/test_imu_sensor.cpp | 40 +- .../test_semantic_component_interface.cpp | 9 +- controller_manager/CMakeLists.txt | 1 - .../controller_manager/controller_manager.hpp | 32 +- controller_manager/src/controller_manager.cpp | 294 +++++++++++++- controller_manager_msgs/CMakeLists.txt | 3 + controller_manager_msgs/msg/HandleName.msg | 2 + .../msg/PublisherDescription.msg | 3 + .../srv/RegisterSubControllerManager.srv | 20 + hardware_interface/CMakeLists.txt | 5 + .../command_forwarder.hpp | 66 ++++ .../publisher_description.hpp | 47 +++ .../state_publisher.hpp | 60 +++ .../sub_controller_manager_wrapper.hpp | 64 +++ .../include/hardware_interface/handle.hpp | 367 ++++++++++++++++-- .../loaned_command_interface.hpp | 23 +- .../loaned_state_interface.hpp | 22 +- .../hardware_interface/resource_manager.hpp | 30 ++ hardware_interface/package.xml | 3 + .../command_forwarder.cpp | 114 ++++++ .../state_publisher.cpp | 100 +++++ hardware_interface/src/resource_manager.cpp | 354 ++++++++++++++++- 23 files changed, 1607 insertions(+), 116 deletions(-) create mode 100644 controller_manager_msgs/msg/HandleName.msg create mode 100644 controller_manager_msgs/msg/PublisherDescription.msg create mode 100644 controller_manager_msgs/srv/RegisterSubControllerManager.srv create mode 100644 hardware_interface/include/hardware_interface/distributed_control_interface/command_forwarder.hpp create mode 100644 hardware_interface/include/hardware_interface/distributed_control_interface/publisher_description.hpp create mode 100644 hardware_interface/include/hardware_interface/distributed_control_interface/state_publisher.hpp create mode 100644 hardware_interface/include/hardware_interface/distributed_control_interface/sub_controller_manager_wrapper.hpp create mode 100644 hardware_interface/src/hardware_interface/distributed_control_interface/command_forwarder.cpp create mode 100644 hardware_interface/src/hardware_interface/distributed_control_interface/state_publisher.cpp diff --git a/controller_interface/test/test_force_torque_sensor.cpp b/controller_interface/test/test_force_torque_sensor.cpp index dd1e55e126..c4cef85162 100644 --- a/controller_interface/test/test_force_torque_sensor.cpp +++ b/controller_interface/test/test_force_torque_sensor.cpp @@ -47,20 +47,20 @@ TEST_F(ForceTorqueSensorTest, validate_all_with_default_names) std::vector interface_names = force_torque_sensor_->get_state_interface_names(); // assign values to force - hardware_interface::StateInterface force_x{ - sensor_name_, fts_interface_names_[0], &force_values_[0]}; - hardware_interface::StateInterface force_y{ - sensor_name_, fts_interface_names_[1], &force_values_[1]}; - hardware_interface::StateInterface force_z{ - sensor_name_, fts_interface_names_[2], &force_values_[2]}; + auto force_x = std::make_shared( + sensor_name_, fts_interface_names_[0], &force_values_[0]); + auto force_y = std::make_shared( + sensor_name_, fts_interface_names_[1], &force_values_[1]); + auto force_z = std::make_shared( + sensor_name_, fts_interface_names_[2], &force_values_[2]); // assign values to torque - hardware_interface::StateInterface torque_x{ - sensor_name_, fts_interface_names_[3], &torque_values_[0]}; - hardware_interface::StateInterface torque_y{ - sensor_name_, fts_interface_names_[4], &torque_values_[1]}; - hardware_interface::StateInterface torque_z{ - sensor_name_, fts_interface_names_[5], &torque_values_[2]}; + auto torque_x = std::make_shared( + sensor_name_, fts_interface_names_[3], &torque_values_[0]); + auto torque_y = std::make_shared( + sensor_name_, fts_interface_names_[4], &torque_values_[1]); + auto torque_z = std::make_shared( + sensor_name_, fts_interface_names_[5], &torque_values_[2]); // create local state interface vector std::vector temp_state_interfaces; @@ -131,16 +131,16 @@ TEST_F(ForceTorqueSensorTest, validate_all_with_custom_names) std::vector interface_names = force_torque_sensor_->get_state_interface_names(); // assign values to force.x and force.z - hardware_interface::StateInterface force_x{ - sensor_name_, fts_interface_names_[0], &force_values_[0]}; - hardware_interface::StateInterface force_z{ - sensor_name_, fts_interface_names_[2], &force_values_[2]}; + auto force_x = std::make_shared( + sensor_name_, fts_interface_names_[0], &force_values_[0]); + auto force_z = std::make_shared( + sensor_name_, fts_interface_names_[2], &force_values_[2]); // assign values to torque.y and torque.z - hardware_interface::StateInterface torque_y{ - sensor_name_, fts_interface_names_[4], &torque_values_[1]}; - hardware_interface::StateInterface torque_z{ - sensor_name_, fts_interface_names_[5], &torque_values_[2]}; + auto torque_y = std::make_shared( + sensor_name_, fts_interface_names_[4], &torque_values_[1]); + auto torque_z = std::make_shared( + sensor_name_, fts_interface_names_[5], &torque_values_[2]); // create local state interface vector std::vector temp_state_interfaces; @@ -213,20 +213,20 @@ TEST_F(ForceTorqueSensorTest, validate_all_custom_names) ASSERT_EQ(force_torque_sensor_->interface_names_[5], sensor_name_ + "/" + "torque.z"); // assign values to force - hardware_interface::StateInterface force_x{ - sensor_name_, fts_interface_names_[0], &force_values_[0]}; - hardware_interface::StateInterface force_y{ - sensor_name_, fts_interface_names_[1], &force_values_[1]}; - hardware_interface::StateInterface force_z{ - sensor_name_, fts_interface_names_[2], &force_values_[2]}; + auto force_x = std::make_shared( + sensor_name_, fts_interface_names_[0], &force_values_[0]); + auto force_y = std::make_shared( + sensor_name_, fts_interface_names_[1], &force_values_[1]); + auto force_z = std::make_shared( + sensor_name_, fts_interface_names_[2], &force_values_[2]); // assign values to torque - hardware_interface::StateInterface torque_x{ - sensor_name_, fts_interface_names_[3], &torque_values_[0]}; - hardware_interface::StateInterface torque_y{ - sensor_name_, fts_interface_names_[4], &torque_values_[1]}; - hardware_interface::StateInterface torque_z{ - sensor_name_, fts_interface_names_[5], &torque_values_[2]}; + auto torque_x = std::make_shared( + sensor_name_, fts_interface_names_[3], &torque_values_[0]); + auto torque_y = std::make_shared( + sensor_name_, fts_interface_names_[4], &torque_values_[1]); + auto torque_z = std::make_shared( + sensor_name_, fts_interface_names_[5], &torque_values_[2]); // create local state interface vector std::vector temp_state_interfaces; diff --git a/controller_interface/test/test_imu_sensor.cpp b/controller_interface/test/test_imu_sensor.cpp index 1fbf205238..4a1575b1e1 100644 --- a/controller_interface/test/test_imu_sensor.cpp +++ b/controller_interface/test/test_imu_sensor.cpp @@ -46,30 +46,30 @@ TEST_F(IMUSensorTest, validate_all) std::vector interface_names = imu_sensor_->get_state_interface_names(); // assign values to orientation - hardware_interface::StateInterface orientation_x{ - sensor_name_, imu_interface_names_[0], &orientation_values_[0]}; - hardware_interface::StateInterface orientation_y{ - sensor_name_, imu_interface_names_[1], &orientation_values_[1]}; - hardware_interface::StateInterface orientation_z{ - sensor_name_, imu_interface_names_[2], &orientation_values_[2]}; - hardware_interface::StateInterface orientation_w{ - sensor_name_, imu_interface_names_[3], &orientation_values_[4]}; + auto orientation_x = std::make_shared( + sensor_name_, imu_interface_names_[0], &orientation_values_[0]); + auto orientation_y = std::make_shared( + sensor_name_, imu_interface_names_[1], &orientation_values_[1]); + auto orientation_z = std::make_shared( + sensor_name_, imu_interface_names_[2], &orientation_values_[2]); + auto orientation_w = std::make_shared( + sensor_name_, imu_interface_names_[3], &orientation_values_[4]); // assign values to angular velocity - hardware_interface::StateInterface angular_velocity_x{ - sensor_name_, imu_interface_names_[4], &angular_velocity_values_[0]}; - hardware_interface::StateInterface angular_velocity_y{ - sensor_name_, imu_interface_names_[5], &angular_velocity_values_[1]}; - hardware_interface::StateInterface angular_velocity_z{ - sensor_name_, imu_interface_names_[6], &angular_velocity_values_[2]}; + auto angular_velocity_x = std::make_shared( + sensor_name_, imu_interface_names_[4], &angular_velocity_values_[0]); + auto angular_velocity_y = std::make_shared( + sensor_name_, imu_interface_names_[5], &angular_velocity_values_[1]); + auto angular_velocity_z = std::make_shared( + sensor_name_, imu_interface_names_[6], &angular_velocity_values_[2]); // assign values to linear acceleration - hardware_interface::StateInterface linear_acceleration_x{ - sensor_name_, imu_interface_names_[7], &linear_acceleration_values_[0]}; - hardware_interface::StateInterface linear_acceleration_y{ - sensor_name_, imu_interface_names_[8], &linear_acceleration_values_[1]}; - hardware_interface::StateInterface linear_acceleration_z{ - sensor_name_, imu_interface_names_[9], &linear_acceleration_values_[2]}; + auto linear_acceleration_x = std::make_shared( + sensor_name_, imu_interface_names_[7], &linear_acceleration_values_[0]); + auto linear_acceleration_y = std::make_shared( + sensor_name_, imu_interface_names_[8], &linear_acceleration_values_[1]); + auto linear_acceleration_z = std::make_shared( + sensor_name_, imu_interface_names_[9], &linear_acceleration_values_[2]); // create local state interface vector std::vector temp_state_interfaces; diff --git a/controller_interface/test/test_semantic_component_interface.cpp b/controller_interface/test/test_semantic_component_interface.cpp index f81b4f64fe..23171fa288 100644 --- a/controller_interface/test/test_semantic_component_interface.cpp +++ b/controller_interface/test/test_semantic_component_interface.cpp @@ -94,9 +94,12 @@ TEST_F(SemanticComponentInterfaceTest, validate_state_interfaces) std::vector interface_values = {1.1, 3.3, 5.5}; // assign 1.1 to interface_1, 3.3 to interface_3 and 5.5 to interface_5 - hardware_interface::StateInterface interface_1{component_name_, "1", &interface_values[0]}; - hardware_interface::StateInterface interface_3{component_name_, "3", &interface_values[1]}; - hardware_interface::StateInterface interface_5{component_name_, "5", &interface_values[2]}; + auto interface_1 = std::make_shared( + component_name_, "1", &interface_values[0]); + auto interface_3 = std::make_shared( + component_name_, "3", &interface_values[1]); + auto interface_5 = std::make_shared( + component_name_, "5", &interface_values[2]); // create local state interface vector std::vector temp_state_interfaces; diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index 3d38403332..b1a6e336ee 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -38,7 +38,6 @@ ament_target_dependencies(controller_manager PUBLIC ${THIS_PACKAGE_INCLUDE_DEPEN # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. target_compile_definitions(controller_manager PRIVATE "CONTROLLER_MANAGER_BUILDING_DLL") - add_executable(ros2_control_node src/ros2_control_node.cpp) target_link_libraries(ros2_control_node PRIVATE controller_manager diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 2c0d1d8fab..fde8e21566 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -35,6 +35,7 @@ #include "controller_manager_msgs/srv/list_hardware_components.hpp" #include "controller_manager_msgs/srv/list_hardware_interfaces.hpp" #include "controller_manager_msgs/srv/load_controller.hpp" +#include "controller_manager_msgs/srv/register_sub_controller_manager.hpp" #include "controller_manager_msgs/srv/reload_controller_libraries.hpp" #include "controller_manager_msgs/srv/set_hardware_component_state.hpp" #include "controller_manager_msgs/srv/switch_controller.hpp" @@ -53,7 +54,6 @@ #include "rclcpp/parameter.hpp" #include "rclcpp/rclcpp.hpp" #include "std_msgs/msg/string.hpp" - namespace controller_manager { using ControllersListIterator = std::vector::const_iterator; @@ -202,6 +202,27 @@ class ControllerManager : public rclcpp::Node CONTROLLER_MANAGER_PUBLIC void init_services(); + CONTROLLER_MANAGER_PUBLIC + void configure_controller_manager(); + + CONTROLLER_MANAGER_PUBLIC + void init_distributed_main_controller_services(); + + CONTROLLER_MANAGER_PUBLIC + void register_sub_controller_manager_srv_cb( + const std::shared_ptr + request, + std::shared_ptr response); + + CONTROLLER_MANAGER_PUBLIC + void add_hardware_state_publishers(); + + CONTROLLER_MANAGER_PUBLIC + void add_hardware_command_forwarders(); + + CONTROLLER_MANAGER_PUBLIC + void register_sub_controller_manager(); + CONTROLLER_MANAGER_PUBLIC controller_interface::ControllerInterfaceBaseSharedPtr add_controller_impl( const ControllerSpec & controller); @@ -407,6 +428,10 @@ class ControllerManager : public rclcpp::Node */ rclcpp::CallbackGroup::SharedPtr best_effort_callback_group_; + bool distributed_ = false; + bool sub_controller_manager_ = false; + + rclcpp::CallbackGroup::SharedPtr distributed_system_srv_callback_group_; /** * The RTControllerListWrapper class wraps a double-buffered list of controllers * to avoid needing to lock the real-time thread when switching controllers in @@ -504,6 +529,11 @@ class ControllerManager : public rclcpp::Node rclcpp::Service::SharedPtr set_hardware_component_state_service_; + // services for distributed control + std::mutex central_controller_manager_srv_lock_; + rclcpp::Service::SharedPtr + register_sub_controller_manager_srv_; + std::vector activate_request_, deactivate_request_; std::vector to_chained_mode_request_, from_chained_mode_request_; std::vector activate_command_interface_request_, diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index c2132cd2d0..976e76909b 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -14,6 +14,7 @@ #include "controller_manager/controller_manager.hpp" +#include #include #include #include @@ -22,7 +23,12 @@ #include "controller_interface/controller_interface_base.hpp" #include "controller_manager_msgs/msg/hardware_component_state.hpp" + +#include "hardware_interface/distributed_control_interface/command_forwarder.hpp" +#include "hardware_interface/distributed_control_interface/state_publisher.hpp" +#include "hardware_interface/handle.hpp" #include "hardware_interface/types/lifecycle_state_names.hpp" + #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/state.hpp" @@ -41,6 +47,10 @@ rclcpp::QoS qos_services = .reliable() .durability_volatile(); +rclcpp::QoSInitialization qos_profile_services_keep_all_persist_init( + RMW_QOS_POLICY_HISTORY_KEEP_ALL, 1); +rclcpp::QoS qos_profile_services_keep_all(qos_profile_services_keep_all_persist_init); + inline bool is_controller_inactive(const controller_interface::ControllerInterfaceBase & controller) { return controller.get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE; @@ -122,6 +132,7 @@ bool command_interface_is_reference_interface_of_controller( namespace controller_manager { + rclcpp::NodeOptions get_cm_node_options() { rclcpp::NodeOptions node_options; @@ -146,7 +157,8 @@ ControllerManager::ControllerManager( { if (!get_parameter("update_rate", update_rate_)) { - RCLCPP_WARN(get_logger(), "'update_rate' parameter not set, using default value."); + RCLCPP_WARN( + get_logger(), "'update_rate' parameter not set, using default value:%iHz", update_rate_); } std::string robot_description = ""; @@ -169,6 +181,7 @@ ControllerManager::ControllerManager( diagnostics_updater_.add( "Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback); init_services(); + configure_controller_manager(); } ControllerManager::ControllerManager( @@ -196,6 +209,7 @@ ControllerManager::ControllerManager( diagnostics_updater_.add( "Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback); init_services(); + configure_controller_manager(); } void ControllerManager::subscribe_to_robot_description_topic() @@ -340,6 +354,284 @@ void ControllerManager::init_services() qos_services, best_effort_callback_group_); } +// TODO(Manuel) don't like this, this is for fast poc +// probably better to create factory and handle creation of correct controller manager type +// there. Since asynchronous control should be supported im the future as well and we don't +// want dozen of ifs. +void ControllerManager::configure_controller_manager() +{ + if (!get_parameter("distributed", distributed_)) + { + RCLCPP_WARN( + get_logger(), "'distributed' parameter not set, using default value:%s", + distributed_ ? "true" : "false"); + } + + if (!get_parameter("sub_controller_manager", sub_controller_manager_)) + { + RCLCPP_WARN( + get_logger(), "'sub_controller_manager' parameter not set, using default value:%s", + sub_controller_manager_ ? "true" : "false"); + } + + bool std_controller_manager = !distributed_ && !sub_controller_manager_; + bool distributed_sub_controller_manager = distributed_ && sub_controller_manager_; + bool central_controller_manager = distributed_ && !sub_controller_manager_; + if (distributed_sub_controller_manager) + { + add_hardware_state_publishers(); + add_hardware_command_forwarders(); + register_sub_controller_manager(); + } + // This means we are the central controller manager + else if (central_controller_manager) + { + init_distributed_main_controller_services(); + } + // std controller manager or error. std controller manager needs no special setup. + else + { + if (!std_controller_manager) + { + throw std::logic_error( + "Controller manager configured with: distributed:false and sub_controller_manager:true. " + "Only distributed controller manager can be a sub controller manager."); + } + } +} + +void ControllerManager::init_distributed_main_controller_services() +{ + distributed_system_srv_callback_group_ = + create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + + register_sub_controller_manager_srv_ = + create_service( + "register_sub_controller_manager", + std::bind( + &ControllerManager::register_sub_controller_manager_srv_cb, this, std::placeholders::_1, + std::placeholders::_2), + qos_profile_services_keep_all, distributed_system_srv_callback_group_); +} + +void ControllerManager::register_sub_controller_manager_srv_cb( + const std::shared_ptr + request, + std::shared_ptr response) +{ + std::lock_guard guard(central_controller_manager_srv_lock_); + + auto sub_ctrl_mng_wrapper = std::make_shared( + request->sub_controller_manager_namespace, request->sub_controller_manager_name, + request->state_publishers, request->command_state_publishers); + + resource_manager_->register_sub_controller_manager(sub_ctrl_mng_wrapper); + + std::vector> + distributed_state_interfaces; + distributed_state_interfaces.reserve(sub_ctrl_mng_wrapper->get_state_publisher_count()); + distributed_state_interfaces = + resource_manager_->import_state_interfaces_of_sub_controller_manager(sub_ctrl_mng_wrapper); + + for (const auto & state_interface : distributed_state_interfaces) + { + try + { + executor_->add_node(state_interface->get_node()->get_node_base_interface()); + } + catch (const std::runtime_error & e) + { + response->ok = false; + RCLCPP_WARN_STREAM( + get_logger(), + "ControllerManager: Caught exception while trying to register sub controller manager. " + "Exception:" + << e.what()); + } + } + + std::vector> + distributed_command_interfaces; + distributed_command_interfaces.reserve(sub_ctrl_mng_wrapper->get_command_forwarder_count()); + distributed_command_interfaces = + resource_manager_->import_command_interfaces_of_sub_controller_manager(sub_ctrl_mng_wrapper); + + for (const auto & command_interface : distributed_command_interfaces) + { + try + { + executor_->add_node(command_interface->get_node()->get_node_base_interface()); + } + catch (const std::runtime_error & e) + { + response->ok = false; + RCLCPP_WARN_STREAM( + get_logger(), + "ControllerManager: Caught exception while trying to register sub controller manager. " + "Exception:" + << e.what()); + } + auto msg = controller_manager_msgs::msg::PublisherDescription(); + msg.ns = get_namespace(); + msg.name.prefix_name = command_interface->get_prefix_name(); + msg.name.interface_name = command_interface->get_interface_name(); + // TODO(Manuel) want topic name relative to namespace, but have to treat "root" namespace separate + msg.publisher_topic = std::string("/") + command_interface->forward_command_topic_name(); + response->command_state_publishers.push_back(msg); + } + + response->ok = true; + RCLCPP_INFO_STREAM( + get_logger(), "ControllerManager: Registered sub controller manager <" + << sub_ctrl_mng_wrapper->get_name() << ">."); +} + +void ControllerManager::add_hardware_state_publishers() +{ + std::vector> state_publishers_vec; + state_publishers_vec.reserve(resource_manager_->available_state_interfaces().size()); + state_publishers_vec = resource_manager_->create_hardware_state_publishers(get_namespace()); + + for (auto const & state_publisher : state_publishers_vec) + { + try + { + executor_->add_node(state_publisher->get_node()->get_node_base_interface()); + } + catch (const std::runtime_error & e) + { + RCLCPP_WARN_STREAM( + get_logger(), "ControllerManager: Can't create StatePublishers<" + << state_publisher->state_interface_name() << ">." << e.what()); + } + } +} + +void ControllerManager::add_hardware_command_forwarders() +{ + std::vector> command_forwarder_vec; + command_forwarder_vec.reserve(resource_manager_->available_command_interfaces().size()); + command_forwarder_vec = resource_manager_->create_hardware_command_forwarders(get_namespace()); + + for (auto const & command_forwarder : command_forwarder_vec) + { + try + { + executor_->add_node(command_forwarder->get_node()->get_node_base_interface()); + } + catch (const std::runtime_error & e) + { + RCLCPP_WARN_STREAM( + get_logger(), "ControllerManager: Can't create CommandForwarder<" + << command_forwarder->command_interface_name() << ">." << e.what()); + } + } +} + +void ControllerManager::register_sub_controller_manager() +{ + RCLCPP_INFO_STREAM( + get_logger(), + "SubControllerManager:<" << get_namespace() << "/" << get_name() << "> trying to register."); + rclcpp::Client::SharedPtr client = + create_client( + "/register_sub_controller_manager"); + + auto request = + std::make_shared(); + request->sub_controller_manager_namespace = get_namespace(); + request->sub_controller_manager_name = get_name(); + + // export the provided StatePublishers + for (auto const & state_publisher : resource_manager_->get_state_publishers()) + { + // create description of StatePublisher including: prefix_name, interface_name and topic. + // So that receiver is able to create a DistributedStateInterface which subscribes to the + // topics provided by this sub controller manager + request->state_publishers.push_back(state_publisher->create_publisher_description_msg()); + } + + // export the provided CommandForwarders + for (auto const & command_forwarders : resource_manager_->get_command_forwarders()) + { + // create description of StatePublisher including: prefix_name, interface_name and topic. + // So that receiver is able to create a DistributedStateInterface which subscribes to the + // topics provided by this sub controller manager + request->command_state_publishers.push_back( + command_forwarders->create_publisher_description_msg()); + } + + using namespace std::chrono_literals; + while (!client->wait_for_service(1s)) + { + if (!rclcpp::ok()) + { + RCLCPP_ERROR_STREAM( + get_logger(), "SubControllerManager:<" + << get_namespace() << "/" << get_name() + << ">. Interrupted while waiting for central controller managers " + "registration service. Exiting."); + return; + } + RCLCPP_INFO_STREAM( + get_logger(), "SubControllerManager:<" + << get_namespace() << "/" << get_name() + << ">. Central controller managers registration service not available, " + "waiting again..."); + } + + auto result = client->async_send_request(request); + if ( + rclcpp::spin_until_future_complete(this->get_node_base_interface(), result) == + rclcpp::FutureReturnCode::SUCCESS) + { + // can call get only once + auto res = result.get(); + if (res->ok) + { + auto command_state_publishers = res->command_state_publishers; + // TODO(Manuel) we should probably make the keys explicit (add key_generation function to handles) + // send keys with request + for (const auto & command_state_publisher : command_state_publishers) + { + std::string key = command_state_publisher.name.prefix_name + "/" + + command_state_publisher.name.interface_name; + auto [found, command_forwarder] = resource_manager_->find_command_forwarder(key); + if (found) + { + command_forwarder->subscribe_to_command_publisher( + command_state_publisher.publisher_topic); + } + else + { + RCLCPP_WARN_STREAM( + get_logger(), "SubControllerManager:<" + << get_namespace() << "/" << get_name() + << ">. Could not find a CommandForwarder for key[" << key + << "]. No subscription to command state possible."); + } + } + RCLCPP_INFO_STREAM( + get_logger(), "SubControllerManager:<" << get_namespace() << "/" << get_name() + << ">. Successfully registered."); + } + else + { + RCLCPP_WARN_STREAM( + get_logger(), "SubControllerManager: <" + << get_namespace() << "/" << get_name() + << ">. Registration of StatePublishers failed. Central ControllerManager " + "returned error code."); + } + } + else + { + RCLCPP_WARN_STREAM( + get_logger(), "SubControllerManager: <" << get_namespace() << "/" << get_name() + << ">. Registration of StatePublishers failed."); + } +} + controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_controller( const std::string & controller_name, const std::string & controller_type) { diff --git a/controller_manager_msgs/CMakeLists.txt b/controller_manager_msgs/CMakeLists.txt index 2a863c29dd..11230df3fb 100644 --- a/controller_manager_msgs/CMakeLists.txt +++ b/controller_manager_msgs/CMakeLists.txt @@ -9,8 +9,10 @@ find_package(rosidl_default_generators REQUIRED) set(msg_files msg/ControllerState.msg msg/ChainConnection.msg + msg/HandleName.msg msg/HardwareComponentState.msg msg/HardwareInterface.msg + msg/PublisherDescription.msg ) set(srv_files srv/ConfigureController.srv @@ -19,6 +21,7 @@ set(srv_files srv/ListHardwareComponents.srv srv/ListHardwareInterfaces.srv srv/LoadController.srv + srv/RegisterSubControllerManager.srv srv/ReloadControllerLibraries.srv srv/SetHardwareComponentState.srv srv/SwitchController.srv diff --git a/controller_manager_msgs/msg/HandleName.msg b/controller_manager_msgs/msg/HandleName.msg new file mode 100644 index 0000000000..bae7974d77 --- /dev/null +++ b/controller_manager_msgs/msg/HandleName.msg @@ -0,0 +1,2 @@ +string prefix_name # prefix name of the handle (ReadOnly or ReadWrite) +string interface_name # interface name of the handle (ReadOnly or ReadWrite) \ No newline at end of file diff --git a/controller_manager_msgs/msg/PublisherDescription.msg b/controller_manager_msgs/msg/PublisherDescription.msg new file mode 100644 index 0000000000..be19be5aad --- /dev/null +++ b/controller_manager_msgs/msg/PublisherDescription.msg @@ -0,0 +1,3 @@ +string ns # the namespace the Publisher is in +controller_manager_msgs/HandleName name # the full qualified name (prefix_name + interface_name) of the Publishers interface +string publisher_topic # the topic via which the values/commands are published. \ No newline at end of file diff --git a/controller_manager_msgs/srv/RegisterSubControllerManager.srv b/controller_manager_msgs/srv/RegisterSubControllerManager.srv new file mode 100644 index 0000000000..97aaf1f0cf --- /dev/null +++ b/controller_manager_msgs/srv/RegisterSubControllerManager.srv @@ -0,0 +1,20 @@ +# The RegisterSubControllerManager service allows a distributed controller +# manager to register itself at the central controller manager + +# Req: +# To register a controller the distributed sub controller manager has to +# define: +# * the full namespace it operates in +# * all statePublishers the sub controller provides + +# Res: +# The response includes: +# * a bool if the registration succeeded + +string sub_controller_manager_namespace +string sub_controller_manager_name +controller_manager_msgs/PublisherDescription[] state_publishers +controller_manager_msgs/PublisherDescription[] command_state_publishers +--- +controller_manager_msgs/PublisherDescription[] command_state_publishers +bool ok \ No newline at end of file diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index 77eec3ec86..b419dbe4f0 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -7,11 +7,14 @@ endif() set(THIS_PACKAGE_INCLUDE_DEPENDS control_msgs + controller_manager_msgs lifecycle_msgs pluginlib + rclcpp rclcpp_lifecycle rcpputils rcutils + std_msgs TinyXML2 tinyxml2_vendor ) @@ -24,6 +27,8 @@ endforeach() add_library(hardware_interface SHARED src/actuator.cpp src/component_parser.cpp + src/hardware_interface/distributed_control_interface/command_forwarder.cpp + src/hardware_interface/distributed_control_interface/state_publisher.cpp src/resource_manager.cpp src/sensor.cpp src/system.cpp diff --git a/hardware_interface/include/hardware_interface/distributed_control_interface/command_forwarder.hpp b/hardware_interface/include/hardware_interface/distributed_control_interface/command_forwarder.hpp new file mode 100644 index 0000000000..3597dbaf00 --- /dev/null +++ b/hardware_interface/include/hardware_interface/distributed_control_interface/command_forwarder.hpp @@ -0,0 +1,66 @@ +#ifndef DISTRIBUTED_CONTROL__COMMAND_FORWARDER_HPP_ +#define DISTRIBUTED_CONTROL__COMMAND_FORWARDER_HPP_ + +#include +#include +#include + +#include "hardware_interface/distributed_control_interface/publisher_description.hpp" +#include "hardware_interface/loaned_command_interface.hpp" + +#include "controller_manager_msgs/msg/publisher_description.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "std_msgs/msg/float64.hpp" + +namespace distributed_control +{ + +class CommandForwarder final +{ +public: + explicit CommandForwarder( + std::unique_ptr loaned_command_interface_ptr_, + const std::string & ns = ""); + + CommandForwarder() = delete; + + ~CommandForwarder() {} + + std::shared_ptr get_node() const; + + std::string get_namespace() const; + + std::string topic_name() const; + + std::string topic_name_relative_to_namespace() const; + + std::string command_interface_name() const; + + std::string command_interface_prefix_name() const; + + std::string command_interface_interface_name() const; + + controller_manager_msgs::msg::PublisherDescription create_publisher_description_msg() const; + + void subscribe_to_command_publisher(const std::string & topic_name); + +private: + void publish_value_on_timer(); + void forward_command(const std_msgs::msg::Float64 & msg); + + std::unique_ptr loaned_command_interface_ptr_; + const std::string namespace_; + const std::string topic_name_; + std::string subscription_topic_name_; + std::shared_ptr node_; + rclcpp::Publisher::SharedPtr state_value_pub_; + rclcpp::Subscription::SharedPtr command_subscription_; + rclcpp::TimerBase::SharedPtr timer_; +}; + +} // namespace distributed_control + +#endif // DISTRIBUTED_CONTROL__COMMAND_FORWARDER_HPP_ \ No newline at end of file diff --git a/hardware_interface/include/hardware_interface/distributed_control_interface/publisher_description.hpp b/hardware_interface/include/hardware_interface/distributed_control_interface/publisher_description.hpp new file mode 100644 index 0000000000..2397d47a1b --- /dev/null +++ b/hardware_interface/include/hardware_interface/distributed_control_interface/publisher_description.hpp @@ -0,0 +1,47 @@ +#ifndef DISTRIBUTED_CONTROL__STATE_PUBLISHER_DESCRIPTION_HPP_ +#define DISTRIBUTED_CONTROL__STATE_PUBLISHER_DESCRIPTION_HPP_ + +#include "controller_manager_msgs/msg/publisher_description.hpp" + +#include "rclcpp/rclcpp.hpp" + +namespace distributed_control +{ + +class PublisherDescription final +{ +public: + explicit PublisherDescription( + const controller_manager_msgs::msg::PublisherDescription & description) + : namespace_(description.ns), + prefix_name_(description.name.prefix_name), + interface_name_(description.name.interface_name), + topic_name_(description.publisher_topic) + { + } + + PublisherDescription() = delete; + + PublisherDescription(const PublisherDescription & other) = default; + + PublisherDescription(PublisherDescription && other) = default; + + ~PublisherDescription() {} + + std::string get_namespace() const { return namespace_; } + + std::string prefix_name() const { return prefix_name_; } + + std::string interface_name() const { return interface_name_; } + + std::string topic_name() const { return topic_name_; } + +private: + std::string namespace_; + std::string prefix_name_; + std::string interface_name_; + std::string topic_name_; +}; + +} // namespace distributed_control +#endif // DISTRIBUTED_CONTROL__STATE_PUBLISHER_DESCRIPTION_HPP_ \ No newline at end of file diff --git a/hardware_interface/include/hardware_interface/distributed_control_interface/state_publisher.hpp b/hardware_interface/include/hardware_interface/distributed_control_interface/state_publisher.hpp new file mode 100644 index 0000000000..6fe316c4b1 --- /dev/null +++ b/hardware_interface/include/hardware_interface/distributed_control_interface/state_publisher.hpp @@ -0,0 +1,60 @@ +#ifndef DISTRIBUTED_CONTROL__STATE_PUBLISHER_HPP_ +#define DISTRIBUTED_CONTROL__STATE_PUBLISHER_HPP_ + +#include +#include +#include + +#include "hardware_interface/loaned_state_interface.hpp" + +#include "controller_manager_msgs/msg/publisher_description.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "std_msgs/msg/float64.hpp" + +namespace distributed_control +{ + +class StatePublisher final +{ +public: + explicit StatePublisher( + std::unique_ptr loaned_state_interface_ptr, + const std::string & ns = ""); + + StatePublisher() = delete; + + ~StatePublisher() {} + + std::shared_ptr get_node() const; + + std::string get_namespace() const; + + std::string topic_name() const; + + std::string topic_name_relative_to_namespace() const; + + std::string state_interface_name() const; + + std::string state_interface_prefix_name() const; + + std::string state_interface_interface_name() const; + + controller_manager_msgs::msg::PublisherDescription create_publisher_description_msg() const; + +private: + void publish_value_on_timer(); + + std::unique_ptr loaned_state_interface_ptr_; + const std::string namespace_; + const std::string topic_name_; + std::shared_ptr node_; + rclcpp::Publisher::SharedPtr state_value_pub_; + rclcpp::TimerBase::SharedPtr timer_; +}; + +} // namespace distributed_control + +#endif // DISTRIBUTED_CONTROL__STATE_PUBLISHER_HPP_ \ No newline at end of file diff --git a/hardware_interface/include/hardware_interface/distributed_control_interface/sub_controller_manager_wrapper.hpp b/hardware_interface/include/hardware_interface/distributed_control_interface/sub_controller_manager_wrapper.hpp new file mode 100644 index 0000000000..24fb64a5ff --- /dev/null +++ b/hardware_interface/include/hardware_interface/distributed_control_interface/sub_controller_manager_wrapper.hpp @@ -0,0 +1,64 @@ +#ifndef DISTRIBUTED_CONTROL__SUB_CONTROLLER_MANAGER_WRAPPER_HPP_ +#define DISTRIBUTED_CONTROL__SUB_CONTROLLER_MANAGER_WRAPPER_HPP_ + +#include +#include +#include + +#include "controller_manager_msgs/msg/publisher_description.hpp" + +#include "hardware_interface/distributed_control_interface/publisher_description.hpp" + +namespace distributed_control +{ + +class SubControllerManagerWrapper final +{ +public: + explicit SubControllerManagerWrapper( + const std::string & ns, const std::string & name, + const std::vector & state_publishers, + std::vector & command_forwarders) + : NAMESPACE_(ns), + NAME_(name), + state_publisher_descriptions_({state_publishers.begin(), state_publishers.end()}), + command_forwarder_descriptions_({command_forwarders.begin(), command_forwarders.end()}) + { + } + + SubControllerManagerWrapper(const SubControllerManagerWrapper & other) = default; + + SubControllerManagerWrapper(SubControllerManagerWrapper && other) = default; + + ~SubControllerManagerWrapper() {} + + std::string get_namespace() const { return NAMESPACE_; } + + std::string get_controller_manager_name() const { return NAME_; } + + std::string get_name() const { return get_namespace() + "/" + get_controller_manager_name(); } + + std::vector get_state_publisher_descriptions() const + { + return state_publisher_descriptions_; + } + + std::vector get_command_forwarder_descriptions() const + { + return command_forwarder_descriptions_; + } + + size_t get_state_publisher_count() const { return state_publisher_descriptions_.size(); } + + size_t get_command_forwarder_count() const { return command_forwarder_descriptions_.size(); } + +private: + const std::string NAMESPACE_; + const std::string NAME_; + std::vector state_publisher_descriptions_; + std::vector command_forwarder_descriptions_; +}; + +} // namespace distributed_control + +#endif // DISTRIBUTED_CONTROL__SUB_CONTROLLER_MANAGER_WRAPPER_HPP_ \ No newline at end of file diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index 1aea017754..1e506bf0b2 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -18,48 +18,62 @@ #include #include +#include "hardware_interface/distributed_control_interface/publisher_description.hpp" #include "hardware_interface/macros.hpp" #include "hardware_interface/visibility_control.h" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "std_msgs/msg/float64.hpp" namespace hardware_interface { /// A handle used to get and set a value on a given interface. -class ReadOnlyHandle +class ReadHandleInterface { public: - ReadOnlyHandle( + virtual double get_value() const = 0; +}; + +class WriteHandleInterface +{ +public: + virtual void set_value(double value) = 0; +}; + +class HandleInterface +{ +public: + HandleInterface( const std::string & prefix_name, const std::string & interface_name, - double * value_ptr = nullptr) - : prefix_name_(prefix_name), interface_name_(interface_name), value_ptr_(value_ptr) + double * value_ptr = nullptr, std::shared_ptr node = nullptr) + : prefix_name_(prefix_name), interface_name_(interface_name), value_ptr_(value_ptr), node_(node) { } - explicit ReadOnlyHandle(const std::string & interface_name) - : interface_name_(interface_name), value_ptr_(nullptr) + explicit HandleInterface(const std::string & interface_name) + : interface_name_(interface_name), value_ptr_(nullptr), node_(nullptr) { } - explicit ReadOnlyHandle(const char * interface_name) - : interface_name_(interface_name), value_ptr_(nullptr) + explicit HandleInterface(const char * interface_name) + : interface_name_(interface_name), value_ptr_(nullptr), node_(nullptr) { } - ReadOnlyHandle(const ReadOnlyHandle & other) = default; + HandleInterface(const HandleInterface & other) = default; - ReadOnlyHandle(ReadOnlyHandle && other) = default; + HandleInterface(HandleInterface && other) = default; - ReadOnlyHandle & operator=(const ReadOnlyHandle & other) = default; + HandleInterface & operator=(const HandleInterface & other) = default; - ReadOnlyHandle & operator=(ReadOnlyHandle && other) = default; + HandleInterface & operator=(HandleInterface && other) = default; - virtual ~ReadOnlyHandle() = default; + virtual ~HandleInterface() = default; /// Returns true if handle references a value. inline operator bool() const { return value_ptr_ != nullptr; } - const std::string get_name() const { return prefix_name_ + "/" + interface_name_; } - - const std::string & get_interface_name() const { return interface_name_; } + std::string get_interface_name() const { return interface_name_; } [[deprecated( "Replaced by get_name method, which is semantically more correct")]] const std::string @@ -68,48 +82,95 @@ class ReadOnlyHandle return get_name(); } - const std::string & get_prefix_name() const { return prefix_name_; } + virtual std::string get_name() const { return prefix_name_ + "/" + interface_name_; } - double get_value() const + // TODO(Manuel): Maybe not the best place to put... + std::shared_ptr get_node() const { - THROW_ON_NULLPTR(value_ptr_); - return *value_ptr_; + THROW_ON_NULLPTR(node_); + if (!node_.get()) + { + throw std::runtime_error("Node not initialized!"); + } + return node_; + } + + std::string get_prefix_name() const { return prefix_name_; } + + /** + * @brief Create the full name consisting of prefix and interface name separated by an underscore. + * Used for e.g. name generation of nodes, where "/" are not allowed. + * + * @return std::string prefix_name + _ + interface_name. + */ + virtual std::string get_underscore_separated_name() const + { + return append_char(get_prefix_name(), '_') + get_interface_name(); } protected: + std::string append_char(std::string str, const char & char_to_append) const + { + if (!str.empty()) + { + return str + char_to_append; + } + return str; + } + + std::string erase_slash_at_start(std::string str) const + { + if (!str.empty()) + { + if (str.at(0) == '/') + { + return str.erase(0, 1); + } + } + return str; + } + + std::string replace_all_chars_from_string( + std::string str, const char & char_to_replace, const char & replace_with_char) const + { + std::replace(str.begin(), str.end(), char_to_replace, replace_with_char); + return str; + } + std::string prefix_name_; std::string interface_name_; double * value_ptr_; + std::shared_ptr node_; }; -class ReadWriteHandle : public ReadOnlyHandle +class ReadOnlyHandle : public HandleInterface, public ReadHandleInterface { public: - ReadWriteHandle( + ReadOnlyHandle( const std::string & prefix_name, const std::string & interface_name, double * value_ptr = nullptr) - : ReadOnlyHandle(prefix_name, interface_name, value_ptr) + : HandleInterface(prefix_name, interface_name, value_ptr) { } - explicit ReadWriteHandle(const std::string & interface_name) : ReadOnlyHandle(interface_name) {} + explicit ReadOnlyHandle(const std::string & interface_name) : HandleInterface(interface_name) {} - explicit ReadWriteHandle(const char * interface_name) : ReadOnlyHandle(interface_name) {} + explicit ReadOnlyHandle(const char * interface_name) : HandleInterface(interface_name) {} - ReadWriteHandle(const ReadWriteHandle & other) = default; + ReadOnlyHandle(const ReadOnlyHandle & other) = default; - ReadWriteHandle(ReadWriteHandle && other) = default; + ReadOnlyHandle(ReadOnlyHandle && other) = default; - ReadWriteHandle & operator=(const ReadWriteHandle & other) = default; + ReadOnlyHandle & operator=(const ReadOnlyHandle & other) = default; - ReadWriteHandle & operator=(ReadWriteHandle && other) = default; + ReadOnlyHandle & operator=(ReadOnlyHandle && other) = default; - virtual ~ReadWriteHandle() = default; + virtual ~ReadOnlyHandle() = default; - void set_value(double value) + double get_value() const override { - THROW_ON_NULLPTR(this->value_ptr_); - *this->value_ptr_ = value; + THROW_ON_NULLPTR(value_ptr_); + return *value_ptr_; } }; @@ -123,6 +184,134 @@ class StateInterface : public ReadOnlyHandle using ReadOnlyHandle::ReadOnlyHandle; }; +class DistributedReadOnlyHandle : public ReadOnlyHandle +{ +public: + // TODO(Manuel): We should pass the initial value via service call, so that the value_ of ReadOnlyHandle + // is initialized with a feasible value. + DistributedReadOnlyHandle( + const distributed_control::PublisherDescription & description, const std::string & ns = "/") + : ReadOnlyHandle(description.prefix_name(), description.interface_name(), &value_), + get_value_topic_name_(description.topic_name()), + namespace_(ns), + interface_namespace_(description.get_namespace()) + { + rclcpp::NodeOptions node_options; + // create node for subscribing to StatePublisher described in StatePublisherDescription + node_ = std::make_shared( + get_underscore_separated_name() + "_state_interface_subscriber", namespace_, node_options, + false); + + // subscribe to topic provided by StatePublisher + state_value_sub_ = node_->create_subscription( + get_value_topic_name_, 10, + std::bind(&DistributedReadOnlyHandle::get_value_cb, this, std::placeholders::_1)); + } + + explicit DistributedReadOnlyHandle(const std::string & interface_name) + : ReadOnlyHandle(interface_name) + { + } + + explicit DistributedReadOnlyHandle(const char * interface_name) : ReadOnlyHandle(interface_name) + { + } + + DistributedReadOnlyHandle() = delete; + + DistributedReadOnlyHandle(const DistributedReadOnlyHandle & other) = delete; + + DistributedReadOnlyHandle(DistributedReadOnlyHandle && other) = default; + + DistributedReadOnlyHandle & operator=(const DistributedReadOnlyHandle & other) = default; + + DistributedReadOnlyHandle & operator=(DistributedReadOnlyHandle && other) = default; + + virtual ~DistributedReadOnlyHandle() = default; + + virtual std::string get_name() const override + { + // concatenate: interface_namespace/prefix_name/interface_name to obtain + // a unique name. + return append_char(interface_namespace_, '/') + append_char(get_prefix_name(), '/') + + get_interface_name(); + } + + virtual std::string get_underscore_separated_name() const override + { + // remove first "/" from namespace and replace all follow occurrences of "/" with "_" + std::string ns = + replace_all_chars_from_string(erase_slash_at_start(interface_namespace_), '/', '_'); + // concatenate: interface_namespace + _ + namespace_prefix + _ + name_interface_name + return append_char(ns, '_') + append_char(get_prefix_name(), '_') + get_interface_name(); + } + +protected: + void get_value_cb(const std_msgs::msg::Float64 & msg) + { + value_ = msg.data; + RCLCPP_WARN_STREAM(node_->get_logger(), "Receiving:[" << value_ << "]."); + } + + std::string get_value_topic_name_; + // the current namespace we are in. Needed to create the node in the correct namespace + std::string namespace_; + // the namespace the actual StateInterface we subscribe to is in. + // We need this to create unique names for the StateInterface. + std::string interface_namespace_; + rclcpp::Subscription::SharedPtr state_value_sub_; + double value_; +}; + +class DistributedStateInterface : public DistributedReadOnlyHandle +{ +public: + DistributedStateInterface(const DistributedStateInterface & other) = default; + + DistributedStateInterface(DistributedStateInterface && other) = default; + + using DistributedReadOnlyHandle::DistributedReadOnlyHandle; +}; + +class ReadWriteHandle : public HandleInterface, + public ReadHandleInterface, + public WriteHandleInterface +{ +public: + ReadWriteHandle( + const std::string & prefix_name, const std::string & interface_name, + double * value_ptr = nullptr) + : HandleInterface(prefix_name, interface_name, value_ptr) + { + } + + explicit ReadWriteHandle(const std::string & interface_name) : HandleInterface(interface_name) {} + + explicit ReadWriteHandle(const char * interface_name) : HandleInterface(interface_name) {} + + ReadWriteHandle(const ReadWriteHandle & other) = default; + + ReadWriteHandle(ReadWriteHandle && other) = default; + + ReadWriteHandle & operator=(const ReadWriteHandle & other) = default; + + ReadWriteHandle & operator=(ReadWriteHandle && other) = default; + + virtual ~ReadWriteHandle() = default; + + virtual double get_value() const override + { + THROW_ON_NULLPTR(value_ptr_); + return *value_ptr_; + } + + virtual void set_value(double value) override + { + THROW_ON_NULLPTR(this->value_ptr_); + *this->value_ptr_ = value; + } +}; + class CommandInterface : public ReadWriteHandle { public: @@ -139,6 +328,118 @@ class CommandInterface : public ReadWriteHandle using ReadWriteHandle::ReadWriteHandle; }; +class DistributedReadWriteHandle : public ReadWriteHandle +{ +public: + DistributedReadWriteHandle( + const distributed_control::PublisherDescription & description, const std::string & ns = "/") + : ReadWriteHandle(description.prefix_name(), description.interface_name(), &value_), + get_value_topic_name_(description.topic_name()), + namespace_(ns), + interface_namespace_(description.get_namespace()), + forward_command_topic_name_(get_underscore_separated_name() + "_command_forwarding") + { + // create node for subscribing to StatePublisher described in StatePublisherDescription + rclcpp::NodeOptions node_options; + node_ = std::make_shared( + get_underscore_separated_name() + "_distributed_command_interface", namespace_, node_options, + false); + + // subscribe to topic provided by StatePublisher + command_value_sub_ = node_->create_subscription( + get_value_topic_name_, 10, + std::bind(&DistributedReadWriteHandle::get_value_cb, this, std::placeholders::_1)); + + // create publisher so that we can forward the commands + command_value_pub_ = + node_->create_publisher(forward_command_topic_name_, 10); + } + + explicit DistributedReadWriteHandle(const std::string & interface_name) + : ReadWriteHandle(interface_name) + { + } + + explicit DistributedReadWriteHandle(const char * interface_name) : ReadWriteHandle(interface_name) + { + } + + DistributedReadWriteHandle(const DistributedReadWriteHandle & other) = default; + + DistributedReadWriteHandle(DistributedReadWriteHandle && other) = default; + + DistributedReadWriteHandle & operator=(const DistributedReadWriteHandle & other) = default; + + DistributedReadWriteHandle & operator=(DistributedReadWriteHandle && other) = default; + + virtual ~DistributedReadWriteHandle() = default; + + virtual std::string get_name() const override + { + // concatenate: interface_namespace/prefix_name/interface_name to obtain + // a unique name. + return append_char(interface_namespace_, '/') + append_char(get_prefix_name(), '/') + + get_interface_name(); + } + + virtual std::string get_underscore_separated_name() const override + { + // remove first "/" from namespace and replace all follow occurrences with "_" + std::string ns = + replace_all_chars_from_string(erase_slash_at_start(interface_namespace_), '/', '_'); + // concatenate: interface_namespace + _ + namespace_prefix + _ + name_interface_name + return append_char(ns, '_') + append_char(get_prefix_name(), '_') + get_interface_name(); + } + + void set_value(double value) override + { + auto msg = std::make_unique(); + msg->data = value; + + RCLCPP_WARN(node_->get_logger(), "DistributedCommandInterface Publishing: '%.7lf'", msg->data); + std::flush(std::cout); + + command_value_pub_->publish(std::move(msg)); + } + + std::string forward_command_topic_name() const { return forward_command_topic_name_; } + +protected: + void get_value_cb(const std_msgs::msg::Float64 & msg) + { + value_ = msg.data; + RCLCPP_WARN_STREAM( + node_->get_logger(), "DistributedCommandInterface Receiving:[" << value_ << "]."); + } + + std::string get_value_topic_name_; + // the current namespace we are in. Needed to create the node in the correct namespace + std::string namespace_; + // the namespace the actual CommandInterface we subscribe to is in. + // We need this to create unique names for the CommandInterface. + std::string interface_namespace_; + rclcpp::Subscription::SharedPtr command_value_sub_; + std::string forward_command_topic_name_; + rclcpp::Publisher::SharedPtr command_value_pub_; + double value_; +}; + +class DistributedCommandInterface : public DistributedReadWriteHandle +{ +public: + /// CommandInterface copy constructor is actively deleted. + /** + * Command interfaces are having a unique ownership and thus + * can't be copied in order to avoid simultaneous writes to + * the same resource. + */ + DistributedCommandInterface(const DistributedCommandInterface & other) = delete; + + DistributedCommandInterface(DistributedCommandInterface && other) = default; + + using DistributedReadWriteHandle::DistributedReadWriteHandle; +}; + } // namespace hardware_interface #endif // HARDWARE_INTERFACE__HANDLE_HPP_ diff --git a/hardware_interface/include/hardware_interface/loaned_command_interface.hpp b/hardware_interface/include/hardware_interface/loaned_command_interface.hpp index bb5807c398..1658261541 100644 --- a/hardware_interface/include/hardware_interface/loaned_command_interface.hpp +++ b/hardware_interface/include/hardware_interface/loaned_command_interface.hpp @@ -28,12 +28,12 @@ class LoanedCommandInterface public: using Deleter = std::function; - explicit LoanedCommandInterface(CommandInterface & command_interface) + explicit LoanedCommandInterface(std::shared_ptr command_interface) : LoanedCommandInterface(command_interface, nullptr) { } - LoanedCommandInterface(CommandInterface & command_interface, Deleter && deleter) + LoanedCommandInterface(std::shared_ptr command_interface, Deleter && deleter) : command_interface_(command_interface), deleter_(std::forward(deleter)) { } @@ -50,25 +50,30 @@ class LoanedCommandInterface } } - const std::string get_name() const { return command_interface_.get_name(); } + std::string get_name() const { return command_interface_->get_name(); } - const std::string & get_interface_name() const { return command_interface_.get_interface_name(); } + std::string get_interface_name() const { return command_interface_->get_interface_name(); } [[deprecated( "Replaced by get_name method, which is semantically more correct")]] const std::string get_full_name() const { - return command_interface_.get_name(); + return command_interface_->get_name(); } - const std::string & get_prefix_name() const { return command_interface_.get_prefix_name(); } + std::string get_prefix_name() const { return command_interface_->get_prefix_name(); } - void set_value(double val) { command_interface_.set_value(val); } + void set_value(double val) { command_interface_->set_value(val); } - double get_value() const { return command_interface_.get_value(); } + double get_value() const { return command_interface_->get_value(); } + + std::string get_underscore_separated_name() const + { + return command_interface_->get_underscore_separated_name(); + } protected: - CommandInterface & command_interface_; + std::shared_ptr command_interface_; Deleter deleter_; }; diff --git a/hardware_interface/include/hardware_interface/loaned_state_interface.hpp b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp index 4fe67d1290..ed4d3bcbd5 100644 --- a/hardware_interface/include/hardware_interface/loaned_state_interface.hpp +++ b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp @@ -20,7 +20,6 @@ #include #include "hardware_interface/handle.hpp" - namespace hardware_interface { class LoanedStateInterface @@ -28,12 +27,12 @@ class LoanedStateInterface public: using Deleter = std::function; - explicit LoanedStateInterface(StateInterface & state_interface) + explicit LoanedStateInterface(std::shared_ptr state_interface) : LoanedStateInterface(state_interface, nullptr) { } - LoanedStateInterface(StateInterface & state_interface, Deleter && deleter) + LoanedStateInterface(std::shared_ptr state_interface, Deleter && deleter) : state_interface_(state_interface), deleter_(std::forward(deleter)) { } @@ -50,23 +49,28 @@ class LoanedStateInterface } } - const std::string get_name() const { return state_interface_.get_name(); } + std::string get_interface_name() const { return state_interface_->get_interface_name(); } - const std::string & get_interface_name() const { return state_interface_.get_interface_name(); } + std::string get_name() const { return state_interface_->get_name(); } [[deprecated( "Replaced by get_name method, which is semantically more correct")]] const std::string get_full_name() const { - return state_interface_.get_name(); + return state_interface_->get_name(); } - const std::string & get_prefix_name() const { return state_interface_.get_prefix_name(); } + std::string get_prefix_name() const { return state_interface_->get_prefix_name(); } + + std::string get_underscore_separated_name() const + { + return state_interface_->get_underscore_separated_name(); + } - double get_value() const { return state_interface_.get_value(); } + double get_value() const { return state_interface_->get_value(); } protected: - StateInterface & state_interface_; + std::shared_ptr state_interface_; Deleter deleter_; }; diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index 836f12eb81..9fafed6457 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -21,11 +21,16 @@ #include #include +#include "hardware_interface/distributed_control_interface/command_forwarder.hpp" +#include "hardware_interface/distributed_control_interface/state_publisher.hpp" +#include "hardware_interface/distributed_control_interface/sub_controller_manager_wrapper.hpp" +#include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_component_info.hpp" #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/loaned_state_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" + #include "rclcpp/duration.hpp" #include "rclcpp/time.hpp" @@ -124,6 +129,31 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager */ bool state_interface_is_available(const std::string & name) const; + void register_sub_controller_manager( + std::shared_ptr sub_controller_manager); + + std::vector> + import_state_interfaces_of_sub_controller_manager( + std::shared_ptr sub_controller_manager); + + std::vector> + import_command_interfaces_of_sub_controller_manager( + std::shared_ptr sub_controller_manager); + + std::vector> + create_hardware_state_publishers(const std::string & ns); + + std::vector> + create_hardware_command_forwarders(const std::string & ns); + + std::pair> find_command_forwarder( + const std::string & key); + + std::vector> get_state_publishers() const; + + std::vector> get_command_forwarders() + const; + /// Add controllers' reference interfaces to resource manager. /** * Interface for transferring management of reference interfaces to resource manager. diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index dda55a198b..1a1dbd275d 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -10,11 +10,14 @@ ament_cmake control_msgs + controller_manager_msgs lifecycle_msgs pluginlib + rclcpp rclcpp_lifecycle rcpputils tinyxml2_vendor + std_msgs rcutils rcutils diff --git a/hardware_interface/src/hardware_interface/distributed_control_interface/command_forwarder.cpp b/hardware_interface/src/hardware_interface/distributed_control_interface/command_forwarder.cpp new file mode 100644 index 0000000000..a8a1eefa98 --- /dev/null +++ b/hardware_interface/src/hardware_interface/distributed_control_interface/command_forwarder.cpp @@ -0,0 +1,114 @@ +#include "hardware_interface/distributed_control_interface/command_forwarder.hpp" + +#include +#include +#include +#include +#include + +using namespace std::chrono_literals; + +namespace distributed_control +{ + +CommandForwarder::CommandForwarder( + std::unique_ptr loaned_command_interface_ptr, + const std::string & ns) +: loaned_command_interface_ptr_(std::move(loaned_command_interface_ptr)), + namespace_(ns), + topic_name_(loaned_command_interface_ptr_->get_underscore_separated_name() + "_command_state") +{ + rclcpp::NodeOptions node_options; + node_ = std::make_shared( + loaned_command_interface_ptr_->get_underscore_separated_name() + "_command_forwarder", + namespace_, node_options, false); + + state_value_pub_ = node_->create_publisher(topic_name_, 10); + // TODO(Manuel): We should check if we cannot detect changes to LoanedStateInterface's value and only publish then + timer_ = + node_->create_wall_timer(50ms, std::bind(&CommandForwarder::publish_value_on_timer, this)); + RCLCPP_INFO(node_->get_logger(), "Creating CommandForwarder<%s>.", topic_name_.c_str()); +} + +std::shared_ptr CommandForwarder::get_node() const +{ + if (!node_.get()) + { + std::string msg( + "CommandForwarder<" + command_interface_name() + ">: Node hasn't been configured yet!"); + throw std::runtime_error(msg); + } + return node_; +} + +std::string CommandForwarder::get_namespace() const { return namespace_; } + +std::string CommandForwarder::topic_name() const { return topic_name_; } + +std::string CommandForwarder::topic_name_relative_to_namespace() const +{ + return get_namespace() + "/" + topic_name(); +} + +std::string CommandForwarder::command_interface_name() const +{ + return loaned_command_interface_ptr_->get_name(); +} + +std::string CommandForwarder::command_interface_prefix_name() const +{ + return loaned_command_interface_ptr_->get_prefix_name(); +} + +std::string CommandForwarder::command_interface_interface_name() const +{ + return loaned_command_interface_ptr_->get_interface_name(); +} + +controller_manager_msgs::msg::PublisherDescription +CommandForwarder::create_publisher_description_msg() const +{ + auto msg = controller_manager_msgs::msg::PublisherDescription(); + + msg.ns = get_namespace(); + msg.name.prefix_name = command_interface_prefix_name(); + msg.name.interface_name = command_interface_interface_name(); + msg.publisher_topic = topic_name_relative_to_namespace(); + + return msg; +} + +void CommandForwarder::subscribe_to_command_publisher(const std::string & topic_name) +{ + subscription_topic_name_ = topic_name; + command_subscription_ = node_->create_subscription( + subscription_topic_name_, 10, + std::bind(&CommandForwarder::forward_command, this, std::placeholders::_1)); +} + +void CommandForwarder::publish_value_on_timer() +{ + // Todo(Manuel) create custom msg and return success or failure not just nan. + auto msg = std::make_unique(); + try + { + msg->data = loaned_command_interface_ptr_->get_value(); + } + catch (const std::runtime_error & e) + { + msg->data = std::numeric_limits::quiet_NaN(); + } + RCLCPP_WARN(node_->get_logger(), "Publishing: '%.7lf'", msg->data); + std::flush(std::cout); + + // Put the message into a queue to be processed by the middleware. + // This call is non-blocking. + state_value_pub_->publish(std::move(msg)); +} + +void CommandForwarder::forward_command(const std_msgs::msg::Float64 & msg) +{ + loaned_command_interface_ptr_->set_value(msg.data); +} + +} // namespace distributed_control \ No newline at end of file diff --git a/hardware_interface/src/hardware_interface/distributed_control_interface/state_publisher.cpp b/hardware_interface/src/hardware_interface/distributed_control_interface/state_publisher.cpp new file mode 100644 index 0000000000..c84bfee1e5 --- /dev/null +++ b/hardware_interface/src/hardware_interface/distributed_control_interface/state_publisher.cpp @@ -0,0 +1,100 @@ +#include "hardware_interface/distributed_control_interface/state_publisher.hpp" + +#include +#include +#include +#include +#include + +using namespace std::chrono_literals; + +namespace distributed_control +{ + +StatePublisher::StatePublisher( + std::unique_ptr loaned_state_interface_ptr, + const std::string & ns) +: loaned_state_interface_ptr_(std::move(loaned_state_interface_ptr)), + namespace_(ns), + topic_name_(loaned_state_interface_ptr_->get_underscore_separated_name() + "_state") +{ + rclcpp::NodeOptions node_options; + node_ = std::make_shared( + loaned_state_interface_ptr_->get_underscore_separated_name() + "_state_publisher", namespace_, + node_options, false); + + state_value_pub_ = node_->create_publisher(topic_name_, 10); + // TODO(Manuel): We should check if we cannot detect changes to LoanedStateInterface's value and only publish then + timer_ = node_->create_wall_timer(50ms, std::bind(&StatePublisher::publish_value_on_timer, this)); + RCLCPP_INFO(node_->get_logger(), "Creating StatePublisher<%s>.", topic_name_.c_str()); +} + +std::shared_ptr StatePublisher::get_node() const +{ + if (!node_.get()) + { + std::string msg( + "StatePublisher<" + state_interface_name() + ">: Node hasn't been configured yet!"); + throw std::runtime_error(msg); + } + return node_; +} + +std::string StatePublisher::get_namespace() const { return namespace_; } + +std::string StatePublisher::topic_name() const { return topic_name_; } + +std::string StatePublisher::topic_name_relative_to_namespace() const +{ + return get_namespace() + "/" + topic_name(); +} + +std::string StatePublisher::state_interface_name() const +{ + return loaned_state_interface_ptr_->get_name(); +} + +std::string StatePublisher::state_interface_prefix_name() const +{ + return loaned_state_interface_ptr_->get_prefix_name(); +} + +std::string StatePublisher::state_interface_interface_name() const +{ + return loaned_state_interface_ptr_->get_interface_name(); +} + +controller_manager_msgs::msg::PublisherDescription +StatePublisher::create_publisher_description_msg() const +{ + auto msg = controller_manager_msgs::msg::PublisherDescription(); + msg.ns = get_namespace(); + msg.name.prefix_name = state_interface_prefix_name(); + msg.name.interface_name = state_interface_interface_name(); + msg.publisher_topic = topic_name_relative_to_namespace(); + + return msg; +} + +void StatePublisher::publish_value_on_timer() +{ + auto msg = std::make_unique(); + try + { + msg->data = loaned_state_interface_ptr_->get_value(); + } + catch (const std::runtime_error & e) + { + // Todo(Manuel) create custom msg and return success or failure not just nan. + // Make explicit note implicit!!! + msg->data = std::numeric_limits::quiet_NaN(); + } + RCLCPP_WARN(node_->get_logger(), "Publishing: '%.7lf'", msg->data); + std::flush(std::cout); + + // Put the message into a queue to be processed by the middleware. + // This call is non-blocking. + state_value_pub_->publish(std::move(msg)); +} + +} // namespace distributed_control \ No newline at end of file diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 42285a2839..35dd84a1aa 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -31,8 +31,10 @@ #include "hardware_interface/system.hpp" #include "hardware_interface/system_interface.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" + #include "lifecycle_msgs/msg/state.hpp" #include "pluginlib/class_loader.hpp" +#include "rclcpp/rclcpp.hpp" #include "rcutils/logging_macros.h" namespace hardware_interface @@ -408,17 +410,42 @@ class ResourceStorage return result; } + void add_state_interface(std::shared_ptr state_interface) + { + const auto [it, success] = + state_interface_map_.insert(std::make_pair(state_interface->get_name(), state_interface)); + if (!success) + { + std::string msg( + "ResourceStorage: Tried to insert StateInterface with already existing key. Insert[" + + state_interface->get_name() + "]"); + throw std::runtime_error(msg); + } + } + + void add_state_interface(const StateInterface & state_interface) + { + const auto [it, success] = state_interface_map_.emplace(std::make_pair( + state_interface.get_name(), std::make_shared(state_interface))); + if (!success) + { + std::string msg( + "ResourceStorage: Tried to insert StateInterface with already existing key. Insert[" + + state_interface.get_name() + "]"); + throw std::runtime_error(msg); + } + } + template void import_state_interfaces(HardwareT & hardware) { auto interfaces = hardware.export_state_interfaces(); std::vector interface_names; interface_names.reserve(interfaces.size()); - for (auto & interface : interfaces) + for (auto const & interface : interfaces) { - auto key = interface.get_name(); - state_interface_map_.emplace(std::make_pair(key, std::move(interface))); - interface_names.push_back(key); + add_state_interface(interface); + interface_names.push_back(interface.get_name()); } hardware_info_map_[hardware.get_name()].state_interfaces = interface_names; available_state_interfaces_.reserve( @@ -432,6 +459,33 @@ class ResourceStorage hardware_info_map_[hardware.get_name()].command_interfaces = add_command_interfaces(interfaces); } + void add_command_interface(std::shared_ptr command_interface) + { + const auto [it, success] = command_interface_map_.insert( + std::make_pair(command_interface->get_name(), command_interface)); + if (!success) + { + std::string msg( + "ResourceStorage: Tried to insert CommandInterface with already existing key. Insert[" + + command_interface->get_name() + "]"); + throw std::runtime_error(msg); + } + } + + void add_command_interface(CommandInterface && command_interface) + { + std::string key = command_interface.get_name(); + const auto [it, success] = command_interface_map_.emplace( + std::make_pair(key, std::make_shared(std::move(command_interface)))); + if (!success) + { + std::string msg( + "ResourceStorage: Tried to insert CommandInterface with already existing key. Insert[" + + key + "]"); + throw std::runtime_error(msg); + } + } + /// Adds exported command interfaces into internal storage. /** * Add command interfaces to the internal storage. Command interfaces exported from hardware or @@ -450,7 +504,7 @@ class ResourceStorage for (auto & interface : interfaces) { auto key = interface.get_name(); - command_interface_map_.emplace(std::make_pair(key, std::move(interface))); + add_command_interface(std::move(interface)); claimed_command_interface_map_.emplace(std::make_pair(key, false)); interface_names.push_back(key); } @@ -475,6 +529,75 @@ class ResourceStorage } } + void add_state_publisher(std::shared_ptr state_publisher) + { + const auto [it, success] = state_interface_state_publisher_map_.insert( + std::pair{state_publisher->state_interface_name(), state_publisher}); + if (!success) + { + std::string msg( + "ResourceStorage: Tried to insert StatePublisher with already existing key. Insert[" + + state_publisher->state_interface_name() + "]"); + throw std::runtime_error(msg); + } + } + + void add_command_forwarder( + std::shared_ptr command_forwarder) + { + const auto [it, success] = command_interface_command_forwarder_map_.insert( + std::pair{command_forwarder->command_interface_name(), command_forwarder}); + if (!success) + { + std::string msg( + "ResourceStorage: Tried to insert CommandForwarder with already existing key. Insert[" + + command_forwarder->command_interface_name() + "]"); + throw std::runtime_error(msg); + } + } + + std::shared_ptr get_state_publisher( + std::string state_interface_name) const + { + return state_interface_state_publisher_map_.at(state_interface_name); + } + + std::vector> get_state_publishers() const + { + std::vector> state_publishers_vec; + state_publishers_vec.reserve(state_interface_state_publisher_map_.size()); + + for (auto state_publisher : state_interface_state_publisher_map_) + { + state_publishers_vec.push_back(state_publisher.second); + } + return state_publishers_vec; + } + + std::vector> get_command_forwarders() const + { + std::vector> command_forwarders_vec; + command_forwarders_vec.reserve(command_interface_command_forwarder_map_.size()); + + for (auto command_forwarder : command_interface_command_forwarder_map_) + { + command_forwarders_vec.push_back(command_forwarder.second); + } + return command_forwarders_vec; + } + + std::pair> find_command_forwarder( + const std::string & key) + { + auto command_forwarder = command_interface_command_forwarder_map_.find(key); + // we could not find a command forwarder for the provided key + if (command_forwarder == command_interface_command_forwarder_map_.end()) + { + return std::make_pair(false, nullptr); + } + return std::make_pair(true, command_forwarder->second); + } + void check_for_duplicates(const HardwareInfo & hardware_info) { // Check for identical names @@ -539,6 +662,128 @@ class ResourceStorage import_command_interfaces(systems_.back()); } + void add_sub_controller_manager( + std::shared_ptr sub_controller_manager) + { + const auto [it, success] = sub_controller_manager_map_.insert( + std::pair{sub_controller_manager->get_name(), sub_controller_manager}); + if (!success) + { + std::string msg( + "ResourceStorage: Tried to add sub ControllerManager with already existing key. Insert[" + + sub_controller_manager->get_name() + "]"); + throw std::runtime_error(msg); + } + } + + std::vector> import_distributed_state_interfaces( + std::shared_ptr sub_controller_manager) + { + std::vector> distributed_state_interfaces; + distributed_state_interfaces.reserve(sub_controller_manager->get_state_publisher_count()); + std::vector interface_names; + interface_names.reserve(sub_controller_manager->get_state_publisher_count()); + + for (const auto & state_publisher_description : + sub_controller_manager->get_state_publisher_descriptions()) + { + // create StateInterface from the Description and store in ResourceStorage. + auto state_interface = + std::make_shared(state_publisher_description); + + add_state_interface(state_interface); + // add to return vector, node needs to added to executor. + distributed_state_interfaces.push_back(state_interface); + interface_names.push_back(state_interface->get_name()); + } + // TODO(Manuel) this should be handled at one point DRY (adding, hardware_info_map, make available...), key should be made explicit + hardware_info_map_[sub_controller_manager->get_name()].state_interfaces = interface_names; + available_state_interfaces_.reserve( + available_state_interfaces_.capacity() + interface_names.size()); + + for (const auto & interface : + hardware_info_map_[sub_controller_manager->get_name()].state_interfaces) + { + // add all state interfaces to available list + auto found_it = std::find( + available_state_interfaces_.begin(), available_state_interfaces_.end(), interface); + + if (found_it == available_state_interfaces_.end()) + { + available_state_interfaces_.emplace_back(interface); + RCUTILS_LOG_DEBUG_NAMED( + "resource_manager", "(hardware '%s'): '%s' state interface added into available list", + sub_controller_manager->get_name().c_str(), interface.c_str()); + } + else + { + // TODO(destogl): do here error management if interfaces are only partially added into + // "available" list - this should never be the case! + RCUTILS_LOG_WARN_NAMED( + "resource_manager", + "(hardware '%s'): '%s' state interface already in available list." + " This can happen due to multiple calls to 'configure'", + sub_controller_manager->get_name().c_str(), interface.c_str()); + } + } + return distributed_state_interfaces; + } + + std::vector> import_distributed_command_interfaces( + std::shared_ptr sub_controller_manager) + { + std::vector> distributed_command_interfaces; + distributed_command_interfaces.reserve(sub_controller_manager->get_command_forwarder_count()); + std::vector interface_names; + interface_names.reserve(sub_controller_manager->get_command_forwarder_count()); + + for (const auto & command_forwarder_description : + sub_controller_manager->get_command_forwarder_descriptions()) + { + // create StateInterface from the Description and store in ResourceStorage. + auto command_interface = + std::make_shared(command_forwarder_description); + add_command_interface(command_interface); + // add to return vector, node needs to added to executor. + distributed_command_interfaces.push_back(command_interface); + // TODO(Manuel) this should be handled at one point DRY (adding, claimed ....), key should be made explicit + claimed_command_interface_map_.emplace(std::make_pair(command_interface->get_name(), false)); + interface_names.push_back(command_interface->get_name()); + } + // TODO(Manuel) this should be handled at one point DRY(adding, claimed,make available....), key should be made explicit + available_command_interfaces_.reserve( + available_command_interfaces_.capacity() + interface_names.size()); + hardware_info_map_[sub_controller_manager->get_name()].command_interfaces = interface_names; + + for (const auto & interface : + hardware_info_map_[sub_controller_manager->get_name()].command_interfaces) + { + // TODO(destogl): check if interface should be available on configure + auto found_it = std::find( + available_command_interfaces_.begin(), available_command_interfaces_.end(), interface); + + if (found_it == available_command_interfaces_.end()) + { + available_command_interfaces_.emplace_back(interface); + RCUTILS_LOG_DEBUG_NAMED( + "resource_manager", "(hardware '%s'): '%s' command interface added into available list", + sub_controller_manager->get_name().c_str(), interface.c_str()); + } + else + { + // TODO(destogl): do here error management if interfaces are only partially added into + // "available" list - this should never be the case! + RCUTILS_LOG_WARN_NAMED( + "resource_manager", + "(hardware '%s'): '%s' command interface already in available list." + " This can happen due to multiple calls to 'configure'", + sub_controller_manager->get_name().c_str(), interface.c_str()); + } + } + + return distributed_command_interfaces; + } + // hardware plugins pluginlib::ClassLoader actuator_loader_; pluginlib::ClassLoader sensor_loader_; @@ -557,9 +802,9 @@ class ResourceStorage std::unordered_map> controllers_reference_interfaces_map_; /// Storage of all available state interfaces - std::map state_interface_map_; + std::map> state_interface_map_; /// Storage of all available command interfaces - std::map command_interface_map_; + std::map> command_interface_map_; /// Vectors with interfaces available to controllers (depending on hardware component state) std::vector available_state_interfaces_; @@ -567,6 +812,16 @@ class ResourceStorage /// List of all claimed command interfaces std::unordered_map claimed_command_interface_map_; + +private: + std::map> + state_interface_state_publisher_map_; + + std::map> + command_interface_command_forwarder_map_; + + std::map> + sub_controller_manager_map_; }; ResourceManager::ResourceManager() : resource_storage_(std::make_unique()) {} @@ -675,6 +930,91 @@ bool ResourceManager::state_interface_is_available(const std::string & name) con name) != resource_storage_->available_state_interfaces_.end(); } +void ResourceManager::register_sub_controller_manager( + std::shared_ptr sub_controller_manager) +{ + std::lock_guard guard(resource_interfaces_lock_); + resource_storage_->add_sub_controller_manager(sub_controller_manager); +} + +std::vector> +ResourceManager::import_state_interfaces_of_sub_controller_manager( + std::shared_ptr sub_controller_manager) +{ + std::lock_guard guard(resource_interfaces_lock_); + return resource_storage_->import_distributed_state_interfaces(sub_controller_manager); +} + +std::vector> +ResourceManager::import_command_interfaces_of_sub_controller_manager( + std::shared_ptr sub_controller_manager) +{ + std::lock_guard guard(resource_interfaces_lock_); + return resource_storage_->import_distributed_command_interfaces(sub_controller_manager); +} + +std::vector> +ResourceManager::create_hardware_state_publishers(const std::string & ns) +{ + std::lock_guard guard(resource_interfaces_lock_); + std::vector> state_publishers_vec; + state_publishers_vec.reserve(available_state_interfaces().size()); + + for (const auto & state_interface : available_state_interfaces()) + { + auto state_publisher = std::make_shared( + std::move(std::make_unique( + claim_state_interface(state_interface))), + ns); + + resource_storage_->add_state_publisher(state_publisher); + state_publishers_vec.push_back(state_publisher); + } + + return state_publishers_vec; +} + +std::vector> +ResourceManager::create_hardware_command_forwarders(const std::string & ns) +{ + std::lock_guard guard(resource_interfaces_lock_); + std::vector> command_forwarders_vec; + command_forwarders_vec.reserve(available_command_interfaces().size()); + + for (const auto & command_interface : available_command_interfaces()) + { + auto command_forwarder = std::make_shared( + std::move(std::make_unique( + claim_command_interface(command_interface))), + ns); + + resource_storage_->add_command_forwarder(command_forwarder); + command_forwarders_vec.push_back(command_forwarder); + } + + return command_forwarders_vec; +} + +std::vector> +ResourceManager::get_state_publishers() const +{ + std::lock_guard guard(resource_interfaces_lock_); + return resource_storage_->get_state_publishers(); +} + +std::vector> +ResourceManager::get_command_forwarders() const +{ + std::lock_guard guard(resource_interfaces_lock_); + return resource_storage_->get_command_forwarders(); +} + +std::pair> +ResourceManager::find_command_forwarder(const std::string & key) +{ + return resource_storage_->find_command_forwarder(key); +} + // CM API: Called in "callback/slow"-thread void ResourceManager::import_controller_reference_interfaces( const std::string & controller_name, std::vector & interfaces) From 984d896bc063851dde91c26b4b40938e125cd74e Mon Sep 17 00:00:00 2001 From: Manuel M Date: Mon, 24 Apr 2023 07:52:33 +0200 Subject: [PATCH 12/26] add parameter for setting publish period --- .../controller_manager/controller_manager.hpp | 8 +++++ controller_manager/src/controller_manager.cpp | 34 ++++++++++++++++--- .../command_forwarder.hpp | 7 ++-- .../state_publisher.hpp | 4 ++- .../hardware_interface/resource_manager.hpp | 5 +-- .../command_forwarder.cpp | 7 ++-- .../state_publisher.cpp | 6 ++-- hardware_interface/src/resource_manager.cpp | 10 +++--- 8 files changed, 62 insertions(+), 19 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index fde8e21566..78996cebe3 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -198,6 +198,10 @@ class ControllerManager : public rclcpp::Node CONTROLLER_MANAGER_PUBLIC unsigned int get_update_rate() const; + // Per controller update rate support + CONTROLLER_MANAGER_PUBLIC + std::chrono::milliseconds distributed_interfaces_publish_period() const; + protected: CONTROLLER_MANAGER_PUBLIC void init_services(); @@ -205,6 +209,9 @@ class ControllerManager : public rclcpp::Node CONTROLLER_MANAGER_PUBLIC void configure_controller_manager(); + CONTROLLER_MANAGER_PUBLIC + void init_distributed_sub_controller_manager(); + CONTROLLER_MANAGER_PUBLIC void init_distributed_main_controller_services(); @@ -430,6 +437,7 @@ class ControllerManager : public rclcpp::Node bool distributed_ = false; bool sub_controller_manager_ = false; + std::chrono::milliseconds distributed_interfaces_publish_period_ = std::chrono::milliseconds(12); rclcpp::CallbackGroup::SharedPtr distributed_system_srv_callback_group_; /** diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 976e76909b..c34b110428 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -379,9 +379,7 @@ void ControllerManager::configure_controller_manager() bool central_controller_manager = distributed_ && !sub_controller_manager_; if (distributed_sub_controller_manager) { - add_hardware_state_publishers(); - add_hardware_command_forwarders(); - register_sub_controller_manager(); + init_distributed_sub_controller_manager(); } // This means we are the central controller manager else if (central_controller_manager) @@ -400,6 +398,25 @@ void ControllerManager::configure_controller_manager() } } +void ControllerManager::init_distributed_sub_controller_manager() +{ + int64_t distributed_interfaces_publish_period; + if (get_parameter("distributed_interfaces_publish_period", distributed_interfaces_publish_period)) + { + distributed_interfaces_publish_period_ = + std::chrono::milliseconds(distributed_interfaces_publish_period); + } + else + { + RCLCPP_WARN( + get_logger(), + "'distributed_interfaces_publish_period' parameter not set, using default value."); + } + add_hardware_state_publishers(); + add_hardware_command_forwarders(); + register_sub_controller_manager(); +} + void ControllerManager::init_distributed_main_controller_services() { distributed_system_srv_callback_group_ = @@ -490,7 +507,8 @@ void ControllerManager::add_hardware_state_publishers() { std::vector> state_publishers_vec; state_publishers_vec.reserve(resource_manager_->available_state_interfaces().size()); - state_publishers_vec = resource_manager_->create_hardware_state_publishers(get_namespace()); + state_publishers_vec = resource_manager_->create_hardware_state_publishers( + get_namespace(), distributed_interfaces_publish_period()); for (auto const & state_publisher : state_publishers_vec) { @@ -511,7 +529,8 @@ void ControllerManager::add_hardware_command_forwarders() { std::vector> command_forwarder_vec; command_forwarder_vec.reserve(resource_manager_->available_command_interfaces().size()); - command_forwarder_vec = resource_manager_->create_hardware_command_forwarders(get_namespace()); + command_forwarder_vec = resource_manager_->create_hardware_command_forwarders( + get_namespace(), distributed_interfaces_publish_period()); for (auto const & command_forwarder : command_forwarder_vec) { @@ -2235,6 +2254,11 @@ std::pair ControllerManager::split_command_interface( unsigned int ControllerManager::get_update_rate() const { return update_rate_; } +std::chrono::milliseconds ControllerManager::distributed_interfaces_publish_period() const +{ + return distributed_interfaces_publish_period_; +} + void ControllerManager::propagate_deactivation_of_chained_mode( const std::vector & controllers) { diff --git a/hardware_interface/include/hardware_interface/distributed_control_interface/command_forwarder.hpp b/hardware_interface/include/hardware_interface/distributed_control_interface/command_forwarder.hpp index 3597dbaf00..dc3937ef5e 100644 --- a/hardware_interface/include/hardware_interface/distributed_control_interface/command_forwarder.hpp +++ b/hardware_interface/include/hardware_interface/distributed_control_interface/command_forwarder.hpp @@ -1,6 +1,7 @@ #ifndef DISTRIBUTED_CONTROL__COMMAND_FORWARDER_HPP_ #define DISTRIBUTED_CONTROL__COMMAND_FORWARDER_HPP_ +#include #include #include #include @@ -22,8 +23,8 @@ class CommandForwarder final { public: explicit CommandForwarder( - std::unique_ptr loaned_command_interface_ptr_, - const std::string & ns = ""); + std::unique_ptr loaned_command_interface_ptr, + std::chrono::milliseconds period_in_ms, const std::string & ns = ""); CommandForwarder() = delete; @@ -53,6 +54,8 @@ class CommandForwarder final std::unique_ptr loaned_command_interface_ptr_; const std::string namespace_; + const std::chrono::milliseconds period_in_ms_; + const std::string topic_name_; std::string subscription_topic_name_; std::shared_ptr node_; diff --git a/hardware_interface/include/hardware_interface/distributed_control_interface/state_publisher.hpp b/hardware_interface/include/hardware_interface/distributed_control_interface/state_publisher.hpp index 6fe316c4b1..2d915a494d 100644 --- a/hardware_interface/include/hardware_interface/distributed_control_interface/state_publisher.hpp +++ b/hardware_interface/include/hardware_interface/distributed_control_interface/state_publisher.hpp @@ -22,7 +22,7 @@ class StatePublisher final public: explicit StatePublisher( std::unique_ptr loaned_state_interface_ptr, - const std::string & ns = ""); + std::chrono::milliseconds period_in_ms, const std::string & ns = ""); StatePublisher() = delete; @@ -49,6 +49,8 @@ class StatePublisher final std::unique_ptr loaned_state_interface_ptr_; const std::string namespace_; + std::chrono::milliseconds period_in_ms_; + const std::string topic_name_; std::shared_ptr node_; rclcpp::Publisher::SharedPtr state_value_pub_; diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index 9fafed6457..b7c3fe3fb7 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -141,10 +141,11 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager std::shared_ptr sub_controller_manager); std::vector> - create_hardware_state_publishers(const std::string & ns); + create_hardware_state_publishers(const std::string & ns, std::chrono::milliseconds update_period); std::vector> - create_hardware_command_forwarders(const std::string & ns); + create_hardware_command_forwarders( + const std::string & ns, std::chrono::milliseconds update_period); std::pair> find_command_forwarder( const std::string & key); diff --git a/hardware_interface/src/hardware_interface/distributed_control_interface/command_forwarder.cpp b/hardware_interface/src/hardware_interface/distributed_control_interface/command_forwarder.cpp index a8a1eefa98..3ed689adba 100644 --- a/hardware_interface/src/hardware_interface/distributed_control_interface/command_forwarder.cpp +++ b/hardware_interface/src/hardware_interface/distributed_control_interface/command_forwarder.cpp @@ -13,8 +13,9 @@ namespace distributed_control CommandForwarder::CommandForwarder( std::unique_ptr loaned_command_interface_ptr, - const std::string & ns) + std::chrono::milliseconds period_in_ms, const std::string & ns) : loaned_command_interface_ptr_(std::move(loaned_command_interface_ptr)), + period_in_ms_(period_in_ms), namespace_(ns), topic_name_(loaned_command_interface_ptr_->get_underscore_separated_name() + "_command_state") { @@ -25,8 +26,8 @@ CommandForwarder::CommandForwarder( state_value_pub_ = node_->create_publisher(topic_name_, 10); // TODO(Manuel): We should check if we cannot detect changes to LoanedStateInterface's value and only publish then - timer_ = - node_->create_wall_timer(50ms, std::bind(&CommandForwarder::publish_value_on_timer, this)); + timer_ = node_->create_wall_timer( + period_in_ms_, std::bind(&CommandForwarder::publish_value_on_timer, this)); RCLCPP_INFO(node_->get_logger(), "Creating CommandForwarder<%s>.", topic_name_.c_str()); } diff --git a/hardware_interface/src/hardware_interface/distributed_control_interface/state_publisher.cpp b/hardware_interface/src/hardware_interface/distributed_control_interface/state_publisher.cpp index c84bfee1e5..957f3673ea 100644 --- a/hardware_interface/src/hardware_interface/distributed_control_interface/state_publisher.cpp +++ b/hardware_interface/src/hardware_interface/distributed_control_interface/state_publisher.cpp @@ -13,8 +13,9 @@ namespace distributed_control StatePublisher::StatePublisher( std::unique_ptr loaned_state_interface_ptr, - const std::string & ns) + std::chrono::milliseconds period_in_ms, const std::string & ns) : loaned_state_interface_ptr_(std::move(loaned_state_interface_ptr)), + period_in_ms_(period_in_ms), namespace_(ns), topic_name_(loaned_state_interface_ptr_->get_underscore_separated_name() + "_state") { @@ -25,7 +26,8 @@ StatePublisher::StatePublisher( state_value_pub_ = node_->create_publisher(topic_name_, 10); // TODO(Manuel): We should check if we cannot detect changes to LoanedStateInterface's value and only publish then - timer_ = node_->create_wall_timer(50ms, std::bind(&StatePublisher::publish_value_on_timer, this)); + timer_ = node_->create_wall_timer( + period_in_ms_, std::bind(&StatePublisher::publish_value_on_timer, this)); RCLCPP_INFO(node_->get_logger(), "Creating StatePublisher<%s>.", topic_name_.c_str()); } diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 35dd84a1aa..20b480be5f 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -954,7 +954,8 @@ ResourceManager::import_command_interfaces_of_sub_controller_manager( } std::vector> -ResourceManager::create_hardware_state_publishers(const std::string & ns) +ResourceManager::create_hardware_state_publishers( + const std::string & ns, std::chrono::milliseconds update_period) { std::lock_guard guard(resource_interfaces_lock_); std::vector> state_publishers_vec; @@ -965,7 +966,7 @@ ResourceManager::create_hardware_state_publishers(const std::string & ns) auto state_publisher = std::make_shared( std::move(std::make_unique( claim_state_interface(state_interface))), - ns); + update_period, ns); resource_storage_->add_state_publisher(state_publisher); state_publishers_vec.push_back(state_publisher); @@ -975,7 +976,8 @@ ResourceManager::create_hardware_state_publishers(const std::string & ns) } std::vector> -ResourceManager::create_hardware_command_forwarders(const std::string & ns) +ResourceManager::create_hardware_command_forwarders( + const std::string & ns, std::chrono::milliseconds update_period) { std::lock_guard guard(resource_interfaces_lock_); std::vector> command_forwarders_vec; @@ -986,7 +988,7 @@ ResourceManager::create_hardware_command_forwarders(const std::string & ns) auto command_forwarder = std::make_shared( std::move(std::make_unique( claim_command_interface(command_interface))), - ns); + update_period, ns); resource_storage_->add_command_forwarder(command_forwarder); command_forwarders_vec.push_back(command_forwarder); From bc8f800b6914b2af9222a9a55d6f1b78aa0c3eea Mon Sep 17 00:00:00 2001 From: Manuel M Date: Mon, 24 Apr 2023 08:21:55 +0200 Subject: [PATCH 13/26] change argument order for state_publisher and command_forwarder --- .../distributed_control_interface/command_forwarder.hpp | 2 +- .../distributed_control_interface/state_publisher.hpp | 4 ++-- .../distributed_control_interface/command_forwarder.cpp | 4 ++-- .../distributed_control_interface/state_publisher.cpp | 4 ++-- hardware_interface/src/resource_manager.cpp | 4 ++-- 5 files changed, 9 insertions(+), 9 deletions(-) diff --git a/hardware_interface/include/hardware_interface/distributed_control_interface/command_forwarder.hpp b/hardware_interface/include/hardware_interface/distributed_control_interface/command_forwarder.hpp index dc3937ef5e..86077913ba 100644 --- a/hardware_interface/include/hardware_interface/distributed_control_interface/command_forwarder.hpp +++ b/hardware_interface/include/hardware_interface/distributed_control_interface/command_forwarder.hpp @@ -24,7 +24,7 @@ class CommandForwarder final public: explicit CommandForwarder( std::unique_ptr loaned_command_interface_ptr, - std::chrono::milliseconds period_in_ms, const std::string & ns = ""); + const std::string & ns, std::chrono::milliseconds period_in_ms); CommandForwarder() = delete; diff --git a/hardware_interface/include/hardware_interface/distributed_control_interface/state_publisher.hpp b/hardware_interface/include/hardware_interface/distributed_control_interface/state_publisher.hpp index 2d915a494d..89f07c1e1d 100644 --- a/hardware_interface/include/hardware_interface/distributed_control_interface/state_publisher.hpp +++ b/hardware_interface/include/hardware_interface/distributed_control_interface/state_publisher.hpp @@ -22,7 +22,7 @@ class StatePublisher final public: explicit StatePublisher( std::unique_ptr loaned_state_interface_ptr, - std::chrono::milliseconds period_in_ms, const std::string & ns = ""); + const std::string & ns, std::chrono::milliseconds period_in_ms); StatePublisher() = delete; @@ -49,7 +49,7 @@ class StatePublisher final std::unique_ptr loaned_state_interface_ptr_; const std::string namespace_; - std::chrono::milliseconds period_in_ms_; + const std::chrono::milliseconds period_in_ms_; const std::string topic_name_; std::shared_ptr node_; diff --git a/hardware_interface/src/hardware_interface/distributed_control_interface/command_forwarder.cpp b/hardware_interface/src/hardware_interface/distributed_control_interface/command_forwarder.cpp index 3ed689adba..12b8024bd9 100644 --- a/hardware_interface/src/hardware_interface/distributed_control_interface/command_forwarder.cpp +++ b/hardware_interface/src/hardware_interface/distributed_control_interface/command_forwarder.cpp @@ -13,10 +13,10 @@ namespace distributed_control CommandForwarder::CommandForwarder( std::unique_ptr loaned_command_interface_ptr, - std::chrono::milliseconds period_in_ms, const std::string & ns) + const std::string & ns, std::chrono::milliseconds period_in_ms) : loaned_command_interface_ptr_(std::move(loaned_command_interface_ptr)), - period_in_ms_(period_in_ms), namespace_(ns), + period_in_ms_(period_in_ms), topic_name_(loaned_command_interface_ptr_->get_underscore_separated_name() + "_command_state") { rclcpp::NodeOptions node_options; diff --git a/hardware_interface/src/hardware_interface/distributed_control_interface/state_publisher.cpp b/hardware_interface/src/hardware_interface/distributed_control_interface/state_publisher.cpp index 957f3673ea..b963d7b3cb 100644 --- a/hardware_interface/src/hardware_interface/distributed_control_interface/state_publisher.cpp +++ b/hardware_interface/src/hardware_interface/distributed_control_interface/state_publisher.cpp @@ -13,10 +13,10 @@ namespace distributed_control StatePublisher::StatePublisher( std::unique_ptr loaned_state_interface_ptr, - std::chrono::milliseconds period_in_ms, const std::string & ns) + const std::string & ns, std::chrono::milliseconds period_in_ms) : loaned_state_interface_ptr_(std::move(loaned_state_interface_ptr)), - period_in_ms_(period_in_ms), namespace_(ns), + period_in_ms_(period_in_ms), topic_name_(loaned_state_interface_ptr_->get_underscore_separated_name() + "_state") { rclcpp::NodeOptions node_options; diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 20b480be5f..cc243caf6e 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -966,7 +966,7 @@ ResourceManager::create_hardware_state_publishers( auto state_publisher = std::make_shared( std::move(std::make_unique( claim_state_interface(state_interface))), - update_period, ns); + ns, update_period); resource_storage_->add_state_publisher(state_publisher); state_publishers_vec.push_back(state_publisher); @@ -988,7 +988,7 @@ ResourceManager::create_hardware_command_forwarders( auto command_forwarder = std::make_shared( std::move(std::make_unique( claim_command_interface(command_interface))), - update_period, ns); + ns, update_period); resource_storage_->add_command_forwarder(command_forwarder); command_forwarders_vec.push_back(command_forwarder); From 7043187958620eb03e507c81f601af4eed4c4ebd Mon Sep 17 00:00:00 2001 From: Manuel M Date: Mon, 24 Apr 2023 16:00:39 +0200 Subject: [PATCH 14/26] make possible to create only on node for pub/sub in distributed cm --- .../controller_manager/controller_manager.hpp | 28 ++++- controller_manager/src/controller_manager.cpp | 118 +++++++++++++----- .../command_forwarder.hpp | 3 +- .../state_publisher.hpp | 4 +- .../include/hardware_interface/handle.hpp | 47 ++++--- .../hardware_interface/resource_manager.hpp | 13 +- .../command_forwarder.cpp | 16 ++- .../state_publisher.cpp | 16 ++- hardware_interface/src/resource_manager.cpp | 30 +++-- 9 files changed, 194 insertions(+), 81 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 78996cebe3..4c6ba20ad7 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -62,6 +62,8 @@ rclcpp::NodeOptions get_cm_node_options(); class ControllerManager : public rclcpp::Node { + enum class controller_manager_type : std::uint8_t; + public: static constexpr bool kWaitForAllResources = false; static constexpr auto kInfiniteTimeout = 0; @@ -198,6 +200,10 @@ class ControllerManager : public rclcpp::Node CONTROLLER_MANAGER_PUBLIC unsigned int get_update_rate() const; + // Per controller update rate support + CONTROLLER_MANAGER_PUBLIC + bool use_multiple_nodes() const; + // Per controller update rate support CONTROLLER_MANAGER_PUBLIC std::chrono::milliseconds distributed_interfaces_publish_period() const; @@ -207,13 +213,21 @@ class ControllerManager : public rclcpp::Node void init_services(); CONTROLLER_MANAGER_PUBLIC - void configure_controller_manager(); + void get_and_initialize_distributed_parameters(); + + CONTROLLER_MANAGER_PUBLIC + controller_manager_type determine_controller_manager_type(); + + CONTROLLER_MANAGER_PUBLIC + void configure_controller_manager(const controller_manager_type & cm_type); CONTROLLER_MANAGER_PUBLIC void init_distributed_sub_controller_manager(); CONTROLLER_MANAGER_PUBLIC - void init_distributed_main_controller_services(); + void init_distributed_central_controller_manager(); + + CONTROLLER_MANAGER_PUBLIC void init_distributed_central_controller_manager_services(); CONTROLLER_MANAGER_PUBLIC void register_sub_controller_manager_srv_cb( @@ -435,8 +449,18 @@ class ControllerManager : public rclcpp::Node */ rclcpp::CallbackGroup::SharedPtr best_effort_callback_group_; + enum class controller_manager_type : std::uint8_t + { + standard_controller_manager, + distributed_central_controller_manager, + distributed_sub_controller_manager, + unkown_type // indicating something went wrong and type could not be determined + }; + bool distributed_ = false; bool sub_controller_manager_ = false; + bool use_multiple_nodes_ = false; + std::shared_ptr distributed_pub_sub_node_ = nullptr; std::chrono::milliseconds distributed_interfaces_publish_period_ = std::chrono::milliseconds(12); rclcpp::CallbackGroup::SharedPtr distributed_system_srv_callback_group_; diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index c34b110428..39dc4fb8b2 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -181,7 +181,9 @@ ControllerManager::ControllerManager( diagnostics_updater_.add( "Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback); init_services(); - configure_controller_manager(); + get_and_initialize_distributed_parameters(); + auto cm_type = determine_controller_manager_type(); + configure_controller_manager(cm_type); } ControllerManager::ControllerManager( @@ -209,7 +211,9 @@ ControllerManager::ControllerManager( diagnostics_updater_.add( "Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback); init_services(); - configure_controller_manager(); + get_and_initialize_distributed_parameters(); + auto cm_type = determine_controller_manager_type(); + configure_controller_manager(cm_type); } void ControllerManager::subscribe_to_robot_description_topic() @@ -354,11 +358,7 @@ void ControllerManager::init_services() qos_services, best_effort_callback_group_); } -// TODO(Manuel) don't like this, this is for fast poc -// probably better to create factory and handle creation of correct controller manager type -// there. Since asynchronous control should be supported im the future as well and we don't -// want dozen of ifs. -void ControllerManager::configure_controller_manager() +void ControllerManager::get_and_initialize_distributed_parameters() { if (!get_parameter("distributed", distributed_)) { @@ -374,50 +374,102 @@ void ControllerManager::configure_controller_manager() sub_controller_manager_ ? "true" : "false"); } + int64_t distributed_interfaces_publish_period; + if (get_parameter("distributed_interfaces_publish_period", distributed_interfaces_publish_period)) + { + distributed_interfaces_publish_period_ = + std::chrono::milliseconds(distributed_interfaces_publish_period); + } + + else + { + RCLCPP_WARN( + get_logger(), + "'distributed_interfaces_publish_period' parameter not set, using default value."); + } + + if (!get_parameter("use_multiple_nodes", use_multiple_nodes_)) + { + RCLCPP_WARN( + get_logger(), "'use_multiple_nodes' parameter not set, using default value:%s", + use_multiple_nodes_ ? "true" : "false"); + } +} + +void ControllerManager::configure_controller_manager(const controller_manager_type & cm_type) +{ + switch (cm_type) + { + case controller_manager_type::distributed_central_controller_manager: + init_distributed_central_controller_manager(); + break; + + case controller_manager_type::distributed_sub_controller_manager: + init_distributed_sub_controller_manager(); + break; + case controller_manager_type::standard_controller_manager: + //nothing special to configure + break; + default: + throw std::logic_error( + "Controller manager configuration not possible. Not a known controller manager type." + "Did you maybe set `distributed:false` and `sub_controller_manager:true`?" + "Note:Only distributed controller manager can be a sub controller manager."); + break; + } +} + +// TODO(Manuel) don't like this, this is for fast poc +// probably better to create factory and handle creation of correct controller manager type +// there. Since asynchronous control should be supported im the future as well and we don't +// want dozen of ifs. +ControllerManager::controller_manager_type ControllerManager::determine_controller_manager_type() +{ bool std_controller_manager = !distributed_ && !sub_controller_manager_; bool distributed_sub_controller_manager = distributed_ && sub_controller_manager_; bool central_controller_manager = distributed_ && !sub_controller_manager_; if (distributed_sub_controller_manager) { - init_distributed_sub_controller_manager(); + return controller_manager_type::distributed_sub_controller_manager; } // This means we are the central controller manager else if (central_controller_manager) { - init_distributed_main_controller_services(); + return controller_manager_type::distributed_central_controller_manager; } // std controller manager or error. std controller manager needs no special setup. - else + else if (std_controller_manager) { - if (!std_controller_manager) - { - throw std::logic_error( - "Controller manager configured with: distributed:false and sub_controller_manager:true. " - "Only distributed controller manager can be a sub controller manager."); - } + return controller_manager_type::standard_controller_manager; } + return controller_manager_type::unkown_type; } void ControllerManager::init_distributed_sub_controller_manager() { - int64_t distributed_interfaces_publish_period; - if (get_parameter("distributed_interfaces_publish_period", distributed_interfaces_publish_period)) - { - distributed_interfaces_publish_period_ = - std::chrono::milliseconds(distributed_interfaces_publish_period); - } - else + if (!use_multiple_nodes()) { - RCLCPP_WARN( - get_logger(), - "'distributed_interfaces_publish_period' parameter not set, using default value."); + rclcpp::NodeOptions node_options; + distributed_pub_sub_node_ = std::make_shared( + std::string(get_name()) + "_pub_sub_node", get_namespace(), node_options, false); } add_hardware_state_publishers(); add_hardware_command_forwarders(); register_sub_controller_manager(); } -void ControllerManager::init_distributed_main_controller_services() +void ControllerManager::init_distributed_central_controller_manager() +{ + if (!use_multiple_nodes()) + { + rclcpp::NodeOptions node_options; + distributed_pub_sub_node_ = std::make_shared( + std::string(get_name()) + "_pub_sub_node", get_namespace(), node_options, false); + } + init_distributed_central_controller_manager_services(); +} + +void ControllerManager::init_distributed_central_controller_manager_services() { distributed_system_srv_callback_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); @@ -448,7 +500,8 @@ void ControllerManager::register_sub_controller_manager_srv_cb( distributed_state_interfaces; distributed_state_interfaces.reserve(sub_ctrl_mng_wrapper->get_state_publisher_count()); distributed_state_interfaces = - resource_manager_->import_state_interfaces_of_sub_controller_manager(sub_ctrl_mng_wrapper); + resource_manager_->import_state_interfaces_of_sub_controller_manager( + sub_ctrl_mng_wrapper, get_namespace(), distributed_pub_sub_node_); for (const auto & state_interface : distributed_state_interfaces) { @@ -471,7 +524,8 @@ void ControllerManager::register_sub_controller_manager_srv_cb( distributed_command_interfaces; distributed_command_interfaces.reserve(sub_ctrl_mng_wrapper->get_command_forwarder_count()); distributed_command_interfaces = - resource_manager_->import_command_interfaces_of_sub_controller_manager(sub_ctrl_mng_wrapper); + resource_manager_->import_command_interfaces_of_sub_controller_manager( + sub_ctrl_mng_wrapper, get_namespace(), distributed_pub_sub_node_); for (const auto & command_interface : distributed_command_interfaces) { @@ -508,7 +562,7 @@ void ControllerManager::add_hardware_state_publishers() std::vector> state_publishers_vec; state_publishers_vec.reserve(resource_manager_->available_state_interfaces().size()); state_publishers_vec = resource_manager_->create_hardware_state_publishers( - get_namespace(), distributed_interfaces_publish_period()); + get_namespace(), distributed_interfaces_publish_period(), distributed_pub_sub_node_); for (auto const & state_publisher : state_publishers_vec) { @@ -530,7 +584,7 @@ void ControllerManager::add_hardware_command_forwarders() std::vector> command_forwarder_vec; command_forwarder_vec.reserve(resource_manager_->available_command_interfaces().size()); command_forwarder_vec = resource_manager_->create_hardware_command_forwarders( - get_namespace(), distributed_interfaces_publish_period()); + get_namespace(), distributed_interfaces_publish_period(), distributed_pub_sub_node_); for (auto const & command_forwarder : command_forwarder_vec) { @@ -2254,6 +2308,8 @@ std::pair ControllerManager::split_command_interface( unsigned int ControllerManager::get_update_rate() const { return update_rate_; } +bool ControllerManager::use_multiple_nodes() const { return use_multiple_nodes_; } + std::chrono::milliseconds ControllerManager::distributed_interfaces_publish_period() const { return distributed_interfaces_publish_period_; diff --git a/hardware_interface/include/hardware_interface/distributed_control_interface/command_forwarder.hpp b/hardware_interface/include/hardware_interface/distributed_control_interface/command_forwarder.hpp index 86077913ba..cfcc6ce9e0 100644 --- a/hardware_interface/include/hardware_interface/distributed_control_interface/command_forwarder.hpp +++ b/hardware_interface/include/hardware_interface/distributed_control_interface/command_forwarder.hpp @@ -24,7 +24,8 @@ class CommandForwarder final public: explicit CommandForwarder( std::unique_ptr loaned_command_interface_ptr, - const std::string & ns, std::chrono::milliseconds period_in_ms); + const std::string & ns, std::chrono::milliseconds period_in_ms, + std::shared_ptr node); CommandForwarder() = delete; diff --git a/hardware_interface/include/hardware_interface/distributed_control_interface/state_publisher.hpp b/hardware_interface/include/hardware_interface/distributed_control_interface/state_publisher.hpp index 89f07c1e1d..8d260beda8 100644 --- a/hardware_interface/include/hardware_interface/distributed_control_interface/state_publisher.hpp +++ b/hardware_interface/include/hardware_interface/distributed_control_interface/state_publisher.hpp @@ -22,8 +22,8 @@ class StatePublisher final public: explicit StatePublisher( std::unique_ptr loaned_state_interface_ptr, - const std::string & ns, std::chrono::milliseconds period_in_ms); - + const std::string & ns, std::chrono::milliseconds period_in_ms, + std::shared_ptr node); StatePublisher() = delete; ~StatePublisher() {} diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index 1e506bf0b2..52fbc3b053 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -148,8 +148,8 @@ class ReadOnlyHandle : public HandleInterface, public ReadHandleInterface public: ReadOnlyHandle( const std::string & prefix_name, const std::string & interface_name, - double * value_ptr = nullptr) - : HandleInterface(prefix_name, interface_name, value_ptr) + double * value_ptr = nullptr, std::shared_ptr node = nullptr) + : HandleInterface(prefix_name, interface_name, value_ptr, node) { } @@ -190,18 +190,22 @@ class DistributedReadOnlyHandle : public ReadOnlyHandle // TODO(Manuel): We should pass the initial value via service call, so that the value_ of ReadOnlyHandle // is initialized with a feasible value. DistributedReadOnlyHandle( - const distributed_control::PublisherDescription & description, const std::string & ns = "/") - : ReadOnlyHandle(description.prefix_name(), description.interface_name(), &value_), + const distributed_control::PublisherDescription & description, const std::string & ns, + std::shared_ptr node) + : ReadOnlyHandle(description.prefix_name(), description.interface_name(), &value_, node), get_value_topic_name_(description.topic_name()), namespace_(ns), interface_namespace_(description.get_namespace()) { - rclcpp::NodeOptions node_options; + // if no node has been passed // create node for subscribing to StatePublisher described in StatePublisherDescription - node_ = std::make_shared( - get_underscore_separated_name() + "_state_interface_subscriber", namespace_, node_options, - false); - + if (!node_.get()) + { + rclcpp::NodeOptions node_options; + node_ = std::make_shared( + get_underscore_separated_name() + "_state_interface_subscriber", namespace_, node_options, + false); + } // subscribe to topic provided by StatePublisher state_value_sub_ = node_->create_subscription( get_value_topic_name_, 10, @@ -280,8 +284,8 @@ class ReadWriteHandle : public HandleInterface, public: ReadWriteHandle( const std::string & prefix_name, const std::string & interface_name, - double * value_ptr = nullptr) - : HandleInterface(prefix_name, interface_name, value_ptr) + double * value_ptr = nullptr, std::shared_ptr node = nullptr) + : HandleInterface(prefix_name, interface_name, value_ptr, node) { } @@ -332,20 +336,25 @@ class DistributedReadWriteHandle : public ReadWriteHandle { public: DistributedReadWriteHandle( - const distributed_control::PublisherDescription & description, const std::string & ns = "/") - : ReadWriteHandle(description.prefix_name(), description.interface_name(), &value_), + const distributed_control::PublisherDescription & description, const std::string & ns, + std::shared_ptr node) + : ReadWriteHandle(description.prefix_name(), description.interface_name(), &value_, node), get_value_topic_name_(description.topic_name()), namespace_(ns), interface_namespace_(description.get_namespace()), forward_command_topic_name_(get_underscore_separated_name() + "_command_forwarding") { - // create node for subscribing to StatePublisher described in StatePublisherDescription - rclcpp::NodeOptions node_options; - node_ = std::make_shared( - get_underscore_separated_name() + "_distributed_command_interface", namespace_, node_options, - false); + // if no node has been passed + // create node for subscribing to CommandForwarder described in CommandForwarderDescription + if (!node_.get()) + { + rclcpp::NodeOptions node_options; + node_ = std::make_shared( + get_underscore_separated_name() + "_distributed_command_interface", namespace_, + node_options, false); + } - // subscribe to topic provided by StatePublisher + // subscribe to topic provided by CommandForwarder command_value_sub_ = node_->create_subscription( get_value_topic_name_, 10, std::bind(&DistributedReadWriteHandle::get_value_cb, this, std::placeholders::_1)); diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index b7c3fe3fb7..c25dc2e9d6 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -134,18 +134,23 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager std::vector> import_state_interfaces_of_sub_controller_manager( - std::shared_ptr sub_controller_manager); + std::shared_ptr sub_controller_manager, + const std::string & ns, std::shared_ptr node); std::vector> import_command_interfaces_of_sub_controller_manager( - std::shared_ptr sub_controller_manager); + std::shared_ptr sub_controller_manager, + const std::string & ns, std::shared_ptr node); std::vector> - create_hardware_state_publishers(const std::string & ns, std::chrono::milliseconds update_period); + create_hardware_state_publishers( + const std::string & ns, std::chrono::milliseconds update_period, + std::shared_ptr node); std::vector> create_hardware_command_forwarders( - const std::string & ns, std::chrono::milliseconds update_period); + const std::string & ns, std::chrono::milliseconds update_period, + std::shared_ptr node); std::pair> find_command_forwarder( const std::string & key); diff --git a/hardware_interface/src/hardware_interface/distributed_control_interface/command_forwarder.cpp b/hardware_interface/src/hardware_interface/distributed_control_interface/command_forwarder.cpp index 12b8024bd9..558b632b2a 100644 --- a/hardware_interface/src/hardware_interface/distributed_control_interface/command_forwarder.cpp +++ b/hardware_interface/src/hardware_interface/distributed_control_interface/command_forwarder.cpp @@ -13,16 +13,22 @@ namespace distributed_control CommandForwarder::CommandForwarder( std::unique_ptr loaned_command_interface_ptr, - const std::string & ns, std::chrono::milliseconds period_in_ms) + const std::string & ns, std::chrono::milliseconds period_in_ms, + std::shared_ptr node) : loaned_command_interface_ptr_(std::move(loaned_command_interface_ptr)), namespace_(ns), period_in_ms_(period_in_ms), + node_(node), topic_name_(loaned_command_interface_ptr_->get_underscore_separated_name() + "_command_state") { - rclcpp::NodeOptions node_options; - node_ = std::make_shared( - loaned_command_interface_ptr_->get_underscore_separated_name() + "_command_forwarder", - namespace_, node_options, false); + // if we did not get a node passed, we create one ourselves + if (!node_.get()) + { + rclcpp::NodeOptions node_options; + node_ = std::make_shared( + loaned_command_interface_ptr_->get_underscore_separated_name() + "_command_forwarder", + namespace_, node_options, false); + } state_value_pub_ = node_->create_publisher(topic_name_, 10); // TODO(Manuel): We should check if we cannot detect changes to LoanedStateInterface's value and only publish then diff --git a/hardware_interface/src/hardware_interface/distributed_control_interface/state_publisher.cpp b/hardware_interface/src/hardware_interface/distributed_control_interface/state_publisher.cpp index b963d7b3cb..0857bbb342 100644 --- a/hardware_interface/src/hardware_interface/distributed_control_interface/state_publisher.cpp +++ b/hardware_interface/src/hardware_interface/distributed_control_interface/state_publisher.cpp @@ -13,16 +13,22 @@ namespace distributed_control StatePublisher::StatePublisher( std::unique_ptr loaned_state_interface_ptr, - const std::string & ns, std::chrono::milliseconds period_in_ms) + const std::string & ns, std::chrono::milliseconds period_in_ms, + std::shared_ptr node) : loaned_state_interface_ptr_(std::move(loaned_state_interface_ptr)), namespace_(ns), period_in_ms_(period_in_ms), + node_(node), topic_name_(loaned_state_interface_ptr_->get_underscore_separated_name() + "_state") { - rclcpp::NodeOptions node_options; - node_ = std::make_shared( - loaned_state_interface_ptr_->get_underscore_separated_name() + "_state_publisher", namespace_, - node_options, false); + // if we did not get a node passed, we create one ourselves + if (!node_.get()) + { + rclcpp::NodeOptions node_options; + node_ = std::make_shared( + loaned_state_interface_ptr_->get_underscore_separated_name() + "_state_publisher", namespace_, + node_options, false); + } state_value_pub_ = node_->create_publisher(topic_name_, 10); // TODO(Manuel): We should check if we cannot detect changes to LoanedStateInterface's value and only publish then diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index cc243caf6e..b5bf5cf937 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -677,7 +677,8 @@ class ResourceStorage } std::vector> import_distributed_state_interfaces( - std::shared_ptr sub_controller_manager) + std::shared_ptr sub_controller_manager, + const std::string & ns, std::shared_ptr node) { std::vector> distributed_state_interfaces; distributed_state_interfaces.reserve(sub_controller_manager->get_state_publisher_count()); @@ -689,7 +690,7 @@ class ResourceStorage { // create StateInterface from the Description and store in ResourceStorage. auto state_interface = - std::make_shared(state_publisher_description); + std::make_shared(state_publisher_description, ns, node); add_state_interface(state_interface); // add to return vector, node needs to added to executor. @@ -730,7 +731,8 @@ class ResourceStorage } std::vector> import_distributed_command_interfaces( - std::shared_ptr sub_controller_manager) + std::shared_ptr sub_controller_manager, + const std::string & ns, std::shared_ptr node) { std::vector> distributed_command_interfaces; distributed_command_interfaces.reserve(sub_controller_manager->get_command_forwarder_count()); @@ -742,7 +744,7 @@ class ResourceStorage { // create StateInterface from the Description and store in ResourceStorage. auto command_interface = - std::make_shared(command_forwarder_description); + std::make_shared(command_forwarder_description, ns, node); add_command_interface(command_interface); // add to return vector, node needs to added to executor. distributed_command_interfaces.push_back(command_interface); @@ -939,23 +941,26 @@ void ResourceManager::register_sub_controller_manager( std::vector> ResourceManager::import_state_interfaces_of_sub_controller_manager( - std::shared_ptr sub_controller_manager) + std::shared_ptr sub_controller_manager, + const std::string & ns, std::shared_ptr node) { std::lock_guard guard(resource_interfaces_lock_); - return resource_storage_->import_distributed_state_interfaces(sub_controller_manager); + return resource_storage_->import_distributed_state_interfaces(sub_controller_manager, ns, node); } std::vector> ResourceManager::import_command_interfaces_of_sub_controller_manager( - std::shared_ptr sub_controller_manager) + std::shared_ptr sub_controller_manager, + const std::string & ns, std::shared_ptr node) { std::lock_guard guard(resource_interfaces_lock_); - return resource_storage_->import_distributed_command_interfaces(sub_controller_manager); + return resource_storage_->import_distributed_command_interfaces(sub_controller_manager, ns, node); } std::vector> ResourceManager::create_hardware_state_publishers( - const std::string & ns, std::chrono::milliseconds update_period) + const std::string & ns, std::chrono::milliseconds update_period, + std::shared_ptr node) { std::lock_guard guard(resource_interfaces_lock_); std::vector> state_publishers_vec; @@ -966,7 +971,7 @@ ResourceManager::create_hardware_state_publishers( auto state_publisher = std::make_shared( std::move(std::make_unique( claim_state_interface(state_interface))), - ns, update_period); + ns, update_period, node); resource_storage_->add_state_publisher(state_publisher); state_publishers_vec.push_back(state_publisher); @@ -977,7 +982,8 @@ ResourceManager::create_hardware_state_publishers( std::vector> ResourceManager::create_hardware_command_forwarders( - const std::string & ns, std::chrono::milliseconds update_period) + const std::string & ns, std::chrono::milliseconds update_period, + std::shared_ptr node) { std::lock_guard guard(resource_interfaces_lock_); std::vector> command_forwarders_vec; @@ -988,7 +994,7 @@ ResourceManager::create_hardware_command_forwarders( auto command_forwarder = std::make_shared( std::move(std::make_unique( claim_command_interface(command_interface))), - ns, update_period); + ns, update_period, node); resource_storage_->add_command_forwarder(command_forwarder); command_forwarders_vec.push_back(command_forwarder); From 9764f8f735f3533b2809b02e87c0547d7aca56d7 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Sun, 7 May 2023 18:34:05 +0200 Subject: [PATCH 15/26] create publishers in cm and only add in rm --- .../controller_manager/controller_manager.hpp | 4 +- controller_manager/src/controller_manager.cpp | 36 +++++++++------ .../hardware_interface/resource_manager.hpp | 14 +++--- hardware_interface/src/resource_manager.cpp | 44 +++---------------- 4 files changed, 35 insertions(+), 63 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 4c6ba20ad7..036370a9a9 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -236,10 +236,10 @@ class ControllerManager : public rclcpp::Node std::shared_ptr response); CONTROLLER_MANAGER_PUBLIC - void add_hardware_state_publishers(); + void create_hardware_state_publishers(); CONTROLLER_MANAGER_PUBLIC - void add_hardware_command_forwarders(); + void create_hardware_command_forwarders(); CONTROLLER_MANAGER_PUBLIC void register_sub_controller_manager(); diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 39dc4fb8b2..567b8e291e 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -453,8 +453,8 @@ void ControllerManager::init_distributed_sub_controller_manager() distributed_pub_sub_node_ = std::make_shared( std::string(get_name()) + "_pub_sub_node", get_namespace(), node_options, false); } - add_hardware_state_publishers(); - add_hardware_command_forwarders(); + create_hardware_state_publishers(); + create_hardware_command_forwarders(); register_sub_controller_manager(); } @@ -557,15 +557,19 @@ void ControllerManager::register_sub_controller_manager_srv_cb( << sub_ctrl_mng_wrapper->get_name() << ">."); } -void ControllerManager::add_hardware_state_publishers() +void ControllerManager::create_hardware_state_publishers() { - std::vector> state_publishers_vec; - state_publishers_vec.reserve(resource_manager_->available_state_interfaces().size()); - state_publishers_vec = resource_manager_->create_hardware_state_publishers( - get_namespace(), distributed_interfaces_publish_period(), distributed_pub_sub_node_); + auto available_state_interfaces = resource_manager_->available_state_interfaces(); - for (auto const & state_publisher : state_publishers_vec) + for (const auto & state_interface : available_state_interfaces) { + auto state_publisher = std::make_shared( + std::move(std::make_unique( + resource_manager_->claim_state_interface(state_interface))), + get_namespace(), distributed_interfaces_publish_period(), distributed_pub_sub_node_); + + resource_manager_->add_hardware_state_publishers(state_publisher); + try { executor_->add_node(state_publisher->get_node()->get_node_base_interface()); @@ -579,15 +583,19 @@ void ControllerManager::add_hardware_state_publishers() } } -void ControllerManager::add_hardware_command_forwarders() +void ControllerManager::create_hardware_command_forwarders() { - std::vector> command_forwarder_vec; - command_forwarder_vec.reserve(resource_manager_->available_command_interfaces().size()); - command_forwarder_vec = resource_manager_->create_hardware_command_forwarders( - get_namespace(), distributed_interfaces_publish_period(), distributed_pub_sub_node_); + auto available_command_interfaces = resource_manager_->available_command_interfaces(); - for (auto const & command_forwarder : command_forwarder_vec) + for (auto const & command_interface : available_command_interfaces) { + auto command_forwarder = std::make_shared( + std::move(std::make_unique( + resource_manager_->claim_command_interface(command_interface))), + get_namespace(), distributed_interfaces_publish_period(), distributed_pub_sub_node_); + + resource_manager_->add_hardware_command_forwarders(command_forwarder); + try { executor_->add_node(command_forwarder->get_node()->get_node_base_interface()); diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index c25dc2e9d6..4e07f5f011 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -142,15 +142,11 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager std::shared_ptr sub_controller_manager, const std::string & ns, std::shared_ptr node); - std::vector> - create_hardware_state_publishers( - const std::string & ns, std::chrono::milliseconds update_period, - std::shared_ptr node); - - std::vector> - create_hardware_command_forwarders( - const std::string & ns, std::chrono::milliseconds update_period, - std::shared_ptr node); + void add_hardware_state_publishers( + std::shared_ptr state_publisher); + + void add_hardware_command_forwarders( + std::shared_ptr command_forwarder); std::pair> find_command_forwarder( const std::string & key); diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index b5bf5cf937..98567a4a27 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -957,50 +957,18 @@ ResourceManager::import_command_interfaces_of_sub_controller_manager( return resource_storage_->import_distributed_command_interfaces(sub_controller_manager, ns, node); } -std::vector> -ResourceManager::create_hardware_state_publishers( - const std::string & ns, std::chrono::milliseconds update_period, - std::shared_ptr node) +void ResourceManager::add_hardware_state_publishers( + std::shared_ptr state_publisher) { std::lock_guard guard(resource_interfaces_lock_); - std::vector> state_publishers_vec; - state_publishers_vec.reserve(available_state_interfaces().size()); - - for (const auto & state_interface : available_state_interfaces()) - { - auto state_publisher = std::make_shared( - std::move(std::make_unique( - claim_state_interface(state_interface))), - ns, update_period, node); - - resource_storage_->add_state_publisher(state_publisher); - state_publishers_vec.push_back(state_publisher); - } - - return state_publishers_vec; + resource_storage_->add_state_publisher(state_publisher); } -std::vector> -ResourceManager::create_hardware_command_forwarders( - const std::string & ns, std::chrono::milliseconds update_period, - std::shared_ptr node) +void ResourceManager::add_hardware_command_forwarders( + std::shared_ptr command_forwarder) { std::lock_guard guard(resource_interfaces_lock_); - std::vector> command_forwarders_vec; - command_forwarders_vec.reserve(available_command_interfaces().size()); - - for (const auto & command_interface : available_command_interfaces()) - { - auto command_forwarder = std::make_shared( - std::move(std::make_unique( - claim_command_interface(command_interface))), - ns, update_period, node); - - resource_storage_->add_command_forwarder(command_forwarder); - command_forwarders_vec.push_back(command_forwarder); - } - - return command_forwarders_vec; + resource_storage_->add_command_forwarder(command_forwarder); } std::vector> From 8b4b553a03eb369e3cb7b9518696f54d63bfcd08 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Mon, 8 May 2023 09:30:31 +0200 Subject: [PATCH 16/26] make possible to export only subset of command/state interfaces, fix adding node multiple times to executor --- .../controller_manager/controller_manager.hpp | 4 + controller_manager/src/controller_manager.cpp | 186 +++++++++++++----- 2 files changed, 136 insertions(+), 54 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 036370a9a9..0b0bccbd2a 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -460,6 +460,10 @@ class ControllerManager : public rclcpp::Node bool distributed_ = false; bool sub_controller_manager_ = false; bool use_multiple_nodes_ = false; + // TODO(Manuel): weak_ptr would probably be a better choice. This way has to be checked + // if pointer points to an object. Don't like the nullptr thing and implicit checks + // associated with it ... (create on distributed Handles and StatePublisher/CommandForwarder) + // needs to be checked if is nullptr before usage std::shared_ptr distributed_pub_sub_node_ = nullptr; std::chrono::milliseconds distributed_interfaces_publish_period_ = std::chrono::milliseconds(12); diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 567b8e291e..e28d970b29 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -403,7 +403,6 @@ void ControllerManager::configure_controller_manager(const controller_manager_ty case controller_manager_type::distributed_central_controller_manager: init_distributed_central_controller_manager(); break; - case controller_manager_type::distributed_sub_controller_manager: init_distributed_sub_controller_manager(); break; @@ -419,7 +418,7 @@ void ControllerManager::configure_controller_manager(const controller_manager_ty } } -// TODO(Manuel) don't like this, this is for fast poc +// TODO(Manuel): don't like this, this is for fast poc // probably better to create factory and handle creation of correct controller manager type // there. Since asynchronous control should be supported im the future as well and we don't // want dozen of ifs. @@ -447,12 +446,27 @@ ControllerManager::controller_manager_type ControllerManager::determine_controll void ControllerManager::init_distributed_sub_controller_manager() { + // if only one node per sub controller manager is used if (!use_multiple_nodes()) { + // create node for publishing/subscribing rclcpp::NodeOptions node_options; distributed_pub_sub_node_ = std::make_shared( std::string(get_name()) + "_pub_sub_node", get_namespace(), node_options, false); + //try to add to executor + try + { + executor_->add_node(distributed_pub_sub_node_->get_node_base_interface()); + } + catch (const std::runtime_error & e) + { + RCLCPP_WARN_STREAM( + get_logger(), + "ControllerManager: can not add node for distributed publishing/subscribing to executor:" + << e.what()); + } } + create_hardware_state_publishers(); create_hardware_command_forwarders(); register_sub_controller_manager(); @@ -465,6 +479,18 @@ void ControllerManager::init_distributed_central_controller_manager() rclcpp::NodeOptions node_options; distributed_pub_sub_node_ = std::make_shared( std::string(get_name()) + "_pub_sub_node", get_namespace(), node_options, false); + //try to add to executor + try + { + executor_->add_node(distributed_pub_sub_node_->get_node_base_interface()); + } + catch (const std::runtime_error & e) + { + RCLCPP_WARN_STREAM( + get_logger(), + "ControllerManager: can not add node for distributed publishing/subscribing to executor:" + << e.what()); + } } init_distributed_central_controller_manager_services(); } @@ -499,54 +525,66 @@ void ControllerManager::register_sub_controller_manager_srv_cb( std::vector> distributed_state_interfaces; distributed_state_interfaces.reserve(sub_ctrl_mng_wrapper->get_state_publisher_count()); + // create distributed state interface and import into resource storage. distributed_state_interfaces = resource_manager_->import_state_interfaces_of_sub_controller_manager( sub_ctrl_mng_wrapper, get_namespace(), distributed_pub_sub_node_); - for (const auto & state_interface : distributed_state_interfaces) + // register every node of state_interface at executor only if multiple nodes + // are used. Otherwise the single nodes has already been added + if (use_multiple_nodes()) { - try - { - executor_->add_node(state_interface->get_node()->get_node_base_interface()); - } - catch (const std::runtime_error & e) + for (const auto & state_interface : distributed_state_interfaces) { - response->ok = false; - RCLCPP_WARN_STREAM( - get_logger(), - "ControllerManager: Caught exception while trying to register sub controller manager. " - "Exception:" - << e.what()); + try + { + executor_->add_node(state_interface->get_node()->get_node_base_interface()); + } + catch (const std::runtime_error & e) + { + response->ok = false; + RCLCPP_WARN_STREAM( + get_logger(), + "ControllerManager: Caught exception while trying to register node of distributed state " + "interface of sub controller manager. Exception:" + << e.what()); + } } } std::vector> distributed_command_interfaces; distributed_command_interfaces.reserve(sub_ctrl_mng_wrapper->get_command_forwarder_count()); + // create distributed command interface and import into resource storage. distributed_command_interfaces = resource_manager_->import_command_interfaces_of_sub_controller_manager( sub_ctrl_mng_wrapper, get_namespace(), distributed_pub_sub_node_); for (const auto & command_interface : distributed_command_interfaces) { - try - { - executor_->add_node(command_interface->get_node()->get_node_base_interface()); - } - catch (const std::runtime_error & e) + // register every node of command_interface at executor only if multiple nodes + // are used. Otherwise the single nodes has already been added + if (use_multiple_nodes()) { - response->ok = false; - RCLCPP_WARN_STREAM( - get_logger(), - "ControllerManager: Caught exception while trying to register sub controller manager. " - "Exception:" - << e.what()); + try + { + executor_->add_node(command_interface->get_node()->get_node_base_interface()); + } + catch (const std::runtime_error & e) + { + response->ok = false; + RCLCPP_WARN_STREAM( + get_logger(), + "ControllerManager: Caught exception while trying to register node of distributed " + "command_interface of sub controller manager. Exception:" + << e.what()); + } } auto msg = controller_manager_msgs::msg::PublisherDescription(); msg.ns = get_namespace(); msg.name.prefix_name = command_interface->get_prefix_name(); msg.name.interface_name = command_interface->get_interface_name(); - // TODO(Manuel) want topic name relative to namespace, but have to treat "root" namespace separate + // TODO(Manuel): want topic name relative to namespace, but have to treat "root" namespace separate msg.publisher_topic = std::string("/") + command_interface->forward_command_topic_name(); response->command_state_publishers.push_back(msg); } @@ -559,52 +597,92 @@ void ControllerManager::register_sub_controller_manager_srv_cb( void ControllerManager::create_hardware_state_publishers() { - auto available_state_interfaces = resource_manager_->available_state_interfaces(); - - for (const auto & state_interface : available_state_interfaces) + std::vector state_interfaces_to_export = std::vector({}); + // export every interface by default + if (!get_parameter("export_state_interfaces", state_interfaces_to_export)) { - auto state_publisher = std::make_shared( - std::move(std::make_unique( - resource_manager_->claim_state_interface(state_interface))), - get_namespace(), distributed_interfaces_publish_period(), distributed_pub_sub_node_); - - resource_manager_->add_hardware_state_publishers(state_publisher); + // get all available state interfaces + state_interfaces_to_export = resource_manager_->available_state_interfaces(); + } + for (const auto & state_interface : state_interfaces_to_export) + { + std::shared_ptr state_publisher; try { - executor_->add_node(state_publisher->get_node()->get_node_base_interface()); + state_publisher = std::make_shared( + std::move(std::make_unique( + resource_manager_->claim_state_interface(state_interface))), + get_namespace(), distributed_interfaces_publish_period(), distributed_pub_sub_node_); } - catch (const std::runtime_error & e) + catch (const std::exception & e) { - RCLCPP_WARN_STREAM( - get_logger(), "ControllerManager: Can't create StatePublishers<" - << state_publisher->state_interface_name() << ">." << e.what()); + RCLCPP_ERROR( + get_logger(), "Can't create StatePublisher for state interface<'%s'>: %s", + state_interface.c_str(), e.what()); + continue; + } + + resource_manager_->add_hardware_state_publishers(state_publisher); + + if (use_multiple_nodes()) + { + try + { + executor_->add_node(state_publisher->get_node()->get_node_base_interface()); + } + catch (const std::runtime_error & e) + { + RCLCPP_WARN_STREAM( + get_logger(), "ControllerManager: Can't create StatePublishers<" + << state_publisher->state_interface_name() << ">." << e.what()); + } } } } void ControllerManager::create_hardware_command_forwarders() { - auto available_command_interfaces = resource_manager_->available_command_interfaces(); - - for (auto const & command_interface : available_command_interfaces) + std::vector command_interfaces_to_export = std::vector({}); + // export every interface by default + if (!get_parameter("export_command_interfaces", command_interfaces_to_export)) { - auto command_forwarder = std::make_shared( - std::move(std::make_unique( - resource_manager_->claim_command_interface(command_interface))), - get_namespace(), distributed_interfaces_publish_period(), distributed_pub_sub_node_); - - resource_manager_->add_hardware_command_forwarders(command_forwarder); + // get all available command interfaces + command_interfaces_to_export = resource_manager_->available_command_interfaces(); + } + for (auto const & command_interface : command_interfaces_to_export) + { + std::shared_ptr command_forwarder; try { - executor_->add_node(command_forwarder->get_node()->get_node_base_interface()); + command_forwarder = std::make_shared( + std::move(std::make_unique( + resource_manager_->claim_command_interface(command_interface))), + get_namespace(), distributed_interfaces_publish_period(), distributed_pub_sub_node_); } - catch (const std::runtime_error & e) + catch (const std::exception & e) { - RCLCPP_WARN_STREAM( - get_logger(), "ControllerManager: Can't create CommandForwarder<" - << command_forwarder->command_interface_name() << ">." << e.what()); + RCLCPP_ERROR( + get_logger(), "Can't create CommandForwarder for command interface<'%s'>: %s", + command_interface.c_str(), e.what()); + continue; + } + + resource_manager_->add_hardware_command_forwarders(command_forwarder); + + if (use_multiple_nodes()) + { + try + { + executor_->add_node(command_forwarder->get_node()->get_node_base_interface()); + } + catch (const std::runtime_error & e) + { + RCLCPP_WARN_STREAM( + get_logger(), "ControllerManager: Can't create CommandForwarder<" + << command_forwarder->command_interface_name() << ">." << e.what()); + } } } } From 14a2c07fad598746d8f8f093d8171cf60bcfe528 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Wed, 17 May 2023 12:27:24 +0200 Subject: [PATCH 17/26] make initialization and parameters more clear and easy --- .../controller_manager/controller_manager.hpp | 12 ++++-- controller_manager/src/controller_manager.cpp | 37 ++++++++++++------- 2 files changed, 31 insertions(+), 18 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 0b0bccbd2a..d5ce930cf0 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -200,11 +200,15 @@ class ControllerManager : public rclcpp::Node CONTROLLER_MANAGER_PUBLIC unsigned int get_update_rate() const; - // Per controller update rate support + CONTROLLER_MANAGER_PUBLIC + bool is_central_controller_manager() const; + + CONTROLLER_MANAGER_PUBLIC + bool is_sub_controller_manager() const; + CONTROLLER_MANAGER_PUBLIC bool use_multiple_nodes() const; - // Per controller update rate support CONTROLLER_MANAGER_PUBLIC std::chrono::milliseconds distributed_interfaces_publish_period() const; @@ -219,7 +223,7 @@ class ControllerManager : public rclcpp::Node controller_manager_type determine_controller_manager_type(); CONTROLLER_MANAGER_PUBLIC - void configure_controller_manager(const controller_manager_type & cm_type); + void configure_controller_manager(); CONTROLLER_MANAGER_PUBLIC void init_distributed_sub_controller_manager(); @@ -457,8 +461,8 @@ class ControllerManager : public rclcpp::Node unkown_type // indicating something went wrong and type could not be determined }; - bool distributed_ = false; bool sub_controller_manager_ = false; + bool central_controller_manager_ = false; bool use_multiple_nodes_ = false; // TODO(Manuel): weak_ptr would probably be a better choice. This way has to be checked // if pointer points to an object. Don't like the nullptr thing and implicit checks diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index e28d970b29..fc6f61ca5b 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -182,8 +182,7 @@ ControllerManager::ControllerManager( "Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback); init_services(); get_and_initialize_distributed_parameters(); - auto cm_type = determine_controller_manager_type(); - configure_controller_manager(cm_type); + configure_controller_manager(); } ControllerManager::ControllerManager( @@ -212,8 +211,7 @@ ControllerManager::ControllerManager( "Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback); init_services(); get_and_initialize_distributed_parameters(); - auto cm_type = determine_controller_manager_type(); - configure_controller_manager(cm_type); + configure_controller_manager(); } void ControllerManager::subscribe_to_robot_description_topic() @@ -360,11 +358,11 @@ void ControllerManager::init_services() void ControllerManager::get_and_initialize_distributed_parameters() { - if (!get_parameter("distributed", distributed_)) + if (!get_parameter("central_controller_manager", central_controller_manager_)) { RCLCPP_WARN( - get_logger(), "'distributed' parameter not set, using default value:%s", - distributed_ ? "true" : "false"); + get_logger(), "'central_controller_manager' parameter not set, using default value:%s", + central_controller_manager_ ? "true" : "false"); } if (!get_parameter("sub_controller_manager", sub_controller_manager_)) @@ -396,8 +394,10 @@ void ControllerManager::get_and_initialize_distributed_parameters() } } -void ControllerManager::configure_controller_manager(const controller_manager_type & cm_type) +void ControllerManager::configure_controller_manager() { + auto cm_type = determine_controller_manager_type(); + switch (cm_type) { case controller_manager_type::distributed_central_controller_manager: @@ -412,8 +412,9 @@ void ControllerManager::configure_controller_manager(const controller_manager_ty default: throw std::logic_error( "Controller manager configuration not possible. Not a known controller manager type." - "Did you maybe set `distributed:false` and `sub_controller_manager:true`?" - "Note:Only distributed controller manager can be a sub controller manager."); + "Did you maybe set `central_controller_manager:true` and `sub_controller_manager:true`?" + "Note:Distributed controller manager can only be central_controller_manager or " + "sub_controller_manager, not both."); break; } } @@ -424,15 +425,16 @@ void ControllerManager::configure_controller_manager(const controller_manager_ty // want dozen of ifs. ControllerManager::controller_manager_type ControllerManager::determine_controller_manager_type() { - bool std_controller_manager = !distributed_ && !sub_controller_manager_; - bool distributed_sub_controller_manager = distributed_ && sub_controller_manager_; - bool central_controller_manager = distributed_ && !sub_controller_manager_; + bool std_controller_manager = !central_controller_manager_ && !sub_controller_manager_; + bool distributed_sub_controller_manager = !central_controller_manager_ && sub_controller_manager_; + bool distributed_central_controller_manager = + central_controller_manager_ && !sub_controller_manager_; if (distributed_sub_controller_manager) { return controller_manager_type::distributed_sub_controller_manager; } // This means we are the central controller manager - else if (central_controller_manager) + else if (distributed_central_controller_manager) { return controller_manager_type::distributed_central_controller_manager; } @@ -2394,6 +2396,13 @@ std::pair ControllerManager::split_command_interface( unsigned int ControllerManager::get_update_rate() const { return update_rate_; } +bool ControllerManager::is_central_controller_manager() const +{ + return central_controller_manager_; +} + +bool ControllerManager::is_sub_controller_manager() const { return sub_controller_manager_; } + bool ControllerManager::use_multiple_nodes() const { return use_multiple_nodes_; } std::chrono::milliseconds ControllerManager::distributed_interfaces_publish_period() const From cab9d52293d71aa40d5b5eba5198714779a3e69c Mon Sep 17 00:00:00 2001 From: Manuel M Date: Wed, 17 May 2023 14:14:35 +0200 Subject: [PATCH 18/26] adapt controller interfaces for distributed chained control --- .../chainable_controller_interface.hpp | 13 +++++ .../controller_interface.hpp | 11 ++++- .../controller_interface_base.hpp | 10 ++++ .../src/chainable_controller_interface.cpp | 47 +++++++++++++++++++ .../src/controller_interface.cpp | 6 +++ 5 files changed, 86 insertions(+), 1 deletion(-) diff --git a/controller_interface/include/controller_interface/chainable_controller_interface.hpp b/controller_interface/include/controller_interface/chainable_controller_interface.hpp index 37b784011c..d5f1192613 100644 --- a/controller_interface/include/controller_interface/chainable_controller_interface.hpp +++ b/controller_interface/include/controller_interface/chainable_controller_interface.hpp @@ -57,6 +57,10 @@ class ChainableControllerInterface : public ControllerInterfaceBase CONTROLLER_INTERFACE_PUBLIC std::vector export_reference_interfaces() final; + CONTROLLER_INTERFACE_PUBLIC + std::vector + export_distributed_reference_interfaces() final; + CONTROLLER_INTERFACE_PUBLIC bool set_chained_mode(bool chained_mode) final; @@ -75,6 +79,15 @@ class ChainableControllerInterface : public ControllerInterfaceBase */ virtual std::vector on_export_reference_interfaces() = 0; + // Only for fast poc should be pure virtual + /** + * Default returns emtpy list, so that not every chainable controller has to be updated for poc. + * + * \returns empty list. + */ + virtual std::vector + on_export_distributed_reference_interfaces(); + /// Virtual method that each chainable controller should implement to switch chained mode. /** * Each chainable controller implements this methods to switch between "chained" and "external" diff --git a/controller_interface/include/controller_interface/controller_interface.hpp b/controller_interface/include/controller_interface/controller_interface.hpp index a3d006755f..71d9c4f4f7 100644 --- a/controller_interface/include/controller_interface/controller_interface.hpp +++ b/controller_interface/include/controller_interface/controller_interface.hpp @@ -43,13 +43,22 @@ class ControllerInterface : public controller_interface::ControllerInterfaceBase bool is_chainable() const final; /** - * Controller has no reference interfaces. + * Controller has no distributed reference interfaces. * * \returns empty list. */ CONTROLLER_INTERFACE_PUBLIC std::vector export_reference_interfaces() final; + /** + * Controller has no reference interfaces. + * + * \returns empty list. + */ + CONTROLLER_INTERFACE_PUBLIC + std::vector + export_distributed_reference_interfaces() final; + /** * Controller is not chainable, therefore no chained mode can be set. * diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index fdaffac06f..5966e0758b 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -197,6 +197,16 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy CONTROLLER_INTERFACE_PUBLIC virtual std::vector export_reference_interfaces() = 0; + /** + * Export interfaces for a chainable controller that can be used as command interface of other + * controllers if controller is distributed. + * + * \returns list of distributed command interfaces for preceding controllers. + */ + CONTROLLER_INTERFACE_PUBLIC + virtual std::vector + export_distributed_reference_interfaces() = 0; + /** * Set chained mode of a chainable controller. This method triggers internal processes to switch * a chainable controller to "chained" mode and vice-versa. Setting controller to "chained" mode diff --git a/controller_interface/src/chainable_controller_interface.cpp b/controller_interface/src/chainable_controller_interface.cpp index 2f7c67741e..56539950f9 100644 --- a/controller_interface/src/chainable_controller_interface.cpp +++ b/controller_interface/src/chainable_controller_interface.cpp @@ -14,6 +14,7 @@ #include "controller_interface/chainable_controller_interface.hpp" +#include #include #include "hardware_interface/types/lifecycle_state_names.hpp" @@ -44,6 +45,46 @@ return_type ChainableControllerInterface::update( return ret; } +std::vector +ChainableControllerInterface::export_distributed_reference_interfaces() +{ + auto reference_interfaces = on_export_distributed_reference_interfaces(); + + // check if the "reference_interfaces_" variable is resized to number of interfaces + if (reference_interfaces_.size() != reference_interfaces.size()) + { + // TODO(destogl): Should here be "FATAL"? It is fatal in terms of controller but not for the + // framework + RCLCPP_FATAL( + get_node()->get_logger(), + "The internal storage for reference values 'reference_interfaces_' variable has size '%zu', " + "but it is expected to have the size '%zu' equal to the number of exported reference " + "interfaces. No reference interface will be exported. Please correct and recompile " + "the controller with name '%s' and try again.", + reference_interfaces_.size(), reference_interfaces.size(), get_node()->get_name()); + reference_interfaces.clear(); + } + + // check if the names of the reference interfaces begin with the controller's name + for (const auto & interface : reference_interfaces) + { + if (interface.get_prefix_name() != get_node()->get_name()) + { + RCLCPP_FATAL( + get_node()->get_logger(), + "The name of the interface '%s' does not begin with the controller's name. This is " + "mandatory " + " for reference interfaces. No reference interface will be exported. Please correct and " + "recompile the controller with name '%s' and try again.", + interface.get_name().c_str(), get_node()->get_name()); + reference_interfaces.clear(); + break; + } + } + + return reference_interfaces; +} + std::vector ChainableControllerInterface::export_reference_interfaces() { @@ -111,6 +152,12 @@ bool ChainableControllerInterface::set_chained_mode(bool chained_mode) bool ChainableControllerInterface::is_in_chained_mode() const { return in_chained_mode_; } +std::vector +ChainableControllerInterface::on_export_distributed_reference_interfaces() +{ + return {}; +} + bool ChainableControllerInterface::on_set_chained_mode(bool /*chained_mode*/) { return true; } } // namespace controller_interface diff --git a/controller_interface/src/controller_interface.cpp b/controller_interface/src/controller_interface.cpp index 392a48ffa4..2c3d87e597 100644 --- a/controller_interface/src/controller_interface.cpp +++ b/controller_interface/src/controller_interface.cpp @@ -33,6 +33,12 @@ std::vector ControllerInterface::export_re return {}; } +std::vector +ControllerInterface::export_distributed_reference_interfaces() +{ + return {}; +} + bool ControllerInterface::set_chained_mode(bool /*chained_mode*/) { return false; } bool ControllerInterface::is_in_chained_mode() const { return false; } From eebc602e6a56e0fe5baecc9ad9f8621f77956f83 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Fri, 19 May 2023 11:28:19 +0200 Subject: [PATCH 19/26] change exporting of state_publishers and command_forwarders --- .../controller_manager/controller_manager.hpp | 17 +++- controller_manager/src/controller_manager.cpp | 98 +++++++++++++++---- 2 files changed, 93 insertions(+), 22 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index d5ce930cf0..9b98801cde 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -240,10 +240,20 @@ class ControllerManager : public rclcpp::Node std::shared_ptr response); CONTROLLER_MANAGER_PUBLIC - void create_hardware_state_publishers(); + void register_sub_controller_manager_references_srv_cb( + const std::shared_ptr< + controller_manager_msgs::srv::RegisterSubControllerManagerReferences::Request> + request, + std::shared_ptr + response); + + CONTROLLER_MANAGER_PUBLIC + void create_hardware_state_publishers( + const std::vector & state_interfaces_to_export); CONTROLLER_MANAGER_PUBLIC - void create_hardware_command_forwarders(); + void create_hardware_command_forwarders( + const std::vector & command_interfaces_to_export); CONTROLLER_MANAGER_PUBLIC void register_sub_controller_manager(); @@ -464,6 +474,9 @@ class ControllerManager : public rclcpp::Node bool sub_controller_manager_ = false; bool central_controller_manager_ = false; bool use_multiple_nodes_ = false; + std::vector command_interfaces_to_export_ = std::vector({}); + std::vector state_interfaces_to_export_ = std::vector({}); + // TODO(Manuel): weak_ptr would probably be a better choice. This way has to be checked // if pointer points to an object. Don't like the nullptr thing and implicit checks // associated with it ... (create on distributed Handles and StatePublisher/CommandForwarder) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index fc6f61ca5b..860f21fe42 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -378,7 +378,6 @@ void ControllerManager::get_and_initialize_distributed_parameters() distributed_interfaces_publish_period_ = std::chrono::milliseconds(distributed_interfaces_publish_period); } - else { RCLCPP_WARN( @@ -392,6 +391,36 @@ void ControllerManager::get_and_initialize_distributed_parameters() get_logger(), "'use_multiple_nodes' parameter not set, using default value:%s", use_multiple_nodes_ ? "true" : "false"); } + + if (!get_parameter("export_command_interfaces", command_interfaces_to_export_)) + { + RCLCPP_WARN( + get_logger(), + "'export_command_interfaces' parameter not set, going to export all available command " + "interfaces"); + } + + if (!get_parameter("export_state_interfaces", state_interfaces_to_export_)) + { + RCLCPP_WARN( + get_logger(), + "'export_state_interfaces' parameter not set, going to export all available command " + "interfaces"); + } + + if (!get_parameter("sub_controller_manager", sub_controller_manager_)) + { + RCLCPP_WARN( + get_logger(), "'sub_controller_manager' parameter not set, using default value:%s", + sub_controller_manager_ ? "true" : "false"); + } + + if (!get_parameter("use_multiple_nodes", use_multiple_nodes_)) + { + RCLCPP_WARN( + get_logger(), "'use_multiple_nodes' parameter not set, using default value:%s", + use_multiple_nodes_ ? "true" : "false"); + } } void ControllerManager::configure_controller_manager() @@ -469,8 +498,26 @@ void ControllerManager::init_distributed_sub_controller_manager() } } - create_hardware_state_publishers(); - create_hardware_command_forwarders(); + // export every interface by default + if (state_interfaces_to_export_.empty()) + { + // get all available state interfaces and export + create_hardware_state_publishers(resource_manager_->available_state_interfaces()); + } + else + { + create_hardware_state_publishers(state_interfaces_to_export_); + } + // export every interface by default + if (command_interfaces_to_export_.empty()) + { + // get all available command interfaces and export + create_hardware_command_forwarders(resource_manager_->available_command_interfaces()); + } + else + { + create_hardware_command_forwarders(command_interfaces_to_export_); + } register_sub_controller_manager(); } @@ -597,16 +644,18 @@ void ControllerManager::register_sub_controller_manager_srv_cb( << sub_ctrl_mng_wrapper->get_name() << ">."); } -void ControllerManager::create_hardware_state_publishers() +void ControllerManager::register_sub_controller_manager_references_srv_cb( + const std::shared_ptr< + controller_manager_msgs::srv::RegisterSubControllerManagerReferences::Request> + request, + std::shared_ptr + response) { - std::vector state_interfaces_to_export = std::vector({}); - // export every interface by default - if (!get_parameter("export_state_interfaces", state_interfaces_to_export)) - { - // get all available state interfaces - state_interfaces_to_export = resource_manager_->available_state_interfaces(); - } +} +void ControllerManager::create_hardware_state_publishers( + const std::vector & state_interfaces_to_export) +{ for (const auto & state_interface : state_interfaces_to_export) { std::shared_ptr state_publisher; @@ -643,16 +692,9 @@ void ControllerManager::create_hardware_state_publishers() } } -void ControllerManager::create_hardware_command_forwarders() +void ControllerManager::create_hardware_command_forwarders( + const std::vector & command_interfaces_to_export) { - std::vector command_interfaces_to_export = std::vector({}); - // export every interface by default - if (!get_parameter("export_command_interfaces", command_interfaces_to_export)) - { - // get all available command interfaces - command_interfaces_to_export = resource_manager_->available_command_interfaces(); - } - for (auto const & command_interface : command_interfaces_to_export) { std::shared_ptr command_forwarder; @@ -1021,8 +1063,24 @@ controller_interface::return_type ControllerManager::configure_controller( controller_name.c_str()); return controller_interface::return_type::ERROR; } + // safe the name for later. Interfaces get moved so they are no longer available after they have been imported. + std::vector reference_interfaces_names; + reference_interfaces_names.reserve(interfaces.size()); + for (const auto & interface : interfaces) + { + reference_interfaces_names.push_back(interface.get_name()); + } + resource_manager_->import_controller_reference_interfaces(controller_name, interfaces); + if (is_sub_controller_manager()) + { + // export all of the just created reference interfaces by default + create_hardware_command_forwarders(reference_interfaces_names); + + // TODO(Manuel) : register + } + // TODO(destogl): check and resort controllers in the vector } From 7cb7d710a1be40ebda78d1fb3f6129d9b7b058f0 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Fri, 19 May 2023 11:30:32 +0200 Subject: [PATCH 20/26] add skeleton for of service for registration of reference interfaces --- .../controller_manager/controller_manager.hpp | 3 +++ controller_manager/src/controller_manager.cpp | 8 ++++++++ controller_manager_msgs/CMakeLists.txt | 1 + ...RegisterSubControllerManagerReferences.srv | 19 +++++++++++++++++++ 4 files changed, 31 insertions(+) create mode 100644 controller_manager_msgs/srv/RegisterSubControllerManagerReferences.srv diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 9b98801cde..8f7944fcd8 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -36,6 +36,7 @@ #include "controller_manager_msgs/srv/list_hardware_interfaces.hpp" #include "controller_manager_msgs/srv/load_controller.hpp" #include "controller_manager_msgs/srv/register_sub_controller_manager.hpp" +#include "controller_manager_msgs/srv/register_sub_controller_manager_references.hpp" #include "controller_manager_msgs/srv/reload_controller_libraries.hpp" #include "controller_manager_msgs/srv/set_hardware_component_state.hpp" #include "controller_manager_msgs/srv/switch_controller.hpp" @@ -586,6 +587,8 @@ class ControllerManager : public rclcpp::Node std::mutex central_controller_manager_srv_lock_; rclcpp::Service::SharedPtr register_sub_controller_manager_srv_; + rclcpp::Service::SharedPtr + register_sub_controller_manager_references_srv_; std::vector activate_request_, deactivate_request_; std::vector to_chained_mode_request_, from_chained_mode_request_; diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 860f21fe42..861855f7f1 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -556,6 +556,14 @@ void ControllerManager::init_distributed_central_controller_manager_services() &ControllerManager::register_sub_controller_manager_srv_cb, this, std::placeholders::_1, std::placeholders::_2), qos_profile_services_keep_all, distributed_system_srv_callback_group_); + + register_sub_controller_manager_references_srv_ = + create_service( + "register_sub_controller_manager_references", + std::bind( + &ControllerManager::register_sub_controller_manager_references_srv_cb, this, + std::placeholders::_1, std::placeholders::_2), + qos_profile_services_keep_all, distributed_system_srv_callback_group_); } void ControllerManager::register_sub_controller_manager_srv_cb( diff --git a/controller_manager_msgs/CMakeLists.txt b/controller_manager_msgs/CMakeLists.txt index 11230df3fb..cd51c6ab22 100644 --- a/controller_manager_msgs/CMakeLists.txt +++ b/controller_manager_msgs/CMakeLists.txt @@ -22,6 +22,7 @@ set(srv_files srv/ListHardwareInterfaces.srv srv/LoadController.srv srv/RegisterSubControllerManager.srv + srv/RegisterSubControllerManagerReferences.srv srv/ReloadControllerLibraries.srv srv/SetHardwareComponentState.srv srv/SwitchController.srv diff --git a/controller_manager_msgs/srv/RegisterSubControllerManagerReferences.srv b/controller_manager_msgs/srv/RegisterSubControllerManagerReferences.srv new file mode 100644 index 0000000000..6963b086a2 --- /dev/null +++ b/controller_manager_msgs/srv/RegisterSubControllerManagerReferences.srv @@ -0,0 +1,19 @@ +# The RegisterSubControllerManagerReferences service allows a distributed controller +# manager to register it's reference interfaces at the central controller manager + +# Req: +# To register a controller the distributed sub controller manager has to +# define: +# * the full namespace it operates in +# * all statePublishers the sub controller provides + +# Res: +# The response includes: +# * a bool if the registration succeeded + +string sub_controller_manager_namespace +string sub_controller_manager_name +controller_manager_msgs/PublisherDescription[] command_state_publishers +--- +controller_manager_msgs/PublisherDescription[] command_state_publishers +bool ok \ No newline at end of file From 92e2965253b28fa810625d0c3ccdd57185414a8a Mon Sep 17 00:00:00 2001 From: Manuel M Date: Fri, 19 May 2023 11:32:37 +0200 Subject: [PATCH 21/26] reorder distributed parameters --- controller_manager/src/controller_manager.cpp | 14 -------------- 1 file changed, 14 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 861855f7f1..e0824f3740 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -365,13 +365,6 @@ void ControllerManager::get_and_initialize_distributed_parameters() central_controller_manager_ ? "true" : "false"); } - if (!get_parameter("sub_controller_manager", sub_controller_manager_)) - { - RCLCPP_WARN( - get_logger(), "'sub_controller_manager' parameter not set, using default value:%s", - sub_controller_manager_ ? "true" : "false"); - } - int64_t distributed_interfaces_publish_period; if (get_parameter("distributed_interfaces_publish_period", distributed_interfaces_publish_period)) { @@ -385,13 +378,6 @@ void ControllerManager::get_and_initialize_distributed_parameters() "'distributed_interfaces_publish_period' parameter not set, using default value."); } - if (!get_parameter("use_multiple_nodes", use_multiple_nodes_)) - { - RCLCPP_WARN( - get_logger(), "'use_multiple_nodes' parameter not set, using default value:%s", - use_multiple_nodes_ ? "true" : "false"); - } - if (!get_parameter("export_command_interfaces", command_interfaces_to_export_)) { RCLCPP_WARN( From 6f9de3208e82cc6424ddcc78e11bf2124efc8d95 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Sat, 20 May 2023 17:14:09 +0200 Subject: [PATCH 22/26] first rough working poc of distributed chained controllers --- .../controller_manager/controller_manager.hpp | 3 + controller_manager/src/controller_manager.cpp | 169 +++++++++++++++++- .../sub_controller_manager_wrapper.hpp | 3 + .../hardware_component_info.hpp | 2 +- .../hardware_interface/resource_manager.hpp | 5 + hardware_interface/src/resource_manager.cpp | 81 ++++++++- 6 files changed, 255 insertions(+), 8 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 8f7944fcd8..ae1c6e2175 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -259,6 +259,9 @@ class ControllerManager : public rclcpp::Node CONTROLLER_MANAGER_PUBLIC void register_sub_controller_manager(); + CONTROLLER_MANAGER_PUBLIC + void register_reference_interfaces(const std::vector & reference_interfaces_names); + CONTROLLER_MANAGER_PUBLIC controller_interface::ControllerInterfaceBaseSharedPtr add_controller_impl( const ControllerSpec & controller); diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index e0824f3740..96625c2f2b 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -23,6 +23,7 @@ #include "controller_interface/controller_interface_base.hpp" #include "controller_manager_msgs/msg/hardware_component_state.hpp" +#include "controller_manager_msgs/msg/publisher_description.hpp" #include "hardware_interface/distributed_control_interface/command_forwarder.hpp" #include "hardware_interface/distributed_control_interface/state_publisher.hpp" @@ -645,6 +646,55 @@ void ControllerManager::register_sub_controller_manager_references_srv_cb( std::shared_ptr response) { + std::lock_guard guard(central_controller_manager_srv_lock_); + + // only command interfaces can be state publishers. We initialize state interfaces to empty list. + std::vector empty_state_publishers{}; + auto sub_ctrl_mng_wrapper = std::make_shared( + request->sub_controller_manager_namespace, request->sub_controller_manager_name, + empty_state_publishers, request->command_state_publishers); + + std::vector> + distributed_command_interfaces; + distributed_command_interfaces.reserve(sub_ctrl_mng_wrapper->get_command_forwarder_count()); + // create distributed command interface and import into resource storage. + distributed_command_interfaces = + resource_manager_->import_reference_interfaces_of_sub_controller_manager( + sub_ctrl_mng_wrapper, get_namespace(), distributed_pub_sub_node_); + + for (const auto & command_interface : distributed_command_interfaces) + { + // register every node of command_interface at executor only if multiple nodes + // are used. Otherwise the single nodes has already been added + if (use_multiple_nodes()) + { + try + { + executor_->add_node(command_interface->get_node()->get_node_base_interface()); + } + catch (const std::runtime_error & e) + { + response->ok = false; + RCLCPP_WARN_STREAM( + get_logger(), + "ControllerManager: Caught exception while trying to register node of reference " + "interface of sub_controller_manager. Exception:" + << e.what()); + } + } + auto msg = controller_manager_msgs::msg::PublisherDescription(); + msg.ns = get_namespace(); + msg.name.prefix_name = command_interface->get_prefix_name(); + msg.name.interface_name = command_interface->get_interface_name(); + // TODO(Manuel): want topic name relative to namespace, but have to treat "root" namespace separate + msg.publisher_topic = std::string("/") + command_interface->forward_command_topic_name(); + response->command_state_publishers.push_back(msg); + } + + response->ok = true; + RCLCPP_INFO_STREAM( + get_logger(), "ControllerManager: Registered reference interfaces of sub_controller_manager <" + << sub_ctrl_mng_wrapper->get_name() << ">."); } void ControllerManager::create_hardware_state_publishers( @@ -829,6 +879,112 @@ void ControllerManager::register_sub_controller_manager() } } +void ControllerManager::register_reference_interfaces( + const std::vector & reference_interfaces_names) +{ + RCLCPP_INFO_STREAM( + get_logger(), "SubControllerManager:<" << get_namespace() << "/" << get_name() + << "> trying to register reference interfaces."); + rclcpp::Client::SharedPtr + client = create_client( + "/register_sub_controller_manager_references"); + + auto request = std::make_shared< + controller_manager_msgs::srv::RegisterSubControllerManagerReferences::Request>(); + request->sub_controller_manager_namespace = get_namespace(); + request->sub_controller_manager_name = get_name(); + + // export the provided CommandForwarders + for (auto const & reference_interface_name : reference_interfaces_names) + { + auto [found, command_forwarder] = + resource_manager_->find_command_forwarder(reference_interface_name); + if (found) + { + // create description of StatePublisher including: prefix_name, interface_name and topic. + // So that receiver is able to create a DistributedStateInterface which subscribes to the + // topics provided by this sub controller manager + request->command_state_publishers.push_back( + command_forwarder->create_publisher_description_msg()); + } + else + { + RCLCPP_WARN_STREAM( + get_logger(), "SubControllerManager: <" + << get_namespace() << "/" << get_name() + << "> could not find command_forwarder for reference interfaces:" + << reference_interface_name); + } + } + + using namespace std::chrono_literals; + while (!client->wait_for_service(1s)) + { + if (!rclcpp::ok()) + { + RCLCPP_ERROR_STREAM( + get_logger(), "SubControllerManager:<" + << get_namespace() << "/" << get_name() + << ">. Interrupted while waiting for central controller managers " + "register_sub_controller_manager_references service. Exiting."); + return; + } + RCLCPP_INFO_STREAM( + get_logger(), "SubControllerManager:<" + << get_namespace() << "/" << get_name() + << ">. Central controller managers " + "register_sub_controller_manager_references service not available, " + "waiting again..."); + } + + auto result = client->async_send_request(request); + // TODO(Manuel): first try to wait synchronous. If this doesn't work we might have to create a + // queue or something similar, add the future and check in update periodically if finished. + + // This blocks... which might be bad... + result.wait(); + // can call get only once + auto res = result.get(); + if (res->ok) + { + auto command_state_publishers = res->command_state_publishers; + // TODO(Manuel) we should probably make the keys explicit (add key_generation function to handles) + // send keys with request + for (const auto & command_state_publisher : command_state_publishers) + { + std::string key = command_state_publisher.name.prefix_name + "/" + + command_state_publisher.name.interface_name; + auto [found, command_forwarder] = resource_manager_->find_command_forwarder(key); + if (found) + { + RCLCPP_WARN_STREAM( + get_logger(), "SubControllerManager: <" << get_namespace() << "/" << get_name() + << "> found commad forwarder for" << key); + command_forwarder->subscribe_to_command_publisher(command_state_publisher.publisher_topic); + } + else + { + RCLCPP_WARN_STREAM( + get_logger(), "SubControllerManager:<" + << get_namespace() << "/" << get_name() + << ">. Could not find a CommandForwarder for key[" << key + << "]. No subscription to command state possible."); + } + } + RCLCPP_INFO_STREAM( + get_logger(), "SubControllerManager:<" << get_namespace() << "/" << get_name() + << ">. Successfully registered."); + } + else + { + RCLCPP_WARN_STREAM( + get_logger(), "SubControllerManager: <" + << get_namespace() << "/" << get_name() + << ">. Registration of StatePublishers failed. Central ControllerManager " + "returned error code."); + } +} + controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_controller( const std::string & controller_name, const std::string & controller_type) { @@ -1069,10 +1225,19 @@ controller_interface::return_type ControllerManager::configure_controller( if (is_sub_controller_manager()) { + // TODO(Manuel); This is only for fast poc, chaining of multiples in sub controller + // is most likely going to lead to issues if handled this way. + // We should only allow the first controller in the chain to be distributed in each + // sub controller manager and chain the successor locally in sub controller manager + // instead of exporting for every. + + // Set chained mode as default true and make references available so that + // hardware_command_forwarders can be created. + controller->set_chained_mode(true); + resource_manager_->make_controller_reference_interfaces_available(controller_name); // export all of the just created reference interfaces by default create_hardware_command_forwarders(reference_interfaces_names); - - // TODO(Manuel) : register + register_reference_interfaces(reference_interfaces_names); } // TODO(destogl): check and resort controllers in the vector diff --git a/hardware_interface/include/hardware_interface/distributed_control_interface/sub_controller_manager_wrapper.hpp b/hardware_interface/include/hardware_interface/distributed_control_interface/sub_controller_manager_wrapper.hpp index 24fb64a5ff..ec7e359da2 100644 --- a/hardware_interface/include/hardware_interface/distributed_control_interface/sub_controller_manager_wrapper.hpp +++ b/hardware_interface/include/hardware_interface/distributed_control_interface/sub_controller_manager_wrapper.hpp @@ -52,9 +52,12 @@ class SubControllerManagerWrapper final size_t get_command_forwarder_count() const { return command_forwarder_descriptions_.size(); } + std::string type() const { return TYPE_; } + private: const std::string NAMESPACE_; const std::string NAME_; + const std::string TYPE_ = "sub_controller_manager"; std::vector state_publisher_descriptions_; std::vector command_forwarder_descriptions_; }; diff --git a/hardware_interface/include/hardware_interface/hardware_component_info.hpp b/hardware_interface/include/hardware_interface/hardware_component_info.hpp index 45afebdb34..44bfafcbc3 100644 --- a/hardware_interface/include/hardware_interface/hardware_component_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_component_info.hpp @@ -36,7 +36,7 @@ struct HardwareComponentInfo /// Component name. std::string name; - /// Component "classification": "actuator", "sensor" or "system" + /// Component "classification": "actuator", "sensor", "system" or "sub_controller_manager" std::string type; /// Component pluginlib plugin name. diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index 4e07f5f011..eb1aa1d79d 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -142,6 +142,11 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager std::shared_ptr sub_controller_manager, const std::string & ns, std::shared_ptr node); + std::vector> + import_reference_interfaces_of_sub_controller_manager( + std::shared_ptr sub_controller_manager, + const std::string & ns, std::shared_ptr node); + void add_hardware_state_publishers( std::shared_ptr state_publisher); diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 98567a4a27..e2cad29e83 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -665,8 +665,13 @@ class ResourceStorage void add_sub_controller_manager( std::shared_ptr sub_controller_manager) { - const auto [it, success] = sub_controller_manager_map_.insert( - std::pair{sub_controller_manager->get_name(), sub_controller_manager}); + HardwareComponentInfo component_info; + component_info.name = sub_controller_manager->get_name(); + component_info.type = sub_controller_manager->type(); + component_info.state_interfaces = {}; + component_info.command_interfaces = {}; + const auto [it, success] = + hardware_info_map_.insert(std::pair{component_info.name, component_info}); if (!success) { std::string msg( @@ -786,6 +791,64 @@ class ResourceStorage return distributed_command_interfaces; } + std::vector> import_distributed_reference_interfaces( + std::shared_ptr sub_controller_manager, + const std::string & ns, std::shared_ptr node) + { + std::vector> distributed_command_interfaces; + distributed_command_interfaces.reserve(sub_controller_manager->get_command_forwarder_count()); + std::vector interface_names; + interface_names.reserve(sub_controller_manager->get_command_forwarder_count()); + + for (const auto & command_forwarder_description : + sub_controller_manager->get_command_forwarder_descriptions()) + { + // create StateInterface from the Description and store in ResourceStorage. + auto command_interface = + std::make_shared(command_forwarder_description, ns, node); + add_command_interface(command_interface); + // add to return vector, node needs to added to executor. + distributed_command_interfaces.push_back(command_interface); + // TODO(Manuel) this should be handled at one point DRY (adding, claimed ....), key should be made explicit + claimed_command_interface_map_.emplace(std::make_pair(command_interface->get_name(), false)); + interface_names.push_back(command_interface->get_name()); + } + // TODO(Manuel) this should be handled at one point DRY(adding, claimed,make available....), key should be made explicit + available_command_interfaces_.reserve( + available_command_interfaces_.capacity() + interface_names.size()); + auto command_interfaces_end = + hardware_info_map_[sub_controller_manager->get_name()].command_interfaces.end(); + hardware_info_map_[sub_controller_manager->get_name()].command_interfaces.insert( + command_interfaces_end, interface_names.begin(), interface_names.end()); + + for (const auto & interface : interface_names) + { + // TODO(destogl): check if interface should be available on configure + auto found_it = std::find( + available_command_interfaces_.begin(), available_command_interfaces_.end(), interface); + + if (found_it == available_command_interfaces_.end()) + { + available_command_interfaces_.emplace_back(interface); + RCUTILS_LOG_DEBUG_NAMED( + "resource_manager", "(hardware '%s'): '%s' command interface added into available list", + sub_controller_manager->get_name().c_str(), interface.c_str()); + } + else + { + // TODO(destogl): do here error management if interfaces are only partially added into + // "available" list - this should never be the case! + RCUTILS_LOG_WARN_NAMED( + "resource_manager", + "(hardware '%s'): '%s' command interface already in available list." + " This can happen due to multiple calls to 'configure'", + sub_controller_manager->get_name().c_str(), interface.c_str()); + } + } + + return distributed_command_interfaces; + } + // hardware plugins pluginlib::ClassLoader actuator_loader_; pluginlib::ClassLoader sensor_loader_; @@ -821,9 +884,6 @@ class ResourceStorage std::map> command_interface_command_forwarder_map_; - - std::map> - sub_controller_manager_map_; }; ResourceManager::ResourceManager() : resource_storage_(std::make_unique()) {} @@ -957,6 +1017,16 @@ ResourceManager::import_command_interfaces_of_sub_controller_manager( return resource_storage_->import_distributed_command_interfaces(sub_controller_manager, ns, node); } +std::vector> +ResourceManager::import_reference_interfaces_of_sub_controller_manager( + std::shared_ptr sub_controller_manager, + const std::string & ns, std::shared_ptr node) +{ + std::lock_guard guard(resource_interfaces_lock_); + return resource_storage_->import_distributed_reference_interfaces( + sub_controller_manager, ns, node); +} + void ResourceManager::add_hardware_state_publishers( std::shared_ptr state_publisher) { @@ -988,6 +1058,7 @@ ResourceManager::get_command_forwarders() const std::pair> ResourceManager::find_command_forwarder(const std::string & key) { + std::lock_guard guard(resource_interfaces_lock_); return resource_storage_->find_command_forwarder(key); } From 6241a0eb866c2e4b68d39da98c4e176cc16686c5 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Mon, 22 May 2023 13:54:10 +0200 Subject: [PATCH 23/26] change warn to debug in distributed handles and state-/commandpublisher --- hardware_interface/CMakeLists.txt | 6 +++--- hardware_interface/include/hardware_interface/handle.hpp | 6 +++--- .../distributed_control_interface/command_forwarder.cpp | 4 +++- .../distributed_control_interface/state_publisher.cpp | 4 +++- 4 files changed, 12 insertions(+), 8 deletions(-) diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index b419dbe4f0..761b95f0bb 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -7,14 +7,14 @@ endif() set(THIS_PACKAGE_INCLUDE_DEPENDS control_msgs - controller_manager_msgs + controller_manager_msgs lifecycle_msgs pluginlib - rclcpp + rclcpp rclcpp_lifecycle rcpputils rcutils - std_msgs + std_msgs TinyXML2 tinyxml2_vendor ) diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index 52fbc3b053..4900b429f4 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -254,7 +254,7 @@ class DistributedReadOnlyHandle : public ReadOnlyHandle void get_value_cb(const std_msgs::msg::Float64 & msg) { value_ = msg.data; - RCLCPP_WARN_STREAM(node_->get_logger(), "Receiving:[" << value_ << "]."); + RCLCPP_DEBUG_STREAM(node_->get_logger(), "Receiving:[" << value_ << "]."); } std::string get_value_topic_name_; @@ -405,7 +405,7 @@ class DistributedReadWriteHandle : public ReadWriteHandle auto msg = std::make_unique(); msg->data = value; - RCLCPP_WARN(node_->get_logger(), "DistributedCommandInterface Publishing: '%.7lf'", msg->data); + RCLCPP_DEBUG(node_->get_logger(), "DistributedCommandInterface Publishing: '%.7lf'", msg->data); std::flush(std::cout); command_value_pub_->publish(std::move(msg)); @@ -417,7 +417,7 @@ class DistributedReadWriteHandle : public ReadWriteHandle void get_value_cb(const std_msgs::msg::Float64 & msg) { value_ = msg.data; - RCLCPP_WARN_STREAM( + RCLCPP_DEBUG_STREAM( node_->get_logger(), "DistributedCommandInterface Receiving:[" << value_ << "]."); } diff --git a/hardware_interface/src/hardware_interface/distributed_control_interface/command_forwarder.cpp b/hardware_interface/src/hardware_interface/distributed_control_interface/command_forwarder.cpp index 558b632b2a..c9db5cbfee 100644 --- a/hardware_interface/src/hardware_interface/distributed_control_interface/command_forwarder.cpp +++ b/hardware_interface/src/hardware_interface/distributed_control_interface/command_forwarder.cpp @@ -6,6 +6,8 @@ #include #include +#include "rclcpp/rclcpp.hpp" + using namespace std::chrono_literals; namespace distributed_control @@ -105,7 +107,7 @@ void CommandForwarder::publish_value_on_timer() { msg->data = std::numeric_limits::quiet_NaN(); } - RCLCPP_WARN(node_->get_logger(), "Publishing: '%.7lf'", msg->data); + RCLCPP_DEBUG(node_->get_logger(), "Publishing: '%.7lf'", msg->data); std::flush(std::cout); // Put the message into a queue to be processed by the middleware. diff --git a/hardware_interface/src/hardware_interface/distributed_control_interface/state_publisher.cpp b/hardware_interface/src/hardware_interface/distributed_control_interface/state_publisher.cpp index 0857bbb342..b353ef63bf 100644 --- a/hardware_interface/src/hardware_interface/distributed_control_interface/state_publisher.cpp +++ b/hardware_interface/src/hardware_interface/distributed_control_interface/state_publisher.cpp @@ -6,6 +6,8 @@ #include #include +#include "rclcpp/rclcpp.hpp" + using namespace std::chrono_literals; namespace distributed_control @@ -97,7 +99,7 @@ void StatePublisher::publish_value_on_timer() // Make explicit note implicit!!! msg->data = std::numeric_limits::quiet_NaN(); } - RCLCPP_WARN(node_->get_logger(), "Publishing: '%.7lf'", msg->data); + RCLCPP_DEBUG(node_->get_logger(), "Publishing: '%.7lf'", msg->data); std::flush(std::cout); // Put the message into a queue to be processed by the middleware. From f8976fc03438767d7c5d304e901e97bfee7a3f65 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Sun, 11 Jun 2023 15:09:27 +0200 Subject: [PATCH 24/26] add helper for evaluation (qos, publish, publish qos) --- .../controller_manager/controller_manager.hpp | 7 + controller_manager/src/controller_manager.cpp | 58 +++++++ hardware_interface/CMakeLists.txt | 1 + .../command_forwarder.hpp | 1 + .../evaluation_helper.hpp | 156 ++++++++++++++++++ .../state_publisher.hpp | 1 + .../include/hardware_interface/handle.hpp | 14 +- .../command_forwarder.cpp | 10 +- .../evaluation_helper.cpp | 5 + .../state_publisher.cpp | 5 +- 10 files changed, 252 insertions(+), 6 deletions(-) create mode 100644 hardware_interface/include/hardware_interface/distributed_control_interface/evaluation_helper.hpp create mode 100644 hardware_interface/src/hardware_interface/distributed_control_interface/evaluation_helper.cpp diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index ae1c6e2175..d0dddc3ebb 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -201,6 +201,9 @@ class ControllerManager : public rclcpp::Node CONTROLLER_MANAGER_PUBLIC unsigned int get_update_rate() const; + CONTROLLER_MANAGER_PUBLIC + rmw_qos_profile_t determine_qos_profile(const std::string & key) const; + CONTROLLER_MANAGER_PUBLIC bool is_central_controller_manager() const; @@ -478,6 +481,10 @@ class ControllerManager : public rclcpp::Node bool sub_controller_manager_ = false; bool central_controller_manager_ = false; bool use_multiple_nodes_ = false; + std::shared_ptr qos_helper_; + std::string handles_qos_key_ = "system_default"; + bool publish_evaluation_msg_ = true; + std::string evaluation_qos_key_ = "system_default"; std::vector command_interfaces_to_export_ = std::vector({}); std::vector state_interfaces_to_export_ = std::vector({}); diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 96625c2f2b..9830ce333b 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -26,6 +26,7 @@ #include "controller_manager_msgs/msg/publisher_description.hpp" #include "hardware_interface/distributed_control_interface/command_forwarder.hpp" +#include "hardware_interface/distributed_control_interface/evaluation_helper.hpp" #include "hardware_interface/distributed_control_interface/state_publisher.hpp" #include "hardware_interface/handle.hpp" #include "hardware_interface/types/lifecycle_state_names.hpp" @@ -408,6 +409,24 @@ void ControllerManager::get_and_initialize_distributed_parameters() get_logger(), "'use_multiple_nodes' parameter not set, using default value:%s", use_multiple_nodes_ ? "true" : "false"); } + if (!get_parameter("handles_qos_key", handles_qos_key_)) + { + RCLCPP_WARN( + get_logger(), "'handles_qos_key' parameter not set, using default value:%s", + handles_qos_key_.c_str()); + } + if (!get_parameter("publish_evaluation_msg", publish_evaluation_msg_)) + { + RCLCPP_WARN( + get_logger(), "'publish_evaluation_msg' parameter not set, using default value:%s", + publish_evaluation_msg_ ? "true" : "false"); + } + if (!get_parameter("evaluation_qos_key", evaluation_qos_key_)) + { + RCLCPP_WARN( + get_logger(), "'evaluation_qos_key' parameter not set, using default value:%s", + evaluation_qos_key_.c_str()); + } } void ControllerManager::configure_controller_manager() @@ -462,8 +481,42 @@ ControllerManager::controller_manager_type ControllerManager::determine_controll return controller_manager_type::unkown_type; } +rmw_qos_profile_t ControllerManager::determine_qos_profile(const std::string & key) const +{ + if (key == "sensor_data") + { + return evaluation_helper::rmw_qos_profile_sensor_data; + } + else if (key == "sensor_data_1") + { + return evaluation_helper::rmw_qos_profile_sensor_data_1; + } + else if (key == "sensor_data_100") + { + return evaluation_helper::rmw_qos_profile_sensor_data_100; + } + else if (key == "reliable") + { + return evaluation_helper::rmw_qos_profile_reliable; + } + else if (key == "reliable_100") + { + return evaluation_helper::rmw_qos_profile_reliable_100; + } + else if (key == "system_default") + { + return evaluation_helper::rmw_qos_profile_system_default; + } + throw std::runtime_error("Given qos profile not know"); +} + void ControllerManager::init_distributed_sub_controller_manager() { + // just for evaluation of concept + auto handle_qos_profile = determine_qos_profile(handles_qos_key_); + auto evaluation_qos_profile = determine_qos_profile(evaluation_qos_key_); + qos_helper_ = evaluation_helper::Evaluation_Helper::create_instance( + handle_qos_profile, publish_evaluation_msg_, evaluation_qos_profile); // if only one node per sub controller manager is used if (!use_multiple_nodes()) { @@ -510,6 +563,11 @@ void ControllerManager::init_distributed_sub_controller_manager() void ControllerManager::init_distributed_central_controller_manager() { + // just for evaluation of concept + auto handle_qos_profile = determine_qos_profile(handles_qos_key_); + auto evaluation_qos_profile = determine_qos_profile(evaluation_qos_key_); + qos_helper_ = evaluation_helper::Evaluation_Helper::create_instance( + handle_qos_profile, publish_evaluation_msg_, evaluation_qos_profile); if (!use_multiple_nodes()) { rclcpp::NodeOptions node_options; diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index 761b95f0bb..9be78cbc6b 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -29,6 +29,7 @@ add_library(hardware_interface SHARED src/component_parser.cpp src/hardware_interface/distributed_control_interface/command_forwarder.cpp src/hardware_interface/distributed_control_interface/state_publisher.cpp + src/hardware_interface/distributed_control_interface/evaluation_helper.cpp src/resource_manager.cpp src/sensor.cpp src/system.cpp diff --git a/hardware_interface/include/hardware_interface/distributed_control_interface/command_forwarder.hpp b/hardware_interface/include/hardware_interface/distributed_control_interface/command_forwarder.hpp index cfcc6ce9e0..fb94a09450 100644 --- a/hardware_interface/include/hardware_interface/distributed_control_interface/command_forwarder.hpp +++ b/hardware_interface/include/hardware_interface/distributed_control_interface/command_forwarder.hpp @@ -6,6 +6,7 @@ #include #include +#include "hardware_interface/distributed_control_interface/evaluation_helper.hpp" #include "hardware_interface/distributed_control_interface/publisher_description.hpp" #include "hardware_interface/loaned_command_interface.hpp" diff --git a/hardware_interface/include/hardware_interface/distributed_control_interface/evaluation_helper.hpp b/hardware_interface/include/hardware_interface/distributed_control_interface/evaluation_helper.hpp new file mode 100644 index 0000000000..84905fd9f6 --- /dev/null +++ b/hardware_interface/include/hardware_interface/distributed_control_interface/evaluation_helper.hpp @@ -0,0 +1,156 @@ +// Copyright 2020 - 2021 ros2_control Development Team +// +// 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. + +#ifndef DISTRIBUTED_CONTROL__QOS_PROFILES_HELPER_HPP_ +#define DISTRIBUTED_CONTROL__QOS_PROFILES_HELPER_HPP_ + +#include +#include + +#include "rmw/qos_profiles.h" +#include "rmw/types.h" + +namespace evaluation_helper +{ + +// All profiles used for evaluation, copy them to make sure that exactly those params are used. +static const rmw_qos_profile_t rmw_qos_profile_sensor_data = { + RMW_QOS_POLICY_HISTORY_KEEP_LAST, + 5, + RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT, + RMW_QOS_POLICY_DURABILITY_VOLATILE, + RMW_QOS_DEADLINE_DEFAULT, + RMW_QOS_LIFESPAN_DEFAULT, + RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT, + RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT, + false}; + +static const rmw_qos_profile_t rmw_qos_profile_sensor_data_1 = { + RMW_QOS_POLICY_HISTORY_KEEP_LAST, + 1, + RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT, + RMW_QOS_POLICY_DURABILITY_VOLATILE, + RMW_QOS_DEADLINE_DEFAULT, + RMW_QOS_LIFESPAN_DEFAULT, + RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT, + RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT, + false}; + +static const rmw_qos_profile_t rmw_qos_profile_sensor_data_100 = { + RMW_QOS_POLICY_HISTORY_KEEP_LAST, + 100, + RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT, + RMW_QOS_POLICY_DURABILITY_VOLATILE, + RMW_QOS_DEADLINE_DEFAULT, + RMW_QOS_LIFESPAN_DEFAULT, + RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT, + RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT, + false}; + +static const rmw_qos_profile_t rmw_qos_profile_reliable = { + RMW_QOS_POLICY_HISTORY_KEEP_LAST, + 1, + RMW_QOS_POLICY_RELIABILITY_RELIABLE, + RMW_QOS_POLICY_DURABILITY_VOLATILE, + RMW_QOS_DEADLINE_DEFAULT, + RMW_QOS_LIFESPAN_DEFAULT, + RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT, + RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT, + false}; + +static const rmw_qos_profile_t rmw_qos_profile_reliable_100 = { + RMW_QOS_POLICY_HISTORY_KEEP_LAST, + 100, + RMW_QOS_POLICY_RELIABILITY_RELIABLE, + RMW_QOS_POLICY_DURABILITY_VOLATILE, + RMW_QOS_DEADLINE_DEFAULT, + RMW_QOS_LIFESPAN_DEFAULT, + RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT, + RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT, + false}; + +static const rmw_qos_profile_t rmw_qos_profile_system_default = { + RMW_QOS_POLICY_HISTORY_SYSTEM_DEFAULT, + RMW_QOS_POLICY_DEPTH_SYSTEM_DEFAULT, + RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT, + RMW_QOS_POLICY_DURABILITY_SYSTEM_DEFAULT, + RMW_QOS_DEADLINE_DEFAULT, + RMW_QOS_LIFESPAN_DEFAULT, + RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT, + RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT, + false}; + +class Evaluation_Helper +{ +protected: + Evaluation_Helper( + const rmw_qos_profile_t & qos_profile, const bool & publish_evaluation_msg, + const rmw_qos_profile_t & evaluation_qos_profile) + : qos_profile_(qos_profile), + publish_evaluation_msg_(publish_evaluation_msg), + evaluation_qos_profile_(evaluation_qos_profile) + { + } + +public: + /** + * Evaluation_Helpers should not be cloneable. + */ + Evaluation_Helper(Evaluation_Helper & other) = delete; + /** + * Evaluation_Helpers should not be assignable. + */ + void operator=(const Evaluation_Helper &) = delete; + + static std::shared_ptr create_instance( + const rmw_qos_profile_t & qos_profile, const bool & publish_evaluation_msg, + const rmw_qos_profile_t & evaluation_qos_profile) + { + std::lock_guard lock(mutex_); + if (qos_profile_instance_ == nullptr) + { + qos_profile_instance_ = std::shared_ptr( + new Evaluation_Helper(qos_profile, publish_evaluation_msg, evaluation_qos_profile)); + } + return qos_profile_instance_; + } + + static std::shared_ptr get_instance() + { + std::lock_guard lock(mutex_); + if (qos_profile_instance_ == nullptr) + { + throw std::runtime_error("Evaluation_Helper not initialized!"); + } + return qos_profile_instance_; + } + + rmw_qos_profile_t get_qos_profile() const { return qos_profile_; } + + rmw_qos_profile_t get_evaluation_qos_profile() const { return evaluation_qos_profile_; } + + bool publish_evaluation_msg() const { return publish_evaluation_msg_; } + +protected: + static std::shared_ptr qos_profile_instance_; + static std::mutex mutex_; + + const rmw_qos_profile_t qos_profile_; + const bool publish_evaluation_msg_; + const rmw_qos_profile_t evaluation_qos_profile_; +}; + +} // namespace evaluation_helper + +#endif // DISTRIBUTED_CONTROL__EVALUATION_HELPER_HPP_ \ No newline at end of file diff --git a/hardware_interface/include/hardware_interface/distributed_control_interface/state_publisher.hpp b/hardware_interface/include/hardware_interface/distributed_control_interface/state_publisher.hpp index 8d260beda8..65a166658a 100644 --- a/hardware_interface/include/hardware_interface/distributed_control_interface/state_publisher.hpp +++ b/hardware_interface/include/hardware_interface/distributed_control_interface/state_publisher.hpp @@ -5,6 +5,7 @@ #include #include +#include "hardware_interface/distributed_control_interface/evaluation_helper.hpp" #include "hardware_interface/loaned_state_interface.hpp" #include "controller_manager_msgs/msg/publisher_description.hpp" diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index 4900b429f4..55678b1bb5 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -18,6 +18,7 @@ #include #include +#include "hardware_interface/distributed_control_interface/evaluation_helper.hpp" #include "hardware_interface/distributed_control_interface/publisher_description.hpp" #include "hardware_interface/macros.hpp" #include "hardware_interface/visibility_control.h" @@ -206,9 +207,13 @@ class DistributedReadOnlyHandle : public ReadOnlyHandle get_underscore_separated_name() + "_state_interface_subscriber", namespace_, node_options, false); } + + auto evaluation_helper = evaluation_helper::Evaluation_Helper::get_instance(); + rclcpp::QoS qos_profile( + rclcpp::QoSInitialization::from_rmw(evaluation_helper->get_qos_profile())); // subscribe to topic provided by StatePublisher state_value_sub_ = node_->create_subscription( - get_value_topic_name_, 10, + get_value_topic_name_, qos_profile, std::bind(&DistributedReadOnlyHandle::get_value_cb, this, std::placeholders::_1)); } @@ -354,14 +359,17 @@ class DistributedReadWriteHandle : public ReadWriteHandle node_options, false); } + auto evaluation_helper = evaluation_helper::Evaluation_Helper::get_instance(); + rclcpp::QoS qos_profile( + rclcpp::QoSInitialization::from_rmw(evaluation_helper->get_qos_profile())); // subscribe to topic provided by CommandForwarder command_value_sub_ = node_->create_subscription( - get_value_topic_name_, 10, + get_value_topic_name_, qos_profile, std::bind(&DistributedReadWriteHandle::get_value_cb, this, std::placeholders::_1)); // create publisher so that we can forward the commands command_value_pub_ = - node_->create_publisher(forward_command_topic_name_, 10); + node_->create_publisher(forward_command_topic_name_, qos_profile); } explicit DistributedReadWriteHandle(const std::string & interface_name) diff --git a/hardware_interface/src/hardware_interface/distributed_control_interface/command_forwarder.cpp b/hardware_interface/src/hardware_interface/distributed_control_interface/command_forwarder.cpp index c9db5cbfee..0ee7be9ad8 100644 --- a/hardware_interface/src/hardware_interface/distributed_control_interface/command_forwarder.cpp +++ b/hardware_interface/src/hardware_interface/distributed_control_interface/command_forwarder.cpp @@ -32,7 +32,10 @@ CommandForwarder::CommandForwarder( namespace_, node_options, false); } - state_value_pub_ = node_->create_publisher(topic_name_, 10); + auto evaluation_helper = evaluation_helper::Evaluation_Helper::get_instance(); + rclcpp::QoS qos_profile( + rclcpp::QoSInitialization::from_rmw(evaluation_helper->get_qos_profile())); + state_value_pub_ = node_->create_publisher(topic_name_, qos_profile); // TODO(Manuel): We should check if we cannot detect changes to LoanedStateInterface's value and only publish then timer_ = node_->create_wall_timer( period_in_ms_, std::bind(&CommandForwarder::publish_value_on_timer, this)); @@ -89,9 +92,12 @@ CommandForwarder::create_publisher_description_msg() const void CommandForwarder::subscribe_to_command_publisher(const std::string & topic_name) { + auto evaluation_helper = evaluation_helper::Evaluation_Helper::get_instance(); + rclcpp::QoS qos_profile( + rclcpp::QoSInitialization::from_rmw(evaluation_helper->get_qos_profile())); subscription_topic_name_ = topic_name; command_subscription_ = node_->create_subscription( - subscription_topic_name_, 10, + subscription_topic_name_, qos_profile, std::bind(&CommandForwarder::forward_command, this, std::placeholders::_1)); } diff --git a/hardware_interface/src/hardware_interface/distributed_control_interface/evaluation_helper.cpp b/hardware_interface/src/hardware_interface/distributed_control_interface/evaluation_helper.cpp new file mode 100644 index 0000000000..35c7f213bd --- /dev/null +++ b/hardware_interface/src/hardware_interface/distributed_control_interface/evaluation_helper.cpp @@ -0,0 +1,5 @@ +#include "hardware_interface/distributed_control_interface/evaluation_helper.hpp" + +std::shared_ptr + evaluation_helper::Evaluation_Helper::qos_profile_instance_{nullptr}; +std::mutex evaluation_helper::Evaluation_Helper::mutex_; \ No newline at end of file diff --git a/hardware_interface/src/hardware_interface/distributed_control_interface/state_publisher.cpp b/hardware_interface/src/hardware_interface/distributed_control_interface/state_publisher.cpp index b353ef63bf..0ceaee8b6c 100644 --- a/hardware_interface/src/hardware_interface/distributed_control_interface/state_publisher.cpp +++ b/hardware_interface/src/hardware_interface/distributed_control_interface/state_publisher.cpp @@ -32,7 +32,10 @@ StatePublisher::StatePublisher( node_options, false); } - state_value_pub_ = node_->create_publisher(topic_name_, 10); + auto evaluation_helper = evaluation_helper::Evaluation_Helper::get_instance(); + rclcpp::QoS qos_profile( + rclcpp::QoSInitialization::from_rmw(evaluation_helper->get_qos_profile())); + state_value_pub_ = node_->create_publisher(topic_name_, qos_profile); // TODO(Manuel): We should check if we cannot detect changes to LoanedStateInterface's value and only publish then timer_ = node_->create_wall_timer( period_in_ms_, std::bind(&StatePublisher::publish_value_on_timer, this)); From 59de6ffb1cf9fe3b906f5483b02967bedbf03d94 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Sun, 11 Jun 2023 16:12:19 +0200 Subject: [PATCH 25/26] change qos for distributed services to reliable --- controller_manager/src/controller_manager.cpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 9830ce333b..69d920f352 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -49,10 +49,6 @@ rclcpp::QoS qos_services = .reliable() .durability_volatile(); -rclcpp::QoSInitialization qos_profile_services_keep_all_persist_init( - RMW_QOS_POLICY_HISTORY_KEEP_ALL, 1); -rclcpp::QoS qos_profile_services_keep_all(qos_profile_services_keep_all_persist_init); - inline bool is_controller_inactive(const controller_interface::ControllerInterfaceBase & controller) { return controller.get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE; @@ -591,6 +587,11 @@ void ControllerManager::init_distributed_central_controller_manager() void ControllerManager::init_distributed_central_controller_manager_services() { + rclcpp::QoS qos_distributed_services_keep_10 = + rclcpp::QoS(rclcpp::QoSInitialization(RMW_QOS_POLICY_HISTORY_KEEP_ALL, 10)) + .reliable() + .durability_volatile(); + distributed_system_srv_callback_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); @@ -600,7 +601,7 @@ void ControllerManager::init_distributed_central_controller_manager_services() std::bind( &ControllerManager::register_sub_controller_manager_srv_cb, this, std::placeholders::_1, std::placeholders::_2), - qos_profile_services_keep_all, distributed_system_srv_callback_group_); + qos_distributed_services_keep_10, distributed_system_srv_callback_group_); register_sub_controller_manager_references_srv_ = create_service( @@ -608,7 +609,7 @@ void ControllerManager::init_distributed_central_controller_manager_services() std::bind( &ControllerManager::register_sub_controller_manager_references_srv_cb, this, std::placeholders::_1, std::placeholders::_2), - qos_profile_services_keep_all, distributed_system_srv_callback_group_); + qos_distributed_services_keep_10, distributed_system_srv_callback_group_); } void ControllerManager::register_sub_controller_manager_srv_cb( From a9a599848bc9d466f5db605db58f66e123f4d520 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Wed, 14 Jun 2023 15:09:00 +0200 Subject: [PATCH 26/26] add steady clock to nodes --- controller_manager/src/controller_manager.cpp | 2 ++ hardware_interface/include/hardware_interface/handle.hpp | 2 ++ .../distributed_control_interface/command_forwarder.cpp | 1 + .../distributed_control_interface/state_publisher.cpp | 1 + 4 files changed, 6 insertions(+) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 69d920f352..bd89445aa5 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -518,6 +518,7 @@ void ControllerManager::init_distributed_sub_controller_manager() { // create node for publishing/subscribing rclcpp::NodeOptions node_options; + node_options.clock_type(rcl_clock_type_t::RCL_STEADY_TIME); distributed_pub_sub_node_ = std::make_shared( std::string(get_name()) + "_pub_sub_node", get_namespace(), node_options, false); //try to add to executor @@ -567,6 +568,7 @@ void ControllerManager::init_distributed_central_controller_manager() if (!use_multiple_nodes()) { rclcpp::NodeOptions node_options; + node_options.clock_type(rcl_clock_type_t::RCL_STEADY_TIME); distributed_pub_sub_node_ = std::make_shared( std::string(get_name()) + "_pub_sub_node", get_namespace(), node_options, false); //try to add to executor diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index 55678b1bb5..0b4aaba12d 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -203,6 +203,7 @@ class DistributedReadOnlyHandle : public ReadOnlyHandle if (!node_.get()) { rclcpp::NodeOptions node_options; + node_options.clock_type(rcl_clock_type_t::RCL_STEADY_TIME); node_ = std::make_shared( get_underscore_separated_name() + "_state_interface_subscriber", namespace_, node_options, false); @@ -354,6 +355,7 @@ class DistributedReadWriteHandle : public ReadWriteHandle if (!node_.get()) { rclcpp::NodeOptions node_options; + node_options.clock_type(rcl_clock_type_t::RCL_STEADY_TIME); node_ = std::make_shared( get_underscore_separated_name() + "_distributed_command_interface", namespace_, node_options, false); diff --git a/hardware_interface/src/hardware_interface/distributed_control_interface/command_forwarder.cpp b/hardware_interface/src/hardware_interface/distributed_control_interface/command_forwarder.cpp index 0ee7be9ad8..203be40c00 100644 --- a/hardware_interface/src/hardware_interface/distributed_control_interface/command_forwarder.cpp +++ b/hardware_interface/src/hardware_interface/distributed_control_interface/command_forwarder.cpp @@ -27,6 +27,7 @@ CommandForwarder::CommandForwarder( if (!node_.get()) { rclcpp::NodeOptions node_options; + node_options.clock_type(rcl_clock_type_t::RCL_STEADY_TIME); node_ = std::make_shared( loaned_command_interface_ptr_->get_underscore_separated_name() + "_command_forwarder", namespace_, node_options, false); diff --git a/hardware_interface/src/hardware_interface/distributed_control_interface/state_publisher.cpp b/hardware_interface/src/hardware_interface/distributed_control_interface/state_publisher.cpp index 0ceaee8b6c..bec3564d89 100644 --- a/hardware_interface/src/hardware_interface/distributed_control_interface/state_publisher.cpp +++ b/hardware_interface/src/hardware_interface/distributed_control_interface/state_publisher.cpp @@ -27,6 +27,7 @@ StatePublisher::StatePublisher( if (!node_.get()) { rclcpp::NodeOptions node_options; + node_options.clock_type(rcl_clock_type_t::RCL_STEADY_TIME); node_ = std::make_shared( loaned_state_interface_ptr_->get_underscore_separated_name() + "_state_publisher", namespace_, node_options, false);