Skip to content

Commit a3bf280

Browse files
mergify[bot]destogl
mergify[bot]
and
destogl
authored
Add additional checks for non existing and not available interfaces. (backport #1218) (#1291)
* Add additional checks for non-existing and not available interfaces. (#1218) (cherry picked from commit 8c34ab6) * Resolve conflicts for backport. * Make tests to use hardware description in tests to be able to activate controllers. * Fix variable name for clarity. --------- Co-authored-by: Dr. Denis <[email protected]>
1 parent 7a3a4a1 commit a3bf280

File tree

13 files changed

+665
-182
lines changed

13 files changed

+665
-182
lines changed

‎controller_manager/src/controller_manager.cpp‎

Lines changed: 25 additions & 17 deletions
Original file line numberDiff line numberDiff line change
@@ -1257,17 +1257,17 @@ void ControllerManager::deactivate_controllers()
12571257
std::vector<ControllerSpec> & rt_controller_list =
12581258
rt_controllers_wrapper_.update_and_get_used_by_rt_list();
12591259
// stop controllers
1260-
for (const auto & request : deactivate_request_)
1260+
for (const auto & controller_name : deactivate_request_)
12611261
{
12621262
auto found_it = std::find_if(
12631263
rt_controller_list.begin(), rt_controller_list.end(),
1264-
std::bind(controller_name_compare, std::placeholders::_1, request));
1264+
std::bind(controller_name_compare, std::placeholders::_1, controller_name));
12651265
if (found_it == rt_controller_list.end())
12661266
{
12671267
RCLCPP_ERROR(
12681268
get_logger(),
12691269
"Got request to stop controller '%s' but it is not in the realtime controller list",
1270-
request.c_str());
1270+
controller_name.c_str());
12711271
continue;
12721272
}
12731273
auto controller = found_it->c;
@@ -1279,7 +1279,7 @@ void ControllerManager::deactivate_controllers()
12791279
{
12801280
RCLCPP_ERROR(
12811281
get_logger(), "After deactivating, controller '%s' is in state '%s', expected Inactive",
1282-
request.c_str(), new_state.label().c_str());
1282+
controller_name.c_str(), new_state.label().c_str());
12831283
}
12841284
}
12851285
}
@@ -1291,18 +1291,18 @@ void ControllerManager::switch_chained_mode(
12911291
std::vector<ControllerSpec> & rt_controller_list =
12921292
rt_controllers_wrapper_.update_and_get_used_by_rt_list();
12931293

1294-
for (const auto & request : chained_mode_switch_list)
1294+
for (const auto & controller_name : chained_mode_switch_list)
12951295
{
12961296
auto found_it = std::find_if(
12971297
rt_controller_list.begin(), rt_controller_list.end(),
1298-
std::bind(controller_name_compare, std::placeholders::_1, request));
1298+
std::bind(controller_name_compare, std::placeholders::_1, controller_name));
12991299
if (found_it == rt_controller_list.end())
13001300
{
13011301
RCLCPP_FATAL(
13021302
get_logger(),
13031303
"Got request to turn %s chained mode for controller '%s', but controller is not in the "
13041304
"realtime controller list. (This should never happen!)",
1305-
(to_chained_mode ? "ON" : "OFF"), request.c_str());
1305+
(to_chained_mode ? "ON" : "OFF"), controller_name.c_str());
13061306
continue;
13071307
}
13081308
auto controller = found_it->c;
@@ -1312,11 +1312,11 @@ void ControllerManager::switch_chained_mode(
13121312
{
13131313
if (to_chained_mode)
13141314
{
1315-
resource_manager_->make_controller_reference_interfaces_available(request);
1315+
resource_manager_->make_controller_reference_interfaces_available(controller_name);
13161316
}
13171317
else
13181318
{
1319-
resource_manager_->make_controller_reference_interfaces_unavailable(request);
1319+
resource_manager_->make_controller_reference_interfaces_unavailable(controller_name);
13201320
}
13211321
}
13221322
else
@@ -1327,7 +1327,7 @@ void ControllerManager::switch_chained_mode(
13271327
"it! The control will probably not work as expected. Try to restart all controllers. "
13281328
"If "
13291329
"the error persist check controllers' individual configuration.",
1330-
(to_chained_mode ? "ON" : "OFF"), request.c_str());
1330+
(to_chained_mode ? "ON" : "OFF"), controller_name.c_str());
13311331
}
13321332
}
13331333
else
@@ -1336,7 +1336,7 @@ void ControllerManager::switch_chained_mode(
13361336
get_logger(),
13371337
"Got request to turn %s chained mode for controller '%s', but this can not happen if "
13381338
"controller is in '%s' state. (This should never happen!)",
1339-
(to_chained_mode ? "ON" : "OFF"), request.c_str(),
1339+
(to_chained_mode ? "ON" : "OFF"), controller_name.c_str(),
13401340
hardware_interface::lifecycle_state_names::ACTIVE);
13411341
}
13421342
}
@@ -1346,17 +1346,17 @@ void ControllerManager::activate_controllers()
13461346
{
13471347
std::vector<ControllerSpec> & rt_controller_list =
13481348
rt_controllers_wrapper_.update_and_get_used_by_rt_list();
1349-
for (const auto & request : activate_request_)
1349+
for (const auto & controller_name : activate_request_)
13501350
{
13511351
auto found_it = std::find_if(
13521352
rt_controller_list.begin(), rt_controller_list.end(),
1353-
std::bind(controller_name_compare, std::placeholders::_1, request));
1353+
std::bind(controller_name_compare, std::placeholders::_1, controller_name));
13541354
if (found_it == rt_controller_list.end())
13551355
{
13561356
RCLCPP_ERROR(
13571357
get_logger(),
13581358
"Got request to activate controller '%s' but it is not in the realtime controller list",
1359-
request.c_str());
1359+
controller_name.c_str());
13601360
continue;
13611361
}
13621362
auto controller = found_it->c;
@@ -1385,7 +1385,7 @@ void ControllerManager::activate_controllers()
13851385
RCLCPP_ERROR(
13861386
get_logger(),
13871387
"Resource conflict for controller '%s'. Command interface '%s' is already claimed.",
1388-
request.c_str(), command_interface.c_str());
1388+
controller_name.c_str(), command_interface.c_str());
13891389
assignment_successful = false;
13901390
break;
13911391
}
@@ -1395,7 +1395,8 @@ void ControllerManager::activate_controllers()
13951395
}
13961396
catch (const std::exception & e)
13971397
{
1398-
RCLCPP_ERROR(get_logger(), "Can't activate controller '%s': %s", request.c_str(), e.what());
1398+
RCLCPP_ERROR(
1399+
get_logger(), "Can't activate controller '%s': %s", controller_name.c_str(), e.what());
13991400
assignment_successful = false;
14001401
break;
14011402
}
@@ -1429,7 +1430,8 @@ void ControllerManager::activate_controllers()
14291430
}
14301431
catch (const std::exception & e)
14311432
{
1432-
RCLCPP_ERROR(get_logger(), "Can't activate controller '%s': %s", request.c_str(), e.what());
1433+
RCLCPP_ERROR(
1434+
get_logger(), "Can't activate controller '%s': %s", controller_name.c_str(), e.what());
14331435
assignment_successful = false;
14341436
break;
14351437
}
@@ -1451,6 +1453,12 @@ void ControllerManager::activate_controllers()
14511453
hardware_interface::lifecycle_state_names::ACTIVE,
14521454
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
14531455
}
1456+
1457+
// if it is a chainable controller, make the reference interfaces available on activation
1458+
if (controller->is_chainable())
1459+
{
1460+
resource_manager_->make_controller_reference_interfaces_available(controller_name);
1461+
}
14541462
}
14551463
// All controllers activated, switching done
14561464
switch_params_.do_switch = false;

