Merge branch 'master' into use_optional/instead_of/get_value

controller_interface/src/chainable_controller_interface.cpp

@@ -53,6 +53,9 @@ ChainableControllerInterface::export_state_interfaces()
   state_interfaces_ptrs_vec.reserve(state_interfaces.size());
   ordered_exported_state_interfaces_.reserve(state_interfaces.size());
   exported_state_interface_names_.reserve(state_interfaces.size());
+  exported_state_interfaces_.clear();
+  exported_state_interface_names_.clear();
+  ordered_exported_state_interfaces_.clear();
 
   // check if the names of the controller state interfaces begin with the controller's name
   for (const auto & interface : state_interfaces)
@@ -117,6 +120,9 @@ ChainableControllerInterface::export_reference_interfaces()
   reference_interfaces_ptrs_vec.reserve(reference_interfaces.size());
   exported_reference_interface_names_.reserve(reference_interfaces.size());
   ordered_exported_reference_interfaces_.reserve(reference_interfaces.size());
+  exported_reference_interfaces_.clear();
+  exported_reference_interface_names_.clear();
+  ordered_exported_reference_interfaces_.clear();
 
   // BEGIN (Handle export change): for backward compatibility
   // check if the "reference_interfaces_" variable is resized to number of interfaces

controller_interface/test/test_chainable_controller_interface.cpp

@@ -53,6 +53,13 @@ TEST_F(ChainableControllerInterfaceTest, export_state_interfaces)
   EXPECT_EQ(exported_state_interfaces[0]->get_interface_name(), "test_state");
 
   EXPECT_EQ(exported_state_interfaces[0]->get_optional().value(), EXPORTED_STATE_INTERFACE_VALUE);
+
+  // calling export_state_interfaces again should return the same interface and shouldn't throw
+  EXPECT_NO_THROW(exported_state_interfaces = controller.export_state_interfaces());
+
+  ASSERT_THAT(exported_state_interfaces, SizeIs(1));
+  EXPECT_EQ(exported_state_interfaces[0]->get_prefix_name(), TEST_CONTROLLER_NAME);
+  EXPECT_EQ(exported_state_interfaces[0]->get_interface_name(), "test_state");
 }
 
 TEST_F(ChainableControllerInterfaceTest, export_reference_interfaces)
@@ -73,6 +80,13 @@ TEST_F(ChainableControllerInterfaceTest, export_reference_interfaces)
   EXPECT_EQ(reference_interfaces[0]->get_interface_name(), "test_itf");
 
   EXPECT_EQ(reference_interfaces[0]->get_optional().value(), INTERFACE_VALUE);
+
+  // calling export_reference_interfaces again should return the same interface and shouldn't throw
+  EXPECT_NO_THROW(reference_interfaces = controller.export_reference_interfaces());
+
+  ASSERT_THAT(reference_interfaces, SizeIs(1));
+  EXPECT_EQ(reference_interfaces[0]->get_prefix_name(), TEST_CONTROLLER_NAME);
+  EXPECT_EQ(reference_interfaces[0]->get_interface_name(), "test_itf");
 }
 
 TEST_F(ChainableControllerInterfaceTest, interfaces_prefix_is_not_node_name)

controller_manager/include/controller_manager/controller_manager.hpp

@@ -445,6 +445,13 @@ class ControllerManager : public rclcpp::Node
    */
   rclcpp::NodeOptions determine_controller_node_options(const ControllerSpec & controller) const;
 
+  /**
+   * @brief cleanup_controller_exported_interfaces - A method that cleans up the exported interfaces
+   * of a chainable controller
+   * @param controller - controller info
+   */
+  void cleanup_controller_exported_interfaces(const ControllerSpec & controller);
+
   std::shared_ptr<controller_manager::ParamListener> cm_param_listener_;
   std::shared_ptr<controller_manager::Params> params_;
   diagnostic_updater::Updater diagnostics_updater_;

controller_manager/src/controller_manager.cpp

