Skip to content

Commit 811d501

Browse files
saikishorsaikishor
committed
Add tests for switching controllers in nonRT loop
1 parent 2bdc00b commit 811d501

File tree

3 files changed

+357
-0
lines changed

3 files changed

+357
-0
lines changed

‎controller_manager/test/test_controller/test_controller.cpp

Lines changed: 13 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -150,6 +150,19 @@ CallbackReturn TestController::on_configure(const rclcpp_lifecycle::State & /*pr
150150
return CallbackReturn::SUCCESS;
151151
}
152152

153+
CallbackReturn TestController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/)
154+
{
155+
if (activation_processing_time > 0.0)
156+
{
157+
RCLCPP_INFO(
158+
get_node()->get_logger(), "Sleeping for %.3f seconds to simulate activation processing time",
159+
activation_processing_time);
160+
std::this_thread::sleep_for(std::chrono::duration<double>(activation_processing_time));
161+
}
162+
163+
return CallbackReturn::SUCCESS;
164+
}
165+
153166
CallbackReturn TestController::on_cleanup(const rclcpp_lifecycle::State & /*previous_state*/)
154167
{
155168
if (simulate_cleanup_failure)

‎controller_manager/test/test_controller/test_controller.hpp

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -52,6 +52,8 @@ class TestController : public controller_interface::ControllerInterface
5252

5353
CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override;
5454

55+
CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override;
56+
5557
CallbackReturn on_cleanup(const rclcpp_lifecycle::State & previous_state) override;
5658

5759
CallbackReturn on_shutdown(const rclcpp_lifecycle::State & previous_state) override;
@@ -67,6 +69,7 @@ class TestController : public controller_interface::ControllerInterface
6769

6870
rclcpp::Service<example_interfaces::srv::SetBool>::SharedPtr service_;
6971
unsigned int internal_counter = 0;
72+
double activation_processing_time = 0.0;
7073
bool simulate_cleanup_failure = false;
7174
// Variable where we store when shutdown was called, pointer because the controller
7275
// is usually destroyed after shutdown

‎controller_manager/test/test_controller_manager_srvs.cpp

Lines changed: 341 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -2216,3 +2216,344 @@ TEST_F(TestControllerManagerSrvs, switch_controller_failure_behaviour_on_unknown
22162216
result->controller[0].required_state_interfaces,
22172217
UnorderedElementsAre("joint1/position", "joint1/velocity", "joint2/position"));
22182218
}
2219+
2220+
TEST_F(TestControllerManagerSrvs, switch_controller_test_activate_asap)
2221+
{
2222+
// create server client and request
2223+
rclcpp::executors::SingleThreadedExecutor srv_executor;
2224+
rclcpp::Node::SharedPtr srv_node = std::make_shared<rclcpp::Node>("srv_client");
2225+
srv_executor.add_node(srv_node);
2226+
rclcpp::Client<ListControllers>::SharedPtr client =
2227+
srv_node->create_client<ListControllers>("test_controller_manager/list_controllers");
2228+
auto request = std::make_shared<ListControllers::Request>();
2229+
2230+
// create set of chained controllers
2231+
static constexpr char TEST_CHAINED_CONTROLLER_1[] = "test_chainable_controller_name_1";
2232+
static constexpr char TEST_CHAINED_CONTROLLER_2[] = "test_chainable_controller_name_2";
2233+
auto test_chained_controller_1 = std::make_shared<TestChainableController>();
2234+
controller_interface::InterfaceConfiguration chained_cmd_cfg = {
2235+
controller_interface::interface_configuration_type::INDIVIDUAL, {"joint1/position"}};
2236+
controller_interface::InterfaceConfiguration chained_state_cfg = {
2237+
controller_interface::interface_configuration_type::INDIVIDUAL,
2238+
{"joint1/position", "joint1/velocity"}};
2239+
test_chained_controller_1->set_command_interface_configuration(chained_cmd_cfg);
2240+
test_chained_controller_1->set_state_interface_configuration(chained_state_cfg);
2241+
test_chained_controller_1->set_reference_interface_names({"joint1/position", "joint1/velocity"});
2242+
2243+
auto test_chained_controller_2 = std::make_shared<TestChainableController>();
2244+
chained_cmd_cfg = {
2245+
controller_interface::interface_configuration_type::INDIVIDUAL,
2246+
{std::string(TEST_CHAINED_CONTROLLER_1) + "/joint1/position"}};
2247+
test_chained_controller_2->set_command_interface_configuration(chained_cmd_cfg);
2248+
test_chained_controller_2->set_state_interface_configuration(chained_state_cfg);
2249+
test_chained_controller_2->set_reference_interface_names({"joint1/position", "joint1/velocity"});
2250+
2251+
// create non-chained controller
2252+
auto test_controller = std::make_shared<TestController>();
2253+
controller_interface::InterfaceConfiguration cmd_cfg = {
2254+
controller_interface::interface_configuration_type::INDIVIDUAL,
2255+
{std::string(TEST_CHAINED_CONTROLLER_2) + "/joint1/position",
2256+
std::string(TEST_CHAINED_CONTROLLER_2) + "/joint1/velocity", "joint2/velocity"}};
2257+
controller_interface::InterfaceConfiguration state_cfg = {
2258+
controller_interface::interface_configuration_type::INDIVIDUAL,
2259+
{"joint1/position", "joint1/velocity"}};
2260+
test_controller->set_command_interface_configuration(cmd_cfg);
2261+
test_controller->set_state_interface_configuration(state_cfg);
2262+
// add controllers
2263+
cm_->add_controller(
2264+
test_chained_controller_1, TEST_CHAINED_CONTROLLER_1,
2265+
test_chainable_controller::TEST_CONTROLLER_CLASS_NAME);
2266+
cm_->add_controller(
2267+
test_controller, test_controller::TEST_CONTROLLER_NAME,
2268+
test_controller::TEST_CONTROLLER_CLASS_NAME);
2269+
cm_->add_controller(
2270+
test_chained_controller_2, TEST_CHAINED_CONTROLLER_2,
2271+
test_chainable_controller::TEST_CONTROLLER_CLASS_NAME);
2272+
// get controller list before configure
2273+
auto result = call_service_and_wait(*client, request, srv_executor);
2274+
// check chainable controller
2275+
ASSERT_EQ(3u, result->controller.size());
2276+
ASSERT_EQ(result->controller[0].name, TEST_CHAINED_CONTROLLER_1);
2277+
ASSERT_EQ(result->controller[2].name, TEST_CHAINED_CONTROLLER_2);
2278+
// check test controller
2279+
ASSERT_EQ(result->controller[1].name, "test_controller_name");
2280+
2281+
// configure controllers
2282+
for (const auto & controller :
2283+
{TEST_CHAINED_CONTROLLER_1, test_controller::TEST_CONTROLLER_NAME,
2284+
TEST_CHAINED_CONTROLLER_2})
2285+
{
2286+
cm_->configure_controller(controller);
2287+
}
2288+
2289+
// get controller list after configure
2290+
result = call_service_and_wait(*client, request, srv_executor);
2291+
ASSERT_EQ(3u, result->controller.size());
2292+
2293+
// reordered controllers
2294+
ASSERT_EQ(result->controller[0].name, "test_controller_name");
2295+
ASSERT_EQ(result->controller[1].name, TEST_CHAINED_CONTROLLER_2);
2296+
ASSERT_EQ(result->controller[2].name, TEST_CHAINED_CONTROLLER_1);
2297+
2298+
// activate controllers all at once
2299+
auto res = cm_->switch_controller(
2300+
{TEST_CHAINED_CONTROLLER_1, TEST_CHAINED_CONTROLLER_2, test_controller::TEST_CONTROLLER_NAME},
2301+
{}, controller_manager_msgs::srv::SwitchController::Request::STRICT, true,
2302+
rclcpp::Duration(0, 0));
2303+
ASSERT_EQ(res, controller_interface::return_type::OK);
2304+
2305+
ASSERT_EQ(
2306+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
2307+
test_chained_controller_1->get_lifecycle_state().id());
2308+
ASSERT_EQ(
2309+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
2310+
test_chained_controller_2->get_lifecycle_state().id());
2311+
ASSERT_EQ(
2312+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_lifecycle_state().id());
2313+
2314+
res = cm_->switch_controller(
2315+
{},
2316+
{TEST_CHAINED_CONTROLLER_1, TEST_CHAINED_CONTROLLER_2, test_controller::TEST_CONTROLLER_NAME},
2317+
controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0));
2318+
ASSERT_EQ(res, controller_interface::return_type::OK);
2319+
2320+
ASSERT_EQ(
2321+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
2322+
test_chained_controller_1->get_lifecycle_state().id());
2323+
ASSERT_EQ(
2324+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
2325+
test_chained_controller_2->get_lifecycle_state().id());
2326+
ASSERT_EQ(
2327+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
2328+
test_controller->get_lifecycle_state().id());
2329+
2330+
// Now test with activate_asap to false
2331+
2332+
res = cm_->switch_controller(
2333+
{TEST_CHAINED_CONTROLLER_1, TEST_CHAINED_CONTROLLER_2, test_controller::TEST_CONTROLLER_NAME},
2334+
{}, controller_manager_msgs::srv::SwitchController::Request::STRICT, false,
2335+
rclcpp::Duration(0, 0));
2336+
ASSERT_EQ(res, controller_interface::return_type::OK);
2337+
2338+
ASSERT_EQ(
2339+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
2340+
test_chained_controller_1->get_lifecycle_state().id());
2341+
ASSERT_EQ(
2342+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
2343+
test_chained_controller_2->get_lifecycle_state().id());
2344+
ASSERT_EQ(
2345+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_lifecycle_state().id());
2346+
2347+
res = cm_->switch_controller(
2348+
{},
2349+
{TEST_CHAINED_CONTROLLER_1, TEST_CHAINED_CONTROLLER_2, test_controller::TEST_CONTROLLER_NAME},
2350+
controller_manager_msgs::srv::SwitchController::Request::STRICT, false, rclcpp::Duration(0, 0));
2351+
ASSERT_EQ(res, controller_interface::return_type::OK);
2352+
2353+
ASSERT_EQ(
2354+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
2355+
test_chained_controller_1->get_lifecycle_state().id());
2356+
ASSERT_EQ(
2357+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
2358+
test_chained_controller_2->get_lifecycle_state().id());
2359+
ASSERT_EQ(
2360+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
2361+
test_controller->get_lifecycle_state().id());
2362+
}
2363+
2364+
TEST_F(TestControllerManagerSrvs, switch_controller_controllers_taking_long_time_to_activate)
2365+
{
2366+
// create server client and request
2367+
rclcpp::executors::SingleThreadedExecutor srv_executor;
2368+
rclcpp::Node::SharedPtr srv_node = std::make_shared<rclcpp::Node>("srv_client");
2369+
srv_executor.add_node(srv_node);
2370+
rclcpp::Client<ListControllers>::SharedPtr client =
2371+
srv_node->create_client<ListControllers>("test_controller_manager/list_controllers");
2372+
auto request = std::make_shared<ListControllers::Request>();
2373+
2374+
// create non-chained controllers
2375+
auto test_controller_1 = std::make_shared<TestController>();
2376+
controller_interface::InterfaceConfiguration cmd_cfg = {
2377+
controller_interface::interface_configuration_type::INDIVIDUAL, {"joint1/position"}};
2378+
controller_interface::InterfaceConfiguration state_cfg = {
2379+
controller_interface::interface_configuration_type::INDIVIDUAL,
2380+
{"joint1/position", "joint1/velocity"}};
2381+
test_controller_1->set_command_interface_configuration(cmd_cfg);
2382+
test_controller_1->set_state_interface_configuration(state_cfg);
2383+
2384+
auto test_controller_2 = std::make_shared<TestController>();
2385+
cmd_cfg = {controller_interface::interface_configuration_type::INDIVIDUAL, {"joint2/velocity"}};
2386+
state_cfg = {
2387+
controller_interface::interface_configuration_type::INDIVIDUAL,
2388+
{"joint1/position", "joint1/velocity"}};
2389+
test_controller_2->set_command_interface_configuration(cmd_cfg);
2390+
test_controller_2->set_state_interface_configuration(state_cfg);
2391+
2392+
auto test_controller_3 = std::make_shared<TestController>();
2393+
cmd_cfg = {controller_interface::interface_configuration_type::INDIVIDUAL, {"joint3/velocity"}};
2394+
state_cfg = {
2395+
controller_interface::interface_configuration_type::INDIVIDUAL,
2396+
{"joint1/position", "joint1/velocity"}};
2397+
test_controller_3->set_command_interface_configuration(cmd_cfg);
2398+
test_controller_3->set_state_interface_configuration(state_cfg);
2399+
// take 2 seconds to activate
2400+
test_controller_3->activation_processing_time = 2.0;
2401+
2402+
// add controllers
2403+
const std::string test_ctrl_1_name = "test_controller_name_1";
2404+
const std::string test_ctrl_2_name = "test_controller_name_2";
2405+
const std::string test_ctrl_3_name = "test_controller_name_3";
2406+
2407+
cm_->add_controller(
2408+
test_controller_1, test_ctrl_1_name, test_controller::TEST_CONTROLLER_CLASS_NAME);
2409+
cm_->add_controller(
2410+
test_controller_2, test_ctrl_2_name, test_controller::TEST_CONTROLLER_CLASS_NAME);
2411+
cm_->add_controller(
2412+
test_controller_3, test_ctrl_3_name, test_controller::TEST_CONTROLLER_CLASS_NAME);
2413+
// get controller list before configure
2414+
auto result = call_service_and_wait(*client, request, srv_executor);
2415+
ASSERT_EQ(3u, result->controller.size());
2416+
ASSERT_EQ(result->controller[0].name, test_ctrl_1_name);
2417+
ASSERT_EQ(result->controller[1].name, test_ctrl_2_name);
2418+
ASSERT_EQ(result->controller[2].name, test_ctrl_3_name);
2419+
2420+
// configure controllers
2421+
for (const auto & controller : {test_ctrl_1_name, test_ctrl_2_name, test_ctrl_3_name})
2422+
{
2423+
cm_->configure_controller(controller);
2424+
}
2425+
2426+
// get controller list after configure
2427+
result = call_service_and_wait(*client, request, srv_executor);
2428+
ASSERT_EQ(3u, result->controller.size());
2429+
2430+
// reordered controllers
2431+
ASSERT_EQ(result->controller[2].name, test_ctrl_1_name);
2432+
ASSERT_EQ(result->controller[1].name, test_ctrl_2_name);
2433+
ASSERT_EQ(result->controller[0].name, test_ctrl_3_name);
2434+
2435+
ASSERT_EQ(0u, test_controller_1->internal_counter);
2436+
ASSERT_EQ(0u, test_controller_2->internal_counter);
2437+
ASSERT_EQ(0u, test_controller_3->internal_counter);
2438+
2439+
EXPECT_EQ(
2440+
controller_interface::return_type::OK,
2441+
cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
2442+
2443+
ASSERT_EQ(0u, test_controller_1->internal_counter);
2444+
ASSERT_EQ(0u, test_controller_2->internal_counter);
2445+
ASSERT_EQ(0u, test_controller_3->internal_counter);
2446+
2447+
// activate non time taking controllers
2448+
{
2449+
auto res = cm_->switch_controller(
2450+
{test_ctrl_1_name, test_ctrl_2_name}, {},
2451+
controller_manager_msgs::srv::SwitchController::Request::STRICT, true,
2452+
rclcpp::Duration(0, 0));
2453+
ASSERT_EQ(res, controller_interface::return_type::OK);
2454+
}
2455+
unsigned int old_counter_1 = test_controller_1->internal_counter;
2456+
unsigned int old_counter_2 = test_controller_2->internal_counter;
2457+
2458+
EXPECT_EQ(
2459+
controller_interface::return_type::OK,
2460+
cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
2461+
ASSERT_GE(test_controller_1->internal_counter, old_counter_1 + 1);
2462+
ASSERT_GE(test_controller_2->internal_counter, old_counter_2 + 1);
2463+
EXPECT_EQ(old_counter_1 + 1, test_controller_1->internal_counter);
2464+
EXPECT_EQ(old_counter_2 + 1, test_controller_2->internal_counter);
2465+
2466+
ASSERT_EQ(
2467+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
2468+
test_controller_1->get_lifecycle_state().id());
2469+
ASSERT_EQ(
2470+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
2471+
test_controller_2->get_lifecycle_state().id());
2472+
ASSERT_EQ(
2473+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
2474+
test_controller_3->get_lifecycle_state().id());
2475+
2476+
// Now let's activate the time taking controller with activate_asap set to true
2477+
old_counter_1 = test_controller_1->internal_counter;
2478+
old_counter_2 = test_controller_2->internal_counter;
2479+
{
2480+
auto res = cm_->switch_controller(
2481+
{test_ctrl_3_name}, {}, controller_manager_msgs::srv::SwitchController::Request::STRICT, true,
2482+
rclcpp::Duration(0, 0));
2483+
ASSERT_EQ(res, controller_interface::return_type::OK);
2484+
}
2485+
2486+
ASSERT_EQ(
2487+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
2488+
test_controller_1->get_lifecycle_state().id());
2489+
ASSERT_EQ(
2490+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
2491+
test_controller_2->get_lifecycle_state().id());
2492+
ASSERT_EQ(
2493+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
2494+
test_controller_3->get_lifecycle_state().id());
2495+
2496+
// In 2 seconds, the others should do much more cycles but as the switch is happening in the RT
2497+
// loop, it blocked the update for the long time.
2498+
ASSERT_THAT(
2499+
test_controller_1->internal_counter,
2500+
testing::AllOf(testing::Gt(old_counter_1), testing::Lt((old_counter_1 + 4))));
2501+
ASSERT_THAT(
2502+
test_controller_2->internal_counter,
2503+
testing::AllOf(testing::Gt(old_counter_2), testing::Lt((old_counter_2 + 4))));
2504+
2505+
std::this_thread::sleep_for(std::chrono::milliseconds(100));
2506+
// Now, let's deactivate and activate again but with activate_asap as false
2507+
{
2508+
auto res = cm_->switch_controller(
2509+
{}, {test_ctrl_3_name}, controller_manager_msgs::srv::SwitchController::Request::STRICT, true,
2510+
rclcpp::Duration(0, 0));
2511+
ASSERT_EQ(res, controller_interface::return_type::OK);
2512+
}
2513+
2514+
ASSERT_EQ(
2515+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
2516+
test_controller_1->get_lifecycle_state().id());
2517+
ASSERT_EQ(
2518+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
2519+
test_controller_2->get_lifecycle_state().id());
2520+
ASSERT_EQ(
2521+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
2522+
test_controller_3->get_lifecycle_state().id());
2523+
2524+
old_counter_1 = test_controller_1->internal_counter;
2525+
old_counter_2 = test_controller_2->internal_counter;
2526+
{
2527+
ControllerManagerRunner cm_runner(this);
2528+
auto res = cm_->switch_controller(
2529+
{test_ctrl_3_name}, {}, controller_manager_msgs::srv::SwitchController::Request::STRICT,
2530+
false, rclcpp::Duration(0, 0));
2531+
ASSERT_EQ(res, controller_interface::return_type::OK);
2532+
}
2533+
const auto ideal_cycles =
2534+
cm_->get_update_rate() *
2535+
static_cast<unsigned int>(test_controller_3->activation_processing_time);
2536+
2537+
ASSERT_GT(test_controller_1->internal_counter, old_counter_1 + ideal_cycles - 20);
2538+
ASSERT_GT(test_controller_2->internal_counter, old_counter_2 + ideal_cycles - 20);
2539+
2540+
ASSERT_EQ(
2541+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
2542+
test_controller_1->get_lifecycle_state().id());
2543+
ASSERT_EQ(
2544+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
2545+
test_controller_2->get_lifecycle_state().id());
2546+
ASSERT_EQ(
2547+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
2548+
test_controller_3->get_lifecycle_state().id());
2549+
2550+
old_counter_1 = test_controller_1->internal_counter;
2551+
old_counter_2 = test_controller_2->internal_counter;
2552+
const auto old_counter_3 = test_controller_3->internal_counter;
2553+
EXPECT_EQ(
2554+
controller_interface::return_type::OK,
2555+
cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
2556+
EXPECT_EQ(old_counter_1 + 1, test_controller_1->internal_counter);
2557+
EXPECT_EQ(old_counter_2 + 1, test_controller_2->internal_counter);
2558+
EXPECT_EQ(old_counter_3 + 1, test_controller_3->internal_counter);
2559+
}

0 commit comments

Comments
 (0)