Skip to content

Commit 5be6ffe

Browse files
saikishorsaikishor
committed
Add some tests to test the logic of the AUTO mode
1 parent b19cf3c commit 5be6ffe

File tree

1 file changed

+168
-0
lines changed

1 file changed

+168
-0
lines changed

‎controller_manager/test/test_controller_manager_srvs.cpp‎

Lines changed: 168 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -836,6 +836,117 @@ TEST_F(TestControllerManagerSrvs, list_sorted_chained_controllers)
836836
ASSERT_EQ(result->controller[4].name, TEST_CHAINED_CONTROLLER_2);
837837
ASSERT_EQ(result->controller[5].name, TEST_CHAINED_CONTROLLER_1);
838838
RCLCPP_ERROR(srv_node->get_logger(), "Check successful!");
839+
840+
auto check_lifecycle_state = [&](const std::vector<uint8_t> & expected_states)
841+
{
842+
ASSERT_EQ(expected_states[0], test_controller->get_lifecycle_state().id())
843+
<< "Unexpected state for controller: " << test_controller->get_name();
844+
ASSERT_EQ(expected_states[1], test_chained_controller_5->get_lifecycle_state().id())
845+
<< "Unexpected state for controller: " << test_chained_controller_5->get_name();
846+
ASSERT_EQ(expected_states[2], test_chained_controller_4->get_lifecycle_state().id())
847+
<< "Unexpected state for controller: " << test_chained_controller_4->get_name();
848+
ASSERT_EQ(expected_states[3], test_chained_controller_3->get_lifecycle_state().id())
849+
<< "Unexpected state for controller: " << test_chained_controller_3->get_name();
850+
ASSERT_EQ(expected_states[4], test_chained_controller_2->get_lifecycle_state().id())
851+
<< "Unexpected state for controller: " << test_chained_controller_2->get_name();
852+
ASSERT_EQ(expected_states[5], test_chained_controller_1->get_lifecycle_state().id())
853+
<< "Unexpected state for controller: " << test_chained_controller_1->get_name();
854+
};
855+
856+
// Activate the whole chain by activating the top level controller
857+
ASSERT_EQ(controller_interface::return_type::OK, cm_->switch_controller(
858+
{"test_controller_name"}, {}, controller_manager_msgs::srv::SwitchController::Request::AUTO,
859+
true, rclcpp::Duration(0, 0)));
860+
check_lifecycle_state(
861+
{lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
862+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
863+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
864+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
865+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
866+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE});
867+
868+
// redoing the switch should be a no-op and not change the states and it should be successful
869+
ASSERT_EQ(controller_interface::return_type::OK, cm_->switch_controller(
870+
{"test_controller_name"}, {}, controller_manager_msgs::srv::SwitchController::Request::AUTO,
871+
true, rclcpp::Duration(0, 0)));
872+
check_lifecycle_state(
873+
{lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
874+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
875+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
876+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
877+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
878+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE});
879+
880+
// Now, deactivate the last controller in the chain and see that all controllers are deactivated
881+
ASSERT_EQ(controller_interface::return_type::OK, cm_->switch_controller(
882+
{}, {TEST_CHAINED_CONTROLLER_1}, controller_manager_msgs::srv::SwitchController::Request::AUTO,
883+
true, rclcpp::Duration(0, 0)));
884+
check_lifecycle_state(
885+
{lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
886+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
887+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
888+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
889+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
890+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE});
891+
892+
// Now activate the middle controller in the chain and see that all controllers below it are
893+
// activated
894+
ASSERT_EQ(controller_interface::return_type::OK, cm_->switch_controller(
895+
{TEST_CHAINED_CONTROLLER_4}, {}, controller_manager_msgs::srv::SwitchController::Request::AUTO,
896+
true, rclcpp::Duration(0, 0)));
897+
check_lifecycle_state(
898+
{lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
899+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
900+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
901+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
902+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
903+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE});
904+
905+
// Now, deactivate 3, and see that 3 and 4 are deactivated
906+
ASSERT_EQ(controller_interface::return_type::OK, cm_->switch_controller(
907+
{}, {TEST_CHAINED_CONTROLLER_3}, controller_manager_msgs::srv::SwitchController::Request::AUTO,
908+
true, rclcpp::Duration(0, 0)));
909+
check_lifecycle_state(
910+
{lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
911+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
912+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
913+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
914+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
915+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE});
916+
917+
// Redoing it should be a no-op and not change the states and it should be successful
918+
ASSERT_EQ(controller_interface::return_type::OK, cm_->switch_controller(
919+
{}, {TEST_CHAINED_CONTROLLER_3}, controller_manager_msgs::srv::SwitchController::Request::AUTO,
920+
true, rclcpp::Duration(0, 0)));
921+
check_lifecycle_state(
922+
{lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
923+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
924+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
925+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
926+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
927+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE});
928+
929+
ASSERT_EQ(controller_interface::return_type::OK, cm_->switch_controller(
930+
{TEST_CHAINED_CONTROLLER_4}, {}, controller_manager_msgs::srv::SwitchController::Request::AUTO,
931+
true, rclcpp::Duration(0, 0)));
932+
check_lifecycle_state(
933+
{lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
934+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
935+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
936+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
937+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
938+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE});
939+
940+
ASSERT_EQ(controller_interface::return_type::OK, cm_->switch_controller(
941+
{"test_controller_name"}, {}, controller_manager_msgs::srv::SwitchController::Request::AUTO,
942+
true, rclcpp::Duration(0, 0)));
943+
check_lifecycle_state(
944+
{lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
945+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
946+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
947+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
948+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
949+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE});
839950
}
840951

