diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index b0db9990b0..a3adf25ff7 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -13,6 +13,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS pluginlib rclcpp realtime_tools + std_msgs ) find_package(ament_cmake REQUIRED) @@ -133,6 +134,17 @@ 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_include_directories(test_controller_manager_urdf_passing PRIVATE include) + target_link_libraries(test_controller_manager_urdf_passing + controller_manager + ) + 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) 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 32e8e13df8..a1ae11b4cb 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" @@ -490,6 +496,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 a1452bd25b..d7ac4d629c 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 acf24095b7..1eb545d0d7 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -270,10 +270,16 @@ ControllerManager::ControllerManager( 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(); } @@ -295,9 +301,54 @@ ControllerManager::ControllerManager( { RCLCPP_WARN(get_logger(), "'update_rate' parameter not set, using default value."); } + + if (!resource_manager_->is_urdf_already_loaded()) + { + subscribe_to_robot_description_topic(); + } 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... 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..fb527d39d2 100644 --- a/hardware_interface/doc/mock_components_userdoc.rst +++ b/hardware_interface/doc/mock_components_userdoc.rst @@ -27,7 +27,12 @@ Features: Parameters ,,,,,,,,,, -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) 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..bff6f7f23e 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,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_; + + 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 98f2727613..da3176e953 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,9 @@ void ResourceManager::load_urdf(const std::string & urdf, bool validate_interfac } } +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) { if (!state_interface_is_available(key)) 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 eca8bbf40a..12cc597dc7 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, when_missing_state_keys_expect_hw_initialization_fails) { // missing state keys { @@ -162,6 +187,10 @@ TEST_F(TestResourceManager, 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( @@ -171,7 +200,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,7 +219,35 @@ TEST_F(TestResourceManager, initialization_with_urdf_unclaimed) } } -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) { hardware_interface::ResourceManager rm(ros2_control_test_assets::minimal_robot_urdf); // Activate components to get all interfaces available @@ -301,7 +358,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); @@ -345,7 +402,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 @@ -379,7 +436,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 @@ -415,7 +472,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); @@ -487,7 +544,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); @@ -630,7 +687,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); @@ -842,7 +899,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); @@ -1216,7 +1273,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);