Address segfault in controller coordinator (#68)

evan-palmer · web-flow · commit 3ee73b7d9843 · 2025-07-17T15:38:03.000-07:00
* Fixed stale demos

* Update package versions

* Fix obsolete tf2 header

* Added a coordinator to activate controllers/hardware

* Address pr comments

* Fix segfault in coordinator on failure service

* bump version

* Address linter errors and update gersemi

* dumb
diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml
@@ -17,7 +17,7 @@ repos:
         types_or: [c++, c]
 
   - repo: https://github.com/BlankSpruce/gersemi
-    rev: 0.13.5
+    rev: 0.21.0
     hooks:
     - id: gersemi
 
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.1 (2025-07-09)
+
 ## 0.3.0 (2025-06-07)
 
 ## 0.2.1 (2025-06-03)
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.0</version>
+  <version>0.3.1</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.1 (2025-07-09)
+
 ## 0.3.0 (2025-06-07)
 
 ## 0.2.1 (2025-06-03)
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.0</version>
+  <version>0.3.1</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.1 (2025-07-09)
+
 ## 0.3.0 (2025-06-07)
 
 - Implements the controller coordinator
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.0</version>
+  <version>0.3.1</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.1 (2025-07-09)
+
 ## 0.3.0 (2025-06-07)
 
 ## 0.2.1 (2025-06-03)
diff --git a/controller_common/CMakeLists.txt b/controller_common/CMakeLists.txt
@@ -18,8 +18,9 @@ target_sources(
     PRIVATE src/common.cpp
     PUBLIC
         FILE_SET HEADERS
-        BASE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include
-        FILES ${CMAKE_CURRENT_SOURCE_DIR}/include/controller_common/common.hpp
+            BASE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include
+            FILES
+                ${CMAKE_CURRENT_SOURCE_DIR}/include/controller_common/common.hpp
 )
 target_compile_features(controller_common PUBLIC cxx_std_23)
 target_link_libraries(
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.0</version>
+  <version>0.3.1</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.1 (2025-07-09)
+
+- Addresses a lifetime bug in the coordinator service responses.
+
 ## 0.3.0 (2025-06-07)
 
 - Implements a simple service endpoint for activating and deactivating a
diff --git a/controller_coordinator/CMakeLists.txt b/controller_coordinator/CMakeLists.txt
@@ -29,11 +29,8 @@ target_link_libraries(
 )
 
 install(
-    TARGETS
-        controller_coordinator
-        controller_coordinator_parameters
-        DESTINATION
-        lib/controller_coordinator
+    TARGETS controller_coordinator controller_coordinator_parameters
+    DESTINATION lib/controller_coordinator
 )
 
 ament_package()
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.0</version>
+  <version>0.3.1</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
@@ -82,18 +82,18 @@ ControllerCoordinator::ControllerCoordinator()
   activate_system_service_ = this->create_service<std_srvs::srv::SetBool>(
     "~/activate",
     [this](
-      const std::shared_ptr<rmw_request_id_t> /*request_header*/,
-      const std::shared_ptr<std_srvs::srv::SetBool::Request> request,
-      const std::shared_ptr<std_srvs::srv::SetBool::Response> response) {
+      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
       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) {
+          [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
@@ -108,7 +108,8 @@ ControllerCoordinator::ControllerCoordinator()
         switch_controller_client_->async_send_request(
           activate_controllers_request_,
           [this,
-           &response](rclcpp::Client<controller_manager_msgs::srv::SwitchController>::SharedFuture result_response) {
+           response](
+            rclcpp::Client<controller_manager_msgs::srv::SwitchController>::SharedFuture result_response) {  // NOLINT
             const auto & result = result_response.get();
             if (result->ok) {
               RCLCPP_INFO(this->get_logger(), "Successfully activated controllers");  // NOLINT
@@ -124,8 +125,8 @@ ControllerCoordinator::ControllerCoordinator()
         // deactivate the hardware interface
         hardware_client_->async_send_request(
           deactivate_hardware_request_,
-          [this, &response](
-            rclcpp::Client<controller_manager_msgs::srv::SetHardwareComponentState>::SharedFuture result_response) {
+          [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
@@ -140,7 +141,8 @@ ControllerCoordinator::ControllerCoordinator()
         switch_controller_client_->async_send_request(
           deactivate_controllers_request_,
           [this,
-           &response](rclcpp::Client<controller_manager_msgs::srv::SwitchController>::SharedFuture result_response) {
+           response](
+            rclcpp::Client<controller_manager_msgs::srv::SwitchController>::SharedFuture result_response) {  // NOLINT
             const auto & result = result_response.get();
             if (result->ok) {
               RCLCPP_INFO(this->get_logger(), "Successfully deactivated controllers");  // NOLINT
diff --git a/end_effector_trajectory_controller/CHANGELOG.md b/end_effector_trajectory_controller/CHANGELOG.md
@@ -1,5 +1,7 @@
 # Changelog for package controller_common
 
+## 0.3.1 (2025-07-09)
+
 ## 0.3.0 (2025-06-07)
 
 ## 0.2.1 (2025-06-03)
diff --git a/end_effector_trajectory_controller/CMakeLists.txt b/end_effector_trajectory_controller/CMakeLists.txt
@@ -36,10 +36,10 @@ target_sources(
     PRIVATE src/end_effector_trajectory_controller.cpp src/trajectory.cpp
     PUBLIC
         FILE_SET HEADERS
-        BASE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include
-        FILES
-            ${CMAKE_CURRENT_SOURCE_DIR}/include/end_effector_trajectory_controller/end_effector_trajectory_controller.hpp
-            ${CMAKE_CURRENT_SOURCE_DIR}/include/end_effector_trajectory_controller/trajectory.hpp
+            BASE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include
+            FILES
+                ${CMAKE_CURRENT_SOURCE_DIR}/include/end_effector_trajectory_controller/end_effector_trajectory_controller.hpp
+                ${CMAKE_CURRENT_SOURCE_DIR}/include/end_effector_trajectory_controller/trajectory.hpp
 )
 target_compile_features(end_effector_trajectory_controller PUBLIC cxx_std_23)
 target_link_libraries(
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.0</version>
+  <version>0.3.1</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.1 (2025-07-09)
+
 ## 0.3.0 (2025-06-07)
 
 ## 0.2.1 (2025-06-03)
diff --git a/ik_solvers/CMakeLists.txt b/ik_solvers/CMakeLists.txt
@@ -30,8 +30,8 @@ target_sources(
     PRIVATE src/solver.cpp
     PUBLIC
         FILE_SET HEADERS
-        BASE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include
-        FILES ${CMAKE_CURRENT_SOURCE_DIR}/include/ik_solvers/solver.hpp
+            BASE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include
+            FILES ${CMAKE_CURRENT_SOURCE_DIR}/include/ik_solvers/solver.hpp
 )
 target_link_libraries(
     solver_base
@@ -52,9 +52,9 @@ target_sources(
     PRIVATE src/task_priority_solver.cpp src/pseudoinverse.cpp
     PUBLIC
         FILE_SET HEADERS
-        BASE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include
-        FILES
-            ${CMAKE_CURRENT_SOURCE_DIR}/include/ik_solvers/task_priority_solver.hpp
+            BASE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include
+            FILES
+                ${CMAKE_CURRENT_SOURCE_DIR}/include/ik_solvers/task_priority_solver.hpp
 )
 target_compile_features(ik_solvers PUBLIC cxx_std_23)
 target_link_libraries(
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.0</version>
+  <version>0.3.1</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.1 (2025-07-09)
+
 ## 0.3.0 (2025-06-07)
 
 ## 0.2.1 (2025-06-03)
diff --git a/thruster_allocation_matrix_controller/CMakeLists.txt b/thruster_allocation_matrix_controller/CMakeLists.txt
@@ -33,9 +33,9 @@ target_sources(
     PRIVATE src/thruster_allocation_matrix_controller.cpp
     PUBLIC
         FILE_SET HEADERS
-        BASE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include
-        FILES
-            ${CMAKE_CURRENT_SOURCE_DIR}/include/thruster_allocation_matrix_controller/thruster_allocation_matrix_controller.hpp
+            BASE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include
+            FILES
+                ${CMAKE_CURRENT_SOURCE_DIR}/include/thruster_allocation_matrix_controller/thruster_allocation_matrix_controller.hpp
 )
 target_compile_features(thruster_allocation_matrix_controller PUBLIC cxx_std_23)
 target_link_libraries(
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.0</version>
+  <version>0.3.1</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.1 (2025-07-09)
+
 ## 0.3.0 (2025-06-07)
 
 ## 0.2.1 (2025-06-03)
diff --git a/thruster_controllers/CMakeLists.txt b/thruster_controllers/CMakeLists.txt
@@ -37,11 +37,11 @@ target_sources(
         src/gz_passthrough_controller.cpp
     PUBLIC
         FILE_SET HEADERS
-        BASE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include
-        FILES
-            ${CMAKE_CURRENT_SOURCE_DIR}/include/thruster_controllers/polynomial_thrust_curve_controller.hpp
-            ${CMAKE_CURRENT_SOURCE_DIR}/include/thruster_controllers/rotation_rate_controller.hpp
-            ${CMAKE_CURRENT_SOURCE_DIR}/include/thruster_controllers/gz_passthrough_controller.hpp
+            BASE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include
+            FILES
+                ${CMAKE_CURRENT_SOURCE_DIR}/include/thruster_controllers/polynomial_thrust_curve_controller.hpp
+                ${CMAKE_CURRENT_SOURCE_DIR}/include/thruster_controllers/rotation_rate_controller.hpp
+                ${CMAKE_CURRENT_SOURCE_DIR}/include/thruster_controllers/gz_passthrough_controller.hpp
 )
 target_compile_features(thruster_controllers PUBLIC cxx_std_23)
 target_link_libraries(
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.0</version>
+  <version>0.3.1</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.1 (2025-07-09)
+
 ## 0.3.0 (2025-06-07)
 
 ## 0.2.1 (2025-06-03)
diff --git a/topic_sensors/CMakeLists.txt b/topic_sensors/CMakeLists.txt
@@ -23,8 +23,9 @@ target_sources(
     PRIVATE src/odom_sensor.cpp
     PUBLIC
         FILE_SET HEADERS
-        BASE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include
-        FILES ${CMAKE_CURRENT_SOURCE_DIR}/include/topic_sensors/odom_sensor.hpp
+            BASE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include
+            FILES
+                ${CMAKE_CURRENT_SOURCE_DIR}/include/topic_sensors/odom_sensor.hpp
 )
 target_compile_features(topic_sensors PUBLIC cxx_std_23)
 target_link_libraries(
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.0</version>
+  <version>0.3.1</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.1 (2025-07-09)
+
 ## 0.3.0 (2025-06-07)
 
 ## 0.2.1 (2025-06-03)
diff --git a/velocity_controllers/CMakeLists.txt b/velocity_controllers/CMakeLists.txt
@@ -42,10 +42,10 @@ target_sources(
         src/adaptive_integral_terminal_sliding_mode_controller.cpp
     PUBLIC
         FILE_SET HEADERS
-        BASE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include
-        FILES
-            ${CMAKE_CURRENT_SOURCE_DIR}/include/velocity_controllers/integral_sliding_mode_controller.hpp
-            ${CMAKE_CURRENT_SOURCE_DIR}/include/velocity_controllers/adaptive_integral_terminal_sliding_mode_controller.hpp
+            BASE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include
+            FILES
+                ${CMAKE_CURRENT_SOURCE_DIR}/include/velocity_controllers/integral_sliding_mode_controller.hpp
+                ${CMAKE_CURRENT_SOURCE_DIR}/include/velocity_controllers/adaptive_integral_terminal_sliding_mode_controller.hpp
 )
 target_compile_features(velocity_controllers PUBLIC cxx_std_23)
 target_link_libraries(
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.0</version>
+  <version>0.3.1</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.1 (2025-07-09)
+
 ## 0.3.0 (2025-06-07)
 
 ## 0.2.1 (2025-06-03)
diff --git a/whole_body_controllers/CMakeLists.txt b/whole_body_controllers/CMakeLists.txt
@@ -37,9 +37,9 @@ target_sources(
     PRIVATE src/ik_controller.cpp
     PUBLIC
         FILE_SET HEADERS
-        BASE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include
-        FILES
-            ${CMAKE_CURRENT_SOURCE_DIR}/include/whole_body_controllers/ik_controller.hpp
+            BASE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include
+            FILES
+                ${CMAKE_CURRENT_SOURCE_DIR}/include/whole_body_controllers/ik_controller.hpp
 )
 target_compile_features(whole_body_controllers PUBLIC cxx_std_23)
 target_link_libraries(
diff --git a/whole_body_controllers/package.xml b/whole_body_controllers/package.xml
