Support multiple hardware interfaces in the coordinator (#72)

evan-palmer · web-flow · commit 734f1a0ffdd4 · 2025-07-30T20:30:35.000-07:00
* add support for multiple hardware interfaces

* changelog

* make hardware activation synchronous

* Fixed coordinator hardware bug
diff --git a/auv_control_demos/CHANGELOG.md b/auv_control_demos/CHANGELOG.md
@@ -1,5 +1,7 @@
 # Changelog for package auv_control_demos
 
+## 0.3.3 (2025-07-29)
+
 ## 0.3.2 (2025-07-22)
 
 ## 0.3.1 (2025-07-09)
diff --git a/auv_control_demos/package.xml b/auv_control_demos/package.xml
@@ -3,7 +3,7 @@
 <package format="3">
 
   <name>auv_control_demos</name>
-  <version>0.3.2</version>
+  <version>0.3.3</version>
   <description>Example package that includes demos for using auv_controllers in individual and chained modes</description>
 
   <maintainer email="mitchcol@oregonstate.edu">Colin Mitchell</maintainer>
diff --git a/auv_control_msgs/CHANGELOG.md b/auv_control_msgs/CHANGELOG.md
@@ -1,5 +1,7 @@
 # Changelog for package auv_control_msgs
 
+## 0.3.3 (2025-07-29)
+
 ## 0.3.2 (2025-07-22)
 
 ## 0.3.1 (2025-07-09)
diff --git a/auv_control_msgs/package.xml b/auv_control_msgs/package.xml
@@ -3,7 +3,7 @@
 <package format="3">
 
   <name>auv_control_msgs</name>
-  <version>0.3.2</version>
+  <version>0.3.3</version>
   <description>Custom messages for AUV controllers</description>
 
   <maintainer email="rakeshvivek97@gmail.com">Rakesh Vivekanandan</maintainer>
diff --git a/auv_controllers/CHANGELOG.md b/auv_controllers/CHANGELOG.md
@@ -1,5 +1,7 @@
 # Changelog for package auv_controllers
 
+## 0.3.3 (2025-07-29)
+
 ## 0.3.2 (2025-07-22)
 
 ## 0.3.1 (2025-07-09)
diff --git a/auv_controllers/package.xml b/auv_controllers/package.xml
@@ -3,7 +3,7 @@
 <package format="3">
 
   <name>auv_controllers</name>
-  <version>0.3.2</version>
+  <version>0.3.3</version>
   <description>Meta package for auv_controllers</description>
 
   <maintainer email="evanp922@gmail.com">Evan Palmer</maintainer>
diff --git a/controller_common/CHANGELOG.md b/controller_common/CHANGELOG.md
@@ -1,5 +1,7 @@
 # Changelog for package controller_common
 
+## 0.3.3 (2025-07-29)
+
 ## 0.3.2 (2025-07-22)
 
 ## 0.3.1 (2025-07-09)
diff --git a/controller_common/package.xml b/controller_common/package.xml
@@ -3,7 +3,7 @@
 <package format="3">
 
   <name>controller_common</name>
-  <version>0.3.2</version>
+  <version>0.3.3</version>
   <description>Common interfaces for controllers used in this project</description>
 
   <maintainer email="evanp922@gmail.com">Evan Palmer</maintainer>
diff --git a/controller_coordinator/CHANGELOG.md b/controller_coordinator/CHANGELOG.md
@@ -1,5 +1,9 @@
 # Changelog for package controller_coordinator
 
+## 0.3.3 (2025-07-29)
+
+- Enabled activating and deactivating multiple hardware interfaces.
+
 ## 0.3.2 (2025-07-22)
 
 ## 0.3.1 (2025-07-09)
diff --git a/controller_coordinator/package.xml b/controller_coordinator/package.xml
@@ -3,7 +3,7 @@
 <package format="3">
 
   <name>controller_coordinator</name>
-  <version>0.3.2</version>
+  <version>0.3.3</version>
   <description>A high-level node used to load and activate/deactivate control systems</description>
 
   <maintainer email="evanp922@gmail.com">Evan Palmer</maintainer>
diff --git a/controller_coordinator/src/coordinator.cpp b/controller_coordinator/src/coordinator.cpp
@@ -29,10 +29,8 @@ namespace coordinator
 
 ControllerCoordinator::ControllerCoordinator()
 : rclcpp::Node("controller_coordinator"),
-  activate_hardware_request_(std::make_shared<controller_manager_msgs::srv::SetHardwareComponentState::Request>()),
-  deactivate_hardware_request_(std::make_shared<controller_manager_msgs::srv::SetHardwareComponentState::Request>()),
-  activate_controllers_request_(std::make_shared<controller_manager_msgs::srv::SwitchController::Request>()),
-  deactivate_controllers_request_(std::make_shared<controller_manager_msgs::srv::SwitchController::Request>())
+  activate_controllers_request_(std::make_shared<ControllerRequest>()),
+  deactivate_controllers_request_(std::make_shared<ControllerRequest>())
 {
   param_listener_ = std::make_shared<controller_coordinator::ParamListener>(this->get_node_parameters_interface());
   params_ = param_listener_->get_params();
@@ -60,11 +58,23 @@ ControllerCoordinator::ControllerCoordinator()
   wait_for_service(switch_controller_client_, switch_controller_name);
 
   // pre-configure the hardware activation/deactivation requests
-  activate_hardware_request_->name = params_.hardware_interface;
-  activate_hardware_request_->target_state.id = lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE;
-
-  deactivate_hardware_request_->name = params_.hardware_interface;
-  deactivate_hardware_request_->target_state.id = lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE;
+  activate_hardware_requests_.reserve(params_.hardware_interfaces.size());
+  std::ranges::transform(
+    params_.hardware_interfaces, std::back_inserter(activate_hardware_requests_), [](const std::string & name) {
+      auto request = std::make_shared<HardwareRequest>();
+      request->name = name;
+      request->target_state.id = lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE;
+      return request;
+    });
+
+  deactivate_hardware_requests_.reserve(params_.hardware_interfaces.size());
+  std::ranges::transform(
+    params_.hardware_interfaces, std::back_inserter(deactivate_hardware_requests_), [](const std::string & name) {
+      auto request = std::make_shared<HardwareRequest>();
+      request->name = name;
+      request->target_state.id = lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE;
+      return request;
+    });
 
   // pre-configure the controller activation/deactivation requests
   activate_controllers_request_->activate_controllers = params_.controller_sequence;
@@ -87,24 +97,23 @@ ControllerCoordinator::ControllerCoordinator()
       const std::shared_ptr<std_srvs::srv::SetBool::Response> response) {  // NOLINT
       response->success = true;
       if (request->data) {
-        RCLCPP_INFO(this->get_logger(), "Activating thruster hardware interface and controllers");  // NOLINT
-
-        // activate the hardware interface
-        hardware_client_->async_send_request(
-          activate_hardware_request_,
-          [this, response](rclcpp::Client<controller_manager_msgs::srv::SetHardwareComponentState>::SharedFuture
-                             result_response) {  // NOLINT
-            const auto & result = result_response.get();
-            if (result->ok) {
-              RCLCPP_INFO(this->get_logger(), "Successfully activated thruster hardware interface");  // NOLINT
-            } else {
-              RCLCPP_ERROR(this->get_logger(), "Failed to activate thruster hardware interface");  // NOLINT
-              response->success = false;
-              response->message = "Failed to activate thruster hardware interface";
-            }
-          });
-
-        // activate the controllers
+        RCLCPP_INFO(this->get_logger(), "Activating hardware interfaces and controllers");  // NOLINT
+        for (const auto & activate_request : activate_hardware_requests_) {
+          hardware_client_->async_send_request(
+            activate_request,
+            [logger = this->get_logger(), response, activate_request](
+              rclcpp::Client<controller_manager_msgs::srv::SetHardwareComponentState>::SharedFuture
+                result_response) {  // NOLINT
+              const auto & result = result_response.get();
+              if (result->ok) {
+                RCLCPP_INFO(logger, "Successfully activated %s", activate_request->name.c_str());  // NOLINT
+              } else {
+                RCLCPP_ERROR(logger, "Failed to activate %s", activate_request->name.c_str());  // NOLINT
+                response->success = false;
+                response->message = "Failed to activate " + activate_request->name;
+              }
+            });
+        }
         switch_controller_client_->async_send_request(
           activate_controllers_request_,
           [this,
@@ -120,24 +129,25 @@ ControllerCoordinator::ControllerCoordinator()
             }
           });
       } else {
-        RCLCPP_INFO(this->get_logger(), "Deactivating controllers and thruster hardware interface");  // NOLINT
-
-        // deactivate the hardware interface
-        hardware_client_->async_send_request(
-          deactivate_hardware_request_,
-          [this, response](rclcpp::Client<controller_manager_msgs::srv::SetHardwareComponentState>::SharedFuture
-                             result_response) {  // NOLINT
-            const auto & result = result_response.get();
-            if (result->ok) {
-              RCLCPP_INFO(this->get_logger(), "Successfully deactivated thruster hardware interface");  // NOLINT
-            } else {
-              RCLCPP_ERROR(this->get_logger(), "Failed to deactivate thruster hardware interface");  // NOLINT
-              response->success = false;
-              response->message = "Failed to deactivate thruster hardware interface";
-            }
-          });
-
-        // deactivate the controllers
+        RCLCPP_INFO(this->get_logger(), "Deactivating controllers and hardware interfaces");  // NOLINT
+        for (const auto & deactivate_request : deactivate_hardware_requests_) {
+          hardware_client_->async_send_request(
+            deactivate_request,
+            [this, response, deactivate_request](
+              rclcpp::Client<controller_manager_msgs::srv::SetHardwareComponentState>::SharedFuture
+                result_response) {  // NOLINT
+              const auto & result = result_response.get();
+              if (result->ok) {
+                RCLCPP_INFO(
+                  this->get_logger(), "Successfully deactivated %s", deactivate_request->name.c_str());  // NOLINT
+              } else {
+                RCLCPP_ERROR(
+                  this->get_logger(), "Failed to deactivate %s", deactivate_request->name.c_str());  // NOLINT
+                response->success = false;
+                response->message = "Failed to deactivate " + deactivate_request->name;
+              }
+            });
+        }
         switch_controller_client_->async_send_request(
           deactivate_controllers_request_,
           [this,
diff --git a/controller_coordinator/src/coordinator.hpp b/controller_coordinator/src/coordinator.hpp
@@ -44,7 +44,7 @@ class ControllerCoordinator : public rclcpp::Node
 
 private:
   // we need clients to:
-  // 1. activate/deactivate the hardware
+  // 1. activate/deactivate the hardware interfaces
   // 2. activate/deactivate the controllers
   std::shared_ptr<rclcpp::Client<controller_manager_msgs::srv::SetHardwareComponentState>> hardware_client_;
   std::shared_ptr<rclcpp::Client<controller_manager_msgs::srv::SwitchController>> switch_controller_client_;
@@ -54,10 +54,13 @@ class ControllerCoordinator : public rclcpp::Node
 
   // pre-configure the activate/deactivate service messages
   // these won't change, so we can set them up once
-  std::shared_ptr<controller_manager_msgs::srv::SetHardwareComponentState::Request> activate_hardware_request_;
-  std::shared_ptr<controller_manager_msgs::srv::SetHardwareComponentState::Request> deactivate_hardware_request_;
-  std::shared_ptr<controller_manager_msgs::srv::SwitchController::Request> activate_controllers_request_;
-  std::shared_ptr<controller_manager_msgs::srv::SwitchController::Request> deactivate_controllers_request_;
+  using HardwareRequest = controller_manager_msgs::srv::SetHardwareComponentState::Request;
+  std::vector<std::shared_ptr<HardwareRequest>> activate_hardware_requests_;
+  std::vector<std::shared_ptr<HardwareRequest>> deactivate_hardware_requests_;
+
+  using ControllerRequest = controller_manager_msgs::srv::SwitchController::Request;
+  std::shared_ptr<ControllerRequest> activate_controllers_request_;
+  std::shared_ptr<ControllerRequest> deactivate_controllers_request_;
 
   std::shared_ptr<controller_coordinator::ParamListener> param_listener_;
   controller_coordinator::Params params_;
diff --git a/controller_coordinator/src/coordinator_parameters.yaml b/controller_coordinator/src/coordinator_parameters.yaml
@@ -1,8 +1,8 @@
 controller_coordinator:
-  hardware_interface:
-    type: string
-    description: The name of the hardware interface to activate/deactivate.
-    default_value: ""
+  hardware_interfaces:
+    type: string_array
+    description: A list of hardware interfaces to activate/deactivate.
+    default_value: []
 
   controller_sequence:
     type: string_array
diff --git a/end_effector_trajectory_controller/CHANGELOG.md b/end_effector_trajectory_controller/CHANGELOG.md
@@ -1,4 +1,6 @@
-# Changelog for package controller_common
+# Changelog for package end_effector_trajectory_controller
+
+## 0.3.3 (2025-07-29)
 
 ## 0.3.2 (2025-07-22)
 
diff --git a/end_effector_trajectory_controller/package.xml b/end_effector_trajectory_controller/package.xml
@@ -3,7 +3,7 @@
 <package format="3">
 
   <name>end_effector_trajectory_controller</name>
-  <version>0.3.2</version>
+  <version>0.3.3</version>
   <description>End effector trajectory tracking controller for UVMS control</description>
 
   <maintainer email="evanp922@gmail.com">Evan Palmer</maintainer>
diff --git a/ik_solvers/CHANGELOG.md b/ik_solvers/CHANGELOG.md
@@ -1,5 +1,7 @@
 # Changelog for package ik_solvers
 
+## 0.3.3 (2025-07-29)
+
 ## 0.3.2 (2025-07-22)
 
 ## 0.3.1 (2025-07-09)
diff --git a/ik_solvers/package.xml b/ik_solvers/package.xml
@@ -3,7 +3,7 @@
 <package format="3">
 
   <name>ik_solvers</name>
-  <version>0.3.2</version>
+  <version>0.3.3</version>
   <description>Inverse kinematics solvers used for whole-body control</description>
 
   <maintainer email="evanp922@gmail.com">Evan Palmer</maintainer>
diff --git a/thruster_allocation_matrix_controller/CHANGELOG.md b/thruster_allocation_matrix_controller/CHANGELOG.md
@@ -1,5 +1,7 @@
 # Changelog for package thruster_allocation_matrix_controller
 
+## 0.3.3 (2025-07-29)
+
 ## 0.3.2 (2025-07-22)
 
 - Replaces the deprecated `unlockAndPublish` API with `try_publish`
diff --git a/thruster_allocation_matrix_controller/package.xml b/thruster_allocation_matrix_controller/package.xml
@@ -3,7 +3,7 @@
 <package format="3">
 
   <name>thruster_allocation_matrix_controller</name>
-  <version>0.3.2</version>
+  <version>0.3.3</version>
   <description>Thruster allocation matrix controller used to convert wrench commands into thrust commands</description>
 
   <maintainer email="evanp922@gmail.com">Evan Palmer</maintainer>
diff --git a/thruster_controllers/CHANGELOG.md b/thruster_controllers/CHANGELOG.md
@@ -1,5 +1,7 @@
 # Changelog for package thruster_controllers
 
+## 0.3.3 (2025-07-29)
+
 ## 0.3.2 (2025-07-22)
 
 - Replaces the deprecated `unlockAndPublish` API with `try_publish`
diff --git a/thruster_controllers/package.xml b/thruster_controllers/package.xml
@@ -3,7 +3,7 @@
 <package format="3">
 
   <name>thruster_controllers</name>
-  <version>0.3.2</version>
+  <version>0.3.3</version>
   <description>A collection of thruster controllers for AUV control</description>
 
   <maintainer email="evanp922@gmail.com">Evan Palmer</maintainer>
diff --git a/topic_sensors/CHANGELOG.md b/topic_sensors/CHANGELOG.md
@@ -1,5 +1,7 @@
 # Changelog for package topic_sensors
 
+## 0.3.3 (2025-07-29)
+
 ## 0.3.2 (2025-07-22)
 
 ## 0.3.1 (2025-07-09)
diff --git a/topic_sensors/package.xml b/topic_sensors/package.xml
@@ -3,7 +3,7 @@
 <package format="3">
 
   <name>topic_sensors</name>
-  <version>0.3.2</version>
+  <version>0.3.3</version>
   <description>Sensor plugins used to write ROS 2 messages to state interfaces</description>
 
   <maintainer email="evanp922@gmail.com">Evan Palmer</maintainer>
diff --git a/velocity_controllers/CHANGELOG.md b/velocity_controllers/CHANGELOG.md
@@ -1,5 +1,7 @@
 # Changelog for package velocity_controllers
 
+## 0.3.3 (2025-07-29)
+
 ## 0.3.2 (2025-07-22)
 
 - Replaces the deprecated `unlockAndPublish` API with `try_publish`
diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml
@@ -3,7 +3,7 @@
 <package format="3">
 
   <name>velocity_controllers</name>
-  <version>0.3.2</version>
+  <version>0.3.3</version>
   <description>A collection of velocity controllers for underwater vehicles</description>
 
   <maintainer email="evanp922@gmail.com">Evan Palmer</maintainer>
diff --git a/whole_body_controllers/CHANGELOG.md b/whole_body_controllers/CHANGELOG.md
@@ -1,5 +1,7 @@
 # Changelog for package whole_body_controllers
 
+## 0.3.3 (2025-07-29)
+
 ## 0.3.2 (2025-07-22)
 
 - Replaces the deprecated `unlockAndPublish` API with `try_publish`
diff --git a/whole_body_controllers/package.xml b/whole_body_controllers/package.xml
@@ -3,7 +3,7 @@
 <package format="3">
 
   <name>whole_body_controllers</name>
-  <version>0.3.2</version>
+  <version>0.3.3</version>
   <description>Whole-body controllers for underwater vehicle manipulator systems</description>
 
   <maintainer email="evanp922@gmail.com">Evan Palmer</maintainer>