841952
TEST_F(TestControllerManagerSrvs, list_sorted_complex_chained_controllers)
@@ -1018,6 +1129,63 @@ TEST_F(TestControllerManagerSrvs, list_sorted_complex_chained_controllers)
10181129
ASSERT_GT(ctrl_5_pos, ctrl_6_pos);
10191130
ASSERT_GT(ctrl_4_pos, ctrl_5_pos);
10201131
ASSERT_GT(ctrl_3_pos, ctrl_4_pos);
1132+
1133+
// Now let's activate the top level controller and see if the whole chain is activated
1134+
cm_->switch_controller(
1135+
{"test_controller_name"}, {}, controller_manager_msgs::srv::SwitchController::Request::AUTO,
1136+
true, rclcpp::Duration(0, 0));
1137+
ASSERT_EQ(
1138+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_lifecycle_state().id());
1139+
ASSERT_EQ(
1140+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
1141+
test_chained_controller_7->get_lifecycle_state().id());
1142+
ASSERT_EQ(
1143+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
1144+
test_chained_controller_6->get_lifecycle_state().id());
1145+
ASSERT_EQ(
1146+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
1147+
test_chained_controller_5->get_lifecycle_state().id());
1148+
ASSERT_EQ(
1149+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
1150+
test_chained_controller_4->get_lifecycle_state().id());
1151+
ASSERT_EQ(
1152+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
1153+
test_chained_controller_3->get_lifecycle_state().id());
1154+
ASSERT_EQ(
1155+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
1156+
test_chained_controller_2->get_lifecycle_state().id());
1157+
ASSERT_EQ(
1158+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
1159+
test_chained_controller_1->get_lifecycle_state().id());
1160+
1161+
// Now deactivate the chained_controller_1 and everything should deactivate
1162+
cm_->switch_controller(
1163+
{}, {TEST_CHAINED_CONTROLLER_1}, controller_manager_msgs::srv::SwitchController::Request::AUTO,
1164+
true, rclcpp::Duration(0, 0));
1165+
ASSERT_EQ(
1166+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
1167+
test_controller->get_lifecycle_state().id());
1168+
ASSERT_EQ(
1169+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
1170+
test_chained_controller_7->get_lifecycle_state().id());
1171+
ASSERT_EQ(
1172+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
1173+
test_chained_controller_6->get_lifecycle_state().id());
1174+
ASSERT_EQ(
1175+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
1176+
test_chained_controller_5->get_lifecycle_state().id());
1177+
ASSERT_EQ(
1178+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
1179+
test_chained_controller_4->get_lifecycle_state().id());
1180+
ASSERT_EQ(
1181+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
1182+
test_chained_controller_3->get_lifecycle_state().id());
1183+
ASSERT_EQ(
1184+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
1185+
test_chained_controller_2->get_lifecycle_state().id());
1186+
ASSERT_EQ(
1187+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
1188+
test_chained_controller_1->get_lifecycle_state().id());
10211189
}
10221190

10231191
TEST_F(TestControllerManagerSrvs, list_sorted_independent_chained_controllers)

0 commit comments

Comments
 (0)