adapt reference interfaces and stateInterfaces of controllers new way of

exporting

controller_interface/include/controller_interface/chainable_controller_interface.hpp

@@ -15,7 +15,9 @@
 #ifndef CONTROLLER_INTERFACE__CHAINABLE_CONTROLLER_INTERFACE_HPP_
 #define CONTROLLER_INTERFACE__CHAINABLE_CONTROLLER_INTERFACE_HPP_
 
+#include <memory>
 #include <string>
+#include <unordered_map>
 #include <vector>
 
 #include "controller_interface/controller_interface_base.hpp"
@@ -57,10 +59,11 @@ class ChainableControllerInterface : public ControllerInterfaceBase
  bool is_chainable() const 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<hardware_interface::CommandInterface> export_reference_interfaces() final;
+ std::vector<std::shared_ptr<hardware_interface::CommandInterface>> export_reference_interfaces()
+ final;
 
  CONTROLLER_INTERFACE_PUBLIC
  bool set_chained_mode(bool chained_mode) final;
@@ -135,7 +138,11 @@ class ChainableControllerInterface : public ControllerInterfaceBase
 
  /// Storage of values for reference interfaces
  std::vector<std::string> exported_reference_interface_names_;
+ // BEGIN (Handle export change): for backward compatibility
  std::vector<double> reference_interfaces_;
+ // END
+ std::unordered_map<std::string, std::shared_ptr<hardware_interface::CommandInterface>>
+ reference_interfaces_ptrs_;
 
 private:
  /// A flag marking if a chainable controller is currently preceded by another controller.

controller_interface/include/controller_interface/controller_interface.hpp

@@ -47,15 +47,16 @@ 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.
  *
  * \returns empty list.
  */
  CONTROLLER_INTERFACE_PUBLIC
- std::vector<hardware_interface::CommandInterface> export_reference_interfaces() final;
+ std::vector<std::shared_ptr<hardware_interface::CommandInterface>> export_reference_interfaces()
+ final;
 
  /**
  * Controller is not chainable, therefore no chained mode can be set.

controller_interface/include/controller_interface/controller_interface_base.hpp

@@ -221,7 +221,8 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy
  * \returns list of command interfaces for preceding controllers.
  */
  CONTROLLER_INTERFACE_PUBLIC
