Skip to content

Commit 74a2b5c

Browse files
saikishorsaikishor
authored
Enable controller activation for inactive hardware component (#2486)
1 parent bce3c67 commit 74a2b5c

File tree

2 files changed

+39
-67
lines changed

2 files changed

+39
-67
lines changed

‎hardware_interface/src/resource_manager.cpp‎

Lines changed: 0 additions & 20 deletions
Original file line numberDiff line numberDiff line change
@@ -2010,16 +2010,6 @@ 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-
}
20232013
if (
20242014
component.get_lifecycle_state().id() ==
20252015
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
@@ -2110,16 +2100,6 @@ bool ResourceManager::perform_command_mode_switch(
21102100
component.get_name().c_str());
21112101
continue;
21122102
}
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-
}
21232103
if (
21242104
component.get_lifecycle_state().id() ==
21252105
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||

‎hardware_interface_testing/test/test_resource_manager_prepare_perform_switch.cpp‎

Lines changed: 39 additions & 47 deletions
Original file line numberDiff line numberDiff line change
@@ -210,32 +210,28 @@ 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 not OK
214-
EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_actuator, legal_keys_actuator));
213+
// When TestActuatorHardware is INACTIVE expect OK
214+
EXPECT_TRUE(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.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));
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));
219218
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0);
220-
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.0, 1e-7)
221-
<< "Start interfaces with inactive should result in no change";
219+
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.101, 1e-7);
222220

223-
EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_actuator, empty_keys));
221+
EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_actuator, empty_keys));
224222
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0);
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));
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));
228225
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0);
229-
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.0, 1e-7)
230-
<< "Start interfaces with inactive should result in no change";
226+
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.202, 1e-7);
231227

232228
EXPECT_TRUE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_actuator));
233229
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0);
234-
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.001, 1e-7);
230+
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.203, 1e-7);
235231
EXPECT_TRUE(rm_->perform_command_mode_switch(empty_keys, legal_keys_actuator))
236-
<< "Inactive with empty start interfaces is OK";
232+
<< "Inactive is OK";
237233
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0);
238-
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.101, 1e-7);
234+
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.303, 1e-7);
239235
};
240236

241237
// System : INACTIVE
@@ -247,58 +243,54 @@ TEST_F(
247243
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, "inactive",
248244
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, "active");
249245

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";
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);
254249
EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0)
255250
<< "System interfaces shouldn't affect the actuator";
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";
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);
259253
EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0)
260254
<< "System interfaces shouldn't affect the actuator";
261255

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";
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);
265258
EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0)
266259
<< "System interfaces shouldn't affect the actuator";
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";
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);
270262
EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0)
271263
<< "System interfaces shouldn't affect the actuator";
272264

273265
EXPECT_TRUE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_system));
274-
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 1.0);
266+
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 203.0);
275267
EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0)
276268
<< "System interfaces shouldn't affect the actuator";
277269
EXPECT_FALSE(rm_->perform_command_mode_switch(empty_keys, legal_keys_system));
278-
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 101.0);
270+
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0);
279271
EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0)
280272
<< "System interfaces shouldn't affect the actuator";
281273

282274
// When TestActuatorHardware is ACTIVE expect OK
283275
EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_actuator, legal_keys_actuator));
284-
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 101.0);
276+
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0);
285277
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.001, 1e-7);
286278
EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, legal_keys_actuator));
287-
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 101.0);
279+
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0);
288280
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.101, 1e-7);
289281

290282
EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_actuator, empty_keys));
291-
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 101.0);
283+
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0);
292284
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.102, 1e-7);
293285
EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, empty_keys));
294-
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 101.0);
286+
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0);
295287
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.202, 1e-7);
296288

297289
EXPECT_TRUE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_actuator));
298-
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 101.0);
290+
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0);
299291
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.203, 1e-7);
300292
EXPECT_TRUE(rm_->perform_command_mode_switch(empty_keys, legal_keys_actuator));
301-
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 101.0);
293+
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0);
302294
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.303, 1e-7);
303295
};
304296

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

453445
// Similar to the proximal activation
454-
EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_actuator, empty_keys));
446+
EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_actuator, empty_keys));
455447
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0);
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))
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))
458450
<< "Start interfaces with inactive should result in no change";
459451
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0);
460-
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.404, 1e-7)
452+
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.505, 1e-7)
461453
<< "Start interfaces with inactive should result in no change";
462454

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

477469
EXPECT_TRUE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_actuator));
478470
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0);
479-
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.405, 1e-7);
471+
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.506, 1e-7);
480472
EXPECT_TRUE(rm_->perform_command_mode_switch(empty_keys, legal_keys_actuator));
481473
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0);
482-
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.505, 1e-7);
474+
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.606, 1e-7);
483475

484-
EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_actuator, empty_keys));
476+
EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_actuator, empty_keys));
485477
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0);
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));
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));
488480
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0);
489-
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.505, 1e-7);
481+
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.707, 1e-7);
490482
};
491483

492484
// System : UNCONFIGURED

0 commit comments

Comments
 (0)