From 62cd260af54f26a11f4c614572389e9d2b1948ce Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Tue, 3 Jun 2025 00:52:08 -0700 Subject: [PATCH 1/5] Fixed stale demos --- .vscode/settings.json | 1 + .../chained_controllers/README.md | 111 ++++++------ .../config/chained_controllers.yaml | 58 ++++-- .../launch/chaining.launch.py | 4 +- ...ined_config.xacro => chained.config.xacro} | 6 +- .../xacro/chained.model.xacro | 42 +++++ ...ntrol.xacro => chained.ros2_control.xacro} | 36 ---- .../xacro/chained.urdf.xacro | 142 +++++++++++++++ .../xacro/chained_urdf.xacro | 170 ------------------ .../individual_controller/README.md | 80 +++++---- .../config/individual_controller.yaml | 56 ++++-- .../launch/individual.launch.py | 4 +- ...l_config.xacro => individual.config.xacro} | 6 +- .../xacro/individual.model.xacro | 42 +++++ ...ol.xacro => individual.ros2_control.xacro} | 0 ...idual_urdf.xacro => individual.urdf.xacro} | 0 16 files changed, 412 insertions(+), 346 deletions(-) rename auv_control_demos/chained_controllers/xacro/{chained_config.xacro => chained.config.xacro} (64%) create mode 100644 auv_control_demos/chained_controllers/xacro/chained.model.xacro rename auv_control_demos/chained_controllers/xacro/{chained_ros2_control.xacro => chained.ros2_control.xacro} (53%) create mode 100644 auv_control_demos/chained_controllers/xacro/chained.urdf.xacro delete mode 100644 auv_control_demos/chained_controllers/xacro/chained_urdf.xacro rename auv_control_demos/individual_controller/xacro/{individual_config.xacro => individual.config.xacro} (62%) create mode 100644 auv_control_demos/individual_controller/xacro/individual.model.xacro rename auv_control_demos/individual_controller/xacro/{individual_ros2_control.xacro => individual.ros2_control.xacro} (100%) rename auv_control_demos/individual_controller/xacro/{individual_urdf.xacro => individual.urdf.xacro} (100%) diff --git a/.vscode/settings.json b/.vscode/settings.json index 0313938..bf272b8 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -21,6 +21,7 @@ "files.trimTrailingWhitespace": true, "editor.formatOnSave": true, "editor.tabSize": 2, + "editor.detectIndentation": false, "autoDocstring.startOnNewLine": false, "autoDocstring.docstringFormat": "google-notypes", "python.autoComplete.extraPaths": [ diff --git a/auv_control_demos/chained_controllers/README.md b/auv_control_demos/chained_controllers/README.md index dd19edb..9cea3b9 100644 --- a/auv_control_demos/chained_controllers/README.md +++ b/auv_control_demos/chained_controllers/README.md @@ -23,16 +23,16 @@ applies a PWM command to the hardware interface. The output should resemble: ```bash - integral_sliding_mode_controller[velocity_controllers/IntegralSlidingModeController] active - thruster_allocation_matrix_controller[thruster_allocation_matrix_controller/ThrusterAllocationMatrixController] active - thruster_1_controller[thruster_controllers/PolynomialThrustCurveController] active - thruster_2_controller[thruster_controllers/PolynomialThrustCurveController] active - thruster_3_controller[thruster_controllers/PolynomialThrustCurveController] active - thruster_4_controller[thruster_controllers/PolynomialThrustCurveController] active - thruster_5_controller[thruster_controllers/PolynomialThrustCurveController] active - thruster_6_controller[thruster_controllers/PolynomialThrustCurveController] active - thruster_7_controller[thruster_controllers/PolynomialThrustCurveController] active - thruster_8_controller[thruster_controllers/PolynomialThrustCurveController] active + adaptive_integral_terminal_sliding_mode_controller velocity_controllers/AdaptiveIntegralTerminalSlidingModeController active + thruster_allocation_matrix_controller thruster_allocation_matrix_controller/ThrusterAllocationMatrixController active + thruster_8_controller thruster_controllers/PolynomialThrustCurveController active + thruster_7_controller thruster_controllers/PolynomialThrustCurveController active + thruster_6_controller thruster_controllers/PolynomialThrustCurveController active + thruster_5_controller thruster_controllers/PolynomialThrustCurveController active + thruster_4_controller thruster_controllers/PolynomialThrustCurveController active + thruster_3_controller thruster_controllers/PolynomialThrustCurveController active + thruster_2_controller thruster_controllers/PolynomialThrustCurveController active + thruster_1_controller thruster_controllers/PolynomialThrustCurveController active ``` 3. Verify that the hardware interfaces have been properly loaded by opening @@ -46,57 +46,51 @@ applies a PWM command to the hardware interface. ```bash command interfaces - integral_sliding_mode_controller/rx/velocity [available] [unclaimed] - integral_sliding_mode_controller/ry/velocity [available] [unclaimed] - integral_sliding_mode_controller/rz/velocity [available] [unclaimed] - integral_sliding_mode_controller/x/velocity [available] [unclaimed] - integral_sliding_mode_controller/y/velocity [available] [unclaimed] - integral_sliding_mode_controller/z/velocity [available] [unclaimed] - thruster_1_controller/thruster_1_joint/effort [available] [claimed] - thruster_1_joint/effort [available] [claimed] - thruster_2_controller/thruster_2_joint/effort [available] [claimed] - thruster_2_joint/effort [available] [claimed] - thruster_3_controller/thruster_3_joint/effort [available] [claimed] - thruster_3_joint/effort [available] [claimed] - thruster_4_controller/thruster_4_joint/effort [available] [claimed] - thruster_4_joint/effort [available] [claimed] - thruster_5_controller/thruster_5_joint/effort [available] [claimed] - thruster_5_joint/effort [available] [claimed] - thruster_6_controller/thruster_6_joint/effort [available] [claimed] - thruster_6_joint/effort [available] [claimed] - thruster_7_controller/thruster_7_joint/effort [available] [claimed] - thruster_7_joint/effort [available] [claimed] - thruster_8_controller/thruster_8_joint/effort [available] [claimed] - thruster_8_joint/effort [available] [claimed] - thruster_allocation_matrix_controller/rx/effort [available] [claimed] - thruster_allocation_matrix_controller/ry/effort [available] [claimed] - thruster_allocation_matrix_controller/rz/effort [available] [claimed] - thruster_allocation_matrix_controller/x/effort [available] [claimed] - thruster_allocation_matrix_controller/y/effort [available] [claimed] - thruster_allocation_matrix_controller/z/effort [available] [claimed] + adaptive_integral_terminal_sliding_mode_controller/rx/velocity [available] [unclaimed] + adaptive_integral_terminal_sliding_mode_controller/ry/velocity [available] [unclaimed] + adaptive_integral_terminal_sliding_mode_controller/rz/velocity [available] [unclaimed] + adaptive_integral_terminal_sliding_mode_controller/x/velocity [available] [unclaimed] + adaptive_integral_terminal_sliding_mode_controller/y/velocity [available] [unclaimed] + adaptive_integral_terminal_sliding_mode_controller/z/velocity [available] [unclaimed] + thruster_1_controller/thruster_1_joint/effort [available] [claimed] + thruster_1_joint/pwm [available] [claimed] + thruster_2_controller/thruster_2_joint/effort [available] [claimed] + thruster_2_joint/pwm [available] [claimed] + thruster_3_controller/thruster_3_joint/effort [available] [claimed] + thruster_3_joint/pwm [available] [claimed] + thruster_4_controller/thruster_4_joint/effort [available] [claimed] + thruster_4_joint/pwm [available] [claimed] + thruster_5_controller/thruster_5_joint/effort [available] [claimed] + thruster_5_joint/pwm [available] [claimed] + thruster_6_controller/thruster_6_joint/effort [available] [claimed] + thruster_6_joint/pwm [available] [claimed] + thruster_7_controller/thruster_7_joint/effort [available] [claimed] + thruster_7_joint/pwm [available] [claimed] + thruster_8_controller/thruster_8_joint/effort [available] [claimed] + thruster_8_joint/pwm [available] [claimed] + thruster_allocation_matrix_controller/rx/effort [available] [claimed] + thruster_allocation_matrix_controller/ry/effort [available] [claimed] + thruster_allocation_matrix_controller/rz/effort [available] [claimed] + thruster_allocation_matrix_controller/x/effort [available] [claimed] + thruster_allocation_matrix_controller/y/effort [available] [claimed] + thruster_allocation_matrix_controller/z/effort [available] [claimed] state interfaces - rx/velocity - ry/velocity - rz/velocity - x/velocity - y/velocity - z/velocity ``` -4. State feedback can be provided to ISMC using the controller's state +4. State feedback can be provided to AITSMC using the controller's state interfaces or a topic. To demonstrate the topic-based interface, run the following command: ```bash - ros2 topic pub /integral_sliding_mode_controller/system_state nav_msgs/msg/Odometry + ros2 topic pub /adaptive_integral_terminal_sliding_mode_controller/system_state nav_msgs/msg/Odometry ``` -5. The ISMC accepts reference commands sent over a topic or a reference +5. The AITSMC accepts reference commands sent over a topic or a reference interface. Run the following command in a separate terminal to provide the controller with a dummy reference input: ```bash - ros2 topic pub /integral_sliding_mode_controller/reference geometry_msgs/msg/Twist "{linear: {x: 0.5, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.2}}" + ros2 topic pub /adaptive_integral_terminal_sliding_mode_controller/reference geometry_msgs/msg/Twist "{linear: {x: 0.5, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.2}}" ``` 6. Each of the implemented controllers publishes controller state information to @@ -111,23 +105,23 @@ applies a PWM command to the hardware interface. ```bash header: stamp: - sec: 1710818346 - nanosec: 252453476 + sec: 1748936962 + nanosec: 907678344 frame_id: '' dof_state: name: thruster_2_joint - reference: -39.79632414479818 + reference: -63.32823451272053 feedback: 0.0 feedback_dot: 0.0 error: 0.0 error_dot: 0.0 - time_step: 0.033380572 + time_step: 0.033281545 output: 1134.0 ``` This output indicates that the controller chaining was successful. In particular, we can see that the velocity commands sent to the - `integral_sliding_mode_controller` are converted to thrust values, which is + `adaptive_integral_terminal_sliding_mode_controller` are converted to thrust values, which is reflected in the `thruster_controller` output. ## Files used for this demo @@ -136,14 +130,15 @@ applies a PWM command to the hardware interface. - [chaining.launch.py](https://github.com/Robotic-Decision-Making-Lab/auv_controllers/blob/main/auv_control_demos/chained_controllers/launch/chaining.launch.py) - Controllers: - - [Integral Sliding Mode Controller](https://github.com/Robotic-Decision-Making-Lab/auv_controllers/tree/main/velocity_controllers) + - [Adaptive Integral Terminal Sliding Mode Controller](https://github.com/Robotic-Decision-Making-Lab/auv_controllers/tree/main/velocity_controllers) - [Thruster Allocation Matrix Controller](https://github.com/Robotic-Decision-Making-Lab/auv_controllers/tree/main/thruster_allocation_matrix_controller) - [Polynomial Thrust Curve Controller](https://github.com/Robotic-Decision-Making-Lab/auv_controllers/tree/main/thruster_controllers) - Controller Config: - - [chained_controllers.yaml](https://github.com/Robotic-Decision-Making-Lab/auv_controllers/blob/main/auv_control_demos/chained_controllers/config/chained_controllers.yaml) + - [chained.controllers.yaml](https://github.com/Robotic-Decision-Making-Lab/auv_controllers/blob/main/auv_control_demos/chained_controllers/config/chained_controllers.yaml) - Xacro Files: - - [chained_config.xacro](https://github.com/Robotic-Decision-Making-Lab/auv_controllers/blob/main/auv_control_demos/chained_controllers/xacro/chained_config.xacro) - - [chained_ros2_control.xacro](https://github.com/Robotic-Decision-Making-Lab/auv_controllers/blob/main/auv_control_demos/chained_controllers/xacro/chained_ros2_control.xacro) - - [chained_urdf.xacro](https://github.com/Robotic-Decision-Making-Lab/auv_controllers/blob/main/auv_control_demos/chained_controllers/xacro/chained_urdf.xacro) + - [chained.config.xacro](https://github.com/Robotic-Decision-Making-Lab/auv_controllers/blob/main/auv_control_demos/chained_controllers/xacro/chained.config.xacro) + - [chained.ros2_control.xacro](https://github.com/Robotic-Decision-Making-Lab/auv_controllers/blob/main/auv_control_demos/chained_controllers/xacro/chained.ros2_control.xacro) + - [chained.urdf.xacro](https://github.com/Robotic-Decision-Making-Lab/auv_controllers/blob/main/auv_control_demos/chained_controllers/xacro/chained.urdf.xacro) + - [chained.model.xacro](https://github.com/Robotic-Decision-Making-Lab/auv_controllers/blob/main/auv_control_demos/chained_controllers/xacro/chained.urdf.xacro) diff --git a/auv_control_demos/chained_controllers/config/chained_controllers.yaml b/auv_control_demos/chained_controllers/config/chained_controllers.yaml index 9ffce54..8c61ded 100644 --- a/auv_control_demos/chained_controllers/config/chained_controllers.yaml +++ b/auv_control_demos/chained_controllers/config/chained_controllers.yaml @@ -2,8 +2,8 @@ controller_manager: ros__parameters: update_rate: 30 # Hz - integral_sliding_mode_controller: - type: velocity_controllers/IntegralSlidingModeController + adaptive_integral_terminal_sliding_mode_controller: + type: velocity_controllers/AdaptiveIntegralTerminalSlidingModeController thruster_allocation_matrix_controller: type: thruster_allocation_matrix_controller/ThrusterAllocationMatrixController @@ -32,24 +32,48 @@ controller_manager: thruster_8_controller: type: thruster_controllers/PolynomialThrustCurveController -integral_sliding_mode_controller: +adaptive_integral_terminal_sliding_mode_controller: ros__parameters: - use_external_measured_states: true reference_controller: thruster_allocation_matrix_controller + use_external_measured_states: true gains: - rho: 20.0 - lambda: 200.0 - Kp: [10.0, 10.0, 6.0, 3.0, 6.0, 10.0] - hydrodynamics: - mass: 13.5 - weight: 114.80 - buoyancy: 112.80 - moments_of_inertia: [0.16, 0.16, 0.16] - added_mass: [-5.50, -12.70, -14.60, -0.12, -0.12, -0.12] - center_of_buoyancy: [0.0, 0.0, 0.0] - center_of_gravity: [0.0, 0.0, 0.0] - linear_damping: [-4.03, -6.22, -5.18, -0.07, -0.07, -0.07] - quadratic_damping: [-18.18, -21.66, -36.99, -1.55, -1.55, -1.55] + lambda: 0.9 + x: + alpha: 12.0 + k1_min: 0.1 + k2: 0.4 + k_theta: 2.0 + mu: 0.01 + y: + alpha: 12.0 + k1_min: 0.1 + k2: 0.4 + k_theta: 2.0 + mu: 0.01 + z: + alpha: 12.0 + k1_min: 0.4 + k2: 0.8 + k_theta: 2.0 + mu: 0.01 + rx: + alpha: 0.5 + k1_min: 0.1 + k2: 0.4 + k_theta: 2.0 + mu: 0.01 + ry: + alpha: 12.0 + k1_min: 0.1 + k2: 1.0 + k_theta: 2.0 + mu: 0.01 + rz: + alpha: 12.0 + k1_min: 0.1 + k2: 0.4 + k_theta: 2.0 + mu: 0.01 thruster_allocation_matrix_controller: ros__parameters: diff --git a/auv_control_demos/chained_controllers/launch/chaining.launch.py b/auv_control_demos/chained_controllers/launch/chaining.launch.py index de4104b..ad4d79f 100644 --- a/auv_control_demos/chained_controllers/launch/chaining.launch.py +++ b/auv_control_demos/chained_controllers/launch/chaining.launch.py @@ -40,7 +40,7 @@ def generate_launch_description(): [ FindPackageShare("auv_control_demos"), "xacro", - "chained_config.xacro", + "chained.config.xacro", ] ), ] @@ -51,7 +51,7 @@ def generate_launch_description(): package="controller_manager", executable="spawner", arguments=[ - "integral_sliding_mode_controller", + "adaptive_integral_terminal_sliding_mode_controller", "--controller-manager", ["", "controller_manager"], ], diff --git a/auv_control_demos/chained_controllers/xacro/chained_config.xacro b/auv_control_demos/chained_controllers/xacro/chained.config.xacro similarity index 64% rename from auv_control_demos/chained_controllers/xacro/chained_config.xacro rename to auv_control_demos/chained_controllers/xacro/chained.config.xacro index 1e17ed6..6d1e771 100644 --- a/auv_control_demos/chained_controllers/xacro/chained_config.xacro +++ b/auv_control_demos/chained_controllers/xacro/chained.config.xacro @@ -2,8 +2,8 @@ - - - + + + diff --git a/auv_control_demos/chained_controllers/xacro/chained.model.xacro b/auv_control_demos/chained_controllers/xacro/chained.model.xacro new file mode 100644 index 0000000..e5a0d2d --- /dev/null +++ b/auv_control_demos/chained_controllers/xacro/chained.model.xacro @@ -0,0 +1,42 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/auv_control_demos/chained_controllers/xacro/chained_ros2_control.xacro b/auv_control_demos/chained_controllers/xacro/chained.ros2_control.xacro similarity index 53% rename from auv_control_demos/chained_controllers/xacro/chained_ros2_control.xacro rename to auv_control_demos/chained_controllers/xacro/chained.ros2_control.xacro index 95696d9..7f00692 100644 --- a/auv_control_demos/chained_controllers/xacro/chained_ros2_control.xacro +++ b/auv_control_demos/chained_controllers/xacro/chained.ros2_control.xacro @@ -7,42 +7,6 @@ false - - - 0.0 - - - - - - 0.0 - - - - - - 0.0 - - - - - - 0.0 - - - - - - 0.0 - - - - - - 0.0 - - - diff --git a/auv_control_demos/chained_controllers/xacro/chained.urdf.xacro b/auv_control_demos/chained_controllers/xacro/chained.urdf.xacro new file mode 100644 index 0000000..c74492c --- /dev/null +++ b/auv_control_demos/chained_controllers/xacro/chained.urdf.xacro @@ -0,0 +1,142 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/auv_control_demos/chained_controllers/xacro/chained_urdf.xacro b/auv_control_demos/chained_controllers/xacro/chained_urdf.xacro deleted file mode 100644 index d663453..0000000 --- a/auv_control_demos/chained_controllers/xacro/chained_urdf.xacro +++ /dev/null @@ -1,170 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/auv_control_demos/individual_controller/README.md b/auv_control_demos/individual_controller/README.md index aabaa0a..20e289b 100644 --- a/auv_control_demos/individual_controller/README.md +++ b/auv_control_demos/individual_controller/README.md @@ -21,7 +21,7 @@ This example uses the [integral sliding mode controller](https://github.com/Robo The output should resemble: ```bash - integral_sliding_mode_controller[velocity_controllers/IntegralSlidingModeController] active + adaptive_integral_terminal_sliding_mode_controller velocity_controllers/AdaptiveIntegralTerminalSlidingModeController active ``` 3. Check that the hardware interfaces have been properly loaded by opening @@ -35,25 +35,25 @@ This example uses the [integral sliding mode controller](https://github.com/Robo ```bash command interfaces - integral_sliding_mode_controller/rx/velocity [available] [unclaimed] - integral_sliding_mode_controller/ry/velocity [available] [unclaimed] - integral_sliding_mode_controller/rz/velocity [available] [unclaimed] - integral_sliding_mode_controller/x/velocity [available] [unclaimed] - integral_sliding_mode_controller/y/velocity [available] [unclaimed] - integral_sliding_mode_controller/z/velocity [available] [unclaimed] - rx/effort [available] [claimed] - ry/effort [available] [claimed] - rz/effort [available] [claimed] - x/effort [available] [claimed] - y/effort [available] [claimed] - z/effort [available] [claimed] + adaptive_integral_terminal_sliding_mode_controller/rx/velocity [available] [unclaimed] + adaptive_integral_terminal_sliding_mode_controller/ry/velocity [available] [unclaimed] + adaptive_integral_terminal_sliding_mode_controller/rz/velocity [available] [unclaimed] + adaptive_integral_terminal_sliding_mode_controller/x/velocity [available] [unclaimed] + adaptive_integral_terminal_sliding_mode_controller/y/velocity [available] [unclaimed] + adaptive_integral_terminal_sliding_mode_controller/z/velocity [available] [unclaimed] + rx/effort [available] [claimed] + ry/effort [available] [claimed] + rz/effort [available] [claimed] + x/effort [available] [claimed] + y/effort [available] [claimed] + z/effort [available] [claimed] state interfaces - rx/velocity - ry/velocity - rz/velocity - x/velocity - y/velocity - z/velocity + rx/velocity + ry/velocity + rz/velocity + x/velocity + y/velocity + z/velocity ``` 4. State feedback can be sent to the controller using a topic or the @@ -61,22 +61,22 @@ This example uses the [integral sliding mode controller](https://github.com/Robo topic-based interface: ```bash - ros2 topic pub /integral_sliding_mode_controller/system_state nav_msgs/msg/Odometry + ros2 topic pub /adaptive_integral_terminal_sliding_mode_controller/system_state nav_msgs/msg/Odometry ``` -5. The ISMC accepts reference commands sent via a topic or the controller's +5. The AITSMC accepts reference commands sent via a topic or the controller's reference interfaces. Run the following command in a separate terminal to provide the controller with a dummy reference input: ```bash - ros2 topic pub /integral_sliding_mode_controller/reference geometry_msgs/msg/Twist "{linear: {x: 0.5, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.2}}" + ros2 topic pub /adaptive_integral_terminal_sliding_mode_controller/reference geometry_msgs/msg/Twist "{linear: {x: 0.5, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.2}}" ``` 6. The current state of the controller can be observed on the `~/status` topic. For example, ```bash - ros2 topic echo /integral_sliding_mode_controller/status + ros2 topic echo /adaptive_integral_terminal_sliding_mode_controller/status ``` should yield an output similar to the following: @@ -84,8 +84,8 @@ This example uses the [integral sliding mode controller](https://github.com/Robo ```bash header: stamp: - sec: 1710817505 - nanosec: 673791332 + sec: 1748936374 + nanosec: 519996894 frame_id: '' dof_states: - name: x @@ -94,15 +94,15 @@ This example uses the [integral sliding mode controller](https://github.com/Robo feedback_dot: 0.0 error: 0.5 error_dot: 0.0 - time_step: 0.004459243 - output: 102.03798479512044 + time_step: 0.033335234 + output: 176.02789696997084 - name: y reference: 0.0 feedback: 0.0 feedback_dot: 0.0 error: 0.0 error_dot: 0.0 - time_step: 0.004459243 + time_step: 0.033335234 output: 0.0 - name: z reference: 0.0 @@ -110,15 +110,15 @@ This example uses the [integral sliding mode controller](https://github.com/Robo feedback_dot: 0.0 error: 0.0 error_dot: 0.0 - time_step: 0.004459243 - output: -2.0 + time_step: 0.033335234 + output: -3.034099999999995 - name: rx reference: 0.0 feedback: 0.0 feedback_dot: 0.0 error: 0.0 error_dot: 0.0 - time_step: 0.004459243 + time_step: 0.033335234 output: 0.0 - name: ry reference: 0.0 @@ -126,7 +126,7 @@ This example uses the [integral sliding mode controller](https://github.com/Robo feedback_dot: 0.0 error: 0.0 error_dot: 0.0 - time_step: 0.004459243 + time_step: 0.033335234 output: 0.0 - name: rz reference: 0.2 @@ -134,11 +134,12 @@ This example uses the [integral sliding mode controller](https://github.com/Robo feedback_dot: 0.0 error: 0.2 error_dot: 0.0 - time_step: 0.004459243 - output: 3.479864118728659 + time_step: 0.033335234 + output: 0.8183158752505321 + --- ``` - This output also demonstrates that the ISMC is functional. Any changes to + This output also demonstrates that the AITSMC is functional. Any changes to the reference command or state are reflected in the output of this topic. ### Files used for this demo @@ -147,12 +148,13 @@ This example uses the [integral sliding mode controller](https://github.com/Robo - [individual.launch.py](https://github.com/Robotic-Decision-Making-Lab/auv_controllers/blob/main/auv_control_demos/individual_controller/launch/individual.launch.py) - Controllers: - - [Integral Sliding Mode Controller](https://github.com/Robotic-Decision-Making-Lab/auv_controllers/tree/main/velocity_controllers) + - [Adaptive Integral Terminal Sliding Mode Controller](https://github.com/Robotic-Decision-Making-Lab/auv_controllers/tree/main/velocity_controllers) - Controller Config: - [individual_controller.yaml](https://github.com/Robotic-Decision-Making-Lab/auv_controllers/blob/main/auv_control_demos/individual_controller/config/individual_controller.yaml) - Xacro Files: - - [individual_config.xacro](https://github.com/Robotic-Decision-Making-Lab/auv_controllers/blob/main/auv_control_demos/individual_controller/xacro/individual_config.xacro) - - [individual_ros2_control.xacro](https://github.com/Robotic-Decision-Making-Lab/auv_controllers/blob/main/auv_control_demos/individual_controller/xacro/individual_ros2_control.xacro) - - [individual_urdf.xacro](https://github.com/Robotic-Decision-Making-Lab/auv_controllers/blob/main/auv_control_demos/individual_controller/xacro/individual_urdf.xacro) + - [individual.config.xacro](https://github.com/Robotic-Decision-Making-Lab/auv_controllers/blob/main/auv_control_demos/individual_controller/xacro/individual.config.xacro) + - [individual.ros2_control.xacro](https://github.com/Robotic-Decision-Making-Lab/auv_controllers/blob/main/auv_control_demos/individual_controller/xacro/individual.ros2_control.xacro) + - [individual.urdf.xacro](https://github.com/Robotic-Decision-Making-Lab/auv_controllers/blob/main/auv_control_demos/individual_controller/xacro/individual.urdf.xacro) + - [individual.model.xacro](https://github.com/Robotic-Decision-Making-Lab/auv_controllers/blob/main/auv_control_demos/individual_controller/xacro/individual.model.xacro) diff --git a/auv_control_demos/individual_controller/config/individual_controller.yaml b/auv_control_demos/individual_controller/config/individual_controller.yaml index d5faa7f..a7f1177 100644 --- a/auv_control_demos/individual_controller/config/individual_controller.yaml +++ b/auv_control_demos/individual_controller/config/individual_controller.yaml @@ -2,23 +2,47 @@ controller_manager: ros__parameters: update_rate: 30 # Hz - integral_sliding_mode_controller: - type: velocity_controllers/IntegralSlidingModeController + adaptive_integral_terminal_sliding_mode_controller: + type: velocity_controllers/AdaptiveIntegralTerminalSlidingModeController -integral_sliding_mode_controller: +adaptive_integral_terminal_sliding_mode_controller: ros__parameters: use_external_measured_states: true gains: - rho: 20.0 - lambda: 200.0 - Kp: [10.0, 10.0, 6.0, 3.0, 6.0, 10.0] - hydrodynamics: - mass: 13.5 - weight: 114.80 - buoyancy: 112.80 - moments_of_inertia: [0.16, 0.16, 0.16] - added_mass: [-5.50, -12.70, -14.60, -0.12, -0.12, -0.12] - center_of_buoyancy: [0.0, 0.0, 0.0] - center_of_gravity: [0.0, 0.0, 0.0] - linear_damping: [-4.03, -6.22, -5.18, -0.07, -0.07, -0.07] - quadratic_damping: [-18.18, -21.66, -36.99, -1.55, -1.55, -1.55] + lambda: 0.9 + x: + alpha: 12.0 + k1_min: 0.1 + k2: 0.4 + k_theta: 2.0 + mu: 0.01 + y: + alpha: 12.0 + k1_min: 0.1 + k2: 0.4 + k_theta: 2.0 + mu: 0.01 + z: + alpha: 12.0 + k1_min: 0.4 + k2: 0.8 + k_theta: 2.0 + mu: 0.01 + rx: + alpha: 0.5 + k1_min: 0.1 + k2: 0.4 + k_theta: 2.0 + mu: 0.01 + ry: + alpha: 12.0 + k1_min: 0.1 + k2: 1.0 + k_theta: 2.0 + mu: 0.01 + rz: + alpha: 12.0 + k1_min: 0.1 + k2: 0.4 + k_theta: 2.0 + mu: 0.01 diff --git a/auv_control_demos/individual_controller/launch/individual.launch.py b/auv_control_demos/individual_controller/launch/individual.launch.py index 5d709f5..155ad85 100644 --- a/auv_control_demos/individual_controller/launch/individual.launch.py +++ b/auv_control_demos/individual_controller/launch/individual.launch.py @@ -38,7 +38,7 @@ def generate_launch_description(): [ FindPackageShare("auv_control_demos"), "xacro", - "individual_config.xacro", + "individual.config.xacro", ] ), ] @@ -74,7 +74,7 @@ def generate_launch_description(): package="controller_manager", executable="spawner", arguments=[ - "integral_sliding_mode_controller", + "adaptive_integral_terminal_sliding_mode_controller", "--controller-manager", ["", "controller_manager"], ], diff --git a/auv_control_demos/individual_controller/xacro/individual_config.xacro b/auv_control_demos/individual_controller/xacro/individual.config.xacro similarity index 62% rename from auv_control_demos/individual_controller/xacro/individual_config.xacro rename to auv_control_demos/individual_controller/xacro/individual.config.xacro index 0706330..ce0a688 100644 --- a/auv_control_demos/individual_controller/xacro/individual_config.xacro +++ b/auv_control_demos/individual_controller/xacro/individual.config.xacro @@ -2,8 +2,8 @@ - - - + + + diff --git a/auv_control_demos/individual_controller/xacro/individual.model.xacro b/auv_control_demos/individual_controller/xacro/individual.model.xacro new file mode 100644 index 0000000..e5a0d2d --- /dev/null +++ b/auv_control_demos/individual_controller/xacro/individual.model.xacro @@ -0,0 +1,42 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/auv_control_demos/individual_controller/xacro/individual_ros2_control.xacro b/auv_control_demos/individual_controller/xacro/individual.ros2_control.xacro similarity index 100% rename from auv_control_demos/individual_controller/xacro/individual_ros2_control.xacro rename to auv_control_demos/individual_controller/xacro/individual.ros2_control.xacro diff --git a/auv_control_demos/individual_controller/xacro/individual_urdf.xacro b/auv_control_demos/individual_controller/xacro/individual.urdf.xacro similarity index 100% rename from auv_control_demos/individual_controller/xacro/individual_urdf.xacro rename to auv_control_demos/individual_controller/xacro/individual.urdf.xacro From da3d86a2681d412a3460135a388d337b673c34f6 Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Tue, 3 Jun 2025 00:58:38 -0700 Subject: [PATCH 2/5] Update package versions --- auv_control_demos/CHANGELOG.md | 6 ++++++ auv_control_demos/package.xml | 2 +- auv_control_msgs/CHANGELOG.md | 2 ++ auv_control_msgs/package.xml | 2 +- auv_controllers/CHANGELOG.md | 4 ++++ auv_controllers/package.xml | 2 +- controller_common/CHANGELOG.md | 2 ++ controller_common/package.xml | 2 +- end_effector_trajectory_controller/CHANGELOG.md | 7 +++++++ end_effector_trajectory_controller/package.xml | 2 +- ik_solvers/CHANGELOG.md | 2 ++ ik_solvers/package.xml | 2 +- thruster_allocation_matrix_controller/CHANGELOG.md | 2 ++ thruster_allocation_matrix_controller/package.xml | 2 +- thruster_controllers/CHANGELOG.md | 2 ++ thruster_controllers/package.xml | 2 +- topic_sensors/CHANGELOG.md | 2 ++ topic_sensors/package.xml | 2 +- velocity_controllers/CHANGELOG.md | 2 ++ velocity_controllers/package.xml | 2 +- whole_body_controllers/CHANGELOG.md | 2 ++ whole_body_controllers/package.xml | 2 +- 22 files changed, 44 insertions(+), 11 deletions(-) create mode 100644 end_effector_trajectory_controller/CHANGELOG.md diff --git a/auv_control_demos/CHANGELOG.md b/auv_control_demos/CHANGELOG.md index d4819d7..5144491 100644 --- a/auv_control_demos/CHANGELOG.md +++ b/auv_control_demos/CHANGELOG.md @@ -1,5 +1,11 @@ # Changelog for package auv_control_demos +## 0.2.1 (2025-06-03) + +- Updates the individual_controller and chained_controllers demos to use the +current `hydrodynamics` API and the new adaptive integral terminal sliding mode +controller. + ## 0.2.0 (2025-05-03) ## 0.1.0 (2025-04-27) diff --git a/auv_control_demos/package.xml b/auv_control_demos/package.xml index eda73df..4da428e 100644 --- a/auv_control_demos/package.xml +++ b/auv_control_demos/package.xml @@ -3,7 +3,7 @@ auv_control_demos - 0.2.0 + 0.2.1 Example package that includes demos for using auv_controllers in individual and chained modes Colin Mitchell diff --git a/auv_control_msgs/CHANGELOG.md b/auv_control_msgs/CHANGELOG.md index cf6d0d6..bf7ce60 100644 --- a/auv_control_msgs/CHANGELOG.md +++ b/auv_control_msgs/CHANGELOG.md @@ -1,5 +1,7 @@ # Changelog for package auv_control_msgs +## 0.2.1 (2025-06-03) + ## 0.2.0 (2025-05-03) - Implements the EndEffectorTrajectory message diff --git a/auv_control_msgs/package.xml b/auv_control_msgs/package.xml index fa31191..6ca9806 100644 --- a/auv_control_msgs/package.xml +++ b/auv_control_msgs/package.xml @@ -2,7 +2,7 @@ auv_control_msgs - 0.2.0 + 0.2.1 Custom messages for AUV controllers Rakesh Vivekanandan diff --git a/auv_controllers/CHANGELOG.md b/auv_controllers/CHANGELOG.md index 48279b9..c868f9e 100644 --- a/auv_controllers/CHANGELOG.md +++ b/auv_controllers/CHANGELOG.md @@ -1,5 +1,9 @@ # Changelog for package auv_controllers +## 0.2.1 (2025-06-03) + +- Fixes the auv_control_demos configurations + ## 0.2.0 (2025-05-03) - Adds the end effector trajectory controller diff --git a/auv_controllers/package.xml b/auv_controllers/package.xml index 11d30cf..0bddd3e 100644 --- a/auv_controllers/package.xml +++ b/auv_controllers/package.xml @@ -3,7 +3,7 @@ auv_controllers - 0.2.0 + 0.2.1 Meta package for auv_controllers Evan Palmer diff --git a/controller_common/CHANGELOG.md b/controller_common/CHANGELOG.md index 63dadc7..9b65e0f 100644 --- a/controller_common/CHANGELOG.md +++ b/controller_common/CHANGELOG.md @@ -1,5 +1,7 @@ # Changelog for package controller_common +## 0.2.1 (2025-06-03) + ## 0.2.0 (2025-05-03) - Adds the common::math::isclose method for comparing doubles diff --git a/controller_common/package.xml b/controller_common/package.xml index 9122575..1ca0cb1 100644 --- a/controller_common/package.xml +++ b/controller_common/package.xml @@ -3,7 +3,7 @@ controller_common - 0.2.0 + 0.2.1 Common interfaces for controllers used in this project Evan Palmer diff --git a/end_effector_trajectory_controller/CHANGELOG.md b/end_effector_trajectory_controller/CHANGELOG.md new file mode 100644 index 0000000..a30279d --- /dev/null +++ b/end_effector_trajectory_controller/CHANGELOG.md @@ -0,0 +1,7 @@ +# Changelog for package controller_common + +## 0.2.1 (2025-06-03) + +## 0.2.0 (2025-05-03) + +- Implements the end_effector_trajectory_controller diff --git a/end_effector_trajectory_controller/package.xml b/end_effector_trajectory_controller/package.xml index 910998f..3cc36fc 100644 --- a/end_effector_trajectory_controller/package.xml +++ b/end_effector_trajectory_controller/package.xml @@ -3,7 +3,7 @@ end_effector_trajectory_controller - 0.2.0 + 0.2.1 End effector trajectory tracking controller for UVMS control Evan Palmer diff --git a/ik_solvers/CHANGELOG.md b/ik_solvers/CHANGELOG.md index 8c9f764..b829884 100644 --- a/ik_solvers/CHANGELOG.md +++ b/ik_solvers/CHANGELOG.md @@ -1,5 +1,7 @@ # Changelog for package ik_solvers +## 0.2.1 (2025-06-03) + ## 0.2.0 (2025-05-03) - Replace instances of `Eigen::Affine3d` with `Eigen::Isometry3d` diff --git a/ik_solvers/package.xml b/ik_solvers/package.xml index cae9040..d788e50 100644 --- a/ik_solvers/package.xml +++ b/ik_solvers/package.xml @@ -3,7 +3,7 @@ ik_solvers - 0.2.0 + 0.2.1 Inverse kinematics solvers used for whole-body control Evan Palmer diff --git a/thruster_allocation_matrix_controller/CHANGELOG.md b/thruster_allocation_matrix_controller/CHANGELOG.md index fb5c325..a6f27e5 100644 --- 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.2.1 (2025-06-03) + ## 0.2.0 (2025-05-03) ## 0.1.0 (2025-04-27) diff --git a/thruster_allocation_matrix_controller/package.xml b/thruster_allocation_matrix_controller/package.xml index 66a1823..8c02b63 100644 --- a/thruster_allocation_matrix_controller/package.xml +++ b/thruster_allocation_matrix_controller/package.xml @@ -3,7 +3,7 @@ thruster_allocation_matrix_controller - 0.2.0 + 0.2.1 Thruster allocation matrix controller used to convert wrench commands into thrust commands Evan Palmer diff --git a/thruster_controllers/CHANGELOG.md b/thruster_controllers/CHANGELOG.md index 6e590ff..71b3e38 100644 --- a/thruster_controllers/CHANGELOG.md +++ b/thruster_controllers/CHANGELOG.md @@ -1,5 +1,7 @@ # Changelog for package thruster_controllers +## 0.2.1 (2025-06-03) + ## 0.2.0 (2025-05-03) ## 0.1.0 (2025-04-27) diff --git a/thruster_controllers/package.xml b/thruster_controllers/package.xml index 0282d47..965421d 100644 --- a/thruster_controllers/package.xml +++ b/thruster_controllers/package.xml @@ -3,7 +3,7 @@ thruster_controllers - 0.2.0 + 0.2.1 A collection of thruster controllers for AUV control Evan Palmer diff --git a/topic_sensors/CHANGELOG.md b/topic_sensors/CHANGELOG.md index e2f4a4e..3b39871 100644 --- a/topic_sensors/CHANGELOG.md +++ b/topic_sensors/CHANGELOG.md @@ -1,5 +1,7 @@ # Changelog for package topic_sensors +## 0.2.1 (2025-06-03) + ## 0.2.0 (2025-05-03) ## 0.1.0 (2025-04-27) diff --git a/topic_sensors/package.xml b/topic_sensors/package.xml index da96fb0..19a21c9 100644 --- a/topic_sensors/package.xml +++ b/topic_sensors/package.xml @@ -3,7 +3,7 @@ topic_sensors - 0.2.0 + 0.2.1 Sensor plugins used to write ROS 2 messages to state interfaces Evan Palmer diff --git a/velocity_controllers/CHANGELOG.md b/velocity_controllers/CHANGELOG.md index 9463314..16f54a0 100644 --- a/velocity_controllers/CHANGELOG.md +++ b/velocity_controllers/CHANGELOG.md @@ -1,5 +1,7 @@ # Changelog for package velocity_controllers +## 0.2.1 (2025-06-03) + ## 0.2.0 (2025-05-03) ## 0.1.0 (2025-04-27) diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index c2f9b1f..09dae57 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -3,7 +3,7 @@ velocity_controllers - 0.2.0 + 0.2.1 A collection of velocity controllers for underwater vehicles Evan Palmer diff --git a/whole_body_controllers/CHANGELOG.md b/whole_body_controllers/CHANGELOG.md index 85f2fb8..2fdcbb6 100644 --- a/whole_body_controllers/CHANGELOG.md +++ b/whole_body_controllers/CHANGELOG.md @@ -1,5 +1,7 @@ # Changelog for package whole_body_controllers +## 0.2.1 (2025-06-03) + ## 0.2.0 (2025-05-03) - Replaces instances of `Eigen::Affine3d` with `Eigen::Isometry3d` diff --git a/whole_body_controllers/package.xml b/whole_body_controllers/package.xml index 70e9197..6342cea 100644 --- a/whole_body_controllers/package.xml +++ b/whole_body_controllers/package.xml @@ -3,7 +3,7 @@ whole_body_controllers - 0.2.0 + 0.2.1 Whole-body controllers for underwater vehicle manipulator systems Evan Palmer From 86d9f2f77376464a69b3704ecad6160c6f8a0e68 Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Tue, 3 Jun 2025 01:07:26 -0700 Subject: [PATCH 3/5] Fix obsolete tf2 header --- .../end_effector_trajectory_controller.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/end_effector_trajectory_controller/include/end_effector_trajectory_controller/end_effector_trajectory_controller.hpp b/end_effector_trajectory_controller/include/end_effector_trajectory_controller/end_effector_trajectory_controller.hpp index 933c91b..8b513fe 100644 --- a/end_effector_trajectory_controller/include/end_effector_trajectory_controller/end_effector_trajectory_controller.hpp +++ b/end_effector_trajectory_controller/include/end_effector_trajectory_controller/end_effector_trajectory_controller.hpp @@ -32,7 +32,7 @@ #include "realtime_tools/realtime_buffer.hpp" #include "realtime_tools/realtime_publisher.hpp" #include "realtime_tools/realtime_server_goal_handle.hpp" -#include "tf2/exceptions.h" +#include "tf2/exceptions.hpp" #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" From 767a8ae200349544362be37670afaf39566cfb79 Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Sat, 7 Jun 2025 14:36:05 -0700 Subject: [PATCH 4/5] Added a coordinator to activate controllers/hardware --- auv_control_demos/CHANGELOG.md | 2 + auv_control_demos/package.xml | 2 +- auv_control_msgs/CHANGELOG.md | 2 + auv_control_msgs/package.xml | 3 +- auv_controllers/CHANGELOG.md | 4 + auv_controllers/package.xml | 2 +- controller_common/CHANGELOG.md | 2 + controller_common/package.xml | 2 +- controller_coordinator/CHANGELOG.md | 6 + controller_coordinator/CMakeLists.txt | 39 ++++ controller_coordinator/LICENSE | 17 ++ controller_coordinator/README.md | 15 ++ controller_coordinator/package.xml | 27 +++ controller_coordinator/src/coordinator.cpp | 170 ++++++++++++++++++ controller_coordinator/src/coordinator.hpp | 70 ++++++++ .../src/coordinator_parameters.yaml | 21 +++ .../CHANGELOG.md | 2 + .../package.xml | 2 +- ik_solvers/CHANGELOG.md | 2 + ik_solvers/package.xml | 2 +- .../CHANGELOG.md | 2 + .../package.xml | 2 +- thruster_controllers/CHANGELOG.md | 2 + thruster_controllers/package.xml | 2 +- topic_sensors/CHANGELOG.md | 2 + topic_sensors/package.xml | 2 +- velocity_controllers/CHANGELOG.md | 2 + velocity_controllers/package.xml | 2 +- whole_body_controllers/CHANGELOG.md | 2 + whole_body_controllers/package.xml | 2 +- 30 files changed, 401 insertions(+), 11 deletions(-) create mode 100644 controller_coordinator/CHANGELOG.md create mode 100644 controller_coordinator/CMakeLists.txt create mode 100644 controller_coordinator/LICENSE create mode 100644 controller_coordinator/README.md create mode 100644 controller_coordinator/package.xml create mode 100644 controller_coordinator/src/coordinator.cpp create mode 100644 controller_coordinator/src/coordinator.hpp create mode 100644 controller_coordinator/src/coordinator_parameters.yaml diff --git a/auv_control_demos/CHANGELOG.md b/auv_control_demos/CHANGELOG.md index 5144491..56a16eb 100644 --- a/auv_control_demos/CHANGELOG.md +++ b/auv_control_demos/CHANGELOG.md @@ -1,5 +1,7 @@ # Changelog for package auv_control_demos +## 0.3.0 (2025-06-07) + ## 0.2.1 (2025-06-03) - Updates the individual_controller and chained_controllers demos to use the diff --git a/auv_control_demos/package.xml b/auv_control_demos/package.xml index 4da428e..eec47da 100644 --- a/auv_control_demos/package.xml +++ b/auv_control_demos/package.xml @@ -3,7 +3,7 @@ auv_control_demos - 0.2.1 + 0.3.0 Example package that includes demos for using auv_controllers in individual and chained modes Colin Mitchell diff --git a/auv_control_msgs/CHANGELOG.md b/auv_control_msgs/CHANGELOG.md index bf7ce60..fb72d13 100644 --- a/auv_control_msgs/CHANGELOG.md +++ b/auv_control_msgs/CHANGELOG.md @@ -1,5 +1,7 @@ # Changelog for package auv_control_msgs +## 0.3.0 (2025-06-07) + ## 0.2.1 (2025-06-03) ## 0.2.0 (2025-05-03) diff --git a/auv_control_msgs/package.xml b/auv_control_msgs/package.xml index 6ca9806..effa430 100644 --- a/auv_control_msgs/package.xml +++ b/auv_control_msgs/package.xml @@ -1,8 +1,9 @@ + auv_control_msgs - 0.2.1 + 0.3.0 Custom messages for AUV controllers Rakesh Vivekanandan diff --git a/auv_controllers/CHANGELOG.md b/auv_controllers/CHANGELOG.md index c868f9e..3d19326 100644 --- a/auv_controllers/CHANGELOG.md +++ b/auv_controllers/CHANGELOG.md @@ -1,5 +1,9 @@ # Changelog for package auv_controllers +## 0.3.0 (2025-06-07) + +- Implements the controller coordinator + ## 0.2.1 (2025-06-03) - Fixes the auv_control_demos configurations diff --git a/auv_controllers/package.xml b/auv_controllers/package.xml index 0bddd3e..7021da3 100644 --- a/auv_controllers/package.xml +++ b/auv_controllers/package.xml @@ -3,7 +3,7 @@ auv_controllers - 0.2.1 + 0.3.0 Meta package for auv_controllers Evan Palmer diff --git a/controller_common/CHANGELOG.md b/controller_common/CHANGELOG.md index 9b65e0f..1811344 100644 --- a/controller_common/CHANGELOG.md +++ b/controller_common/CHANGELOG.md @@ -1,5 +1,7 @@ # Changelog for package controller_common +## 0.3.0 (2025-06-07) + ## 0.2.1 (2025-06-03) ## 0.2.0 (2025-05-03) diff --git a/controller_common/package.xml b/controller_common/package.xml index 1ca0cb1..7dfc607 100644 --- a/controller_common/package.xml +++ b/controller_common/package.xml @@ -3,7 +3,7 @@ controller_common - 0.2.1 + 0.3.0 Common interfaces for controllers used in this project Evan Palmer diff --git a/controller_coordinator/CHANGELOG.md b/controller_coordinator/CHANGELOG.md new file mode 100644 index 0000000..93baee0 --- /dev/null +++ b/controller_coordinator/CHANGELOG.md @@ -0,0 +1,6 @@ +# Changelog for package controller_coordinator + +## 0.3.0 (2025-06-07) + +- Implements a simple service endpoint for activating and deactivating a + control system. diff --git a/controller_coordinator/CMakeLists.txt b/controller_coordinator/CMakeLists.txt new file mode 100644 index 0000000..5c4b862 --- /dev/null +++ b/controller_coordinator/CMakeLists.txt @@ -0,0 +1,39 @@ +cmake_minimum_required(VERSION 3.23) +project(controller_coordinator) + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +include(GNUInstallDirs) + +find_package(rclcpp REQUIRED) +find_package(controller_manager_msgs REQUIRED) +find_package(ament_cmake REQUIRED) +find_package(generate_parameter_library REQUIRED) +find_package(std_srvs REQUIRED) + +generate_parameter_library(controller_coordinator_parameters src/coordinator_parameters.yaml) + +add_executable(controller_coordinator) +target_sources(controller_coordinator PRIVATE src/coordinator.cpp) + +target_compile_features(controller_coordinator PUBLIC cxx_std_20) +target_link_libraries( + controller_coordinator + PUBLIC + controller_coordinator_parameters + rclcpp::rclcpp + ${controller_manager_msgs_TARGETS} + ${std_srvs_TARGETS} +) + +install( + TARGETS + controller_coordinator + controller_coordinator_parameters + DESTINATION + lib/controller_coordinator +) + +ament_package() diff --git a/controller_coordinator/LICENSE b/controller_coordinator/LICENSE new file mode 100644 index 0000000..30e8e2e --- /dev/null +++ b/controller_coordinator/LICENSE @@ -0,0 +1,17 @@ +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. diff --git a/controller_coordinator/README.md b/controller_coordinator/README.md new file mode 100644 index 0000000..5a9a8f8 --- /dev/null +++ b/controller_coordinator/README.md @@ -0,0 +1,15 @@ +# Controller Coordinator + +The controller coordinator is a high-level interface for activating and +deactivating a control system. This is useful in scenarios where you want to +switch between a custom control framework and a company-provided control +framework. + +## Clients + +- controller_manager/set_hardware_component_state [controller_manager_msgs::srv::SetHardwareComponentState] +- controller_manager/switch_controller [controller_manager_msgs::srv::SwitchController] + +## Services + +- controller_coordinator/activate [std_srvs/srv/SetBool] diff --git a/controller_coordinator/package.xml b/controller_coordinator/package.xml new file mode 100644 index 0000000..daa0337 --- /dev/null +++ b/controller_coordinator/package.xml @@ -0,0 +1,27 @@ + + + + + controller_coordinator + 0.3.0 + A high-level node used to load and activate/deactivate controllers + + Evan Palmer + MIT + + https://github.com/Robotic-Decision-Making-Lab/ardusub_driver.git + https://github.com/Robotic-Decision-Making-Lab/ardusub_driver/issues + + Evan Palmer + + ament_cmake + + rclcpp + std_srvs + controller_manager_msgs + generate_parameter_library + + + ament_cmake + + diff --git a/controller_coordinator/src/coordinator.cpp b/controller_coordinator/src/coordinator.cpp new file mode 100644 index 0000000..87bfe27 --- /dev/null +++ b/controller_coordinator/src/coordinator.cpp @@ -0,0 +1,170 @@ +// Copyright 2025, Evan Palmer +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#include "coordinator.hpp" + +#include + +#include "lifecycle_msgs/msg/state.hpp" + +namespace coordinator +{ + +ControllerCoordinator::ControllerCoordinator() +: rclcpp::Node("controller_coordinator"), + activate_hardware_request_(std::make_shared()), + deactivate_hardware_request_(std::make_shared()), + activate_controllers_request_(std::make_shared()), + deactivate_controllers_request_(std::make_shared()) +{ + param_listener_ = std::make_shared(this->get_node_parameters_interface()); + params_ = param_listener_->get_params(); + + client_callback_group_ = this->create_callback_group(rclcpp::CallbackGroupType::Reentrant, true); + + // 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) { + 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 + } + RCLCPP_INFO(this->get_logger(), "%s service available", service_name.c_str()); // NOLINT + }; + + // create clients + const std::string hardware_service = "controller_manager/set_hardware_component_state"; + hardware_client_ = this->create_client( + hardware_service, rclcpp::ServicesQoS(), client_callback_group_); + wait_for_service(hardware_client_, hardware_service); + + const std::string switch_controller_name = "controller_manager/switch_controller"; + switch_controller_client_ = this->create_client( + switch_controller_name, rclcpp::ServicesQoS(), client_callback_group_); + 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; + + // pre-configure the controller activation/deactivation requests + activate_controllers_request_->activate_controllers = params_.controller_sequence; + activate_controllers_request_->strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT; + activate_controllers_request_->activate_asap = true; + activate_controllers_request_->timeout = rclcpp::Duration::from_seconds(params_.timeout); + + deactivate_controllers_request_->deactivate_controllers = params_.controller_sequence; + deactivate_controllers_request_->strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT; + deactivate_controllers_request_->activate_asap = true; + deactivate_controllers_request_->timeout = rclcpp::Duration::from_seconds(params_.timeout); + + // create a service endpoint for users to activate or deactivate their system + service_callback_group_ = this->create_callback_group(rclcpp::CallbackGroupType::Reentrant, true); + activate_system_service_ = this->create_service( + "~/activate", + [this]( + const std::shared_ptr /*request_header*/, + const std::shared_ptr request, + const std::shared_ptr response) { + 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::SharedFuture result_response) { + 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 + switch_controller_client_->async_send_request( + activate_controllers_request_, + [this, + &response](rclcpp::Client::SharedFuture result_response) { + const auto & result = result_response.get(); + if (result->ok) { + RCLCPP_INFO(this->get_logger(), "Successfully activated controllers"); // NOLINT + } else { + RCLCPP_ERROR(this->get_logger(), "Failed to activate controllers"); // NOLINT + response->success = false; + response->message = "Failed to activate controllers"; + } + }); + } 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::SharedFuture result_response) { + 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 + switch_controller_client_->async_send_request( + deactivate_controllers_request_, + [this, + &response](rclcpp::Client::SharedFuture result_response) { + const auto & result = result_response.get(); + if (result->ok) { + RCLCPP_INFO(this->get_logger(), "Successfully deactivated controllers"); // NOLINT + } else { + RCLCPP_ERROR(this->get_logger(), "Failed to deactivate controllers"); // NOLINT + response->success = false; + response->message = "Failed to deactivate controllers"; + } + }); + } + }, + rclcpp::ServicesQoS(), + service_callback_group_); +} + +} // namespace coordinator + +auto main(int argc, char * argv[]) -> int +{ + rclcpp::init(argc, argv); + rclcpp::executors::MultiThreadedExecutor executor; + auto node = std::make_shared(); + executor.add_node(node->get_node_base_interface()); + executor.spin(); + rclcpp::shutdown(); + return 0; +} diff --git a/controller_coordinator/src/coordinator.hpp b/controller_coordinator/src/coordinator.hpp new file mode 100644 index 0000000..cc37083 --- /dev/null +++ b/controller_coordinator/src/coordinator.hpp @@ -0,0 +1,70 @@ +// Copyright 2025, Evan Palmer +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#pragma once + +#include "controller_manager_msgs/srv/configure_controller.hpp" +#include "controller_manager_msgs/srv/list_controllers.hpp" +#include "controller_manager_msgs/srv/load_controller.hpp" +#include "controller_manager_msgs/srv/set_hardware_component_state.hpp" +#include "controller_manager_msgs/srv/switch_controller.hpp" +#include "controller_manager_msgs/srv/unload_controller.hpp" +#include "rclcpp/rclcpp.hpp" +#include "std_srvs/srv/set_bool.hpp" + +// auto-generated by generate_parameter_library +#include + +namespace coordinator +{ + +class ControllerCoordinator : public rclcpp::Node +{ +public: + ControllerCoordinator(); + + ~ControllerCoordinator() override = default; + +private: + // we need clients to: + // 1. activate/deactivate the hardware + // 2. activate/deactivate the controllers + std::shared_ptr> hardware_client_; + std::shared_ptr> switch_controller_client_; + + // create a user-facing service that will allow users to activate/deactivate the hardware and all controllers + std::shared_ptr> activate_system_service_; + + // pre-configure the activate/deactivate service messages + // these won't change, so we can set them up once + std::shared_ptr activate_hardware_request_; + std::shared_ptr deactivate_hardware_request_; + std::shared_ptr activate_controllers_request_; + std::shared_ptr deactivate_controllers_request_; + + std::shared_ptr param_listener_; + controller_coordinator::Params params_; + + // we need separate callback groups for the clients and service + std::shared_ptr service_callback_group_; + std::shared_ptr client_callback_group_; +}; + +} // namespace coordinator diff --git a/controller_coordinator/src/coordinator_parameters.yaml b/controller_coordinator/src/coordinator_parameters.yaml new file mode 100644 index 0000000..f932f7e --- /dev/null +++ b/controller_coordinator/src/coordinator_parameters.yaml @@ -0,0 +1,21 @@ +controller_coordinator: + hardware_interface: + type: string + description: The name of the hardware interface to activate/deactivate. + default_value: "" + + controller_sequence: + type: string_array + description: An ordered list of controllers to activate/deactivate. + default_value: [] + + timeout: + type: double + description: The timeout (s) for activating/deactivating controllers. + default_value: 5.0 + + max_attempts: + type: int + description: > + The maximum number of attempts to activate/deactivate the controllers. + default_value: 3 diff --git a/end_effector_trajectory_controller/CHANGELOG.md b/end_effector_trajectory_controller/CHANGELOG.md index a30279d..f17bccc 100644 --- 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.0 (2025-06-07) + ## 0.2.1 (2025-06-03) ## 0.2.0 (2025-05-03) diff --git a/end_effector_trajectory_controller/package.xml b/end_effector_trajectory_controller/package.xml index 3cc36fc..3d28221 100644 --- a/end_effector_trajectory_controller/package.xml +++ b/end_effector_trajectory_controller/package.xml @@ -3,7 +3,7 @@ end_effector_trajectory_controller - 0.2.1 + 0.3.0 End effector trajectory tracking controller for UVMS control Evan Palmer diff --git a/ik_solvers/CHANGELOG.md b/ik_solvers/CHANGELOG.md index b829884..f5c4b1b 100644 --- a/ik_solvers/CHANGELOG.md +++ b/ik_solvers/CHANGELOG.md @@ -1,5 +1,7 @@ # Changelog for package ik_solvers +## 0.3.0 (2025-06-07) + ## 0.2.1 (2025-06-03) ## 0.2.0 (2025-05-03) diff --git a/ik_solvers/package.xml b/ik_solvers/package.xml index d788e50..20d6824 100644 --- a/ik_solvers/package.xml +++ b/ik_solvers/package.xml @@ -3,7 +3,7 @@ ik_solvers - 0.2.1 + 0.3.0 Inverse kinematics solvers used for whole-body control Evan Palmer diff --git a/thruster_allocation_matrix_controller/CHANGELOG.md b/thruster_allocation_matrix_controller/CHANGELOG.md index a6f27e5..a12c4c6 100644 --- 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.0 (2025-06-07) + ## 0.2.1 (2025-06-03) ## 0.2.0 (2025-05-03) diff --git a/thruster_allocation_matrix_controller/package.xml b/thruster_allocation_matrix_controller/package.xml index 8c02b63..eacf7e8 100644 --- a/thruster_allocation_matrix_controller/package.xml +++ b/thruster_allocation_matrix_controller/package.xml @@ -3,7 +3,7 @@ thruster_allocation_matrix_controller - 0.2.1 + 0.3.0 Thruster allocation matrix controller used to convert wrench commands into thrust commands Evan Palmer diff --git a/thruster_controllers/CHANGELOG.md b/thruster_controllers/CHANGELOG.md index 71b3e38..eac65f5 100644 --- a/thruster_controllers/CHANGELOG.md +++ b/thruster_controllers/CHANGELOG.md @@ -1,5 +1,7 @@ # Changelog for package thruster_controllers +## 0.3.0 (2025-06-07) + ## 0.2.1 (2025-06-03) ## 0.2.0 (2025-05-03) diff --git a/thruster_controllers/package.xml b/thruster_controllers/package.xml index 965421d..edfa124 100644 --- a/thruster_controllers/package.xml +++ b/thruster_controllers/package.xml @@ -3,7 +3,7 @@ thruster_controllers - 0.2.1 + 0.3.0 A collection of thruster controllers for AUV control Evan Palmer diff --git a/topic_sensors/CHANGELOG.md b/topic_sensors/CHANGELOG.md index 3b39871..918e00c 100644 --- a/topic_sensors/CHANGELOG.md +++ b/topic_sensors/CHANGELOG.md @@ -1,5 +1,7 @@ # Changelog for package topic_sensors +## 0.3.0 (2025-06-07) + ## 0.2.1 (2025-06-03) ## 0.2.0 (2025-05-03) diff --git a/topic_sensors/package.xml b/topic_sensors/package.xml index 19a21c9..707cdb1 100644 --- a/topic_sensors/package.xml +++ b/topic_sensors/package.xml @@ -3,7 +3,7 @@ topic_sensors - 0.2.1 + 0.3.0 Sensor plugins used to write ROS 2 messages to state interfaces Evan Palmer diff --git a/velocity_controllers/CHANGELOG.md b/velocity_controllers/CHANGELOG.md index 16f54a0..5d0da7b 100644 --- a/velocity_controllers/CHANGELOG.md +++ b/velocity_controllers/CHANGELOG.md @@ -1,5 +1,7 @@ # Changelog for package velocity_controllers +## 0.3.0 (2025-06-07) + ## 0.2.1 (2025-06-03) ## 0.2.0 (2025-05-03) diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index 09dae57..f4f4947 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -3,7 +3,7 @@ velocity_controllers - 0.2.1 + 0.3.0 A collection of velocity controllers for underwater vehicles Evan Palmer diff --git a/whole_body_controllers/CHANGELOG.md b/whole_body_controllers/CHANGELOG.md index 2fdcbb6..8f3fbf6 100644 --- a/whole_body_controllers/CHANGELOG.md +++ b/whole_body_controllers/CHANGELOG.md @@ -1,5 +1,7 @@ # Changelog for package whole_body_controllers +## 0.3.0 (2025-06-07) + ## 0.2.1 (2025-06-03) ## 0.2.0 (2025-05-03) diff --git a/whole_body_controllers/package.xml b/whole_body_controllers/package.xml index 6342cea..d5c5866 100644 --- a/whole_body_controllers/package.xml +++ b/whole_body_controllers/package.xml @@ -3,7 +3,7 @@ whole_body_controllers - 0.2.1 + 0.3.0 Whole-body controllers for underwater vehicle manipulator systems Evan Palmer From b915c8a97036846820810aae500367e8ca2f5a9d Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Sat, 7 Jun 2025 15:24:03 -0700 Subject: [PATCH 5/5] Address pr comments --- controller_coordinator/package.xml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/controller_coordinator/package.xml b/controller_coordinator/package.xml index daa0337..8dc4261 100644 --- a/controller_coordinator/package.xml +++ b/controller_coordinator/package.xml @@ -4,13 +4,13 @@ controller_coordinator 0.3.0 - A high-level node used to load and activate/deactivate controllers + A high-level node used to load and activate/deactivate control systems Evan Palmer MIT - https://github.com/Robotic-Decision-Making-Lab/ardusub_driver.git - https://github.com/Robotic-Decision-Making-Lab/ardusub_driver/issues + https://github.com/Robotic-Decision-Making-Lab/auv_controllers.git + https://github.com/Robotic-Decision-Making-Lab/auv_controllers/issues Evan Palmer