adapt to new changes and remove some rebase artefacts

mamueluth · mamueluth · commit e9c2669b64d1 · 2024-08-14T11:04:52.000+02:00
diff --git a/controller_interface/include/controller_interface/chainable_controller_interface.hpp b/controller_interface/include/controller_interface/chainable_controller_interface.hpp
@@ -59,10 +59,7 @@ class ChainableControllerInterface : public ControllerInterfaceBase
   bool is_chainable() const final;
 
   CONTROLLER_INTERFACE_PUBLIC
-  std::vector<hardware_interface::StateInterface> export_state_interfaces() final;
-
-  CONTROLLER_INTERFACE_PUBLIC
-  std::vector<hardware_interface::StateInterface> export_state_interfaces() final;
+  std::vector<std::shared_ptr<hardware_interface::StateInterface>> export_state_interfaces() final;
 
   CONTROLLER_INTERFACE_PUBLIC
   std::vector<std::shared_ptr<hardware_interface::CommandInterface>> export_reference_interfaces()
diff --git a/controller_interface/include/controller_interface/controller_interface.hpp b/controller_interface/include/controller_interface/controller_interface.hpp
@@ -47,7 +47,7 @@ class ControllerInterface : public controller_interface::ControllerInterfaceBase
    * \returns empty list.
    */
   CONTROLLER_INTERFACE_PUBLIC
-  std::vector<hardware_interface::StateInterface> export_state_interfaces() final;
+  std::vector<std::shared_ptr<hardware_interface::StateInterface>> export_state_interfaces() final;
 
   /**
    * Controller has no reference interfaces.
diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp
@@ -231,7 +231,8 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy
    * \returns list of state interfaces for preceding controllers.
    */
   CONTROLLER_INTERFACE_PUBLIC
