Skip to content

Commit d46c412

Browse files
destogldestogl
committed
Added functionality and tests for getting robot description from the topic.
1 parent 4b5d5ff commit d46c412

File tree

5 files changed

+256
-65
lines changed

5 files changed

+256
-65
lines changed

‎controller_manager/CMakeLists.txt

+1
Original file line numberDiff line numberDiff line change
@@ -150,6 +150,7 @@ if(BUILD_TESTING)
150150
)
151151
target_link_libraries(test_controller_manager_urdf_passing
152152
controller_manager
153+
test_controller
153154
ros2_control_test_assets::ros2_control_test_assets
154155
)
155156
ament_target_dependencies(test_controller_manager_urdf_passing

‎controller_manager/include/controller_manager/controller_manager.hpp

+2-1
Original file line numberDiff line numberDiff line change
@@ -322,7 +322,7 @@ class ControllerManager : public rclcpp::Node
322322
std::vector<std::string> get_controller_names();
323323
std::pair<std::string, std::string> split_command_interface(
324324
const std::string & command_interface);
325-
void subscribe_to_robot_description_topic();
325+
void init_controller_manager();
326326

327327
/**
328328
* Clear request lists used when switching controllers. The lists are shared between "callback"
@@ -546,6 +546,7 @@ class ControllerManager : public rclcpp::Node
546546

547547
std::string robot_description_;
548548
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr robot_description_subscription_;
549+
rclcpp::TimerBase::SharedPtr robot_description_notification_timer_;
549550

550551
struct SwitchParams
551552
{

‎controller_manager/src/controller_manager.cpp

+41-40
Original file line numberDiff line numberDiff line change
@@ -267,31 +267,7 @@ ControllerManager::ControllerManager(
267267
std::make_shared<pluginlib::ClassLoader<controller_interface::ChainableControllerInterface>>(
268268
kControllerInterfaceNamespace, kChainableControllerInterfaceClassName))
269269
{
270-
if (!get_parameter("update_rate", update_rate_))
271-
{
272-
RCLCPP_WARN(get_logger(), "'update_rate' parameter not set, using default value.");
273-
}
274-
275-
robot_description_ = "";
276-
// TODO(destogl): remove support at the end of 2023
277-
get_parameter("robot_description", robot_description_);
278-
if (robot_description_.empty())
279-
{
280-
subscribe_to_robot_description_topic();
281-
}
282-
else
283-
{
284-
RCLCPP_WARN(
285-
get_logger(),
286-
"[Deprecated] Passing the robot description parameter directly to the control_manager node "
287-
"is deprecated. Use '~/robot_description' topic from 'robot_state_publisher' instead.");
288-
init_resource_manager(robot_description_);
289-
init_services();
290-
}
291-
292-
diagnostics_updater_.setHardwareID("ros2_control");
293-
diagnostics_updater_.add(
294-
"Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback);
270+
init_controller_manager();
295271
}
296272

297273
ControllerManager::ControllerManager(
@@ -308,24 +284,17 @@ ControllerManager::ControllerManager(
308284
std::make_shared<pluginlib::ClassLoader<controller_interface::ChainableControllerInterface>>(
309285
kControllerInterfaceNamespace, kChainableControllerInterfaceClassName))
310286
{
287+
init_controller_manager();
288+
}
289+
290+
void ControllerManager::init_controller_manager()
291+
{
292+
// Get parameters needed for RT "update" loop to work
311293
if (!get_parameter("update_rate", update_rate_))
312294
{
313295
RCLCPP_WARN(get_logger(), "'update_rate' parameter not set, using default value.");
314296
}
315297

316-
if (resource_manager_->is_urdf_already_loaded())
317-
{
318-
init_services();
319-
}
320-
subscribe_to_robot_description_topic();
321-
322-
diagnostics_updater_.setHardwareID("ros2_control");
323-
diagnostics_updater_.add(
324-
"Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback);
325-
}
326-
327-
void ControllerManager::subscribe_to_robot_description_topic()
328-
{
329298
// set QoS to transient local to get messages that have already been published
330299
// (if robot state publisher starts before controller manager)
331300
robot_description_subscription_ = create_subscription<std_msgs::msg::String>(
@@ -334,6 +303,19 @@ void ControllerManager::subscribe_to_robot_description_topic()
334303
RCLCPP_INFO(
335304
get_logger(), "Subscribing to '%s' topic for robot description.",
336305
robot_description_subscription_->get_topic_name());
306+
307+
// Setup diagnostics
308+
diagnostics_updater_.setHardwareID("ros2_control");
309+
diagnostics_updater_.add(
310+
"Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback);
311+
312+
robot_description_notification_timer_ = create_wall_timer(
313+
std::chrono::seconds(1),
314+
[&]()
315+
{
316+
RCLCPP_WARN(
317+
get_logger(), "Waiting for data on '~/robot_description' topic to finish initialization");
318+
});
337319
}
338320

339321
void ControllerManager::robot_description_callback(const std_msgs::msg::String & robot_description)
@@ -355,8 +337,14 @@ void ControllerManager::robot_description_callback(const std_msgs::msg::String &
355337

356338
void ControllerManager::init_resource_manager(const std::string & robot_description)
357339
{
358-
// TODO(destogl): manage this when there is an error - CM should not die because URDF is wrong...
359-
resource_manager_->load_urdf(robot_description);
340+
if (!resource_manager_->load_urdf(robot_description))
341+
{
342+
RCLCPP_WARN(
343+
get_logger(),
344+
"URDF validation went wrong check the previous output. This might only mean that interfaces "
345+
"defined in URDF and exported by the hardware do not match. Therefore continue initializing "
346+
"controller manager...");
347+
}
360348

361349
// Get all components and if they are not defined in parameters activate them automatically
362350
auto components_to_activate = resource_manager_->get_components_status();
@@ -451,6 +439,10 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript
451439
resource_manager_->set_component_state(component, active_state);
452440
}
453441
}
442+
443+
// Init CM services first after the URDF is loaded an components are set
444+
init_services();
445+
robot_description_notification_timer_->cancel();
454446
}
455447

456448
void ControllerManager::init_services()
@@ -824,6 +816,15 @@ controller_interface::return_type ControllerManager::switch_controller(
824816
const std::vector<std::string> & deactivate_controllers, int strictness, bool activate_asap,
825817
const rclcpp::Duration & timeout)
826818
{
819+
if (!resource_manager_->is_urdf_already_loaded())
820+
{
821+
RCLCPP_ERROR(
822+
get_logger(),
823+
"Resource Manager is not initialized yet! Please provide robot description on "
824+
"'~/robot_description' topic before trying to switch controllers.");
825+
return controller_interface::return_type::ERROR;
826+
}
827+
827828
switch_params_ = SwitchParams();
828829

829830
if (!deactivate_request_.empty() || !activate_request_.empty())

‎controller_manager/test/test_controller_manager_urdf_passing.cpp

+132-24
Original file line numberDiff line numberDiff line change
@@ -37,7 +37,14 @@ class TestableControllerManager : public controller_manager::ControllerManager
3737
FRIEND_TEST(TestControllerManagerWithTestableCM, load_urdf_called_after_callback);
3838
FRIEND_TEST(TestControllerManagerWithTestableCM, load_urdf_called_after_invalid_urdf_passed);
3939
FRIEND_TEST(
40-
TestControllerManagerWithTestableCM, when_starting_controller_without_hw_expect_error);
40+
TestControllerManagerWithTestableCM,
41+
when_starting_broadcaster_expect_error_before_rm_is_initialized_with_robot_description);
42+
FRIEND_TEST(
43+
TestControllerManagerWithTestableCM,
44+
when_starting_controller_expect_error_before_rm_is_initialized_with_robot_description);
45+
FRIEND_TEST(
46+
TestControllerManagerWithTestableCM,
47+
when_starting_controller_expect_error_before_rm_is_initialized_after_some_time);
4148

4249
public:
4350
TestableControllerManager(
@@ -58,6 +65,45 @@ class TestControllerManagerWithTestableCM
5865
public:
5966
// create cm with no urdf
6067
TestControllerManagerWithTestableCM() : ControllerManagerFixture<TestableControllerManager>("") {}
68+
69+
void prepare_controller()
70+
{
71+
ASSERT_FALSE(cm_->resource_manager_->is_urdf_already_loaded());
72+
test_controller_ = std::make_shared<test_controller::TestController>();
73+
cm_->add_controller(test_controller_, CONTROLLER_NAME, TEST_CONTROLLER_CLASS_NAME);
74+
ASSERT_NE(test_controller_, nullptr);
75+
ASSERT_EQ(
76+
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller_->get_state().id());
77+
}
78+
79+
void configure_and_try_switch_that_returns_error()
80+
{
81+
// configure controller
82+
cm_->configure_controller(CONTROLLER_NAME);
83+
EXPECT_EQ(
84+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller_->get_state().id());
85+
86+
// Set ControllerManager into Debug-Mode output to have detailed output on updating controllers
87+
cm_->get_logger().set_level(rclcpp::Logger::Level::Debug);
88+
89+
switch_test_controllers(
90+
strvec{CONTROLLER_NAME}, strvec{}, STRICT, std::future_status::ready,
91+
controller_interface::return_type::ERROR);
92+
93+
EXPECT_EQ(
94+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller_->get_state().id());
95+
}
96+
97+
void try_successful_switch()
98+
{
99+
switch_test_controllers(
100+
strvec{CONTROLLER_NAME}, strvec{}, STRICT, std::future_status::timeout,
101+
controller_interface::return_type::OK);
102+
103+
EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller_->get_state().id());
104+
}
105+
106+
std::shared_ptr<test_controller::TestController> test_controller_;
61107
};
62108

63109
TEST_P(TestControllerManagerWithTestableCM, initial_no_load_urdf_called)
@@ -80,30 +126,92 @@ TEST_P(TestControllerManagerWithTestableCM, load_urdf_called_after_invalid_urdf_
80126
ASSERT_TRUE(cm_->resource_manager_->is_urdf_already_loaded());
81127
}
82128

83-
TEST_P(TestControllerManagerWithTestableCM, when_starting_controller_without_hw_expect_error)
129+
TEST_P(
130+
TestControllerManagerWithTestableCM,
131+
when_starting_broadcaster_expect_error_before_rm_is_initialized_with_robot_description)
84132
{
85-
ASSERT_FALSE(cm_->resource_manager_->is_urdf_already_loaded());
86-
auto controller_if = cm_->load_controller(CONTROLLER_NAME, TEST_CONTROLLER_CLASS_NAME);
87-
ASSERT_NE(controller_if, nullptr);
88-
ASSERT_EQ(
89-
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if->get_state().id());
90-
cm_->configure_controller(CONTROLLER_NAME);
91-
EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_state().id());
92-
93-
// Set ControllerManager into Debug-Mode output to have detailed output on updating controllers
94-
cm_->get_logger().set_level(rclcpp::Logger::Level::Debug);
95-
96-
switch_test_controllers(
97-
strvec{CONTROLLER_NAME}, strvec{}, STRICT, std::future_status::timeout,
98-
controller_interface::return_type::ERROR);
99-
100-
EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_state().id());
101-
102-
// mimic callback
103-
// auto msg = std_msgs::msg::String();
104-
// msg.data = ros2_control_test_assets::minimal_robot_missing_command_keys_urdf;
105-
// cm_->robot_description_callback(msg);
106-
// ASSERT_TRUE(cm_->resource_manager_->is_urdf_already_loaded());
133+
prepare_controller();
134+
135+
// setup state interfaces to claim from controllers
136+
controller_interface::InterfaceConfiguration state_itfs_cfg;
137+
state_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL;
138+
for (const auto & interface : ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_STATE_INTERFACES)
139+
{
140+
state_itfs_cfg.names.push_back(interface);
141+
}
142+
for (const auto & interface : ros2_control_test_assets::TEST_SENSOR_HARDWARE_STATE_INTERFACES)
143+
{
144+
state_itfs_cfg.names.push_back(interface);
145+
}
146+
test_controller_->set_state_interface_configuration(state_itfs_cfg);
147+
148+
configure_and_try_switch_that_returns_error();
149+
150+
pass_robot_description_to_cm_and_rm();
151+
ASSERT_TRUE(cm_->resource_manager_->is_urdf_already_loaded());
152+
153+
try_successful_switch();
154+
}
155+
156+
TEST_P(
157+
TestControllerManagerWithTestableCM,
158+
when_starting_controller_expect_error_before_rm_is_initialized_with_robot_description)
159+
{
160+
prepare_controller();
161+
162+
// setup command interfaces to claim from controllers
163+
controller_interface::InterfaceConfiguration cmd_itfs_cfg;
164+
cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL;
165+
for (const auto & interface : ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES)
166+
{
167+
cmd_itfs_cfg.names.push_back(interface);
168+
}
169+
test_controller_->set_command_interface_configuration(cmd_itfs_cfg);
170+
171+
configure_and_try_switch_that_returns_error();
172+
173+
pass_robot_description_to_cm_and_rm();
174+
ASSERT_TRUE(cm_->resource_manager_->is_urdf_already_loaded());
175+
176+
try_successful_switch();
177+
}
178+
179+
TEST_P(
180+
TestControllerManagerWithTestableCM,
181+
when_starting_controller_expect_error_before_rm_is_initialized_after_some_time)
182+
{
183+
prepare_controller();
184+
185+
// setup state interfaces to claim from controllers
186+
controller_interface::InterfaceConfiguration state_itfs_cfg;
187+
state_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL;
188+
for (const auto & interface : ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_STATE_INTERFACES)
189+
{
190+
state_itfs_cfg.names.push_back(interface);
191+
}
192+
for (const auto & interface : ros2_control_test_assets::TEST_SENSOR_HARDWARE_STATE_INTERFACES)
193+
{
194+
state_itfs_cfg.names.push_back(interface);
195+
}
196+
test_controller_->set_state_interface_configuration(state_itfs_cfg);
197+
198+
// setup command interfaces to claim from controllers
199+
controller_interface::InterfaceConfiguration cmd_itfs_cfg;
200+
cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL;
201+
for (const auto & interface : ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES)
202+
{
203+
cmd_itfs_cfg.names.push_back(interface);
204+
}
205+
test_controller_->set_command_interface_configuration(cmd_itfs_cfg);
206+
207+
configure_and_try_switch_that_returns_error();
208+
209+
std::this_thread::sleep_for(std::chrono::milliseconds(3000));
210+
211+
pass_robot_description_to_cm_and_rm();
212+
ASSERT_TRUE(cm_->resource_manager_->is_urdf_already_loaded());
213+
214+
try_successful_switch();
107215
}
108216

109217
INSTANTIATE_TEST_SUITE_P(

0 commit comments

Comments
 (0)