Create poc for distributed control

Author: mamueluth

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

@@ -37,7 +37,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

controller_manager/include/controller_manager/controller_manager.hpp

@@ -35,6 +35,7 @@
 #include "controller_manager_msgs/srv/list_hardware_components.hpp"
 #include "controller_manager_msgs/srv/list_hardware_interfaces.hpp"
 #include "controller_manager_msgs/srv/load_controller.hpp"
+#include "controller_manager_msgs/srv/register_sub_controller_manager.hpp"
 #include "controller_manager_msgs/srv/reload_controller_libraries.hpp"
 #include "controller_manager_msgs/srv/set_hardware_component_state.hpp"
 #include "controller_manager_msgs/srv/switch_controller.hpp"
@@ -51,7 +52,6 @@
 #include "rclcpp/node_interfaces/node_logging_interface.hpp"
 #include "rclcpp/node_interfaces/node_parameters_interface.hpp"
 #include "rclcpp/parameter.hpp"
-
 namespace controller_manager
 {
 using ControllersListIterator = std::vector<controller_manager::ControllerSpec>::const_iterator;
@@ -194,6 +194,27 @@ class ControllerManager : public rclcpp::Node
   CONTROLLER_MANAGER_PUBLIC
   void init_services();
 
+  CONTROLLER_MANAGER_PUBLIC
+  void configure_controller_manager();
+
+  CONTROLLER_MANAGER_PUBLIC
+  void init_distributed_main_controller_services();
+
+  CONTROLLER_MANAGER_PUBLIC
+  void register_sub_controller_manager_srv_cb(
+    const std::shared_ptr<controller_manager_msgs::srv::RegisterSubControllerManager::Request>
+      request,
+    std::shared_ptr<controller_manager_msgs::srv::RegisterSubControllerManager::Response> response);
+
+  CONTROLLER_MANAGER_PUBLIC
+  void add_hardware_state_publishers();
+
+  CONTROLLER_MANAGER_PUBLIC
+  void add_hardware_command_forwarders();
+
+  CONTROLLER_MANAGER_PUBLIC
+  void register_sub_controller_manager();
+
   CONTROLLER_MANAGER_PUBLIC
   controller_interface::ControllerInterfaceBaseSharedPtr add_controller_impl(
     const ControllerSpec & controller);
@@ -399,6 +420,10 @@ class ControllerManager : public rclcpp::Node
    */
   rclcpp::CallbackGroup::SharedPtr best_effort_callback_group_;
 
+  bool distributed_ = false;
+  bool sub_controller_manager_ = false;
+
+  rclcpp::CallbackGroup::SharedPtr distributed_system_srv_callback_group_;
   /**
    * The RTControllerListWrapper class wraps a double-buffered list of controllers
    * to avoid needing to lock the real-time thread when switching controllers in
@@ -496,6 +521,11 @@ class ControllerManager : public rclcpp::Node
   rclcpp::Service<controller_manager_msgs::srv::SetHardwareComponentState>::SharedPtr
     set_hardware_component_state_service_;
 
+  // services for distributed control
+  std::mutex central_controller_manager_srv_lock_;
+  rclcpp::Service<controller_manager_msgs::srv::RegisterSubControllerManager>::SharedPtr
+    register_sub_controller_manager_srv_;
+
   std::vector<std::string> activate_request_, deactivate_request_;
   std::vector<std::string> to_chained_mode_request_, from_chained_mode_request_;
   std::vector<std::string> activate_command_interface_request_,