diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index 1bd47cd923..739fdc020d 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -63,6 +63,7 @@ if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) find_package(ros2_control_test_assets REQUIRED) find_package(example_interfaces REQUIRED) + find_package(ament_cmake_gtest REQUIRED) # Plugin Libraries that are built and installed for use in testing add_library(test_controller SHARED test/test_controller/test_controller.cpp) @@ -243,6 +244,15 @@ if(BUILD_TESTING) ${controller_manager_msgs_TARGETS} ) + ament_add_gtest(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..259bee233b 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -242,6 +242,17 @@ class ControllerManager : public rclcpp::Node */ rclcpp::Clock::SharedPtr get_trigger_clock() const; + /** + * @brief Return the robot description timer. + * + * It can be used to determine whether the Controller Manager + * is currently waiting for a robot description to be published. + * + * @return rclcpp::TimerBase::SharedPtr if the timer exists, nullptr otherwise + */ + rclcpp::TimerBase::SharedPtr get_robot_description_timer() const { + return robot_description_notification_timer_; + } protected: void init_services(); diff --git a/controller_manager/package.xml b/controller_manager/package.xml index b63d3c9582..b8c3feb448 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -36,6 +36,7 @@ python3-filelock ament_cmake_gmock + ament_cmake_gtest ament_cmake_pytest hardware_interface_testing launch_testing_ros diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index a7d3af2478..873b8a95cd 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -541,34 +541,38 @@ 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()) { - if (params_->enforce_command_limits) + // fallback state + resource_manager_ = std::make_unique(trigger_clock_, get_logger()); + if (!robot_description_notification_timer_) { - resource_manager_->import_joint_limiters(robot_description_); + 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"); + }); } - init_services(); - } - else + }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 has been 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( @@ -674,9 +678,15 @@ void ControllerManager::robot_description_callback(const std_msgs::msg::String & get_logger(), "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()) + { + // The RM failed to init AFTER we received the description - 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()); + } else { RCLCPP_INFO( get_logger(), @@ -688,10 +698,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,15 +705,44 @@ 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( + 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(), - "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."); + "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(); @@ -855,6 +890,7 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript } } + void ControllerManager::init_services() { // TODO(anyone): Due to issues with the MutliThreadedExecutor, this control loop does not rely on 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..7310182824 --- /dev/null +++ b/controller_manager/test/test_controller_manager_with_resource_manager.cpp @@ -0,0 +1,122 @@ +#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_FALSE(cm.get_robot_description_timer()->is_canceled()); + +} + +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_FALSE(cm.get_robot_description_timer()->is_canceled()); + +} + +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_FALSE(cm.get_robot_description_timer()->is_canceled()); +} + +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_FALSE(cm.get_robot_description_timer()->is_canceled()); +} + +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_FALSE(cm.get_robot_description_timer()->is_canceled()); +} + +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_FALSE(cm.get_robot_description_timer()->is_canceled()); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + int result = RUN_ALL_TESTS(); + return result; +} \ No newline at end of file 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..78fdf7daa3 --- /dev/null +++ b/controller_manager/test/test_controller_manager_with_resource_manager.hpp @@ -0,0 +1,41 @@ +#ifndef CONTROLLER_MANAGER_TEST_HPP_ +#define CONTROLLER_MANAGER_TEST_HPP_ + +#include +#include + +#include "gtest/gtest.h" +#include "rclcpp/node.hpp" +#include "std_msgs/msg/string.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "controller_manager/controller_manager.hpp" +#include "hardware_interface/resource_manager.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "ros2_control_test_assets/descriptions.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::get_robot_description_timer; +}; + +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 \ No newline at end of file diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 0d300eed05..39290551a2 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -1420,9 +1420,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."); + return; + } if (params.activate_all) { for (auto const & hw_info : resource_storage_->hardware_info_map_) @@ -1433,6 +1441,14 @@ 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()); + return; + } } bool ResourceManager::shutdown_components() @@ -1455,8 +1471,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; @@ -1473,6 +1487,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) 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..b81a0ca985 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) +