From 7bb83b67705e04d17f545708d111be23b2f17c6d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Fri, 26 Jan 2024 18:32:28 +0100 Subject: [PATCH] Add small behavior fixes. --- controller_manager/src/controller_manager.cpp | 16 ++++++++-------- .../test/test_resource_manager.cpp | 6 ++---- 2 files changed, 10 insertions(+), 12 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 857cbd3479..6c16a552f3 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -295,6 +295,14 @@ void ControllerManager::init_controller_manager() RCLCPP_WARN(get_logger(), "'update_rate' parameter not set, using default value."); } + 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"); + }); + // 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( @@ -308,14 +316,6 @@ void ControllerManager::init_controller_manager() diagnostics_updater_.setHardwareID("ros2_control"); diagnostics_updater_.add( "Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback); - - 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"); - }); } void ControllerManager::robot_description_callback(const std_msgs::msg::String & robot_description) diff --git a/hardware_interface_testing/test/test_resource_manager.cpp b/hardware_interface_testing/test/test_resource_manager.cpp index c85087f78b..897b97ca54 100644 --- a/hardware_interface_testing/test/test_resource_manager.cpp +++ b/hardware_interface_testing/test/test_resource_manager.cpp @@ -106,9 +106,7 @@ TEST_F(ResourceManagerTest, test_unitilizable_hardware_validation) // If the the hardware can not be initialized and load_urdf tried to validate the interfaces a // runtime exception is thrown TestableResourceManager rm; - ASSERT_THROW( - rm.load_urdf(ros2_control_test_assets::minimal_unitilizable_robot_urdf, true), - std::runtime_error); + EXPECT_FALSE(rm.load_urdf(ros2_control_test_assets::minimal_unitilizable_robot_urdf, true)); } TEST_F(ResourceManagerTest, test_unitilizable_hardware_no_validation) @@ -116,7 +114,7 @@ TEST_F(ResourceManagerTest, test_unitilizable_hardware_no_validation) // If the the hardware can not be initialized and load_urdf didn't try to validate the interfaces, // the interface should not show up TestableResourceManager rm; - EXPECT_NO_THROW(rm.load_urdf(ros2_control_test_assets::minimal_unitilizable_robot_urdf, false)); + EXPECT_TRUE(rm.load_urdf(ros2_control_test_assets::minimal_unitilizable_robot_urdf, false)); // test actuator EXPECT_FALSE(rm.state_interface_exists("joint1/position"));