Skip to content

Commit

Permalink
check for state of the controller node before cleanup (#1363)
Browse files Browse the repository at this point in the history
(cherry picked from commit 6f57faf)

# Conflicts:
#	controller_manager/test/test_controller_manager_srvs.cpp
  • Loading branch information
saikishor authored and christophfroehlich committed Mar 27, 2024
1 parent 0c647de commit e23d1e0
Show file tree
Hide file tree
Showing 2 changed files with 74 additions and 1 deletion.
15 changes: 14 additions & 1 deletion controller_manager/src/controller_manager.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -667,7 +667,20 @@ controller_interface::return_type ControllerManager::unload_controller(
RCLCPP_DEBUG(get_logger(), "Cleanup controller");
// TODO(destogl): remove reference interface if chainable; i.e., add a separate method for
// cleaning-up controllers?
controller.c->get_node()->cleanup();
if (is_controller_inactive(*controller.c))
{
RCLCPP_DEBUG(
get_logger(), "Controller '%s' is cleaned-up before unloading!", controller_name.c_str());
// TODO(destogl): remove reference interface if chainable; i.e., add a separate method for
// cleaning-up controllers?
const auto new_state = controller.c->get_node()->cleanup();
if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED)
{
RCLCPP_WARN(
get_logger(), "Failed to clean-up the controller '%s' before unloading!",
controller_name.c_str());
}
}
executor_->remove_node(controller.c->get_node()->get_node_base_interface());
to.erase(found_it);

Expand Down
60 changes: 60 additions & 0 deletions controller_manager/test/test_controller_manager_srvs.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -462,9 +462,57 @@ TEST_F(TestControllerManagerSrvs, unload_controller_srv)

result = call_service_and_wait(*client, request, srv_executor, true);
ASSERT_TRUE(result->ok);
EXPECT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id());
EXPECT_EQ(0u, cm_->get_loaded_controllers().size());
}

<<<<<<< HEAD
=======
TEST_F(TestControllerManagerSrvs, robot_description_on_load_and_unload_controller)
{
rclcpp::executors::SingleThreadedExecutor srv_executor;
rclcpp::Node::SharedPtr srv_node = std::make_shared<rclcpp::Node>("srv_client");
srv_executor.add_node(srv_node);
rclcpp::Client<controller_manager_msgs::srv::UnloadController>::SharedPtr unload_client =
srv_node->create_client<controller_manager_msgs::srv::UnloadController>(
"test_controller_manager/unload_controller");

auto test_controller = std::make_shared<TestController>();
auto abstract_test_controller = cm_->add_controller(
test_controller, test_controller::TEST_CONTROLLER_NAME,
test_controller::TEST_CONTROLLER_CLASS_NAME);
EXPECT_EQ(1u, cm_->get_loaded_controllers().size());

// check the robot description
ASSERT_EQ(ros2_control_test_assets::minimal_robot_urdf, test_controller->get_robot_description());

// Now change the robot description and then see that the controller maintains the old URDF until
// it is unloaded and loaded again
auto msg = std_msgs::msg::String();
msg.data = ros2_control_test_assets::minimal_robot_missing_state_keys_urdf;
cm_->robot_description_callback(msg);
ASSERT_EQ(ros2_control_test_assets::minimal_robot_urdf, test_controller->get_robot_description());

// now unload and load the controller and see if the controller gets the new robot description
auto unload_request = std::make_shared<controller_manager_msgs::srv::UnloadController::Request>();
unload_request->name = test_controller::TEST_CONTROLLER_NAME;
auto result = call_service_and_wait(*unload_client, unload_request, srv_executor, true);
EXPECT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id());
EXPECT_EQ(0u, cm_->get_loaded_controllers().size());

// now load it and check if it got the new robot description
cm_->add_controller(
test_controller, test_controller::TEST_CONTROLLER_NAME,
test_controller::TEST_CONTROLLER_CLASS_NAME);
EXPECT_EQ(1u, cm_->get_loaded_controllers().size());
ASSERT_EQ(
ros2_control_test_assets::minimal_robot_missing_state_keys_urdf,
test_controller->get_robot_description());
}

>>>>>>> 6f57faf (check for state of the controller node before cleanup (#1363))
TEST_F(TestControllerManagerSrvs, configure_controller_srv)
{
rclcpp::executors::SingleThreadedExecutor srv_executor;
Expand All @@ -473,6 +521,9 @@ TEST_F(TestControllerManagerSrvs, configure_controller_srv)
rclcpp::Client<controller_manager_msgs::srv::ConfigureController>::SharedPtr client =
srv_node->create_client<controller_manager_msgs::srv::ConfigureController>(
"test_controller_manager/configure_controller");
rclcpp::Client<controller_manager_msgs::srv::UnloadController>::SharedPtr unload_client =
srv_node->create_client<controller_manager_msgs::srv::UnloadController>(
"test_controller_manager/unload_controller");

auto request = std::make_shared<controller_manager_msgs::srv::ConfigureController::Request>();
request->name = test_controller::TEST_CONTROLLER_NAME;
Expand All @@ -491,6 +542,15 @@ TEST_F(TestControllerManagerSrvs, configure_controller_srv)
EXPECT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
cm_->get_loaded_controllers()[0].c->get_state().id());
EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state().id());

// now unload the controller and check the state
auto unload_request = std::make_shared<controller_manager_msgs::srv::UnloadController::Request>();
unload_request->name = test_controller::TEST_CONTROLLER_NAME;
ASSERT_TRUE(call_service_and_wait(*unload_client, unload_request, srv_executor, true)->ok);
EXPECT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id());
EXPECT_EQ(0u, cm_->get_loaded_controllers().size());
}

TEST_F(TestControllerManagerSrvs, list_sorted_chained_controllers)
Expand Down

0 comments on commit e23d1e0

Please sign in to comment.