-  virtual std::vector<hardware_interface::StateInterface> export_state_interfaces() = 0;
+  virtual std::vector<std::shared_ptr<hardware_interface::StateInterface>>
+  export_state_interfaces() = 0;
 
   /**
    * Set chained mode of a chainable controller. This method triggers internal processes to switch
diff --git a/controller_interface/src/chainable_controller_interface.cpp b/controller_interface/src/chainable_controller_interface.cpp
@@ -44,52 +44,32 @@ return_type ChainableControllerInterface::update(
   return ret;
 }
 
-std::vector<hardware_interface::StateInterface>
+std::vector<std::shared_ptr<hardware_interface::StateInterface>>
 ChainableControllerInterface::export_state_interfaces()
 {
   auto state_interfaces = on_export_state_interfaces();
+  std::vector<std::shared_ptr<hardware_interface::StateInterface>> state_interfaces_ptrs_vec;
+  state_interfaces_ptrs_vec.reserve(state_interfaces.size());
 
   // check if the names of the controller state interfaces begin with the controller's name
-  for (const auto & interface : state_interfaces)
+  for (auto & interface : state_interfaces)
   {
     if (interface.get_prefix_name() != get_node()->get_name())
     {
-      RCLCPP_FATAL(
-        get_node()->get_logger(),
-        "The name of the interface '%s' does not begin with the controller's name. This is "
+      std::string error_msg =
+        "The name of the interface '" + interface.get_name() +
+        "' does not begin with the controller's name. This is "
         "mandatory for state interfaces. No state interface will be exported. Please "
-        "correct and recompile the controller with name '%s' and try again.",
-        interface.get_name().c_str(), get_node()->get_name());
-      state_interfaces.clear();
-      break;
+        "correct and recompile the controller with name '" +
+        get_node()->get_name() + "' and try again.";
+      throw std::runtime_error(error_msg);
     }
-  }
-
-  return state_interfaces;
-}
-
-std::vector<std::shared_ptr<hardware_interface::CommandInterface>>
-ChainableControllerInterface::export_state_interfaces()
-{
-  auto state_interfaces = on_export_state_interfaces();
 
-  // check if the names of the controller state interfaces begin with the controller's name
-  for (const auto & interface : state_interfaces)
-  {
-    if (interface.get_prefix_name() != get_node()->get_name())
-    {
-      RCLCPP_FATAL(
-        get_node()->get_logger(),
-        "The name of the interface '%s' does not begin with the controller's name. This is "
-        "mandatory for state interfaces. No state interface will be exported. Please "
-        "correct and recompile the controller with name '%s' and try again.",
-        interface.get_name().c_str(), get_node()->get_name());
-      state_interfaces.clear();
-      break;
-    }
+    state_interfaces_ptrs_vec.push_back(
+      std::make_shared<hardware_interface::StateInterface>(interface));
   }
 
-  return state_interfaces;
+  return state_interfaces_ptrs_vec;
 }
 
 std::vector<std::shared_ptr<hardware_interface::CommandInterface>>
@@ -187,8 +167,8 @@ ChainableControllerInterface::on_export_state_interfaces()
   std::vector<hardware_interface::StateInterface> state_interfaces;
   for (size_t i = 0; i < exported_state_interface_names_.size(); ++i)
   {
-    state_interfaces.emplace_back(hardware_interface::StateInterface(
-      get_node()->get_name(), exported_state_interface_names_[i], &state_interfaces_values_[i]));
+    state_interfaces.emplace_back(
+      get_node()->get_name(), exported_state_interface_names_[i], &state_interfaces_values_[i]);
   }
   return state_interfaces;
 }
diff --git a/controller_interface/src/controller_interface.cpp b/controller_interface/src/controller_interface.cpp
@@ -22,7 +22,8 @@ ControllerInterface::ControllerInterface() : ControllerInterfaceBase() {}
 
 bool ControllerInterface::is_chainable() const { return false; }
 
-std::vector<hardware_interface::StateInterface> ControllerInterface::export_state_interfaces()
+std::vector<std::shared_ptr<hardware_interface::StateInterface>>
+ControllerInterface::export_state_interfaces()
 {
   return {};
 }
diff --git a/controller_interface/test/test_chainable_controller_interface.cpp b/controller_interface/test/test_chainable_controller_interface.cpp
@@ -47,11 +47,11 @@ TEST_F(ChainableControllerInterfaceTest, export_state_interfaces)
 
   auto exported_state_interfaces = controller.export_state_interfaces();
 
-  ASSERT_EQ(reference_interfaces.size(), 1u);
-  EXPECT_EQ(reference_interfaces[0]->get_prefix_name(), TEST_CONTROLLER_NAME);
-  EXPECT_EQ(reference_interfaces[0]->get_interface_name(), "test_itf");
+  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");
 
-  EXPECT_EQ(reference_interfaces[0]->get_value(), INTERFACE_VALUE);
+  EXPECT_EQ(exported_state_interfaces[0]->get_value(), INTERFACE_VALUE);
 }
 
 TEST_F(ChainableControllerInterfaceTest, export_reference_interfaces)
@@ -68,10 +68,10 @@ TEST_F(ChainableControllerInterfaceTest, export_reference_interfaces)
   auto 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");
+  EXPECT_EQ(reference_interfaces[0]->get_prefix_name(), TEST_CONTROLLER_NAME);
+  EXPECT_EQ(reference_interfaces[0]->get_interface_name(), "test_itf");
 
-  EXPECT_EQ(reference_interfaces[0].get_value(), INTERFACE_VALUE);
+  EXPECT_EQ(reference_interfaces[0]->get_value(), INTERFACE_VALUE);
 }
 
 TEST_F(ChainableControllerInterfaceTest, interfaces_prefix_is_not_node_name)
@@ -127,7 +127,7 @@ TEST_F(ChainableControllerInterfaceTest, setting_chained_mode)
 
   EXPECT_TRUE(controller.set_chained_mode(true));
   EXPECT_TRUE(controller.is_in_chained_mode());
-  EXPECT_EQ(exported_state_interfaces[0].get_value(), EXPORTED_STATE_INTERFACE_VALUE_IN_CHAINMODE);
+  EXPECT_EQ(exported_state_interfaces[0]->get_value(), EXPORTED_STATE_INTERFACE_VALUE_IN_CHAINMODE);
 
   controller.configure();
   EXPECT_TRUE(controller.set_chained_mode(false));
diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp
@@ -752,12 +752,13 @@ controller_interface::return_type ControllerManager::configure_controller(
       get_logger(),
       "Controller '%s' is chainable. Interfaces are being exported to resource manager.",
       controller_name.c_str());
-    auto state_interfaces = controller->export_state_interfaces();
-    std::vector<std::shared_ptr<hardware_interface::CommandInterface>> interfaces;
+    std::vector<std::shared_ptr<hardware_interface::StateInterface>> state_interfaces;
+    std::vector<std::shared_ptr<hardware_interface::CommandInterface>> ref_interfaces;
     try
     {
-      interfaces = controller->export_reference_interfaces();
-      if (interfaces.empty() && state_interfaces.empty())
+      state_interfaces = controller->export_state_interfaces();
+      ref_interfaces = controller->export_reference_interfaces();
+      if (ref_interfaces.empty() && state_interfaces.empty())
       {
         // TODO(destogl): Add test for this!
         RCLCPP_ERROR(
diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp
@@ -120,7 +120,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
    * \returns CallbackReturn::SUCCESS if required data are provided and can be parsed.
    * \returns CallbackReturn::ERROR if any error happens or data are missing.
    */
