Robotic-Decision-Making-Lab · evan-palmer · Apr 14, 2026 · Mar 30, 2026 · Mar 30, 2026 · Mar 30, 2026
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.4.2 (2026-03-30)
+
 ## 0.4.1 (2026-02-23)
 
 ## 0.4.0 (2025-08-01)

diff --git a/auv_control_demos/package.xml b/auv_control_demos/package.xml
@@ -3,12 +3,9 @@
 <package format="3">
 
   <name>auv_control_demos</name>
-  <version>0.4.1</version>
+  <version>0.4.2</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>
-  <maintainer email="everardo.a.gonzalez@gmail.com">Everardo Gonzalez</maintainer>
-  <maintainer email="rakeshvivek97@gmail.com">Rakesh Vivekanandan</maintainer>
   <maintainer email="evanp922@gmail.com">Evan Palmer</maintainer>
   <license>MIT</license>
 

diff --git a/auv_control_msgs/CHANGELOG.md b/auv_control_msgs/CHANGELOG.md
@@ -1,5 +1,9 @@
 # Changelog for package auv_control_msgs
 
+## 0.4.2 (2026-03-30)
+
+- Implements the ImpedanceStateStamped message.
+
 ## 0.4.1 (2026-02-23)
 
 ## 0.4.0 (2025-08-01)

diff --git a/auv_control_msgs/CMakeLists.txt b/auv_control_msgs/CMakeLists.txt
@@ -16,6 +16,7 @@ rosidl_generate_interfaces(auv_control_msgs
   "msg/CartesianTrajectoryControllerStateStamped.msg"
   "msg/ImpedanceCommand.msg"
   "action/FollowCartesianTrajectory.action"
+  "msg/ImpedanceStateStamped.msg"
   DEPENDENCIES builtin_interfaces std_msgs geometry_msgs trajectory_msgs
 )
 

diff --git a/auv_control_msgs/msg/ImpedanceCommand.msg b/auv_control_msgs/msg/ImpedanceCommand.msg
@@ -1,9 +1,13 @@
 std_msgs/Header header
 
+# The frame ID of the velocity and force/torque setpoints.
 string child_frame_id
 
+# The configuration setpoint.
 geometry_msgs/Pose pose
 
+# The velocity setpoint.
 geometry_msgs/Twist twist
 
+# The force/torque setpoint.
 geometry_msgs/Wrench wrench
diff --git a/auv_control_msgs/msg/ImpedanceStateStamped.msg b/auv_control_msgs/msg/ImpedanceStateStamped.msg
@@ -0,0 +1,30 @@
+# This message represents the state of the impedance controller.
+
+std_msgs/Header header
+
+# The configuration setpoint.
+geometry_msgs/Pose reference_pose
+
+# The velocity setpoint.
+geometry_msgs/Twist reference_twist
+
+# The force/torque feed-forward setpoint.
+geometry_msgs/Wrench reference_wrench
+
+# The measured configuration.
+geometry_msgs/Pose feedback_pose
+
+# The measured velocity.
+geometry_msgs/Twist feedback_twist
+
+# The vector representation of the left-trivialized error.
+geometry_msgs/Twist error_pose
+
+# The velocity error defined in the child frame ID.
+geometry_msgs/Twist error_twist
+
+# Time between two consecutive updates/execution of the control law.
+float64 time_step
+
+# The calculated control output.
+geometry_msgs/Wrench output
diff --git a/auv_control_msgs/package.xml b/auv_control_msgs/package.xml
@@ -3,10 +3,9 @@
 <package format="3">
 
   <name>auv_control_msgs</name>
-  <version>0.4.1</version>
+  <version>0.4.2</version>
   <description>Custom messages for AUV controllers</description>
 
-  <maintainer email="rakeshvivek97@gmail.com">Rakesh Vivekanandan</maintainer>
   <maintainer email="evanp922@gmail.com">Evan Palmer</maintainer>
   <license>MIT</license>
 

diff --git a/auv_controllers/CHANGELOG.md b/auv_controllers/CHANGELOG.md
@@ -1,5 +1,11 @@
 # Changelog for package auv_controllers
 
+## 0.4.2 (2026-03-30)
+
+- Implements the ImpedanceStateStamped message
+- Fixes controller coordinator hardware interface activation
+- Fixes impedance controller memory leak and miscellaneous bugs
+
 ## 0.4.1 (2026-02-23)
 
 ## 0.4.0 (2025-08-01)

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.4.1</version>
+  <version>0.4.2</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.4.2 (2026-03-30)
+
 ## 0.4.1 (2026-02-23)
 
 ## 0.4.0 (2025-08-01)

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.4.1</version>
+  <version>0.4.2</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.4.2 (2026-03-30)
+
+- Fixes hardware activation bug
+
 ## 0.4.1 (2026-02-23)
 
 ## 0.4.0 (2025-08-01)

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.4.1</version>
+  <version>0.4.2</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
@@ -20,6 +20,7 @@
 
 #include "coordinator.hpp"
 
