Create poc for distributed control

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

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,8 @@
 #include "rclcpp/node_interfaces/node_logging_interface.hpp"
 #include "rclcpp/node_interfaces/node_parameters_interface.hpp"
 #include "rclcpp/parameter.hpp"
-
+#include "rclcpp/rclcpp.hpp"
+#include "std_msgs/msg/string.hpp"
 namespace controller_manager
 {
 using ControllersListIterator = std::vector<controller_manager::ControllerSpec>::const_iterator;
@@ -60,6 +62,8 @@ rclcpp::NodeOptions get_cm_node_options();
 
 class ControllerManager : public rclcpp::Node
 {
+  enum class controller_manager_type : std::uint8_t;
+
 public:
   static constexpr bool kWaitForAllResources = false;
   static constexpr auto kInfiniteTimeout = 0;
@@ -82,6 +86,12 @@ class ControllerManager : public rclcpp::Node
   CONTROLLER_MANAGER_PUBLIC
   virtual ~ControllerManager() = default;
 
+  CONTROLLER_MANAGER_PUBLIC
+  void subscribe_to_robot_description_topic();
+
+  CONTROLLER_MANAGER_PUBLIC
+  void robot_description_callback(const std_msgs::msg::String & msg);
+
   CONTROLLER_MANAGER_PUBLIC
   void init_resource_manager(const std::string & robot_description);
 
@@ -190,10 +200,50 @@ class ControllerManager : public rclcpp::Node
   CONTROLLER_MANAGER_PUBLIC
   unsigned int get_update_rate() const;
 
+  // Per controller update rate support
+  CONTROLLER_MANAGER_PUBLIC
+  bool use_multiple_nodes() const;
+
+  // Per controller update rate support
+  CONTROLLER_MANAGER_PUBLIC
+  std::chrono::milliseconds distributed_interfaces_publish_period() const;
+
 protected:
   CONTROLLER_MANAGER_PUBLIC
   void init_services();
 
+  CONTROLLER_MANAGER_PUBLIC
+  void get_and_initialize_distributed_parameters();
+
+  CONTROLLER_MANAGER_PUBLIC
+  controller_manager_type determine_controller_manager_type();
+
+  CONTROLLER_MANAGER_PUBLIC
+  void configure_controller_manager(const controller_manager_type & cm_type);
+
+  CONTROLLER_MANAGER_PUBLIC
+  void init_distributed_sub_controller_manager();
+
+  CONTROLLER_MANAGER_PUBLIC
+  void init_distributed_central_controller_manager();
+
+  CONTROLLER_MANAGER_PUBLIC void init_distributed_central_controller_manager_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 create_hardware_state_publishers();
+
+  CONTROLLER_MANAGER_PUBLIC
+  void create_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 +449,25 @@ class ControllerManager : public rclcpp::Node
    */
   rclcpp::CallbackGroup::SharedPtr best_effort_callback_group_;
 
+  enum class controller_manager_type : std::uint8_t
+  {
+    standard_controller_manager,
+    distributed_central_controller_manager,
+    distributed_sub_controller_manager,
+    unkown_type  // indicating something went wrong and type could not be determined
+  };
+
+  bool distributed_ = false;
+  bool sub_controller_manager_ = false;
+  bool use_multiple_nodes_ = false;
+  // TODO(Manuel): weak_ptr would probably be a better choice. This way has to be checked
+  // if pointer points to an object. Don't like the nullptr thing and implicit checks
+  // associated with it ... (create on distributed Handles and StatePublisher/CommandForwarder)
+  // needs to be checked if is nullptr before usage
+  std::shared_ptr<rclcpp_lifecycle::LifecycleNode> distributed_pub_sub_node_ = nullptr;
+  std::chrono::milliseconds distributed_interfaces_publish_period_ = std::chrono::milliseconds(12);
+
+  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,11 +565,18 @@ 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_,
     deactivate_command_interface_request_;
 
+  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr robot_description_subscription_;
+
   struct SwitchParams
   {
     bool do_switch = {false};

controller_manager/package.xml

@@ -26,6 +26,7 @@
   <depend>ros2_control_test_assets</depend>
   <depend>ros2param</depend>
   <depend>ros2run</depend>
+  <depend>std_msgs</depend>
 
   <test_depend>ament_cmake_gmock</test_depend>
   <test_depend>ros2_control_test_assets</test_depend>