check for state of the controller node before cleanup (#1363)

(cherry picked from commit 6f57faf)

# Conflicts:
#	controller_manager/test/test_controller_manager_srvs.cpp

controller_manager/src/controller_manager.cpp

@@ -652,7 +652,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);
 

controller_manager/test/test_controller_manager_srvs.cpp

@@ -461,9 +461,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;
@@ -472,6 +520,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;
@@ -490,6 +541,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)