+#include <chrono>
 #include <ranges>
 
 #include "lifecycle_msgs/msg/state.hpp"
@@ -39,7 +40,7 @@ ControllerCoordinator::ControllerCoordinator()
 
   // helper function used to wait for services to come up
   // this will block indefinitely
-  auto wait_for_service = [this](const auto & client, const std::string & service_name) {
+  auto wait_for_service = [this](const auto & client, const std::string & service_name) -> void {
     while (!client->wait_for_service(std::chrono::seconds(1))) {
       RCLCPP_INFO(this->get_logger(), "Waiting for %s service to come up", service_name.c_str());  // NOLINT
     }
@@ -60,7 +61,9 @@ ControllerCoordinator::ControllerCoordinator()
   // pre-configure the hardware activation/deactivation requests
   activate_hardware_requests_.reserve(params_.hardware_interfaces.size());
   std::ranges::transform(
-    params_.hardware_interfaces, std::back_inserter(activate_hardware_requests_), [](const std::string & name) {
+    params_.hardware_interfaces,
+    std::back_inserter(activate_hardware_requests_),
+    [](const std::string & name) -> std::shared_ptr<HardwareRequest> {
       auto request = std::make_shared<HardwareRequest>();
       request->name = name;
       request->target_state.id = lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE;
@@ -69,7 +72,9 @@ ControllerCoordinator::ControllerCoordinator()
 
   deactivate_hardware_requests_.reserve(params_.hardware_interfaces.size());
   std::ranges::transform(
-    params_.hardware_interfaces, std::back_inserter(deactivate_hardware_requests_), [](const std::string & name) {
+    params_.hardware_interfaces,
+    std::back_inserter(deactivate_hardware_requests_),
+    [](const std::string & name) -> std::shared_ptr<HardwareRequest> {
       auto request = std::make_shared<HardwareRequest>();
       request->name = name;
       request->target_state.id = lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE;
@@ -92,18 +97,19 @@ ControllerCoordinator::ControllerCoordinator()
   activate_system_service_ = this->create_service<std_srvs::srv::SetBool>(
     "~/activate",
     [this](
-      const std::shared_ptr<rmw_request_id_t> /*request_header*/,          // NOLINT
-      const std::shared_ptr<std_srvs::srv::SetBool::Request> request,      // NOLINT
-      const std::shared_ptr<std_srvs::srv::SetBool::Response> response) {  // NOLINT
+      const std::shared_ptr<rmw_request_id_t> /*request_header*/,                  // NOLINT
+      const std::shared_ptr<std_srvs::srv::SetBool::Request> request,              // NOLINT
+      const std::shared_ptr<std_srvs::srv::SetBool::Response> response) -> void {  // NOLINT
       response->success = true;
       if (request->data) {
         RCLCPP_INFO(this->get_logger(), "Activating hardware interfaces and controllers");  // NOLINT
         for (const auto & activate_request : activate_hardware_requests_) {
-          hardware_client_->async_send_request(
+          auto future = 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
+                result_response)  // NOLINT
+            -> void {
               const auto & result = result_response.get();
               if (result->ok) {
                 RCLCPP_INFO(logger, "Successfully activated %s", activate_request->name.c_str());  // NOLINT
@@ -113,12 +119,28 @@ ControllerCoordinator::ControllerCoordinator()
                 response->message = "Failed to activate " + activate_request->name;
               }
             });
+
+          const std::future_status future_result = future.wait_for(std::chrono::duration<double>(params_.timeout));
+          if (future_result != std::future_status::ready) {
+            RCLCPP_ERROR(this->get_logger(), "Timeout while activating %s", activate_request->name.c_str());  // NOLINT
+            response->success = false;
+            response->message = "Timeout while activating " + activate_request->name;
+          }
+
+          if (!response->success) {
+            RCLCPP_ERROR(  // NOLINT
+              this->get_logger(),
+              "Aborting activation of remaining hardware interfaces and controllers");
+            return;
+          }
         }
+
         switch_controller_client_->async_send_request(
           activate_controllers_request_,
           [this,
            response](
-            rclcpp::Client<controller_manager_msgs::srv::SwitchController>::SharedFuture result_response) {  // NOLINT
+            rclcpp::Client<controller_manager_msgs::srv::SwitchController>::SharedFuture result_response)  // NOLINT
+          -> void {
             const auto & result = result_response.get();
             if (result->ok) {
               RCLCPP_INFO(this->get_logger(), "Successfully activated controllers");  // NOLINT
@@ -131,28 +153,33 @@ ControllerCoordinator::ControllerCoordinator()
       } else {
         RCLCPP_INFO(this->get_logger(), "Deactivating controllers and hardware interfaces");  // NOLINT
         for (const auto & deactivate_request : deactivate_hardware_requests_) {
-          hardware_client_->async_send_request(
+          auto future = hardware_client_->async_send_request(
             deactivate_request,
             [this, response, deactivate_request](
               rclcpp::Client<controller_manager_msgs::srv::SetHardwareComponentState>::SharedFuture
-                result_response) {  // NOLINT
+                result_response)  // NOLINT
+            -> void {
               const auto & result = result_response.get();
               if (result->ok) {
-                RCLCPP_INFO(
-                  this->get_logger(), "Successfully deactivated %s", deactivate_request->name.c_str());  // NOLINT
+                RCLCPP_INFO(  // NOLINT
+                  this->get_logger(),
+                  "Successfully deactivated %s",
+                  deactivate_request->name.c_str());
               } else {
-                RCLCPP_ERROR(
-                  this->get_logger(), "Failed to deactivate %s", deactivate_request->name.c_str());  // NOLINT
+                RCLCPP_ERROR(  // NOLINT
+                  this->get_logger(),
+                  "Failed to deactivate %s",
+                  deactivate_request->name.c_str());
                 response->success = false;
                 response->message = "Failed to deactivate " + deactivate_request->name;
               }
             });
         }
         switch_controller_client_->async_send_request(
           deactivate_controllers_request_,
-          [this,
-           response](
-            rclcpp::Client<controller_manager_msgs::srv::SwitchController>::SharedFuture result_response) {  // NOLINT
+          [this, response](
+            rclcpp::Client<controller_manager_msgs::srv::SwitchController>::SharedFuture result_response)  // NOLINT
+          -> void {
             const auto & result = result_response.get();
             if (result->ok) {
               RCLCPP_INFO(this->get_logger(), "Successfully deactivated controllers");  // NOLINT

diff --git a/ik_solvers/CHANGELOG.md b/ik_solvers/CHANGELOG.md
@@ -1,5 +1,7 @@
 # Changelog for package ik_solvers
 
+## 0.4.2 (2026-03-30)
+
 ## 0.4.1 (2026-02-23)
 
 ## 0.4.0 (2025-08-01)

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.4.1</version>
+  <version>0.4.2</version>
   <description>Inverse kinematics solvers used for whole-body control</description>
 
   <maintainer email="evanp922@gmail.com">Evan Palmer</maintainer>

diff --git a/impedance_controller/CHANGELOG.md b/impedance_controller/CHANGELOG.md
@@ -1,5 +1,10 @@
 # Changelog for package impedance_controller
 
+## 0.4.2 (2026-03-30)
+
+- Addresses various bugs in implementation
+- Replaces MultiDOFStateStamped message with ImpedanceStateStamped message
+
 ## 0.4.1 (2026-02-23)
 
 - Addresses upstream deprecation of the `tf2_ros/buffer.h` and

diff --git a/impedance_controller/include/impedance_controller/impedance_controller.hpp b/impedance_controller/include/impedance_controller/impedance_controller.hpp
@@ -23,7 +23,7 @@
 #include <Eigen/Dense>
 
 #include "auv_control_msgs/msg/impedance_command.hpp"
-#include "control_msgs/msg/multi_dof_state_stamped.hpp"
+#include "auv_control_msgs/msg/impedance_state_stamped.hpp"
 #include "controller_interface/chainable_controller_interface.hpp"
 #include "controller_interface/controller_interface.hpp"
 #include "hydrodynamics/hydrodynamics.hpp"
@@ -84,7 +84,7 @@ class ImpedanceController : public controller_interface::ChainableControllerInte
   std::shared_ptr<rclcpp::Subscription<nav_msgs::msg::Odometry>> system_state_sub_;
   std::vector<double> system_state_values_;
 
-  using ControllerState = control_msgs::msg::MultiDOFStateStamped;
+  using ControllerState = auv_control_msgs::msg::ImpedanceStateStamped;
   std::shared_ptr<rclcpp::Publisher<ControllerState>> controller_state_pub_;
   std::unique_ptr<realtime_tools::RealtimePublisher<ControllerState>> rt_controller_state_pub_;
   ControllerState controller_state_;

diff --git a/impedance_controller/package.xml b/impedance_controller/package.xml
@@ -3,7 +3,7 @@
 <package format="3">
 
   <name>impedance_controller</name>
-  <version>0.4.1</version>
+  <version>0.4.2</version>
   <description>An impedance controller for underwater vehicles</description>
 
   <maintainer email="evanp922@gmail.com">Evan Palmer</maintainer>
@@ -25,12 +25,12 @@
   <depend>hardware_interface</depend>
   <depend>rclcpp_lifecycle</depend>
   <depend>generate_parameter_library</depend>
-  <depend>control_msgs</depend>
   <depend>tf2_eigen</depend>
   <depend>geometry_msgs</depend>
   <depend>nav_msgs</depend>
   <depend>controller_common</depend>
   <depend>hydrodynamics</depend>
+  <depend>auv_control_msgs</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>