Distributed control aio

controller_interface/include/controller_interface/chainable_controller_interface.hpp

@@ -57,6 +57,10 @@ class ChainableControllerInterface : public ControllerInterfaceBase
   CONTROLLER_INTERFACE_PUBLIC
   std::vector<hardware_interface::CommandInterface> export_reference_interfaces() final;
 
+  CONTROLLER_INTERFACE_PUBLIC
+  std::vector<hardware_interface::DistributedCommandInterface>
+  export_distributed_reference_interfaces() final;
+
   CONTROLLER_INTERFACE_PUBLIC
   bool set_chained_mode(bool chained_mode) final;
 
@@ -75,6 +79,15 @@ class ChainableControllerInterface : public ControllerInterfaceBase
    */
   virtual std::vector<hardware_interface::CommandInterface> on_export_reference_interfaces() = 0;
 
+  // Only for fast poc should be pure virtual
+  /**
+   * Default returns emtpy list, so that not every chainable controller has to be updated for poc.
+   *
+   * \returns empty list.
+   */
+  virtual std::vector<hardware_interface::DistributedCommandInterface>
+  on_export_distributed_reference_interfaces();
+
   /// Virtual method that each chainable controller should implement to switch chained mode.
   /**
    * Each chainable controller implements this methods to switch between "chained" and "external"

controller_interface/include/controller_interface/controller_interface.hpp

@@ -43,13 +43,22 @@ class ControllerInterface : public controller_interface::ControllerInterfaceBase
   bool is_chainable() const final;
 
   /**
-   * Controller has no reference interfaces.
+   * Controller has no distributed reference interfaces.
    *
    * \returns empty list.
    */
   CONTROLLER_INTERFACE_PUBLIC
   std::vector<hardware_interface::CommandInterface> export_reference_interfaces() final;
 
+  /**
+   * Controller has no reference interfaces.
+   *
+   * \returns empty list.
+   */
+  CONTROLLER_INTERFACE_PUBLIC
+  std::vector<hardware_interface::DistributedCommandInterface>
+  export_distributed_reference_interfaces() final;
+
   /**
    * Controller is not chainable, therefore no chained mode can be set.
    *

controller_interface/include/controller_interface/controller_interface_base.hpp

@@ -197,6 +197,16 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy
   CONTROLLER_INTERFACE_PUBLIC
   virtual std::vector<hardware_interface::CommandInterface> export_reference_interfaces() = 0;
 
+  /**
+   * Export interfaces for a chainable controller that can be used as command interface of other
+   * controllers if controller is distributed.
+   *
+   * \returns list of distributed command interfaces for preceding controllers.
+   */
+  CONTROLLER_INTERFACE_PUBLIC
+  virtual std::vector<hardware_interface::DistributedCommandInterface>
+  export_distributed_reference_interfaces() = 0;
+
   /**
    * Set chained mode of a chainable controller. This method triggers internal processes to switch
    * a chainable controller to "chained" mode and vice-versa. Setting controller to "chained" mode

controller_interface/src/chainable_controller_interface.cpp

@@ -14,6 +14,7 @@
 
 #include "controller_interface/chainable_controller_interface.hpp"
 
+#include <exception>
 #include <vector>
 
 #include "hardware_interface/types/lifecycle_state_names.hpp"
@@ -44,6 +45,46 @@ return_type ChainableControllerInterface::update(
   return ret;
 }
 
+std::vector<hardware_interface::DistributedCommandInterface>
+ChainableControllerInterface::export_distributed_reference_interfaces()
+{
+  auto reference_interfaces = on_export_distributed_reference_interfaces();
+
+  // 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
+    RCLCPP_FATAL(
+      get_node()->get_logger(),
+      "The internal storage for reference values 'reference_interfaces_' variable has size '%zu', "
+      "but it is expected to have the size '%zu' equal to the number of exported reference "
+      "interfaces. No reference interface will be exported. Please correct and recompile "
+      "the controller with name '%s' and try again.",
+      reference_interfaces_.size(), reference_interfaces.size(), get_node()->get_name());
+    reference_interfaces.clear();
+  }
+
+  // check if the names of the reference interfaces begin with the controller's name
+  for (const 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;
+    }
+  }
+
+  return reference_interfaces;
+}
+
 std::vector<hardware_interface::CommandInterface>
 ChainableControllerInterface::export_reference_interfaces()
 {
@@ -111,6 +152,12 @@ bool ChainableControllerInterface::set_chained_mode(bool chained_mode)
 
 bool ChainableControllerInterface::is_in_chained_mode() const { return in_chained_mode_; }
 
+std::vector<hardware_interface::DistributedCommandInterface>
+ChainableControllerInterface::on_export_distributed_reference_interfaces()
+{
+  return {};
+}
+
 bool ChainableControllerInterface::on_set_chained_mode(bool /*chained_mode*/) { return true; }
 
 }  // namespace controller_interface

