From 0e05f2ebd9a340e9af503ccbab4233d011331193 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Mon, 6 Feb 2023 11:17:43 +0100 Subject: [PATCH 01/11] 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 dbfeab078515b6c5653e86c9a9e0ad066f727e38 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Thu, 16 Feb 2023 08:08:57 +0100 Subject: [PATCH 02/11] 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 fc2cbb5eb80de8032683daa9cde2c28ffab573ac Mon Sep 17 00:00:00 2001 From: Manuel M Date: Thu, 16 Feb 2023 09:58:35 +0100 Subject: [PATCH 03/11] 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 a08c3866160f40ac574427084cdc3ceebaae23eb Mon Sep 17 00:00:00 2001 From: Manuel M Date: Tue, 21 Feb 2023 08:40:29 +0100 Subject: [PATCH 04/11] 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 22a114ec6eb3df0992cdd19eef018f33f92f917a Mon Sep 17 00:00:00 2001 From: Manuel M Date: Fri, 31 Mar 2023 15:47:25 +0200 Subject: [PATCH 05/11] 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 997c3050c248296b66d61ba17728f85c30650b40 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Mon, 3 Apr 2023 11:56:50 +0200 Subject: [PATCH 06/11] 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 a0ac528555292fd251b00eb9568a9a0d759999c6 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Tue, 4 Apr 2023 13:24:23 +0200 Subject: [PATCH 07/11] 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 f27f0f91755352aa1eb6ca13f6dc5ca3f80b4e99 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Fri, 7 Apr 2023 11:50:52 +0200 Subject: [PATCH 08/11] 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 6f521cedc8..b64edb3135 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 c338a94ce06f90d66a627feee6f90aac70f0b3b9 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Fri, 14 Apr 2023 09:03:41 +0200 Subject: [PATCH 09/11] 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 ecc22182025f38bcf881b24bea2bb473c2eab4c4 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Tue, 25 Apr 2023 11:56:37 +0200 Subject: [PATCH 10/11] 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 b66d186ffbf729cd8753f59cfc0102b76bd50269 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Wed, 7 Jun 2023 22:47:57 +0200 Subject: [PATCH 11/11] Cleanup some tiny issues with the code. --- controller_manager/CMakeLists.txt | 2 +- .../controller_manager/controller_manager.hpp | 4 +--- controller_manager/src/controller_manager.cpp | 12 ++++++------ .../test/test_controller_manager_urdf_passing.cpp | 10 +++++----- hardware_interface/doc/mock_components_userdoc.rst | 2 +- .../include/hardware_interface/resource_manager.hpp | 4 ++-- hardware_interface/src/resource_manager.cpp | 4 ++-- hardware_interface/test/test_resource_manager.cpp | 10 +++++----- 8 files changed, 23 insertions(+), 25 deletions(-) diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index 3d38403332..1118856e48 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -145,7 +145,7 @@ if(BUILD_TESTING) controller_manager_msgs ) ament_add_gmock(test_controller_manager_urdf_passing - test/test_controller_manager_urdf_passing.cpp + test/test_controller_manager_urdf_passing.cpp ) target_link_libraries(test_controller_manager_urdf_passing controller_manager diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index f05de22d9c..631d5173aa 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -84,9 +84,6 @@ 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); @@ -324,6 +321,7 @@ class ControllerManager : public rclcpp::Node std::vector get_controller_names(); std::pair split_command_interface( const std::string & command_interface); + void subscribe_to_robot_description_topic(); /** * Clear request lists used when switching controllers. The lists are shared between "callback" diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 0111b6f67e..69a3bbb5f3 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -151,7 +151,7 @@ ControllerManager::ControllerManager( } std::string robot_description = ""; - // TODO(Manuel): robot_description parameter is deprecated and should be removed. + // TODO(destogl): remove support at the end of 2023 get_parameter("robot_description", robot_description); if (robot_description.empty()) { @@ -161,8 +161,8 @@ ControllerManager::ControllerManager( { RCLCPP_WARN( get_logger(), - "[Deprecated] Passing the robot description file directly to the control_manager node is " - "deprecated. Use robot_state_publisher instead."); + "[Deprecated] Passing the robot description parameter directly to the control_manager node " + "is deprecated. Use '~/robot_description' topic from 'robot_state_publisher' instead."); init_resource_manager(robot_description); } @@ -203,7 +203,7 @@ 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( + 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(), @@ -219,7 +219,7 @@ 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_->load_urdf_called()) + if (resource_manager_->is_urdf_already_loaded()) { RCLCPP_WARN( get_logger(), @@ -249,7 +249,7 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript std::vector configure_components_on_start = std::vector({}); if (get_parameter("configure_components_on_start", configure_components_on_start)) { - RCLCPP_WARN_STREAM( + RCLCPP_WARN( get_logger(), "[Deprecated]: Usage of parameter \"configure_components_on_start\" is deprecated. Use " "hardware_spawner instead."); diff --git a/controller_manager/test/test_controller_manager_urdf_passing.cpp b/controller_manager/test/test_controller_manager_urdf_passing.cpp index c73a45e291..102cbdfbd4 100644 --- a/controller_manager/test/test_controller_manager_urdf_passing.cpp +++ b/controller_manager/test/test_controller_manager_urdf_passing.cpp @@ -61,27 +61,27 @@ class TestControllerManagerWithTestableCM TEST_P(TestControllerManagerWithTestableCM, initial_no_load_urdf_called) { - ASSERT_FALSE(cm_->resource_manager_->load_urdf_called()); + ASSERT_FALSE(cm_->resource_manager_->is_urdf_already_loaded()); } TEST_P(TestControllerManagerWithTestableCM, load_urdf_called_after_callback) { - ASSERT_FALSE(cm_->resource_manager_->load_urdf_called()); + ASSERT_FALSE(cm_->resource_manager_->is_urdf_already_loaded()); // 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()); + ASSERT_TRUE(cm_->resource_manager_->is_urdf_already_loaded()); } TEST_P(TestControllerManagerWithTestableCM, load_urdf_called_after_invalid_urdf_passed) { - ASSERT_FALSE(cm_->resource_manager_->load_urdf_called()); + ASSERT_FALSE(cm_->resource_manager_->is_urdf_already_loaded()); // 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()); + ASSERT_TRUE(cm_->resource_manager_->is_urdf_already_loaded()); } INSTANTIATE_TEST_SUITE_P( diff --git a/hardware_interface/doc/mock_components_userdoc.rst b/hardware_interface/doc/mock_components_userdoc.rst index f143ea816c..573037a58d 100644 --- a/hardware_interface/doc/mock_components_userdoc.rst +++ b/hardware_interface/doc/mock_components_userdoc.rst @@ -28,7 +28,7 @@ Parameters ,,,,,,,,,, disable_commands (optional; boolean; default: false) - Disables mirroring commands to states. + Disables mirroring commands to states. This option is helpful to simulate an erroneous connection to the hardware when nothing breaks, but suddenly there is no feedback from a hardware interface. Or it can help you to test your setup when the hardware is running without feedback, i.e., in open loop configuration. diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index 1afd78b582..4ea3ae9a5f 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -98,7 +98,7 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager * @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; + bool is_urdf_already_loaded() const; /// Claim a state interface given its key. /** @@ -424,7 +424,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 load_urdf_called_ = false; + bool is_urdf_loaded__ = false; }; } // namespace hardware_interface diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index b24e812b07..f32d24f890 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -703,7 +703,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; + is_urdf_loaded__ = true; const std::string system_type = "system"; const std::string sensor_type = "sensor"; const std::string actuator_type = "actuator"; @@ -742,7 +742,7 @@ void ResourceManager::load_urdf(const std::string & urdf, bool validate_interfac resource_storage_->systems_.size()); } -bool ResourceManager::load_urdf_called() const { return load_urdf_called_; } +bool ResourceManager::is_urdf_already_loaded() const { return is_urdf_loaded__; } // 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 c1c054713c..aedf2b862d 100644 --- a/hardware_interface/test/test_resource_manager.cpp +++ b/hardware_interface/test/test_resource_manager.cpp @@ -215,7 +215,7 @@ TEST_F(ResourceManagerTest, initialization_with_urdf_unclaimed) TEST_F(ResourceManagerTest, no_load_urdf_function_called) { TestableResourceManager rm; - ASSERT_FALSE(rm.load_urdf_called()); + ASSERT_FALSE(rm.is_urdf_already_loaded()); } TEST_F(ResourceManagerTest, load_urdf_called_if_urdf_is_invalid) @@ -223,21 +223,21 @@ 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()); + ASSERT_TRUE(rm.is_urdf_already_loaded()); } 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()); + ASSERT_TRUE(rm.is_urdf_already_loaded()); } TEST_F(ResourceManagerTest, can_load_urdf_later) { TestableResourceManager rm; - ASSERT_FALSE(rm.load_urdf_called()); + ASSERT_FALSE(rm.is_urdf_already_loaded()); rm.load_urdf(ros2_control_test_assets::minimal_robot_urdf); - ASSERT_TRUE(rm.load_urdf_called()); + ASSERT_TRUE(rm.is_urdf_already_loaded()); } TEST_F(ResourceManagerTest, resource_claiming)