@@ -812,14 +812,11 @@ controller_interface::return_type ControllerManager::unload_controller(
 
   RCLCPP_DEBUG(get_logger(), "Shutdown controller");
   controller_chain_spec_cleanup(controller_chain_spec_, controller_name);
-  // TODO(destogl): remove reference interface if chainable; i.e., add a separate method for
-  // cleaning-up controllers?
+  cleanup_controller_exported_interfaces(controller);
   if (is_controller_inactive(*controller.c) || is_controller_unconfigured(*controller.c))
   {
     RCLCPP_DEBUG(
       get_logger(), "Controller '%s' is shutdown before unloading!", controller_name.c_str());
-    // TODO(destogl): remove reference interface if chainable; i.e., add a separate method for
-    // cleaning-up controllers?
     shutdown_controller(controller);
   }
   executor_->remove_node(controller.c->get_node()->get_node_base_interface());
@@ -908,10 +905,9 @@ controller_interface::return_type ControllerManager::configure_controller(
   {
     RCLCPP_DEBUG(
       get_logger(), "Controller '%s' is cleaned-up before configuring", controller_name.c_str());
-    // TODO(destogl): remove reference interface if chainable; i.e., add a separate method for
-    // cleaning-up controllers?
     try
     {
+      cleanup_controller_exported_interfaces(*found_it);
       new_state = controller->get_node()->cleanup();
       if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED)
       {
@@ -3642,4 +3638,16 @@ rclcpp::NodeOptions ControllerManager::determine_controller_node_options(
   return controller_node_options;
 }
 
+void ControllerManager::cleanup_controller_exported_interfaces(const ControllerSpec & controller)
+{
+  if (is_controller_inactive(controller.c) && controller.c->is_chainable())
+  {
+    RCLCPP_DEBUG(
+      get_logger(), "Removing controller '%s' exported interfaces from resource manager.",
+      controller.info.name.c_str());
+    resource_manager_->remove_controller_exported_state_interfaces(controller.info.name);
+    resource_manager_->remove_controller_reference_interfaces(controller.info.name);
+  }
+}
+
 }  // namespace controller_manager

controller_manager/test/test_controller_manager_srvs.cpp

@@ -573,13 +573,81 @@ TEST_F(TestControllerManagerSrvs, configure_controller_srv)
     lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
     test_controller->get_lifecycle_state().id());
 
+  // call configure again and check the state + it shouldn't throw any exception
+  result = call_service_and_wait(*client, request, srv_executor, true);
+  ASSERT_TRUE(result->ok);
+  EXPECT_EQ(1u, cm_->get_loaded_controllers().size());
+  EXPECT_EQ(
+    lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
+    cm_->get_loaded_controllers()[0].c->get_lifecycle_state().id());
+  EXPECT_EQ(
+    lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
+    test_controller->get_lifecycle_state().id());
+
+  // Now repeat the same with the chainable controller
+  request->name = test_chainable_controller::TEST_CONTROLLER_NAME;
+  result = call_service_and_wait(*client, request, srv_executor);
+  ASSERT_FALSE(result->ok) << "Controller not loaded: " << request->name;
+
+  auto test_chainable_controller = std::make_shared<TestChainableController>();
+  controller_interface::InterfaceConfiguration chained_cmd_cfg = {
+    controller_interface::interface_configuration_type::INDIVIDUAL, {"joint1/position"}};
+  controller_interface::InterfaceConfiguration chained_state_cfg = {
+    controller_interface::interface_configuration_type::INDIVIDUAL,
+    {"joint1/position", "joint1/velocity"}};
+  test_chainable_controller->set_command_interface_configuration(chained_cmd_cfg);
+  test_chainable_controller->set_state_interface_configuration(chained_state_cfg);
+  test_chainable_controller->set_reference_interface_names({"joint1/position", "joint1/velocity"});
+  test_chainable_controller->set_exported_state_interface_names(
+    {"joint1/position", "joint1/velocity"});
+
+  auto abstract_test_chainable_controller = cm_->add_controller(
+    test_chainable_controller, test_chainable_controller::TEST_CONTROLLER_NAME,
+    test_chainable_controller::TEST_CONTROLLER_CLASS_NAME);
+  ASSERT_NE(nullptr, abstract_test_chainable_controller);
+  EXPECT_EQ(2u, cm_->get_loaded_controllers().size());
+
+  result = call_service_and_wait(*client, request, srv_executor, true);
+  ASSERT_TRUE(result->ok);
+  EXPECT_EQ(2u, cm_->get_loaded_controllers().size());
+  EXPECT_EQ(
+    test_chainable_controller::TEST_CONTROLLER_NAME,
+    cm_->get_loaded_controllers()[0].c->get_name());
+  EXPECT_EQ(
+    lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
+    cm_->get_loaded_controllers()[0].c->get_lifecycle_state().id());
+  EXPECT_EQ(
+    lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
+    test_chainable_controller->get_lifecycle_state().id());
+
+  // call configure again and check the state + it shouldn't throw any exception
+  result = call_service_and_wait(*client, request, srv_executor, true);
+  ASSERT_TRUE(result->ok);
+  EXPECT_EQ(2u, cm_->get_loaded_controllers().size());
+  EXPECT_EQ(
+    test_chainable_controller::TEST_CONTROLLER_NAME,
+    cm_->get_loaded_controllers()[0].c->get_name());
+  EXPECT_EQ(
+    lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
+    cm_->get_loaded_controllers()[0].c->get_lifecycle_state().id());
+  EXPECT_EQ(
+    lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
+    test_chainable_controller->get_lifecycle_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_FINALIZED,
     test_controller->get_lifecycle_state().id());
+  EXPECT_EQ(1u, cm_->get_loaded_controllers().size());
+
+  unload_request->name = test_chainable_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_FINALIZED,
+    test_chainable_controller->get_lifecycle_state().id());
   EXPECT_EQ(0u, cm_->get_loaded_controllers().size());
 }