Skip to content

Commit 0301b0a

Browse files
saikishorsaikishor
authored
Revert "Enable controller activation for inactive hardware component (#2486)" (#2534)
This reverts commit 74a2b5c.
1 parent 74a2b5c commit 0301b0a

File tree

2 files changed

+67
-39
lines changed

2 files changed

+67
-39
lines changed

‎hardware_interface/src/resource_manager.cpp‎

Lines changed: 20 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -2010,6 +2010,16 @@ bool ResourceManager::prepare_command_mode_switch(
20102010
component.get_name().c_str());
20112011
continue;
20122012
}
2013+
if (
2014+
!start_interfaces_buffer.empty() &&
2015+
component.get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)
2016+
{
2017+
RCLCPP_WARN(
2018+
logger, "Component '%s' is in INACTIVE state, but has start interfaces to switch: \n%s",
2019+
component.get_name().c_str(),
2020+
interfaces_to_string(start_interfaces_buffer, stop_interfaces_buffer).c_str());
2021+
return false;
2022+
}
20132023
if (
20142024
component.get_lifecycle_state().id() ==
20152025
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
@@ -2100,6 +2110,16 @@ bool ResourceManager::perform_command_mode_switch(
21002110
component.get_name().c_str());
21012111
continue;
21022112
}
2113+
if (
2114+
!start_interfaces_buffer.empty() &&
2115+
component.get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)
2116+
{
2117+
RCLCPP_WARN(
2118+
logger, "Component '%s' is in INACTIVE state, but has start interfaces to switch: \n%s",
2119+
component.get_name().c_str(),
2120+
interfaces_to_string(start_interfaces_buffer, stop_interfaces_buffer).c_str());
2121+
return false;
2122+
}
21032123
if (
21042124
component.get_lifecycle_state().id() ==
21052125
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||

‎hardware_interface_testing/test/test_resource_manager_prepare_perform_switch.cpp‎

Lines changed: 47 additions & 39 deletions
Original file line numberDiff line numberDiff line change
@@ -210,28 +210,32 @@ TEST_F(
210210
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0);
211211
EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0);
212212

213-
// When TestActuatorHardware is INACTIVE expect OK
214-
EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_actuator, legal_keys_actuator));
213+
// When TestActuatorHardware is INACTIVE expect not OK
214+
EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_actuator, legal_keys_actuator));
215215
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0);
216-
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.001, 1e-7);
217-
EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, legal_keys_actuator));
216+
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.0, 1e-7)
217+
<< "Start interfaces with inactive should result in no change";
218+
EXPECT_FALSE(rm_->perform_command_mode_switch(legal_keys_actuator, legal_keys_actuator));
218219
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0);
219-
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.101, 1e-7);
220+
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.0, 1e-7)
221+
<< "Start interfaces with inactive should result in no change";
220222

221-
EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_actuator, empty_keys));
223+
EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_actuator, empty_keys));
222224
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0);
223-
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.102, 1e-7);
224-
EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, empty_keys));
225+
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.0, 1e-7)
226+
<< "Start interfaces with inactive should result in no change";
227+
EXPECT_FALSE(rm_->perform_command_mode_switch(legal_keys_actuator, empty_keys));
225228
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0);
226-
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.202, 1e-7);
229+
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.0, 1e-7)
230+
<< "Start interfaces with inactive should result in no change";
227231

228232
EXPECT_TRUE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_actuator));
229233
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0);
230-
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.203, 1e-7);
234+
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.001, 1e-7);
231235
EXPECT_TRUE(rm_->perform_command_mode_switch(empty_keys, legal_keys_actuator))
232-
<< "Inactive is OK";
236+
<< "Inactive with empty start interfaces is OK";
233237
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0);
234-
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.303, 1e-7);
238+
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.101, 1e-7);
235239
};
236240

237241
// System : INACTIVE
@@ -243,54 +247,58 @@ TEST_F(
243247
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, "inactive",
244248
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, "active");
245249