-  virtual CallbackReturn on_init(const HardwareInfo & /*hardware_info*/)
+  virtual CallbackReturn on_init(const HardwareInfo & hardware_info)
   {
     info_ = hardware_info;
     import_state_interface_descriptions(info_);
diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp
@@ -141,7 +141,7 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager
    * \param[in] interfaces list of controller's state interfaces as StateInterfaces.
    */
   void import_controller_exported_state_interfaces(
-    const std::string & controller_name, std::vector<StateInterface> & interfaces);
+    const std::string & controller_name, std::vector<std::shared_ptr<StateInterface>> & interfaces);
 
   /// Get list of exported tate interface of a controller.
   /**
diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp
@@ -120,7 +120,7 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
    * \returns CallbackReturn::SUCCESS if required data are provided and can be parsed.
    * \returns CallbackReturn::ERROR if any error happens or data are missing.
    */
-  virtual CallbackReturn on_init(const HardwareInfo & /*hardware_info*/)
+  virtual CallbackReturn on_init(const HardwareInfo & hardware_info)
   {
     info_ = hardware_info;
     import_state_interface_descriptions(info_);
diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp
@@ -122,7 +122,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
    * \returns CallbackReturn::SUCCESS if required data are provided and can be parsed.
    * \returns CallbackReturn::ERROR if any error happens or data are missing.
    */
-  virtual CallbackReturn on_init(const HardwareInfo & /*hardware_info*/)
+  virtual CallbackReturn on_init(const HardwareInfo & hardware_info)
   {
     info_ = hardware_info;
     import_state_interface_descriptions(info_);
diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp
@@ -606,40 +606,29 @@ class ResourceStorage
   template <class HardwareT>
   void import_state_interfaces(HardwareT & hardware)
   {
-    try
-    {
-      std::vector<std::string> interface_names;
-      std::vector<std::shared_ptr<StateInterface>> interfaces = hardware.export_state_interfaces();
+    std::vector<std::string> interface_names;
+    std::vector<std::shared_ptr<StateInterface>> interfaces = hardware.export_state_interfaces();
 
-      interface_names.reserve(interfaces.size());
-      for (auto const & interface : interfaces)
+    interface_names.reserve(interfaces.size());
+    for (auto const & interface : interfaces)
+    {
+      try
       {
-        const auto [it, success] =
-          state_interface_map_.insert(std::make_pair(interface->get_name(), interface));
-        if (!success)
-        {
-          std::string msg(
-            "ResourceStorage: Tried to insert StateInterface with already existing key. Insert[" +
-            interface->get_name() + "]");
-          throw std::runtime_error(msg);
-        }
-        interface_names.push_back(interface->get_name());
+        interface_names.push_back(add_state_interface(interface));
+      }
+      // We don't want to crash during runtime because a StateInterface could not be added
+      catch (const std::exception & e)
+      {
+        RCLCPP_WARN(
+          get_logger(),
+          "Exception occurred while importing state interfaces for the hardware '%s' : %s",
+          hardware.get_name().c_str(), e.what());
       }
     }
-    catch (const std::exception & e)
-    {
-      RCLCPP_ERROR(
-        get_logger(),
-        "Exception occurred while importing state interfaces for the hardware '%s' : %s",
-        hardware.get_name().c_str(), e.what());
-    }
-    catch (...)
-    {
-      RCLCPP_ERROR(
-        get_logger(),
-        "Unknown exception occurred while importing state interfaces for the hardware '%s'",
-        hardware.get_name().c_str());
-    }
+
+    hardware_info_map_[hardware.get_name()].state_interfaces = interface_names;
+    available_state_interfaces_.reserve(
+      available_state_interfaces_.capacity() + interface_names.size());
   }
 
   void insert_command_interface(const std::shared_ptr<CommandInterface> command_interface)
@@ -700,6 +689,19 @@ class ResourceStorage
     }
   }
 
+  std::string add_state_interface(std::shared_ptr<StateInterface> interface)
+  {
+    auto interface_name = interface->get_name();
+    const auto [it, success] = state_interface_map_.emplace(interface_name, interface);
+    if (!success)
+    {
+      std::string msg(
+        "ResourceStorage: Tried to insert StateInterface with already existing key. Insert[" +
+        interface->get_name() + "]");
+      throw std::runtime_error(msg);
+    }
+    return interface_name;
+  }
   /// Adds exported state interfaces into internal storage.
   /**
    * Adds state interfaces to the internal storage. State interfaces exported from hardware or
@@ -711,15 +713,23 @@ class ResourceStorage
    * \returns list of interface names that are added into internal storage. The output is used to
    * avoid additional iterations to cache interface names, e.g., for initializing info structures.
    */
-  std::vector<std::string> add_state_interfaces(std::vector<StateInterface> & interfaces)
+  std::vector<std::string> add_state_interfaces(
+    std::vector<std::shared_ptr<StateInterface>> & interfaces)
   {
     std::vector<std::string> interface_names;
     interface_names.reserve(interfaces.size());
     for (auto & interface : interfaces)
     {
-      auto key = interface.get_name();
-      state_interface_map_.emplace(std::make_pair(key, std::move(interface)));
-      interface_names.push_back(key);
+      try
+      {
+        interface_names.push_back(add_state_interface(interface));
+      }
+      // We don't want to crash during runtime because a StateInterface could not be added
+      catch (const std::exception & e)
+      {
+        RCLCPP_WARN(
+          get_logger(), "Exception occurred while importing state interfaces: %s", e.what());
+      }
     }
     available_state_interfaces_.reserve(
       available_state_interfaces_.capacity() + interface_names.size());
@@ -1243,7 +1253,7 @@ bool ResourceManager::state_interface_is_available(const std::string & name) con
 
 // CM API: Called in "callback/slow"-thread
 void ResourceManager::import_controller_exported_state_interfaces(
-  const std::string & controller_name, std::vector<StateInterface> & interfaces)
+  const std::string & controller_name, std::vector<std::shared_ptr<StateInterface>> & interfaces)
 {
   std::lock_guard<std::recursive_mutex> guard(resource_interfaces_lock_);
   auto interface_names = resource_storage_->add_state_interfaces(interfaces);
diff --git a/hardware_interface/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp
diff --git a/hardware_interface/test/test_component_interfaces_custom_export.cpp b/hardware_interface/test/test_component_interfaces_custom_export.cpp

Original file line number	Diff line number	Diff line change
`@@ -22,7 +22,8 @@ ControllerInterface::ControllerInterface() : ControllerInterfaceBase() {}`
`22`	`22`
`23`	`23`	`bool ControllerInterface::is_chainable() const { return false; }`
`24`	`24`
`25`		`-std::vector<hardware_interface::StateInterface> ControllerInterface::export_state_interfaces()`
	`25`	`+std::vector<std::shared_ptr<hardware_interface::StateInterface>>`
	`26`	`+ControllerInterface::export_state_interfaces()`
`26`	`27`	`{`
`27`	`28`	`return {};`
`28`	`29`	`}`
Original file line number	Diff line number	Diff line change
`@@ -120,7 +120,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod`
`120`	`120`	`* \returns CallbackReturn::SUCCESS if required data are provided and can be parsed.`
`121`	`121`	`* \returns CallbackReturn::ERROR if any error happens or data are missing.`
`122`	`122`	`*/`
`123`		`- virtual CallbackReturn on_init(const HardwareInfo & /hardware_info/)`
	`123`	`+ virtual CallbackReturn on_init(const HardwareInfo & hardware_info)`
`124`	`124`	`{`
`125`	`125`	`info_ = hardware_info;`
`126`	`126`	`import_state_interface_descriptions(info_);`
Original file line number	Diff line number	Diff line change
`@@ -120,7 +120,7 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI`
`120`	`120`	`* \returns CallbackReturn::SUCCESS if required data are provided and can be parsed.`
`121`	`121`	`* \returns CallbackReturn::ERROR if any error happens or data are missing.`
`122`	`122`	`*/`
`123`		`- virtual CallbackReturn on_init(const HardwareInfo & /hardware_info/)`
	`123`	`+ virtual CallbackReturn on_init(const HardwareInfo & hardware_info)`
`124`	`124`	`{`
`125`	`125`	`info_ = hardware_info;`
`126`	`126`	`import_state_interface_descriptions(info_);`