Henrik/velocity/merge

auv_setup/launch/autopilot.launch.py

@@ -5,7 +5,8 @@
 from launch.actions import (
     OpaqueFunction,
 )
-from launch_ros.actions import Node
+from launch_ros.actions import ComposableNodeContainer
+from launch_ros.descriptions import ComposableNode
 
 from auv_setup.launch_arg_common import (
     declare_drone_and_namespace_args,
@@ -15,45 +16,52 @@
 
 def launch_setup(context, *args, **kwargs):
     drone, namespace = resolve_drone_and_namespace(context)
-
-    velocity_lqr_config = os.path.join(
-        get_package_share_directory("velocity_controller_lqr"),
-        "config",
-        "param_velocity_controller_lqr.yaml",
-    )
-
-    los_config = os.path.join(
-        get_package_share_directory("los_guidance"),
-        "config",
-        "guidance_params.yaml",
-    )
-
     drone_params = os.path.join(
-        get_package_share_directory("auv_setup"),
-        "config",
-        "robots",
-        f"{drone}.yaml",
+        get_package_share_directory('auv_setup'),
+        'config',
+        'robots',
+        f'{drone}.yaml',
     )
-
-    los_node = Node(
-        package="los_guidance",
-        executable="los_guidance_node",
-        name="los_guidance_node",
-        namespace=namespace,
-        parameters=[drone_params, los_config],
-        output="screen",
+    velocity_control_params = os.path.join(
+        get_package_share_directory('velocity_controller'),
+        'config',
+        f'{drone}_params.yaml',
     )
-
-    lqr_node = Node(
-        package="velocity_controller_lqr",
-        executable="velocity_controller_lqr_node.py",
-        name="velocity_controller_lqr_node",
+    los_config = os.path.join(
+        get_package_share_directory('los_guidance'),
+        'config',
+        'guidance_params.yaml',
+    )
+    container = ComposableNodeContainer(
+        name='autopilot_container',
         namespace=namespace,
-        output="screen",
-        parameters=[drone_params, velocity_lqr_config],
+        package='rclcpp_components',
+        executable='component_container_mt',
+        composable_node_descriptions=[
+            ComposableNode(
+                package='velocity_controller',
+                plugin='velocity_node',
+                name='velocity_controller_node',
+                namespace=namespace,
+                parameters=[velocity_control_params, drone_params],
+                extra_arguments=[{"use_intra_process_comms": True}],
+            ),
+            ComposableNode(
+                package='los_guidance',
+                plugin='vortex::guidance::los::LosGuidanceNode',
+                name='los_guidance_node',
+                namespace=namespace,
+                parameters=[
+                    drone_params,
+                    {"los_config_file": los_config, "time_step": 0.1},
+                ],
+                extra_arguments=[{"use_intra_process_comms": True}],
+            ),
+        ],
+        output='screen',
+        arguments=['--ros-args', '--log-level', 'error'],
     )
-
-    return [los_node, lqr_node]
+    return [container]
 
 
 def generate_launch_description():

control/velocity_controller/CMakeLists.txt

@@ -0,0 +1,77 @@
+cmake_minimum_required(VERSION 3.8)
+project(velocity_controller)
+
+set(CMAKE_CXX_STANDARD 20)
+set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_lifecycle REQUIRED)
+find_package(rclcpp_components REQUIRED)
+find_package(lifecycle_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(vortex_msgs REQUIRED)
+find_package(vortex_utils REQUIRED)
+find_package(Eigen3 REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(ct_optcon REQUIRED)
+find_package(ct_core REQUIRED)
+find_package(casadi REQUIRED)
+
+include_directories(
+  include
+)
+
+
+set(LIB_NAME velocity_controller_component)
+add_library(${LIB_NAME} SHARED
+  src/velocity_controller_ros.cpp
+  src/PID_controller.cpp
+  src/LQR_setup.cpp
+  src/utilities.cpp
+  src/ct_instantiations.cpp
+  src/control_manager.cpp
+  src/3DOF_PID.cpp
+)
+rclcpp_components_register_nodes(${LIB_NAME} "Velocity_node")
+ament_target_dependencies(${LIB_NAME}
+  rclcpp
+  rclcpp_components
+  rclcpp_lifecycle
+  lifecycle_msgs
+  std_msgs
+  vortex_msgs
+  geometry_msgs
+  nav_msgs
+  vortex_utils
+)
+target_link_libraries(${LIB_NAME} Eigen3::Eigen casadi::casadi ct_optcon ct_core)
+install(TARGETS
+  ${LIB_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+install(
+  DIRECTORY include/
+  DESTINATION include
+)
+
+install(DIRECTORY
+  launch
+  config
+  DESTINATION share/${PROJECT_NAME}/
+)
+
+if(BUILD_TESTING)
+  add_subdirectory(tests)
+endif()
+
+ament_package()

control/velocity_controller/README.md

@@ -0,0 +1,201 @@
+# velocity_controller
+
+ROS2 lifecycle node for velocity control of an AUV (autonomous underwater vehicle). Supports two control strategies — a PID controller and an LQR controller.
+
+---
+
+## Overview
+
+The package implements a `Velocity_node` that subscribes to odometry and guidance inputs, computes thrust commands, and publishes them as `WrenchStamped` messages. The node is managed as a ROS2 lifecycle node, meaning it can be managed by a lifecycle manager, however if you do not want to use a lifecycle manager you can change the parameter autostart in the parameter file so that it automatically goes into active state.
+
+The LQR controller linearizes the vehicle dynamics around the current state at each timestep (gain-scheduled LQR), using a body-frame model that includes linear hydrodynamic damping, Coriolis effects, and integral action for steady-state error rejection. The PID controller serves as a simpler backup.
+
+---
+
+## Dependencies
+//TODO(henrimha): fix the dependencies
+| Dependency | Purpose |
+|---|---|
+| `rclcpp` / `rclcpp_lifecycle` | ROS2 node and lifecycle management |
+| `Eigen3` | Matrix math for LQR |
+| `control_toolbox` (`ct::optcon`) | Riccati equation solver for LQR gain |
+| `CasADi` | Used in utilities (NMPC-related) |
+| `vortex_msgs` | Custom guidance message (`LOSGuidance`) |
+| `nav_msgs` | Odometry input |
+| `geometry_msgs` | Thrust output (`WrenchStamped`) |
+
+---
+
+## Topics
+
+| Topic | Type | Direction | Description |
+|---|---|---|---|
+| `topic_thrust` | `geometry_msgs/WrenchStamped` | Published | Force and torque commands to thruster allocator |
+| `topic_guidance` | `vortex_msgs/LOSGuidance` | Subscribed | Desired surge, pitch, yaw from guidance system |
+| `topic_odometry` | `nav_msgs/Odometry` | Subscribed | Current vehicle state from state estimator |
+
+Topic names are configurable via ROS2 global parameter file.
+
+---
+
+## Parameters
+
+All parameters are loaded in the constructor via `get_new_parameters()`.
+
+### Controller selection
+
+| Parameter | Type | Description |
+|---|---|---|
+| `controller_type` | `int` | `1` = PID, `2` = LQR |
+| `publish_rate` | `int` | Control loop frequency in Hz |
+| `max_force` | `double` | Saturation limit applied to all outputs (N / Nm) |
+
+### LQR parameters
+
+| Parameter | Type | Dimension | Description |
+|---|---|---|---|
+| `Q` | `double[]` | 8 | Diagonal of state weight matrix. States: `[surge_err, pitch_err, yaw_err, pitch_rate_err, yaw_rate_err, ∫surge, ∫pitch, ∫yaw]` |
+| `R` | `double[]` | 3 | Diagonal of input weight matrix. Inputs: `[Fx, Ty, Tz]` |
+| `inertia_matrix` | `double[]` | 36 | Row-major 6x6 rigid body inertia matrix |
+| `dampening_matrix_low` | `double[]` | 36 | Row-major 6x6 hydrodynamic damping matrix at low speed |
+
+### PID parameters
+
+Each axis (surge, pitch, yaw) takes a parameter vector of the form `[Kp, Ki, Kd]`.
+
+| Parameter | Description |
+|---|---|
+| `surge_params` | PID gains for surge |
+| `pitch_params` | PID gains for pitch |
+| `yaw_params` | PID gains for yaw |
+
+### Behaviour flags
+
+| Parameter | Type | Description |
+|---|---|---|
+| `reset_on_new_ref` | `bool` | Reset integrators when a new guidance reference arrives |
+| `anti_overshoot` | `bool` | Enable anti-overshoot logic |
+| `odometry_dropout_guard` | `bool` | Stop publishing if odometry stops arriving |
+| `auto_start` | `bool` | If true, node self-transitions to active on startup |
+
+
+---
+
+## Lifecycle states
+
+The node uses the standard ROS2 managed lifecycle:
+
+```
+Unconfigured → [configure] → Inactive → [activate] → Active
+                                       ← [deactivate] ←
+```
+
+If `auto_start` is set to `true` in the parameters, the node will automatically call configure and activate itself after startup without needing an external lifecycle manager.
+
+To manually manage the node:
+
+```bash
+# Configure
+ros2 lifecycle set /velocity_controller configure
+
+# Activate
+ros2 lifecycle set /velocity_controller activate
+
+# Deactivate
+ros2 lifecycle set /velocity_controller deactivate
+
+# Cleanup
+ros2 lifecycle set /velocity_controller cleanup
+
+# Shutdown
+ros2 lifecycle set /velocity_controller shutdown
+
+```
+
+---
+
+## Controller details
+
+### LQR
+
+The LQR controller uses an 8-state augmented model in the body frame:
+
+```
+x = [surge_err, pitch_err, yaw_err, pitch_rate_err, yaw_rate_err, ∫surge, ∫pitch, ∫yaw]
+u = [Fx, Ty, Tz]
+```
+
+The system matrix `A` is re-linearized around the current state every control timestep. Guidance references in NED are converted to body-frame errors using the rotation matrix method before being passed to the controller — not by angle subtraction.
+
+The gain `K` is computed by solving the continuous-time algebraic Riccati equation via `ct::optcon::LQR`. The control law is:
+
+```
+u = K * x_error
+```
+//TODO(henrimha): give the riccatti equation
+where `ct::optcon` produces `K` such that this is equivalent to `u = -K * (x - x_ref)`.
+
+If the Riccati solver fails (e.g. due to an unstabilizable operating point), the node automatically falls back to PID and logs an error.
+
+### PID
+
+Three independent PID controllers run on surge, pitch, and yaw. Each supports anti-windup via integrator clamping. The derivative term can be computed either from the error signal or from a separately provided error derivative, depending on which `calculate_thrust` overload is called.
+
+---
+
+## Building
+
+```bash
+colcon build --packages-select velocity_controller --symlink-install
+source install/setup.bash
+```
+or with
+
+```bash
+colcon build --packages-up-to velocity_controller --symlink-install
+source install/setup.bash
+```
+Done in root of workspace
+---
+
+## Running
+
+Via a launch file with a parameter file:
+
+```bash
+ros2 launch velocity_controller velocity_controller.launch.py
+```
+
+---
+
+## Tests
+There are system tests and a helper node that generates a reference for the controller to follow.
+Tests are build like this:
+```bash
+colcon build --packages-select velocity_controller --symlink-install --cmake-args -DBUILD_TESTING=ON
+source install/setup.bash
+```
+System tests are run with the command
+
+```bash
+colcon test
+```
+
+Helper node is run with
+
+```bash
+ros2 launch velocity_controller VCnTest.launch.py
+```
+
+## Notes for new team members
+
+- The guidance input is expected in NED frame (north-east-down). The controller handles the NED-to-body conversion internally.
+- All angle errors are wrapped to `[-π, π]` using `ssa()` (smallest signed angle) before being fed to the controller.
+- The LQR Q matrix ordering matters — the 8 diagonal values correspond exactly to `[surge_err, pitch_err, yaw_err, pitch_rate_err, yaw_rate_err, ∫surge, ∫pitch, ∫yaw]` in that order.
+- If the vehicle behaves oddly, check that `interval_` (the control timestep) is being set correctly — a value of `0` disables integral action silently.
+
+## Adding new controllers
+After adding the hpp file, add the calculation to calc_thrust function in a new switch case, add to the reset_controller function, with options to reset only one integral, lastly update documentation. Remember to initialize correctly, either in 'on_configure' or in constructor, add the appropriate parameters, and update all the {drone}_params.yaml files.
+
+## Adding new drones
+Copy a {drone}_params.yaml file and change the name to the new name of the drone. Add the appropriate matrices, and tune to satisfying behaviour.