controller_interface/src/controller_interface.cpp

@@ -33,6 +33,12 @@ std::vector<hardware_interface::CommandInterface> ControllerInterface::export_re
   return {};
 }
 
+std::vector<hardware_interface::DistributedCommandInterface>
+ControllerInterface::export_distributed_reference_interfaces()
+{
+  return {};
+}
+
 bool ControllerInterface::set_chained_mode(bool /*chained_mode*/) { return false; }
 
 bool ControllerInterface::is_in_chained_mode() const { return false; }

controller_interface/test/test_force_torque_sensor.cpp

@@ -47,20 +47,20 @@ TEST_F(ForceTorqueSensorTest, validate_all_with_default_names)
   std::vector<std::string> interface_names = force_torque_sensor_->get_state_interface_names();
 
   // assign values to force
-  hardware_interface::StateInterface force_x{
-    sensor_name_, fts_interface_names_[0], &force_values_[0]};
-  hardware_interface::StateInterface force_y{
-    sensor_name_, fts_interface_names_[1], &force_values_[1]};
-  hardware_interface::StateInterface force_z{
-    sensor_name_, fts_interface_names_[2], &force_values_[2]};
+  auto force_x = std::make_shared<hardware_interface::StateInterface>(
+    sensor_name_, fts_interface_names_[0], &force_values_[0]);
+  auto force_y = std::make_shared<hardware_interface::StateInterface>(
+    sensor_name_, fts_interface_names_[1], &force_values_[1]);
+  auto force_z = std::make_shared<hardware_interface::StateInterface>(
+    sensor_name_, fts_interface_names_[2], &force_values_[2]);
 
   // assign values to torque
-  hardware_interface::StateInterface torque_x{
-    sensor_name_, fts_interface_names_[3], &torque_values_[0]};
-  hardware_interface::StateInterface torque_y{
-    sensor_name_, fts_interface_names_[4], &torque_values_[1]};
-  hardware_interface::StateInterface torque_z{
-    sensor_name_, fts_interface_names_[5], &torque_values_[2]};
+  auto torque_x = std::make_shared<hardware_interface::StateInterface>(
+    sensor_name_, fts_interface_names_[3], &torque_values_[0]);
+  auto torque_y = std::make_shared<hardware_interface::StateInterface>(
+    sensor_name_, fts_interface_names_[4], &torque_values_[1]);
+  auto torque_z = std::make_shared<hardware_interface::StateInterface>(
+    sensor_name_, fts_interface_names_[5], &torque_values_[2]);
 
   // create local state interface vector
   std::vector<hardware_interface::LoanedStateInterface> temp_state_interfaces;
@@ -131,16 +131,16 @@ TEST_F(ForceTorqueSensorTest, validate_all_with_custom_names)
   std::vector<std::string> interface_names = force_torque_sensor_->get_state_interface_names();
 
   // assign values to force.x and force.z
-  hardware_interface::StateInterface force_x{
-    sensor_name_, fts_interface_names_[0], &force_values_[0]};
-  hardware_interface::StateInterface force_z{
-    sensor_name_, fts_interface_names_[2], &force_values_[2]};
+  auto force_x = std::make_shared<hardware_interface::StateInterface>(
+    sensor_name_, fts_interface_names_[0], &force_values_[0]);
+  auto force_z = std::make_shared<hardware_interface::StateInterface>(
+    sensor_name_, fts_interface_names_[2], &force_values_[2]);
 
   // assign values to torque.y and torque.z