‎controller_manager/test/controller_manager_test_common.hpp‎

Lines changed: 5 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -164,7 +164,11 @@ class TestControllerManagerSrvs
164164
: public ControllerManagerFixture<controller_manager::ControllerManager>
165165
{
166166
public:
167-
TestControllerManagerSrvs() {}
167+
TestControllerManagerSrvs()
168+
: ControllerManagerFixture<controller_manager::ControllerManager>(
169+
ros2_control_test_assets::minimal_robot_urdf, true)
170+
{
171+
}
168172

169173
void SetUp() override
170174
{

‎controller_manager/test/test_controller_manager_srvs.cpp‎

Lines changed: 10 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -284,12 +284,20 @@ TEST_F(TestControllerManagerSrvs, list_chained_controllers_srv)
284284
ASSERT_EQ("joint1/position", result->controller[1].reference_interfaces[0]);
285285
ASSERT_EQ("joint1/velocity", result->controller[1].reference_interfaces[1]);
286286
// activate controllers
287-
cm_->switch_controller(
287+
auto res = cm_->switch_controller(
288288
{test_chainable_controller::TEST_CONTROLLER_NAME}, {},
289289
controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0));
290-
cm_->switch_controller(
290+
ASSERT_EQ(res, controller_interface::return_type::OK);
291+
// we should here wait for the first controller to be activated, i.e., for its reference
292+
// interface to become available (mail loop runs on 100 Hz) - so we check the status at least once
293+
while (result->controller[1].state != "active")
294+
{
295+
result = call_service_and_wait(*client, request, srv_executor);
296+
}
297+
res = cm_->switch_controller(
291298
{test_controller::TEST_CONTROLLER_NAME}, {},
292299
controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0));
300+
ASSERT_EQ(res, controller_interface::return_type::OK);
293301
// get controller list after activate
294302
result = call_service_and_wait(*client, request, srv_executor);
295303
// check test controller

‎controller_manager/test/test_controllers_chaining_with_controller_manager.cpp‎

Lines changed: 11 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -87,6 +87,12 @@ class TestControllerChainingWithControllerManager
8787
public testing::WithParamInterface<Strictness>
8888
{
8989
public:
90+
TestControllerChainingWithControllerManager()
91+
: ControllerManagerFixture<TestableControllerManager>(
92+
ros2_control_test_assets::minimal_robot_urdf, true)
93+
{
94+
}
95+
9096
void SetUp()
9197
{
9298
executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
@@ -245,7 +251,7 @@ class TestControllerChainingWithControllerManager
245251
void check_after_de_activate(
246252
std::shared_ptr<T> & controller, const std::vector<std::string> & claimed_command_itfs,
247253
size_t expected_internal_counter, const controller_interface::return_type expected_return,
248-
bool deactivated, bool claimed_interfaces_from_hw = false)
254+
bool deactivated)
249255
{
250256
for (const auto & interface : claimed_command_itfs)
251257
{
@@ -258,14 +264,7 @@ class TestControllerChainingWithControllerManager
258264
}
259265
else
260266
{
261-
if (claimed_interfaces_from_hw)
262-
{
263-
EXPECT_TRUE(cm_->resource_manager_->command_interface_is_available(interface));
264-
}
265-
else
266-
{
267-
EXPECT_FALSE(cm_->resource_manager_->command_interface_is_available(interface));
268-
}
267+
EXPECT_TRUE(cm_->resource_manager_->command_interface_is_available(interface));
269268
EXPECT_FALSE(cm_->resource_manager_->command_interface_is_claimed(interface));
270269
}
271270
}
@@ -297,14 +296,12 @@ class TestControllerChainingWithControllerManager
297296
void DeactivateAndCheckController(
298297
std::shared_ptr<T> & controller, const std::string & controller_name,
299298
const std::vector<std::string> & claimed_command_itfs, size_t expected_internal_counter = 0u,
300-
const bool claimed_interfaces_from_hw = false,
301299
const controller_interface::return_type expected_return = controller_interface::return_type::OK)
302300
{
303301
switch_test_controllers(
304302
{}, {controller_name}, test_param.strictness, std::future_status::timeout, expected_return);
305303
check_after_de_activate(
306-
controller, claimed_command_itfs, expected_internal_counter, expected_return, true,
307-
claimed_interfaces_from_hw);
304+
controller, claimed_command_itfs, expected_internal_counter, expected_return, true);
308305
}
309306

310307
void UpdateAllControllerAndCheck(
@@ -606,9 +603,9 @@ TEST_P(
606603

607604
// all controllers are deactivated --> chained mode is not changed
608605
DeactivateAndCheckController(
609-
pid_left_wheel_controller, PID_LEFT_WHEEL, PID_LEFT_WHEEL_CLAIMED_INTERFACES, 14u, true);
606+
pid_left_wheel_controller, PID_LEFT_WHEEL, PID_LEFT_WHEEL_CLAIMED_INTERFACES, 14u);
610607
DeactivateAndCheckController(
611-
pid_right_wheel_controller, PID_RIGHT_WHEEL, PID_RIGHT_WHEEL_CLAIMED_INTERFACES, 14u, true);
608+
pid_right_wheel_controller, PID_RIGHT_WHEEL, PID_RIGHT_WHEEL_CLAIMED_INTERFACES, 14u);
612609
EXPECT_FALSE(pid_left_wheel_controller->is_in_chained_mode());
613610
EXPECT_FALSE(pid_right_wheel_controller->is_in_chained_mode());
614611
ASSERT_FALSE(diff_drive_controller->is_in_chained_mode());

‎hardware_interface/CMakeLists.txt‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -171,6 +171,10 @@ if(BUILD_TESTING)
171171
target_link_libraries(test_resource_manager ${PROJECT_NAME})
172172
ament_target_dependencies(test_resource_manager ros2_control_test_assets)
173173

174+
ament_add_gmock(test_resource_manager_prepare_perform_switch test/test_resource_manager_prepare_perform_switch.cpp)
175+
target_link_libraries(test_resource_manager_prepare_perform_switch hardware_interface)
176+
ament_target_dependencies(test_resource_manager_prepare_perform_switch ros2_control_test_assets)
177+
174178
ament_add_gmock(test_generic_system test/mock_components/test_generic_system.cpp)
175179
target_include_directories(test_generic_system PRIVATE include)
176180
target_link_libraries(test_generic_system ${PROJECT_NAME})

‎hardware_interface/include/hardware_interface/resource_manager.hpp‎

Lines changed: 5 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -325,7 +325,9 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager
325325
* by default
326326
* \param[in] start_interfaces vector of string identifiers for the command interfaces starting.
327327
* \param[in] stop_interfaces vector of string identifiers for the command interfaces stopping.
328-
* \return true if switch can be prepared, false if a component rejects switch request.
328+
* \return true if switch can be prepared; false if a component rejects switch request, and if
329+
* at least one of the input interfaces are not existing or not available (i.e., component is not
330+
* in ACTIVE or INACTIVE state).
329331
*/
330332
bool prepare_command_mode_switch(
331333
const std::vector<std::string> & start_interfaces,
@@ -338,6 +340,8 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager
338340
* \note this is intended for mode-switching when a hardware interface needs to change
339341
* control mode depending on which command interface is claimed.
340342
* \note this is for realtime switching of the command interface.
343+
* \note it is assumed that `prepare_command_mode_switch` is called just before this method
344+
* with the same input arguments.
341345
* \param[in] start_interfaces vector of string identifiers for the command interfaces starting.
342346
* \param[in] stop_interfaces vector of string identifiers for the command interfacs stopping.
343347
* \return true if switch is performed, false if a component rejects switching.

0 commit comments

Comments
 (0)