246-
// When TestSystemCommandModes is INACTIVE expect OK
247-
EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_system, legal_keys_system));
248-
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 1.0);
250+
// When TestSystemCommandModes is INACTIVE expect not OK
251+
EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_system, legal_keys_system));
252+
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0)
253+
<< "Start interfaces with inactive should result in no change";
249254
EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0)
250255
<< "System interfaces shouldn't affect the actuator";
251-
EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_system, legal_keys_system));
252-
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 101.0);
256+
EXPECT_FALSE(rm_->perform_command_mode_switch(legal_keys_system, legal_keys_system));
257+
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0)
258+
<< "Start interfaces with inactive should result in no change";
253259
EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0)
254260
<< "System interfaces shouldn't affect the actuator";
255261

256-
EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_system, empty_keys));
257-
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 102.0);
262+
EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_system, empty_keys));
263+
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0)
264+
<< "Start interfaces with inactive should result in no change";
258265
EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0)
259266
<< "System interfaces shouldn't affect the actuator";
260-
EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_system, empty_keys));
261-
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 202.0);
267+
EXPECT_FALSE(rm_->perform_command_mode_switch(legal_keys_system, empty_keys));
268+
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0)
269+
<< "Start interfaces with inactive should result in no change";
262270
EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0)
263271
<< "System interfaces shouldn't affect the actuator";
264272

265273
EXPECT_TRUE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_system));
266-
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 203.0);
274+
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 1.0);
267275
EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0)
268276
<< "System interfaces shouldn't affect the actuator";
269277
EXPECT_FALSE(rm_->perform_command_mode_switch(empty_keys, legal_keys_system));
270-
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0);
278+
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 101.0);
271279
EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0)
272280
<< "System interfaces shouldn't affect the actuator";
273281

274282
// When TestActuatorHardware is ACTIVE expect OK
275283
EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_actuator, legal_keys_actuator));
276-
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0);
284+
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 101.0);
277285
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.001, 1e-7);
278286
EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, legal_keys_actuator));
279-
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0);
287+
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 101.0);
280288
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.101, 1e-7);
281289

282290
EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_actuator, empty_keys));
283-
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0);
291+
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 101.0);
284292
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.102, 1e-7);
285293
EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, empty_keys));
286-
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0);
294+
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 101.0);
287295
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.202, 1e-7);
288296

289297
EXPECT_TRUE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_actuator));
290-
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0);
298+
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 101.0);
291299
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.203, 1e-7);
292300
EXPECT_TRUE(rm_->perform_command_mode_switch(empty_keys, legal_keys_actuator));
293-
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0);
301+
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 101.0);
294302
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.303, 1e-7);
295303
};
296304

@@ -443,13 +451,13 @@ TEST_F(
443451
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.404, 1e-7);
444452

445453
// Similar to the proximal activation
446-
EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_actuator, empty_keys));
454+
EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_actuator, empty_keys));
447455
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0);
448-
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.405, 1e-7);
449-
EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, empty_keys))
456+
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.404, 1e-7);
457+
EXPECT_FALSE(rm_->perform_command_mode_switch(legal_keys_actuator, empty_keys))
450458
<< "Start interfaces with inactive should result in no change";
451459
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0);
452-
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.505, 1e-7)
460+
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.404, 1e-7)
453461
<< "Start interfaces with inactive should result in no change";
454462

455463
// Now return ERROR with write fail value
@@ -468,17 +476,17 @@ TEST_F(
468476

469477
EXPECT_TRUE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_actuator));
470478
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0);
471-
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.506, 1e-7);
479+
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.405, 1e-7);
472480
EXPECT_TRUE(rm_->perform_command_mode_switch(empty_keys, legal_keys_actuator));
473481
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0);
474-
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.606, 1e-7);
482+
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.505, 1e-7);
475483

476-
EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_actuator, empty_keys));
484+
EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_actuator, empty_keys));
477485
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0);
478-
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.607, 1e-7);
479-
EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, empty_keys));
486+
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.505, 1e-7);
487+
EXPECT_FALSE(rm_->perform_command_mode_switch(legal_keys_actuator, empty_keys));
480488
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0);
481-
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.707, 1e-7);
489+
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.505, 1e-7);
482490
};
483491

484492
// System : UNCONFIGURED

0 commit comments

Comments
 (0)