-  hardware_interface::StateInterface torque_y{
-    sensor_name_, fts_interface_names_[4], &torque_values_[1]};
-  hardware_interface::StateInterface torque_z{
-    sensor_name_, fts_interface_names_[5], &torque_values_[2]};
+  auto torque_y = std::make_shared<hardware_interface::StateInterface>(
+    sensor_name_, fts_interface_names_[4], &torque_values_[1]);
+  auto torque_z = std::make_shared<hardware_interface::StateInterface>(
+    sensor_name_, fts_interface_names_[5], &torque_values_[2]);
 
   // create local state interface vector
   std::vector<hardware_interface::LoanedStateInterface> temp_state_interfaces;
@@ -213,20 +213,20 @@ TEST_F(ForceTorqueSensorTest, validate_all_custom_names)
   ASSERT_EQ(force_torque_sensor_->interface_names_[5], sensor_name_ + "/" + "torque.z");
 
   // assign values to force
-  hardware_interface::StateInterface force_x{
-    sensor_name_, fts_interface_names_[0], &force_values_[0]};
-  hardware_interface::StateInterface force_y{
-    sensor_name_, fts_interface_names_[1], &force_values_[1]};
-  hardware_interface::StateInterface force_z{
-    sensor_name_, fts_interface_names_[2], &force_values_[2]};
+  auto force_x = std::make_shared<hardware_interface::StateInterface>(
+    sensor_name_, fts_interface_names_[0], &force_values_[0]);
+  auto force_y = std::make_shared<hardware_interface::StateInterface>(
+    sensor_name_, fts_interface_names_[1], &force_values_[1]);
+  auto force_z = std::make_shared<hardware_interface::StateInterface>(
+    sensor_name_, fts_interface_names_[2], &force_values_[2]);
 
   // assign values to torque
-  hardware_interface::StateInterface torque_x{
-    sensor_name_, fts_interface_names_[3], &torque_values_[0]};
-  hardware_interface::StateInterface torque_y{
-    sensor_name_, fts_interface_names_[4], &torque_values_[1]};
-  hardware_interface::StateInterface torque_z{
-    sensor_name_, fts_interface_names_[5], &torque_values_[2]};
+  auto torque_x = std::make_shared<hardware_interface::StateInterface>(
+    sensor_name_, fts_interface_names_[3], &torque_values_[0]);
+  auto torque_y = std::make_shared<hardware_interface::StateInterface>(
+    sensor_name_, fts_interface_names_[4], &torque_values_[1]);
+  auto torque_z = std::make_shared<hardware_interface::StateInterface>(
+    sensor_name_, fts_interface_names_[5], &torque_values_[2]);
 
   // create local state interface vector
   std::vector<hardware_interface::LoanedStateInterface> temp_state_interfaces;

controller_interface/test/test_imu_sensor.cpp

@@ -46,30 +46,30 @@ TEST_F(IMUSensorTest, validate_all)
   std::vector<std::string> interface_names = imu_sensor_->get_state_interface_names();
 
   // assign values to orientation
-  hardware_interface::StateInterface orientation_x{
-    sensor_name_, imu_interface_names_[0], &orientation_values_[0]};
-  hardware_interface::StateInterface orientation_y{
-    sensor_name_, imu_interface_names_[1], &orientation_values_[1]};
-  hardware_interface::StateInterface orientation_z{
-    sensor_name_, imu_interface_names_[2], &orientation_values_[2]};
-  hardware_interface::StateInterface orientation_w{
-    sensor_name_, imu_interface_names_[3], &orientation_values_[4]};
+  auto orientation_x = std::make_shared<hardware_interface::StateInterface>(
+    sensor_name_, imu_interface_names_[0], &orientation_values_[0]);
+  auto orientation_y = std::make_shared<hardware_interface::StateInterface>(
+    sensor_name_, imu_interface_names_[1], &orientation_values_[1]);
+  auto orientation_z = std::make_shared<hardware_interface::StateInterface>(
+    sensor_name_, imu_interface_names_[2], &orientation_values_[2]);
+  auto orientation_w = std::make_shared<hardware_interface::StateInterface>(
+    sensor_name_, imu_interface_names_[3], &orientation_values_[4]);
 
   // assign values to angular velocity
