From 6d23033575f65abeba31bfc56a346f1ba7d31b92 Mon Sep 17 00:00:00 2001 From: Manuel Muth Date: Wed, 7 Jun 2023 23:12:26 +0200 Subject: [PATCH 1/7] =?UTF-8?q?[CM]=20Use=20`robot=5Fdescription`=20topic?= =?UTF-8?q?=20instead=20of=20parameter=20and=20don't=20crash=20on=20empty?= =?UTF-8?q?=20URDF=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 From db0b5459e2eedb2f730f8397299c113653bae02d Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Wed, 14 Jun 2023 17:43:06 +0200 Subject: [PATCH 2/7] Apply suggestions from code review --- controller_manager/CMakeLists.txt | 13 +--------- controller_manager/src/controller_manager.cpp | 26 +++---------------- .../doc/mock_components_userdoc.rst | 4 --- .../hardware_interface/resource_manager.hpp | 6 ----- hardware_interface/src/resource_manager.cpp | 3 --- .../test/test_resource_manager.cpp | 12 +++------ 6 files changed, 9 insertions(+), 55 deletions(-) diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index 125a1a23c5..4aa8b426a5 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -6,7 +6,6 @@ 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 @@ -14,17 +13,7 @@ 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)) + std_msgs ) find_package(ament_cmake REQUIRED) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index fb0ab3ec5b..f79d7d2cb6 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -155,7 +155,6 @@ 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()) { @@ -190,15 +189,11 @@ 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)) + if (!resource_manager->is_urdf_already_loaded()) + { + subscribe_to_robot_description_topic(); + } init_services(); } @@ -255,20 +250,7 @@ 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/hardware_interface/doc/mock_components_userdoc.rst b/hardware_interface/doc/mock_components_userdoc.rst index 50e2be53f6..fb527d39d2 100644 --- a/hardware_interface/doc/mock_components_userdoc.rst +++ b/hardware_interface/doc/mock_components_userdoc.rst @@ -27,16 +27,12 @@ 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 4bc106c6b0..bff6f7f23e 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -384,14 +384,8 @@ 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 a82450e017..da3176e953 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -701,12 +701,9 @@ 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 8bc4fc9874..9b6eafec9f 100644 --- a/hardware_interface/test/test_resource_manager.cpp +++ b/hardware_interface/test/test_resource_manager.cpp @@ -190,16 +190,13 @@ TEST_F(TestResourceManager, initialization_with_urdf_unclaimed) } } -<<<<<<< HEAD -TEST_F(TestResourceManager, resource_claiming) -======= -TEST_F(ResourceManagerTest, no_load_urdf_function_called) +TEST_F(TestResourceManager, no_load_urdf_function_called) { TestableResourceManager rm; ASSERT_FALSE(rm.is_urdf_already_loaded()); } -TEST_F(ResourceManagerTest, load_urdf_called_if_urdf_is_invalid) +TEST_F(TestResourceManager, load_urdf_called_if_urdf_is_invalid) { TestableResourceManager rm; EXPECT_THROW( @@ -207,7 +204,7 @@ TEST_F(ResourceManagerTest, load_urdf_called_if_urdf_is_invalid) ASSERT_TRUE(rm.is_urdf_already_loaded()); } -TEST_F(ResourceManagerTest, load_urdf_called_if_urdf_is_valid) +TEST_F(TestResourceManager, load_urdf_called_if_urdf_is_valid) { TestableResourceManager rm(ros2_control_test_assets::minimal_robot_urdf); ASSERT_TRUE(rm.is_urdf_already_loaded()); @@ -221,8 +218,7 @@ TEST_F(ResourceManagerTest, can_load_urdf_later) 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)) +TEST_F(TestResourceManager, resource_claiming) { hardware_interface::ResourceManager rm(ros2_control_test_assets::minimal_robot_urdf); // Activate components to get all interfaces available From 2309703d4c005f730cf2dac931f69a3355fb79fb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Wed, 14 Jun 2023 18:04:24 +0200 Subject: [PATCH 3/7] Fix to compile. --- controller_manager/CMakeLists.txt | 3 +- .../test/test_resource_manager.cpp | 63 +++++++++++++------ 2 files changed, 46 insertions(+), 20 deletions(-) diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index 4aa8b426a5..a3adf25ff7 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -137,12 +137,13 @@ if(BUILD_TESTING) ament_add_gmock(test_controller_manager_urdf_passing test/test_controller_manager_urdf_passing.cpp ) + target_include_directories(test_controller_manager_urdf_passing PRIVATE include) 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 + ros2_control_test_assets ) add_library(test_controller_with_interfaces SHARED test/test_controller_with_interfaces/test_controller_with_interfaces.cpp) diff --git a/hardware_interface/test/test_resource_manager.cpp b/hardware_interface/test/test_resource_manager.cpp index 9b6eafec9f..34f6bfaa01 100644 --- a/hardware_interface/test/test_resource_manager.cpp +++ b/hardware_interface/test/test_resource_manager.cpp @@ -45,7 +45,7 @@ using ros2_control_test_assets::TEST_SYSTEM_HARDWARE_NAME; using ros2_control_test_assets::TEST_SYSTEM_HARDWARE_STATE_INTERFACES; using ros2_control_test_assets::TEST_SYSTEM_HARDWARE_TYPE; -class TestResourceManager : public ::testing::Test +class ResourceManagerTest : public ::testing::Test { public: static void SetUpTestCase() {} @@ -53,6 +53,31 @@ class TestResourceManager : public ::testing::Test void SetUp() {} }; +// Forward declaration +namespace hardware_interface +{ + class ResourceStorage; +} + +class TestableResourceManager : public hardware_interface::ResourceManager +{ +public: + friend ResourceManagerTest; + + FRIEND_TEST(ResourceManagerTest, initialization_with_urdf_manual_validation); + FRIEND_TEST(ResourceManagerTest, post_initialization_add_components); + FRIEND_TEST(ResourceManagerTest, managing_controllers_reference_interfaces); + FRIEND_TEST(ResourceManagerTest, resource_availability_and_claiming_in_lifecycle); + + TestableResourceManager() : hardware_interface::ResourceManager() {} + + TestableResourceManager( + const std::string & urdf, bool validate_interfaces = true, bool activate_all = false) + : hardware_interface::ResourceManager(urdf, validate_interfaces, activate_all) + { + } +}; + std::vector set_components_state( hardware_interface::ResourceManager & rm, const std::vector & components, const uint8_t state_id, const std::string & state_name) @@ -112,24 +137,24 @@ auto shutdown_components = hardware_interface::lifecycle_state_names::FINALIZED); }; -TEST_F(TestResourceManager, initialization_empty) +TEST_F(ResourceManagerTest, initialization_empty) { ASSERT_ANY_THROW(hardware_interface::ResourceManager rm("")); } -TEST_F(TestResourceManager, initialization_with_urdf) +TEST_F(ResourceManagerTest, initialization_with_urdf) { ASSERT_NO_THROW( hardware_interface::ResourceManager rm(ros2_control_test_assets::minimal_robot_urdf)); } -TEST_F(TestResourceManager, post_initialization_with_urdf) +TEST_F(ResourceManagerTest, post_initialization_with_urdf) { hardware_interface::ResourceManager rm; ASSERT_NO_THROW(rm.load_urdf(ros2_control_test_assets::minimal_robot_urdf)); } -TEST_F(TestResourceManager, initialization_with_urdf_manual_validation) +TEST_F(ResourceManagerTest, initialization_with_urdf_manual_validation) { // we validate the results manually hardware_interface::ResourceManager rm(ros2_control_test_assets::minimal_robot_urdf, false); @@ -153,7 +178,7 @@ TEST_F(TestResourceManager, initialization_with_urdf_manual_validation) EXPECT_TRUE(rm.command_interface_exists("joint3/velocity")); } -TEST_F(TestResourceManager, initialization_with_wrong_urdf) +TEST_F(ResourceManagerTest, initialization_with_wrong_urdf) { // missing state keys { @@ -171,7 +196,7 @@ TEST_F(TestResourceManager, initialization_with_wrong_urdf) } } -TEST_F(TestResourceManager, initialization_with_urdf_unclaimed) +TEST_F(ResourceManagerTest, initialization_with_urdf_unclaimed) { // we validate the results manually hardware_interface::ResourceManager rm(ros2_control_test_assets::minimal_robot_urdf); @@ -190,13 +215,13 @@ TEST_F(TestResourceManager, initialization_with_urdf_unclaimed) } } -TEST_F(TestResourceManager, no_load_urdf_function_called) +TEST_F(ResourceManagerTest, no_load_urdf_function_called) { TestableResourceManager rm; ASSERT_FALSE(rm.is_urdf_already_loaded()); } -TEST_F(TestResourceManager, load_urdf_called_if_urdf_is_invalid) +TEST_F(ResourceManagerTest, load_urdf_called_if_urdf_is_invalid) { TestableResourceManager rm; EXPECT_THROW( @@ -204,7 +229,7 @@ TEST_F(TestResourceManager, load_urdf_called_if_urdf_is_invalid) ASSERT_TRUE(rm.is_urdf_already_loaded()); } -TEST_F(TestResourceManager, load_urdf_called_if_urdf_is_valid) +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()); @@ -218,7 +243,7 @@ TEST_F(ResourceManagerTest, can_load_urdf_later) ASSERT_TRUE(rm.is_urdf_already_loaded()); } -TEST_F(TestResourceManager, resource_claiming) +TEST_F(ResourceManagerTest, resource_claiming) { hardware_interface::ResourceManager rm(ros2_control_test_assets::minimal_robot_urdf); // Activate components to get all interfaces available @@ -329,7 +354,7 @@ class ExternalComponent : public hardware_interface::ActuatorInterface } }; -TEST_F(TestResourceManager, post_initialization_add_components) +TEST_F(ResourceManagerTest, post_initialization_add_components) { // we validate the results manually hardware_interface::ResourceManager rm(ros2_control_test_assets::minimal_robot_urdf, false); @@ -373,7 +398,7 @@ TEST_F(TestResourceManager, post_initialization_add_components) EXPECT_NO_THROW(rm.claim_command_interface("external_joint/external_command_interface")); } -TEST_F(TestResourceManager, default_prepare_perform_switch) +TEST_F(ResourceManagerTest, default_prepare_perform_switch) { hardware_interface::ResourceManager rm(ros2_control_test_assets::minimal_robot_urdf); // Activate components to get all interfaces available @@ -407,7 +432,7 @@ const auto command_mode_urdf = std::string(ros2_control_test_assets::urdf_head) std::string(hardware_resources_command_modes) + std::string(ros2_control_test_assets::urdf_tail); -TEST_F(TestResourceManager, custom_prepare_perform_switch) +TEST_F(ResourceManagerTest, custom_prepare_perform_switch) { hardware_interface::ResourceManager rm(command_mode_urdf); // Scenarios defined by example criteria @@ -443,7 +468,7 @@ TEST_F(TestResourceManager, custom_prepare_perform_switch) EXPECT_FALSE(rm.perform_command_mode_switch(empty_keys, legal_keys_position)); } -TEST_F(TestResourceManager, resource_status) +TEST_F(ResourceManagerTest, resource_status) { hardware_interface::ResourceManager rm(ros2_control_test_assets::minimal_robot_urdf); @@ -515,7 +540,7 @@ TEST_F(TestResourceManager, resource_status) status_map[TEST_SYSTEM_HARDWARE_NAME].state_interfaces, TEST_SYSTEM_HARDWARE_STATE_INTERFACES); } -TEST_F(TestResourceManager, lifecycle_all_resources) +TEST_F(ResourceManagerTest, lifecycle_all_resources) { hardware_interface::ResourceManager rm(ros2_control_test_assets::minimal_robot_urdf); @@ -658,7 +683,7 @@ TEST_F(TestResourceManager, lifecycle_all_resources) } } -TEST_F(TestResourceManager, lifecycle_individual_resources) +TEST_F(ResourceManagerTest, lifecycle_individual_resources) { hardware_interface::ResourceManager rm(ros2_control_test_assets::minimal_robot_urdf); @@ -870,7 +895,7 @@ TEST_F(TestResourceManager, lifecycle_individual_resources) } } -TEST_F(TestResourceManager, resource_availability_and_claiming_in_lifecycle) +TEST_F(ResourceManagerTest, resource_availability_and_claiming_in_lifecycle) { using std::placeholders::_1; hardware_interface::ResourceManager rm(ros2_control_test_assets::minimal_robot_urdf); @@ -1244,7 +1269,7 @@ TEST_F(TestResourceManager, resource_availability_and_claiming_in_lifecycle) } } -TEST_F(TestResourceManager, managing_controllers_reference_interfaces) +TEST_F(ResourceManagerTest, managing_controllers_reference_interfaces) { hardware_interface::ResourceManager rm(ros2_control_test_assets::minimal_robot_urdf); From d89e9f73da97121b81b9f9806b1a796fa4ffa1d5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Wed, 14 Jun 2023 21:35:49 +0200 Subject: [PATCH 4/7] Adding output to test so they would not fail. --- hardware_interface/test/test_components/test_system.cpp | 3 +++ hardware_interface/test/test_resource_manager.cpp | 6 +++++- 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/hardware_interface/test/test_components/test_system.cpp b/hardware_interface/test/test_components/test_system.cpp index 73659ca506..f198e057da 100644 --- a/hardware_interface/test/test_components/test_system.cpp +++ b/hardware_interface/test/test_components/test_system.cpp @@ -19,6 +19,8 @@ #include "hardware_interface/system_interface.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rcutils/logging_macros.h" + using hardware_interface::CommandInterface; using hardware_interface::return_type; using hardware_interface::StateInterface; @@ -54,6 +56,7 @@ class TestSystem : public SystemInterface std::vector export_command_interfaces() override { + RCUTILS_LOG_INFO_NAMED("test_system", "Exporting configuration interfaces."); std::vector command_interfaces; for (auto i = 0u; i < info_.joints.size(); ++i) { diff --git a/hardware_interface/test/test_resource_manager.cpp b/hardware_interface/test/test_resource_manager.cpp index 34f6bfaa01..6d184c5abf 100644 --- a/hardware_interface/test/test_resource_manager.cpp +++ b/hardware_interface/test/test_resource_manager.cpp @@ -178,7 +178,7 @@ TEST_F(ResourceManagerTest, initialization_with_urdf_manual_validation) EXPECT_TRUE(rm.command_interface_exists("joint3/velocity")); } -TEST_F(ResourceManagerTest, initialization_with_wrong_urdf) +TEST_F(ResourceManagerTest, when_missing_state_keys_expect_hw_initialization_fails) { // missing state keys { @@ -187,6 +187,10 @@ TEST_F(ResourceManagerTest, initialization_with_wrong_urdf) ros2_control_test_assets::minimal_robot_missing_state_keys_urdf), std::exception); } +} + +TEST_F(ResourceManagerTest, when_missing_command_keys_expect_hw_initialization_fails) +{ // missing command keys { EXPECT_THROW( From 39d1fd9d15a06ab2693ec33e9974b018f586c0b0 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Mon, 14 Aug 2023 21:56:46 +0100 Subject: [PATCH 5/7] reformat --- hardware_interface/test/test_handle.cpp | 8 ++------ hardware_interface/test/test_resource_manager.cpp | 2 +- 2 files changed, 3 insertions(+), 7 deletions(-) diff --git a/hardware_interface/test/test_handle.cpp b/hardware_interface/test/test_handle.cpp index 16ca710e9d..da8258c643 100644 --- a/hardware_interface/test/test_handle.cpp +++ b/hardware_interface/test/test_handle.cpp @@ -27,9 +27,7 @@ constexpr auto FOO_INTERFACE = "FooInterface"; TEST(TestHandle, command_interface) { double value = 1.337; - CommandInterface interface { - JOINT_NAME, FOO_INTERFACE, &value - }; + CommandInterface interface{JOINT_NAME, FOO_INTERFACE, &value}; EXPECT_DOUBLE_EQ(interface.get_value(), value); EXPECT_NO_THROW(interface.set_value(0.0)); EXPECT_DOUBLE_EQ(interface.get_value(), 0.0); @@ -38,9 +36,7 @@ TEST(TestHandle, command_interface) TEST(TestHandle, state_interface) { double value = 1.337; - StateInterface interface { - JOINT_NAME, FOO_INTERFACE, &value - }; + StateInterface interface{JOINT_NAME, FOO_INTERFACE, &value}; EXPECT_DOUBLE_EQ(interface.get_value(), value); // interface.set_value(5); compiler error, no set_value function } diff --git a/hardware_interface/test/test_resource_manager.cpp b/hardware_interface/test/test_resource_manager.cpp index 6d184c5abf..12cc597dc7 100644 --- a/hardware_interface/test/test_resource_manager.cpp +++ b/hardware_interface/test/test_resource_manager.cpp @@ -56,7 +56,7 @@ class ResourceManagerTest : public ::testing::Test // Forward declaration namespace hardware_interface { - class ResourceStorage; +class ResourceStorage; } class TestableResourceManager : public hardware_interface::ResourceManager From 56b346ce5623028d99300f3a7e3dce011d19ebb1 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Thu, 17 Aug 2023 17:45:42 +0100 Subject: [PATCH 6/7] Fix segmentation fault MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Noel Jiménez García --- controller_manager/src/controller_manager.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index f79d7d2cb6..69667cd410 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -190,7 +190,7 @@ ControllerManager::ControllerManager( RCLCPP_WARN(get_logger(), "'update_rate' parameter not set, using default value."); } - if (!resource_manager->is_urdf_already_loaded()) + if (!resource_manager_->is_urdf_already_loaded()) { subscribe_to_robot_description_topic(); } From 50a4a766f1cd4a13a4f2d734658bc999154310f6 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Fri, 18 Aug 2023 12:01:07 +0100 Subject: [PATCH 7/7] fix format Co-authored-by: Sai Kishor Kothakota --- hardware_interface/test/test_handle.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/hardware_interface/test/test_handle.cpp b/hardware_interface/test/test_handle.cpp index da8258c643..16ca710e9d 100644 --- a/hardware_interface/test/test_handle.cpp +++ b/hardware_interface/test/test_handle.cpp @@ -27,7 +27,9 @@ constexpr auto FOO_INTERFACE = "FooInterface"; TEST(TestHandle, command_interface) { double value = 1.337; - CommandInterface interface{JOINT_NAME, FOO_INTERFACE, &value}; + CommandInterface interface { + JOINT_NAME, FOO_INTERFACE, &value + }; EXPECT_DOUBLE_EQ(interface.get_value(), value); EXPECT_NO_THROW(interface.set_value(0.0)); EXPECT_DOUBLE_EQ(interface.get_value(), 0.0); @@ -36,7 +38,9 @@ TEST(TestHandle, command_interface) TEST(TestHandle, state_interface) { double value = 1.337; - StateInterface interface{JOINT_NAME, FOO_INTERFACE, &value}; + StateInterface interface { + JOINT_NAME, FOO_INTERFACE, &value + }; EXPECT_DOUBLE_EQ(interface.get_value(), value); // interface.set_value(5); compiler error, no set_value function }