diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index c445ddb73b..6f8858b49f 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -245,6 +245,15 @@ if(BUILD_TESTING) ${controller_manager_msgs_TARGETS} ) + ament_add_gmock(test_controller_manager_with_resource_manager + test/test_controller_manager_with_resource_manager.cpp + ) + target_link_libraries(test_controller_manager_with_resource_manager + controller_manager + hardware_interface::hardware_interface + ros2_control_test_assets::ros2_control_test_assets + ) + find_package(ament_cmake_pytest REQUIRED) install(FILES test/test_ros2_control_node.yaml DESTINATION test) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index ece49f44f2..44515fa8c1 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -242,6 +242,16 @@ class ControllerManager : public rclcpp::Node */ rclcpp::Clock::SharedPtr get_trigger_clock() const; + /** + * @brief Returns true if we have a valid robot description, currently based on whether the timer + * for waiting on description is still on. + */ + bool has_valid_robot_description() const + { + return robot_description_notification_timer_ && + !robot_description_notification_timer_->is_canceled(); + } + protected: void init_services(); diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 0e38af810d..e81e79ee9b 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -23,6 +23,7 @@ #include #include "controller_interface/controller_interface_base.hpp" +#include "controller_manager/controller_manager_parameters.hpp" #include "controller_manager_msgs/msg/hardware_component_state.hpp" #include "hardware_interface/helpers.hpp" #include "hardware_interface/introspection.hpp" @@ -32,8 +33,6 @@ #include "rclcpp/version.h" #include "rclcpp_lifecycle/state.hpp" -#include "controller_manager/controller_manager_parameters.hpp" - namespace // utility { static constexpr const char * kControllerInterfaceNamespace = "controller_interface"; @@ -541,34 +540,56 @@ bool ControllerManager::shutdown_controllers() void ControllerManager::init_controller_manager() { - controller_manager_activity_publisher_ = - create_publisher( - "~/activity", rclcpp::QoS(1).reliable().transient_local()); - rt_controllers_wrapper_.set_on_switch_callback( - std::bind(&ControllerManager::publish_activity, this)); - resource_manager_->set_on_component_state_switch_callback( - std::bind(&ControllerManager::publish_activity, this)); + if (!resource_manager_ && !robot_description_.empty()) + { + init_resource_manager(robot_description_); + } - // Get parameters needed for RT "update" loop to work - if (is_resource_manager_initialized()) + if ( + is_resource_manager_initialized() && !(resource_manager_->get_joint_limiters_imported()) && + params_->enforce_command_limits) { - if (params_->enforce_command_limits) + try { resource_manager_->import_joint_limiters(robot_description_); } - init_services(); + catch (const std::exception & e) + { + RCLCPP_ERROR(get_logger(), "Error importing joint limiters: %s", e.what()); + } + } + else if (!is_resource_manager_initialized()) + { + // The RM failed to initialize after receiving the robot description, or no description was + // received at all. This is a critical error. Don't finalize controller manager, instead keep + // waiting for robot description - fallback state + resource_manager_ = + std::make_unique(trigger_clock_, get_logger()); + if (!robot_description_notification_timer_) + { + robot_description_notification_timer_ = create_wall_timer( + std::chrono::seconds(1), + [&]() + { + RCLCPP_WARN( + get_logger(), "Waiting for data on 'robot_description' topic to finish initialization"); + }); + } } else { - robot_description_notification_timer_ = create_wall_timer( - std::chrono::seconds(1), - [&]() - { - RCLCPP_WARN( - get_logger(), "Waiting for data on 'robot_description' topic to finish initialization"); - }); + RCLCPP_INFO( + get_logger(), + "Resource Manager successfully initialized. Starting Controller Manager services..."); + init_services(); } + controller_manager_activity_publisher_ = + create_publisher( + "~/activity", rclcpp::QoS(1).reliable().transient_local()); + rt_controllers_wrapper_.set_on_switch_callback( + std::bind(&ControllerManager::publish_activity, this)); + // set QoS to transient local to get messages that have already been published // (if robot state publisher starts before controller manager) robot_description_subscription_ = create_subscription( @@ -589,15 +610,6 @@ void ControllerManager::init_controller_manager() diagnostics_updater_.add( "Controller Manager Activity", this, &ControllerManager::controller_manager_diagnostic_callback); - - INITIALIZE_ROS2_CONTROL_INTROSPECTION_REGISTRY( - this, hardware_interface::DEFAULT_INTROSPECTION_TOPIC, - hardware_interface::DEFAULT_REGISTRY_KEY); - START_ROS2_CONTROL_INTROSPECTION_PUBLISHER_THREAD(hardware_interface::DEFAULT_REGISTRY_KEY); - INITIALIZE_ROS2_CONTROL_INTROSPECTION_REGISTRY( - this, hardware_interface::CM_STATISTICS_TOPIC, hardware_interface::CM_STATISTICS_KEY); - START_ROS2_CONTROL_INTROSPECTION_PUBLISHER_THREAD(hardware_interface::CM_STATISTICS_KEY); - // Add on_shutdown callback to stop the controller manager rclcpp::Context::SharedPtr context = this->get_node_base_interface()->get_context(); preshutdown_cb_handle_ = @@ -675,8 +687,14 @@ void ControllerManager::robot_description_callback(const std_msgs::msg::String & "ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description."); return; } + init_resource_manager(robot_description_); - if (is_resource_manager_initialized()) + if (!is_resource_manager_initialized()) + { + resource_manager_ = + std::make_unique(trigger_clock_, get_logger()); + } + else { RCLCPP_INFO( get_logger(), @@ -688,10 +706,6 @@ void ControllerManager::robot_description_callback(const std_msgs::msg::String & void ControllerManager::init_resource_manager(const std::string & robot_description) { - if (params_->enforce_command_limits) - { - resource_manager_->import_joint_limiters(robot_description_); - } hardware_interface::ResourceManagerParams params; params.robot_description = robot_description; params.clock = trigger_clock_; @@ -699,14 +713,43 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript params.executor = executor_; params.node_namespace = this->get_namespace(); params.update_rate = static_cast(params_->update_rate); - if (!resource_manager_->load_and_initialize_components(params)) + resource_manager_ = std::make_unique(params, false); + + RCLCPP_INFO_EXPRESSION( + get_logger(), params_->enforce_command_limits, "Enforcing command limits is enabled..."); + if (params_->enforce_command_limits) { - RCLCPP_WARN( - get_logger(), - "Could not load and initialize hardware. Please check previous output for more details. " - "After you have corrected your URDF, try to publish robot description again."); + try + { + resource_manager_->import_joint_limiters(robot_description); + } + catch (const std::exception & e) + { + RCLCPP_ERROR(get_logger(), "Error importing joint limiters: %s", e.what()); + return; + } + } + + try + { + if (!resource_manager_->load_and_initialize_components(params)) + { + RCLCPP_WARN( + get_logger(), + "Could not load and initialize hardware. Please check previous output for more details. " + "After you have corrected your URDF, try to publish robot description again."); + return; + } + } + catch (const std::exception & e) + { + // Other possible errors when loading components + RCLCPP_ERROR( + get_logger(), "Exception caught while loading and initializing components: %s", e.what()); return; } + resource_manager_->set_on_component_state_switch_callback( + std::bind(&ControllerManager::publish_activity, this)); // Get all components and if they are not defined in parameters activate them automatically auto components_to_activate = resource_manager_->get_components_status(); diff --git a/controller_manager/test/test_controller_manager_with_resource_manager.cpp b/controller_manager/test/test_controller_manager_with_resource_manager.cpp new file mode 100644 index 0000000000..fd868dca20 --- /dev/null +++ b/controller_manager/test/test_controller_manager_with_resource_manager.cpp @@ -0,0 +1,139 @@ +// Copyright 2022 Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// 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 "test_controller_manager_with_resource_manager.hpp" + +std::shared_ptr ControllerManagerTest::node_ = nullptr; +std::unique_ptr ControllerManagerTest::test_resource_manager_ = + nullptr; +std::shared_ptr ControllerManagerTest::executor_ = + nullptr; + +using LifecycleCallbackReturn = + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + +void ControllerManagerTest::SetUp() +{ + if (!rclcpp::ok()) + { + rclcpp::init(0, nullptr); + } + + node_ = std::make_shared("controller_manager_test_node"); + auto clock = node_->get_clock(); + auto logger = node_->get_logger(); + + test_resource_manager_ = std::make_unique(clock, logger); + executor_ = std::make_shared(); +} + +void ControllerManagerTest::TearDown() +{ + node_.reset(); + test_resource_manager_.reset(); + executor_.reset(); + if (rclcpp::ok()) + { + rclcpp::shutdown(); + } +} + +TEST_F(ControllerManagerTest, robot_description_callback_handles_urdf_without_hardware_plugin) +{ + TestControllerManager cm(std::move(test_resource_manager_), executor_); + + std_msgs::msg::String invalid_urdf_msg; + invalid_urdf_msg.data = ros2_control_test_assets::invalid_urdf_without_hardware_plugin; + + cm.robot_description_callback(invalid_urdf_msg); + + EXPECT_FALSE(cm.is_resource_manager_initialized()); + + EXPECT_TRUE(cm.has_valid_robot_description()); +} + +TEST_F(ControllerManagerTest, robot_description_callback_handles_invalid_urdf) +{ + TestControllerManager cm(std::move(test_resource_manager_), executor_); + + std_msgs::msg::String invalid_urdf_msg; + invalid_urdf_msg.data = R"()"; + + cm.robot_description_callback(invalid_urdf_msg); + + EXPECT_FALSE(cm.is_resource_manager_initialized()); + + EXPECT_TRUE(cm.has_valid_robot_description()); +} + +TEST_F(ControllerManagerTest, robot_description_callback_handles_empty_urdf) +{ + TestControllerManager cm(std::move(test_resource_manager_), executor_); + + std_msgs::msg::String invalid_urdf_msg; + invalid_urdf_msg.data = ""; + + cm.robot_description_callback(invalid_urdf_msg); + + EXPECT_FALSE(cm.is_resource_manager_initialized()); + + EXPECT_TRUE(cm.has_valid_robot_description()); +} + +TEST_F(ControllerManagerTest, robot_description_callback_handles_wrong_plugins) +{ + TestControllerManager cm(std::move(test_resource_manager_), executor_); + + std_msgs::msg::String invalid_urdf_msg; + invalid_urdf_msg.data = ros2_control_test_assets::invalid_urdf_with_wrong_plugin; + + cm.robot_description_callback(invalid_urdf_msg); + + EXPECT_FALSE(cm.is_resource_manager_initialized()); + + EXPECT_TRUE(cm.has_valid_robot_description()); +} + +TEST_F(ControllerManagerTest, robot_description_callback_handles_no_geometry) +{ + TestControllerManager cm(std::move(test_resource_manager_), executor_); + + std_msgs::msg::String invalid_urdf_msg; + invalid_urdf_msg.data = ros2_control_test_assets::invalid_urdf_no_geometry; + + cm.robot_description_callback(invalid_urdf_msg); + + EXPECT_FALSE(cm.is_resource_manager_initialized()); + + EXPECT_TRUE(cm.has_valid_robot_description()); +} + +TEST_F(ControllerManagerTest, init_controller_manager_with_invalid_urdf) +{ + const std::string invalid_urdf = ros2_control_test_assets::invalid_urdf_with_wrong_plugin; + + TestControllerManager cm( + executor_, invalid_urdf, false, "test_controller_manager", "", rclcpp::NodeOptions{}); + + EXPECT_FALSE(cm.is_resource_manager_initialized()); + + EXPECT_TRUE(cm.has_valid_robot_description()); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + int result = RUN_ALL_TESTS(); + return result; +} diff --git a/controller_manager/test/test_controller_manager_with_resource_manager.hpp b/controller_manager/test/test_controller_manager_with_resource_manager.hpp new file mode 100644 index 0000000000..6b2015813b --- /dev/null +++ b/controller_manager/test/test_controller_manager_with_resource_manager.hpp @@ -0,0 +1,56 @@ +// Copyright 2022 Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TEST_CONTROLLER_MANAGER_WITH_RESOURCE_MANAGER_HPP_ +#define TEST_CONTROLLER_MANAGER_WITH_RESOURCE_MANAGER_HPP_ + +#include +#include + +#include +#include "controller_manager/controller_manager.hpp" +#include "gtest/gtest.h" +#include "hardware_interface/resource_manager.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp/node.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "ros2_control_test_assets/descriptions.hpp" +#include "std_msgs/msg/string.hpp" + +class TestControllerManager : public controller_manager::ControllerManager +{ +public: + using ControllerManager::ControllerManager; + + // Expose callbacks + using ControllerManager::robot_description_callback; + + using ControllerManager::is_resource_manager_initialized; + + using ControllerManager::resource_manager_; + + using ControllerManager::has_valid_robot_description; +}; + +class ControllerManagerTest : public ::testing::Test +{ +protected: + static std::shared_ptr node_; + static std::shared_ptr executor_; + static std::unique_ptr test_resource_manager_; + virtual void SetUp(); + virtual void TearDown(); +}; + +#endif // TEST_CONTROLLER_MANAGER_WITH_RESOURCE_MANAGER_HPP_ diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index f56aad93d2..5de9bfe44f 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -558,6 +558,8 @@ class ResourceManager const std::string & get_robot_description() const; + bool get_joint_limiters_imported() const; + protected: /// Gets the logger for the resource manager /** @@ -599,6 +601,7 @@ class ResourceManager // Structure to store read and write status so it is not initialized in the real-time loop HardwareReadWriteStatus read_write_status; + mutable bool joint_limiters_imported_ = false; }; } // namespace hardware_interface diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index a7acd80e2d..fd6bf10b1d 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -758,7 +758,7 @@ class ResourceStorage } } - void import_joint_limiters(const std::vector & hardware_infos) + bool import_joint_limiters(const std::vector & hardware_infos) { for (const auto & hw_info : hardware_infos) { @@ -807,6 +807,7 @@ class ResourceStorage joint_limiters_interface_[hw_info.name].insert({joint_name, std::move(limits_interface)}); } } + return true; } template @@ -1449,9 +1450,17 @@ ResourceManager::ResourceManager( params.allow_controller_activation_with_inactive_hardware; return_failed_hardware_names_on_return_deactivate_write_cycle_ = params.return_failed_hardware_names_on_return_deactivate_write_cycle_; - if (load) + + try { - load_and_initialize_components(params); + if (load && !load_and_initialize_components(params)) + { + RCLCPP_WARN( + get_logger(), + "Could not load and initialize hardware. Please check previous output for more details. " + "After you have corrected your URDF, try to publish robot description again."); + throw std::runtime_error("Failed to load and initialize components"); + } if (params.activate_all) { for (auto const & hw_info : resource_storage_->hardware_info_map_) @@ -1462,6 +1471,12 @@ ResourceManager::ResourceManager( } } } + catch (const std::exception & e) + { + // Other possible errors when loading components + RCLCPP_ERROR( + get_logger(), "Exception caught while loading and initializing components: %s", e.what()); + } } bool ResourceManager::shutdown_components() @@ -1484,8 +1499,6 @@ bool ResourceManager::shutdown_components() bool ResourceManager::load_and_initialize_components( const hardware_interface::ResourceManagerParams & params) { - components_are_loaded_and_initialized_ = true; - resource_storage_->robot_description_ = params.robot_description; resource_storage_->cm_update_rate_ = params.update_rate; params_.robot_description = params.robot_description; @@ -1504,6 +1517,7 @@ bool ResourceManager::load_and_initialize_components( const std::string sensor_type = "sensor"; const std::string actuator_type = "actuator"; + components_are_loaded_and_initialized_ = true; std::lock_guard resource_guard(resources_lock_); std::lock_guard limiters_guard(joint_limiters_lock_); for (const auto & individual_hardware_info : hardware_info) @@ -1579,9 +1593,15 @@ void ResourceManager::import_joint_limiters(const std::string & urdf) { std::lock_guard guard(joint_limiters_lock_); const auto hardware_info = hardware_interface::parse_control_resources_from_urdf(urdf); - resource_storage_->import_joint_limiters(hardware_info); + bool success = resource_storage_->import_joint_limiters(hardware_info); + if (success) + { + joint_limiters_imported_ = true; + } } +bool ResourceManager::get_joint_limiters_imported() const { return joint_limiters_imported_; } + bool ResourceManager::are_components_initialized() const { return components_are_loaded_and_initialized_; diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp index 136ba5d6d0..6a03a5e8ad 100644 --- a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp +++ b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp @@ -2171,6 +2171,54 @@ const auto diff_drive_robot_sdf = )"; +const auto invalid_urdf_without_hardware_plugin = + R"( + + + + + + + + + + + + + +)"; + +const auto invalid_urdf_with_wrong_plugin = + R"( + + + + + + + + + + + + + mock_components/NonExistentSystem + + + +)"; + +const auto invalid_urdf_no_geometry = + R"( + + + + mock_components/NonExistentSystem + + + +)"; + const auto minimal_robot_urdf = std::string(urdf_head) + std::string(hardware_resources) + std::string(urdf_tail); const auto minimal_robot_urdf_no_limits = std::string(urdf_head_continuous_missing_limits) +