- virtual std::vector<hardware_interface::CommandInterface> export_reference_interfaces() = 0;
+ virtual std::vector<std::shared_ptr<hardware_interface::CommandInterface>>
+ export_reference_interfaces() = 0;
 
  /**
  * Export interfaces for a chainable controller that can be used as state interface by other
@@ -230,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

controller_interface/src/chainable_controller_interface.cpp

@@ -44,53 +44,91 @@ 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)
  {
  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;
+ std::string error_msg =
+ "The prefix of the interface '" + interface.get_prefix_name() +
+ "' does not equal the controller's name '" + get_node()->get_name() +
+ "'. This is mandatory for state interfaces. No state interface will be exported. Please "
+ "correct and recompile the controller with name '" +
+ get_node()->get_name() + "' and try again.";
+ throw std::runtime_error(error_msg);
  }
+
+ state_interfaces_ptrs_vec.push_back(
+ std::make_shared<hardware_interface::StateInterface>(interface));
  }
 
- return state_interfaces;
+ return state_interfaces_ptrs_vec;
 }
 
-std::vector<hardware_interface::CommandInterface>
+std::vector<std::shared_ptr<hardware_interface::CommandInterface>>
 ChainableControllerInterface::export_reference_interfaces()
 {
  auto reference_interfaces = on_export_reference_interfaces();
+ std::vector<std::shared_ptr<hardware_interface::CommandInterface>> reference_interfaces_ptrs_vec;
+ reference_interfaces_ptrs_vec.reserve(reference_interfaces.size());
+
+ // BEGIN (Handle export change): for backward compatibility
+ // check if the "reference_interfaces_" variable is resized to number of interfaces
+ if (reference_interfaces_.size() != reference_interfaces.size())
+ {
+ // TODO(destogl): Should here be "FATAL"? It is fatal in terms of controller but not for the
+ // framework
+ std::string error_msg =
+ "The internal storage for reference values 'reference_interfaces_' variable has size '" +
+ std::to_string(reference_interfaces_.size()) + "', but it is expected to have the size '" +
+ std::to_string(reference_interfaces.size()) +
+ "' equal to the number of exported reference interfaces. Please correct and recompile the "
+ "controller with name '" +
+ get_node()->get_name() + "' and try again.";
+ throw std::runtime_error(error_msg);
+ }
+ // END
 
  // check if the names of the reference interfaces begin with the controller's name
- for (const auto & interface : reference_interfaces)
+ const auto ref_interface_size = reference_interfaces.size();
+ for (auto & interface : reference_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 reference interfaces. No reference 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());
- reference_interfaces.clear();
- break;
+ std::string error_msg = "The name of the interface " + interface.get_name() +
+ " does not begin with the controller's name. This is mandatory for "
+ "reference interfaces. Please "
+ "correct and recompile the controller with name " +
+ get_node()->get_name() + " and try again.";
+ throw std::runtime_error(error_msg);
  }
+
+ std::shared_ptr<hardware_interface::CommandInterface> interface_ptr =
+ std::make_shared<hardware_interface::CommandInterface>(std::move(interface));
+ reference_interfaces_ptrs_vec.push_back(interface_ptr);
+ reference_interfaces_ptrs_.insert(std::make_pair(interface_ptr->get_name(), interface_ptr));
  }
 
- return reference_interfaces;
+ if (reference_interfaces_ptrs_.size() != ref_interface_size)
+ {
+ std::string error_msg =
+ "The internal storage for reference ptrs 'reference_interfaces_ptrs_' variable has size '" +
+ std::to_string(reference_interfaces_ptrs_.size()) +
+ "', but it is expected to have the size '" + std::to_string(ref_interface_size) +
+ "' equal to the number of exported reference interfaces. Please correct and recompile the "
+ "controller with name '" +
+ get_node()->get_name() + "' and try again.";
+ throw std::runtime_error(error_msg);
+ }
+
+ return reference_interfaces_ptrs_vec;
 }
 
 bool ChainableControllerInterface::set_chained_mode(bool chained_mode)
@@ -129,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;
 }

controller_interface/src/controller_interface.cpp

@@ -22,12 +22,14 @@ 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 {};
 }
 
-std::vector<hardware_interface::CommandInterface> ControllerInterface::export_reference_interfaces()
+std::vector<std::shared_ptr<hardware_interface::CommandInterface>>
+ControllerInterface::export_reference_interfaces()
 {
  return {};
 }

controller_interface/test/test_chainable_controller_interface.cpp

@@ -15,6 +15,7 @@
 #include "test_chainable_controller_interface.hpp"
 
 #include <gmock/gmock.h>
+#include <memory>
 
 using ::testing::IsEmpty;
 using ::testing::SizeIs;
@@ -48,10 +49,10 @@ TEST_F(ChainableControllerInterfaceTest, export_state_interfaces)
  auto 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");
+ 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(exported_state_interfaces[0].get_value(), EXPORTED_STATE_INTERFACE_VALUE);
+ EXPECT_EQ(exported_state_interfaces[0]->get_value(), EXPORTED_STATE_INTERFACE_VALUE);
 }
 
 TEST_F(ChainableControllerInterfaceTest, export_reference_interfaces)
@@ -68,10 +69,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)
@@ -88,10 +89,15 @@ TEST_F(ChainableControllerInterfaceTest, interfaces_prefix_is_not_node_name)
  controller.set_name_prefix_of_reference_interfaces("some_not_correct_interface_prefix");
 
  // expect empty return because interface prefix is not equal to the node name
- auto reference_interfaces = controller.export_reference_interfaces();
- ASSERT_THAT(reference_interfaces, IsEmpty());
+ std::vector<std::shared_ptr<hardware_interface::CommandInterface>> exported_reference_interfaces;
+ EXPECT_THROW(
+ { exported_reference_interfaces = controller.export_reference_interfaces(); },
+ std::runtime_error);
+ ASSERT_THAT(exported_reference_interfaces, IsEmpty());
  // expect empty return because interface prefix is not equal to the node name
- auto exported_state_interfaces = controller.export_state_interfaces();
+ std::vector<std::shared_ptr<hardware_interface::StateInterface>> exported_state_interfaces;
+ EXPECT_THROW(
+ { exported_state_interfaces = controller.export_state_interfaces(); }, std::runtime_error);
  ASSERT_THAT(exported_state_interfaces, IsEmpty());
 }
 
@@ -114,8 +120,7 @@ TEST_F(ChainableControllerInterfaceTest, setting_chained_mode)
  EXPECT_FALSE(controller.is_in_chained_mode());
 
  // Fail setting chained mode
- EXPECT_EQ(reference_interfaces[0].get_value(), INTERFACE_VALUE);
- EXPECT_EQ(exported_state_interfaces[0].get_value(), EXPORTED_STATE_INTERFACE_VALUE);
+ EXPECT_EQ(reference_interfaces[0]->get_value(), INTERFACE_VALUE);
 
  EXPECT_FALSE(controller.set_chained_mode(true));
  EXPECT_FALSE(controller.is_in_chained_mode());
@@ -124,11 +129,11 @@ TEST_F(ChainableControllerInterfaceTest, setting_chained_mode)
  EXPECT_FALSE(controller.is_in_chained_mode());
 
  // Success setting chained mode
- reference_interfaces[0].set_value(0.0);
+ reference_interfaces[0]->set_value(0.0);
 
  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));