From 6d23033575f65abeba31bfc56a346f1ba7d31b92 Mon Sep 17 00:00:00 2001 From: Manuel Muth Date: Wed, 7 Jun 2023 23:12:26 +0200 Subject: [PATCH] =?UTF-8?q?[CM]=20Use=20`robot=5Fdescription`=20topic=20in?= =?UTF-8?q?stead=20of=20parameter=20and=20don't=20crash=20on=20empty=20URD?= =?UTF-8?q?F=20=F0=9F=A6=BF=20(#940)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * on startup wait for robot description, however allow receiving later --------- Co-authored-by: Dr. Denis (cherry picked from commit d299208e8e2c6b927adbf27391f68aced974892c) # Conflicts: # controller_manager/CMakeLists.txt # controller_manager/src/controller_manager.cpp # hardware_interface/doc/mock_components_userdoc.rst # hardware_interface/include/hardware_interface/resource_manager.hpp # hardware_interface/src/resource_manager.cpp # hardware_interface/test/test_resource_manager.cpp --- controller_manager/CMakeLists.txt | 22 +++++ .../controller_manager/controller_manager.hpp | 8 ++ controller_manager/package.xml | 1 + controller_manager/src/controller_manager.cpp | 75 +++++++++++++++- .../test/controller_manager_test_common.hpp | 57 +++++++++--- .../test_controller_manager_urdf_passing.cpp | 88 +++++++++++++++++++ .../doc/mock_components_userdoc.rst | 9 ++ .../hardware_interface/resource_manager.hpp | 18 ++++ hardware_interface/src/resource_manager.cpp | 7 ++ .../test/test_resource_manager.cpp | 32 +++++++ 10 files changed, 304 insertions(+), 13 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 b0db9990b0..125a1a23c5 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -6,6 +6,7 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") endif() set(THIS_PACKAGE_INCLUDE_DEPENDS +<<<<<<< HEAD ament_index_cpp controller_interface controller_manager_msgs @@ -13,6 +14,17 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS pluginlib rclcpp realtime_tools +======= + ament_index_cpp + controller_interface + controller_manager_msgs + diagnostic_updater + hardware_interface + pluginlib + rclcpp + realtime_tools + std_msgs +>>>>>>> d299208 ([CM] Use `robot_description` topic instead of parameter and don't crash on empty URDF 🦿 (#940)) ) find_package(ament_cmake REQUIRED) @@ -133,6 +145,16 @@ if(BUILD_TESTING) controller_manager_msgs ros2_control_test_assets ) + 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) target_include_directories(test_controller_with_interfaces PRIVATE include) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 6482248e3e..24af4b2e83 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -50,6 +50,8 @@ #include "rclcpp/node_interfaces/node_logging_interface.hpp" #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 { @@ -81,6 +83,9 @@ class ControllerManager : public rclcpp::Node CONTROLLER_MANAGER_PUBLIC virtual ~ControllerManager() = default; + 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); @@ -282,6 +287,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" @@ -467,6 +473,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/package.xml b/controller_manager/package.xml index 02eec626be..f5944404b7 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -25,6 +25,7 @@ ros2_control_test_assets ros2param ros2run + std_msgs ament_cmake_gmock diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 9f9b836f55..fb0ab3ec5b 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -155,13 +155,20 @@ ControllerManager::ControllerManager( } std::string robot_description = ""; + // TODO(destogl): remove support at the end of 2023 get_parameter("robot_description", robot_description); if (robot_description.empty()) { - throw std::runtime_error("Unable to initialize resource manager, no robot description found."); + subscribe_to_robot_description_topic(); + } + else + { + RCLCPP_WARN( + get_logger(), + "[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); } - - init_resource_manager(robot_description); init_services(); } @@ -183,9 +190,58 @@ ControllerManager::ControllerManager( { RCLCPP_WARN(get_logger(), "'update_rate' parameter not set, using default value."); } +<<<<<<< HEAD +======= + + subscribe_to_robot_description_topic(); + + diagnostics_updater_.setHardwareID("ros2_control"); + diagnostics_updater_.add( + "Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback); +>>>>>>> d299208 ([CM] Use `robot_description` topic instead of parameter and don't crash on empty URDF 🦿 (#940)) 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( + 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."); + 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 + { + if (resource_manager_->is_urdf_already_loaded()) + { + RCLCPP_WARN( + get_logger(), + "ResourceManager has already loaded an urdf file. Ignoring attempt to reload a robot " + "description file."); + return; + } + 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. The 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... @@ -199,7 +255,20 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript State::PRIMARY_STATE_INACTIVE, hardware_interface::lifecycle_state_names::INACTIVE); for (const auto & component : configure_components_on_start) { +<<<<<<< HEAD resource_manager_->set_component_state(component, inactive_state); +======= + RCLCPP_WARN( + get_logger(), + "[Deprecated]: Usage of parameter \"configure_components_on_start\" is deprecated. Use " + "hardware_spawner instead."); + rclcpp_lifecycle::State inactive_state( + State::PRIMARY_STATE_INACTIVE, hardware_interface::lifecycle_state_names::INACTIVE); + for (const auto & component : configure_components_on_start) + { + resource_manager_->set_component_state(component, inactive_state); + } +>>>>>>> d299208 ([CM] Use `robot_description` topic instead of parameter and don't crash on empty URDF 🦿 (#940)) } std::vector activate_components_on_start = std::vector({}); diff --git a/controller_manager/test/controller_manager_test_common.hpp b/controller_manager/test/controller_manager_test_common.hpp index 11154e1022..78b3c3250e 100644 --- a/controller_manager/test/controller_manager_test_common.hpp +++ b/controller_manager/test/controller_manager_test_common.hpp @@ -22,15 +22,20 @@ #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" #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 +65,51 @@ template class ControllerManagerFixture : public ::testing::Test { public: + explicit ControllerManagerFixture( + const std::string & robot_description = ros2_control_test_assets::minimal_robot_urdf, + 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 + 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 + { + // 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); + // mimic topic call + auto msg = std_msgs::msg::String(); + msg.data = robot_description_; + cm_->robot_description_callback(msg); + } + } + } + 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() { @@ -121,6 +156,8 @@ class ControllerManagerFixture : public ::testing::Test std::thread updater_; bool run_updater_; + const std::string robot_description_; + const bool pass_urdf_as_parameter_; }; 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..102cbdfbd4 --- /dev/null +++ b/controller_manager/test/test_controller_manager_urdf_passing.cpp @@ -0,0 +1,88 @@ +// 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" + +#include "ros2_control_test_assets/descriptions.hpp" + +class TestControllerManagerWithTestableCM; + +class TestableControllerManager : public controller_manager::ControllerManager +{ + friend TestControllerManagerWithTestableCM; + + 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( + 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 +{ +public: + // create cm with no urdf + TestControllerManagerWithTestableCM() + : ControllerManagerFixture("", false) + { + } +}; + +TEST_P(TestControllerManagerWithTestableCM, initial_no_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_->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_->is_urdf_already_loaded()); +} + +TEST_P(TestControllerManagerWithTestableCM, load_urdf_called_after_invalid_urdf_passed) +{ + 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_->is_urdf_already_loaded()); +} + +INSTANTIATE_TEST_SUITE_P( + test_best_effort, TestControllerManagerWithTestableCM, testing::Values(best_effort)); diff --git a/hardware_interface/doc/mock_components_userdoc.rst b/hardware_interface/doc/mock_components_userdoc.rst index 5bcedb19ea..50e2be53f6 100644 --- a/hardware_interface/doc/mock_components_userdoc.rst +++ b/hardware_interface/doc/mock_components_userdoc.rst @@ -27,7 +27,16 @@ Features: Parameters ,,,,,,,,,, +<<<<<<< HEAD fake_sensor_commands (optional; boolean; default: false) +======= +disable_commands (optional; boolean; default: false) + 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. + +mock_sensor_commands (optional; boolean; default: false) +>>>>>>> d299208 ([CM] Use `robot_description` topic instead of parameter and don't crash on empty URDF 🦿 (#940)) Creates fake command interfaces for faking sensor measurements with an external command. Those interfaces are usually used by a :ref:`forward controller ` to provide access from ROS-world. diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index 5a919289b8..4bc106c6b0 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -77,6 +77,16 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager */ void load_urdf(const std::string & urdf, bool validate_interfaces = 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's load_urdf() has been already called. + * @return false if resource manager's load_urdf() has not been yet called. + */ + bool is_urdf_already_loaded() const; + /// Claim a state interface given its key. /** * The resource is claimed as long as being in scope. @@ -374,6 +384,14 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager mutable std::recursive_mutex resource_interfaces_lock_; mutable std::recursive_mutex claimed_command_interfaces_lock_; std::unique_ptr resource_storage_; +<<<<<<< HEAD +======= + + // Structure to store read and write status so it is not initialized in the real-time loop + HardwareReadWriteStatus read_write_status; + + bool is_urdf_loaded__ = false; +>>>>>>> d299208 ([CM] Use `robot_description` topic instead of parameter and don't crash on empty URDF 🦿 (#940)) }; } // namespace hardware_interface diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 98f2727613..a82450e017 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -667,6 +667,7 @@ ResourceManager::ResourceManager( void ResourceManager::load_urdf(const std::string & urdf, bool validate_interfaces) { + is_urdf_loaded__ = true; const std::string system_type = "system"; const std::string sensor_type = "sensor"; const std::string actuator_type = "actuator"; @@ -700,6 +701,12 @@ void ResourceManager::load_urdf(const std::string & urdf, bool validate_interfac } } +<<<<<<< HEAD +======= +bool ResourceManager::is_urdf_already_loaded() const { return is_urdf_loaded__; } + +// CM API: Called in "update"-thread +>>>>>>> d299208 ([CM] Use `robot_description` topic instead of parameter and don't crash on empty URDF 🦿 (#940)) LoanedStateInterface ResourceManager::claim_state_interface(const std::string & key) { if (!state_interface_is_available(key)) diff --git a/hardware_interface/test/test_resource_manager.cpp b/hardware_interface/test/test_resource_manager.cpp index eca8bbf40a..8bc4fc9874 100644 --- a/hardware_interface/test/test_resource_manager.cpp +++ b/hardware_interface/test/test_resource_manager.cpp @@ -190,7 +190,39 @@ TEST_F(TestResourceManager, initialization_with_urdf_unclaimed) } } +<<<<<<< HEAD TEST_F(TestResourceManager, resource_claiming) +======= +TEST_F(ResourceManagerTest, no_load_urdf_function_called) +{ + TestableResourceManager rm; + ASSERT_FALSE(rm.is_urdf_already_loaded()); +} + +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.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.is_urdf_already_loaded()); +} + +TEST_F(ResourceManagerTest, can_load_urdf_later) +{ + TestableResourceManager rm; + ASSERT_FALSE(rm.is_urdf_already_loaded()); + rm.load_urdf(ros2_control_test_assets::minimal_robot_urdf); + ASSERT_TRUE(rm.is_urdf_already_loaded()); +} + +TEST_F(ResourceManagerTest, resource_claiming) +>>>>>>> d299208 ([CM] Use `robot_description` topic instead of parameter and don't crash on empty URDF 🦿 (#940)) { hardware_interface::ResourceManager rm(ros2_control_test_assets::minimal_robot_urdf); // Activate components to get all interfaces available