-  hardware_interface::StateInterface angular_velocity_x{
-    sensor_name_, imu_interface_names_[4], &angular_velocity_values_[0]};
-  hardware_interface::StateInterface angular_velocity_y{
-    sensor_name_, imu_interface_names_[5], &angular_velocity_values_[1]};
-  hardware_interface::StateInterface angular_velocity_z{
-    sensor_name_, imu_interface_names_[6], &angular_velocity_values_[2]};
+  auto angular_velocity_x = std::make_shared<hardware_interface::StateInterface>(
+    sensor_name_, imu_interface_names_[4], &angular_velocity_values_[0]);
+  auto angular_velocity_y = std::make_shared<hardware_interface::StateInterface>(
+    sensor_name_, imu_interface_names_[5], &angular_velocity_values_[1]);
+  auto angular_velocity_z = std::make_shared<hardware_interface::StateInterface>(
+    sensor_name_, imu_interface_names_[6], &angular_velocity_values_[2]);
 
   // assign values to linear acceleration
-  hardware_interface::StateInterface linear_acceleration_x{
-    sensor_name_, imu_interface_names_[7], &linear_acceleration_values_[0]};
-  hardware_interface::StateInterface linear_acceleration_y{
-    sensor_name_, imu_interface_names_[8], &linear_acceleration_values_[1]};
-  hardware_interface::StateInterface linear_acceleration_z{
-    sensor_name_, imu_interface_names_[9], &linear_acceleration_values_[2]};
+  auto linear_acceleration_x = std::make_shared<hardware_interface::StateInterface>(
+    sensor_name_, imu_interface_names_[7], &linear_acceleration_values_[0]);
+  auto linear_acceleration_y = std::make_shared<hardware_interface::StateInterface>(
+    sensor_name_, imu_interface_names_[8], &linear_acceleration_values_[1]);
+  auto linear_acceleration_z = std::make_shared<hardware_interface::StateInterface>(
+    sensor_name_, imu_interface_names_[9], &linear_acceleration_values_[2]);
 
   // create local state interface vector
   std::vector<hardware_interface::LoanedStateInterface> temp_state_interfaces;

controller_interface/test/test_semantic_component_interface.cpp

@@ -94,9 +94,12 @@ TEST_F(SemanticComponentInterfaceTest, validate_state_interfaces)
   std::vector<double> interface_values = {1.1, 3.3, 5.5};
 
   // assign 1.1 to interface_1, 3.3 to interface_3 and 5.5 to interface_5
-  hardware_interface::StateInterface interface_1{component_name_, "1", &interface_values[0]};
-  hardware_interface::StateInterface interface_3{component_name_, "3", &interface_values[1]};
-  hardware_interface::StateInterface interface_5{component_name_, "5", &interface_values[2]};
+  auto interface_1 = std::make_shared<hardware_interface::StateInterface>(
+    component_name_, "1", &interface_values[0]);
+  auto interface_3 = std::make_shared<hardware_interface::StateInterface>(
+    component_name_, "3", &interface_values[1]);
+  auto interface_5 = std::make_shared<hardware_interface::StateInterface>(
+    component_name_, "5", &interface_values[2]);
 
   // create local state interface vector
   std::vector<hardware_interface::LoanedStateInterface> temp_state_interfaces;

controller_manager/CMakeLists.txt

@@ -14,6 +14,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
   pluginlib
   rclcpp
   realtime_tools
+  std_msgs
 )
 
 find_package(ament_cmake REQUIRED)
@@ -37,7 +38,6 @@ ament_target_dependencies(controller_manager PUBLIC ${THIS_PACKAGE_INCLUDE_DEPEN
 # Causes the visibility macros to use dllexport rather than dllimport,
 # which is appropriate when building the dll but not consuming it.
 target_compile_definitions(controller_manager PRIVATE "CONTROLLER_MANAGER_BUILDING_DLL")
-
 add_executable(ros2_control_node src/ros2_control_node.cpp)
 target_link_libraries(ros2_control_node PRIVATE
   controller_manager
@@ -143,6 +143,16 @@ if(BUILD_TESTING)
   ament_target_dependencies(test_controller_manager_srvs
     controller_manager_msgs
   )
+  ament_add_gmock(test_controller_manager_urdf_passing
+  test/test_controller_manager_urdf_passing.cpp
+  )
+  target_link_libraries(test_controller_manager_urdf_passing
+    controller_manager
+    ros2_control_test_assets::ros2_control_test_assets
+  )
+  ament_target_dependencies(test_controller_manager_urdf_passing
+    controller_manager_msgs
+  )
 
   add_library(test_controller_with_interfaces SHARED
     test/test_controller_with_interfaces/test_controller_with_interfaces.cpp