diff --git a/.gitignore b/.gitignore index 41eed4018..31e22429e 100644 --- a/.gitignore +++ b/.gitignore @@ -59,3 +59,8 @@ qtcreator-* .#* # End of https://www.gitignore.io/api/ros +control/pid_controller_dp/test/test_main.cpp +control/pid_controller_dp/test/test_pid_basic.cpp +control/pid_controller_dp/test/test_pid_controller.cpp +control/pid_controller_dp/test/test_type_casting.cpp +scripts/ci_install_dependencies.sh diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index ef82f0dfb..aea315598 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -12,6 +12,8 @@ # # See https://github.com/pre-commit/pre-commit +exclude: (^|/)(test|tests)/.* + repos: # Standard hooks - repo: https://github.com/pre-commit/pre-commit-hooks diff --git a/auv_setup/README.md b/auv_setup/README.md index ead2200b4..56a4b2172 100644 --- a/auv_setup/README.md +++ b/auv_setup/README.md @@ -10,8 +10,36 @@ The config folder contains physical parameters related to the AUV and the enviro * thrusters: Thruster configs for different thruster types ### Launch -This package contains a launchfile for each specific AUV. Additionally the topside.launch file is to -be used on the topside computer that the joystick is connected to, for ROV operations. + +This package contains launchfiles for each specific AUV. Additionally `topside.launch.py` is used on the topside computer that the joystick is connected to, for ROV operations. + +#### dp.launch.py + +Launches the DP (dynamic positioning) stack — a reference filter and a controller — as a composable node container. + +```bash +ros2 launch auv_setup dp.launch.py +``` + +| Argument | Default | Description | +|---|---|---| +| `drone` | `nautilus` | Drone model — loads `auv_setup/config/robots/.yaml` | +| `namespace` | `` | ROS namespace. Defaults to the drone name if left empty | +| `controller_type` | `adaptive` | Controller to use: `adaptive` or `pid` | + +**`adaptive`** — launches `dp_adapt_backs_controller` with `reference_filter_dp` (Euler/RPY reference filter). Controller params are loaded from `dp_adapt_backs_controller/config/adapt_params_.yaml`. + +```bash +ros2 launch auv_setup dp.launch.py controller_type:=adaptive drone:=nautilus +``` + +**`pid`** — launches `pid_controller_dp` with `reference_filter_dp_quat` (quaternion reference filter). Controller params are loaded from `pid_controller_dp/config/pid_params.yaml`. + +```bash +ros2 launch auv_setup dp.launch.py controller_type:=pid drone:=nautilus +``` + +> When using the joystick to send references, make sure `orientation_mode` in `joystick_interface_auv` matches the controller: use `euler` with `adaptive` and `quat` with `pid`. ### Description The description folder contains the URDF and xacro files for the AUVs. The main description launch file is drone_description.launch.py, which makes all static transforms available to the ros graph. diff --git a/auv_setup/config/robots/nautilus.yaml b/auv_setup/config/robots/nautilus.yaml index 61fa6b061..653c0a2a8 100644 --- a/auv_setup/config/robots/nautilus.yaml +++ b/auv_setup/config/robots/nautilus.yaml @@ -16,8 +16,8 @@ /**: ros__parameters: physical: - center_of_mass: [0.0, 0.0, 0.01] # CO is aligned with CM Position (x,y) in meters (M), small cg offset in z to keep drone naturally stable in roll/pitch - mass_matrix: [53.7, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 53.7, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 53.7, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 11.0628, 1.086, -3.17502, 0.0, 0.0, 0.0, 1.086, 23.1128, 0.1025, 0.0, 0.0, 0.0, -3.17502, 0.1025, 26.23998] + center_of_mass: [0.0, 0.0, 0.05] # CO is aligned with CM Position (x,y) in meters (M), small cg offset in z to keep drone naturally stable in roll/pitch + mass_matrix: [55.7, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 55.7, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 55.7, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 11.0628, 1.086, -3.17502, 0.0, 0.0, 0.0, 1.086, 23.1128, 0.1025, 0.0, 0.0, 0.0, -3.17502, 0.1025, 26.23998] # 6x6 mass_inertia_matrix propulsion: dofs: @@ -53,22 +53,22 @@ 0.00000, # Heave ] thruster_position: [ # Position (x0,x1 ... x7,y1,y2, ...,y7,z1,z2, ... ,z7) in meters (M). i.e thruster0 has position (x0,y0,z0) - 0.413892, - 0.140095, - -0.163904, - -0.413892, - -0.413892, - -0.163904, - 0.140095, - 0.413892, # x-positions of the thrusters + 0.45015, + 0.24060, + -0.22970, + -0.43861, + -0.43861, + -0.22970, + 0.240600, + 0.450150, # x-positions of the thrusters + 0.305680, 0.313022, 0.313022, - 0.313022, - 0.313022, - -0.313022, + 0.305680, + -0.305680, -0.313022, -0.313022, - -0.313022, # y-positions of the thrusters + -0.305680, # y-positions of the thrusters 0.021736, 0.021736, 0.021736, @@ -80,8 +80,8 @@ ] constraints: - max_force: 40.0 - min_force: -40.0 + max_force: 39.0 + min_force: -41.0 input_matrix_weights: [1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0] slack_matrix_weights: [2000.0,2000.0,2000.0,2000.0,2000.0,2000.0] @@ -90,6 +90,10 @@ thrust_update_rate: 100.0 # [Hz] watchdog_timeout: 1.0 # [s] + thruster_to_pin_mapping: [4, 5, 7, 6, 0, 1, 2, 3] # I.e. if thruster_to_pin = [ 7, 6, 5, 4, 3, 2, 1, 0 ] then thruster 0 is pin 1 etc.. + thruster_direction: [-1, -1, 1, 1, -1, -1, 1, 1] # Disclose during thruster mapping (+/- 1) + thruster_PWM_min: [1050, 1050, 1050, 1050, 1050, 1050, 1050, 1050] # Minimum PWM value, Recommended: [1100, 1100, 1100, 1100, 1100, 1100, 1100, 1100] + thruster_PWM_max: [1950, 1950, 1950, 1950, 1950, 1950, 1950, 1950] # Maximum PWM value, Recommended: [1900, 1900, 1900, 1900, 1900, 1900, 1900, 1900] topics: wrench_input: "wrench_input" @@ -110,7 +114,7 @@ waypoint: "waypoint" guidance: los: "guidance/los" - dp: "guidance/dp" + dp_rpy: "guidance/dp_rpy" dp_quat: "guidance/dp_quat" dvl_twist: "dvl/twist" dvl_altitude: "dvl/altitude" diff --git a/auv_setup/config/robots/nautilus_bak.yaml b/auv_setup/config/robots/nautilus_bak.yaml new file mode 100644 index 000000000..a0407a309 --- /dev/null +++ b/auv_setup/config/robots/nautilus_bak.yaml @@ -0,0 +1,140 @@ +# This file defines parameters specific to Nautilus. +# When looking at the AUV from above, the thruster placement is: +# +# front +# |======| +# |=7↗=| |=0↖=| +# | | | | +# | 6• | | 1• | +# | | | | +# | | | | +# | 5• | | 2• | +# | | | | +# |=4↘=|==||==|=3↙=| +# + +/**: + ros__parameters: + physical: + center_of_mass: [0.0, 0.0, 0.1] # CO is aligned with CM Position (x,y) in meters (M), small cg offset in z to keep drone naturally stable in roll/pitch + mass_matrix: [53.7, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 53.7, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 53.7, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 11.0628, 1.086, -3.17502, 0.0, 0.0, 0.0, 1.086, 23.1128, 0.1025, 0.0, 0.0, 0.0, -3.17502, 0.1025, 26.23998] + # 6x6 mass_inertia_matrix + propulsion: + dofs: + num: 6 + dimensions: + num: 3 + thrusters: + num: 8 + thruster_force_direction: [ # Direction of forces X,Y,Z, same logic as thruster_position thruster0 produces thrust in (X0,Y0,Z0) and ||(X0,Y0,Z0)|| = 1 + 0.70711, + 0.00000, + 0.00000, + -0.70711, + -0.70711, + 0.00000, + 0.00000, + 0.70711, # Surge + -0.70711, + 0.00000, + 0.00000, + -0.70711, + 0.70711, + 0.00000, + 0.00000, + 0.70711, # Sway + 0.00000, + 1.00000, + 1.00000, + 0.00000, + 0.00000, + 1.00000, + 1.00000, + 0.00000, # Heave + ] + thruster_position: [ # Position (x0,x1 ... x7,y1,y2, ...,y7,z1,z2, ... ,z7) in meters (M). i.e thruster0 has position (x0,y0,z0) + 0.45015, + 0.24060, + -0.22970, + -0.43861, + -0.43861, + -0.22970, + 0.240600, + 0.450150, # x-positions of the thrusters + 0.305680, + 0.313022, + 0.313022, + 0.305680, + -0.305680, + -0.313022, + -0.313022, + -0.305680, # y-positions of the thrusters + 0.021736, + 0.021736, + 0.021736, + 0.021736, + 0.021736, + 0.021736, + 0.021736, + 0.021736, # z-positions of the thrusters + ] + + constraints: + max_force: 40.0 + min_force: -40.0 + input_matrix_weights: [1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0] + slack_matrix_weights: [2000.0,2000.0,2000.0,2000.0,2000.0,2000.0] + + rate_of_change: + max: 1 # Maximum rate of change in newton per second for a thruster + + thrust_update_rate: 100.0 # [Hz] + watchdog_timeout: 1.0 # [s] + thruster_to_pin_mapping: [4, 5, 7, 6, 0, 1, 2, 3] # I.e. if thruster_to_pin = [ 7, 6, 5, 4, 3, 2, 1, 0 ] then thruster 0 is pin 1 etc.. + thruster_direction: [-1, -1, 1, 1, -1, -1, 1, 1] # Disclose during thruster mapping (+/- 1) + thruster_PWM_min: [1050, 1050, 1050, 1050, 1050, 1050, 1050, 1050] # Minimum PWM value, Recommended: [1100, 1100, 1100, 1100, 1100, 1100, 1100, 1100] + thruster_PWM_max: [1950, 1950, 1950, 1950, 1950, 1950, 1950, 1950] # Maximum PWM value, Recommended: [1900, 1900, 1900, 1900, 1900, 1900, 1900, 1900] + + topics: + wrench_input: "wrench_input" + thruster_forces: "thruster_forces" + pwm_output: "pwm" + current: "power_sense_module/current" + voltage: "power_sense_module/voltage" + pressure: "pressure" + temperature: "temperature" + joy: "joy" + pose: "pose" + twist: "twist" + odom: "odom" + operation_mode: "operation_mode" + killswitch: "killswitch" + reference_pose: "reference_pose" + landmarks: "landmarks" + waypoint: "waypoint" + guidance: + los: "guidance/los" + dp_rpy: "guidance/dp_rpy" + dp_quat: "guidance/dp_quat" + dvl_twist: "dvl/twist" + dvl_altitude: "dvl/altitude" + imu: "imu/data_raw" + sonar_info: "fls/sonar_info" + pressure_sensor: "pressure_sensor" + gripper_servos: "gripper_servos" + + + action_servers: + reference_filter: "reference_filter" + los: "los_guidance" + landmark_polling: "landmark_polling" + landmark_convergence: "landmark_convergence" + waypoint_manager: "waypoint_manager" + + services: + set_operation_mode: "set_operation_mode" + set_killswitch: "set_killswitch" + toggle_killswitch: "toggle_killswitch" + get_operation_mode: "get_operation_mode" + waypoint_addition: "waypoint_addition" + start_mission: "start_mission" diff --git a/auv_setup/config/robots/orca.yaml b/auv_setup/config/robots/orca.yaml index 93425c34f..37111d771 100644 --- a/auv_setup/config/robots/orca.yaml +++ b/auv_setup/config/robots/orca.yaml @@ -134,7 +134,7 @@ waypoint: "waypoint" guidance: los: "guidance/los" - dp: "guidance/dp" + dp_rpy: "guidance/dp_rpy" dp_quat: "guidance/dp_quat" fsm: active_controller: "fsm/active_controller" @@ -156,6 +156,7 @@ set_killswitch: "set_killswitch" toggle_killswitch: "toggle_killswitch" get_operation_mode: "get_operation_mode" + los_mode: "set_los_mode" waypoint_addition: "waypoint_addition" start_mission: "start_mission" diff --git a/auv_setup/description/nautilus.urdf.xacro b/auv_setup/description/nautilus.urdf.xacro index 12b7aba27..ac43afe3c 100644 --- a/auv_setup/description/nautilus.urdf.xacro +++ b/auv_setup/description/nautilus.urdf.xacro @@ -18,7 +18,7 @@ - + @@ -46,7 +46,7 @@ - + @@ -66,7 +66,7 @@ - + diff --git a/auv_setup/launch/autopilot.launch.py b/auv_setup/launch/autopilot.launch.py index 3a84897c4..e282cd55d 100644 --- a/auv_setup/launch/autopilot.launch.py +++ b/auv_setup/launch/autopilot.launch.py @@ -5,55 +5,67 @@ from launch.actions import ( OpaqueFunction, ) -from launch_ros.actions import Node - +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration from auv_setup.launch_arg_common import ( declare_drone_and_namespace_args, resolve_drone_and_namespace, ) - - +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode 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", + drone_params = os.path.join( + get_package_share_directory('auv_setup'), + 'config', + 'robots', + f'{drone}.yaml', + ) + velocity_control_params = os.path.join( + get_package_share_directory('velocity_controller'), + 'config', + f'{drone}_params.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", - ) - - los_node = Node( - package="los_guidance", - executable="los_guidance_node", - name="los_guidance_node", + container=ComposableNodeContainer( + name='autopilot_container', namespace=namespace, - parameters=[drone_params, los_config], - output="screen", - ) + 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}], + ), - lqr_node = Node( - package="velocity_controller_lqr", - executable="velocity_controller_lqr_node.py", - name="velocity_controller_lqr_node", - namespace=namespace, - output="screen", - parameters=[drone_params, velocity_lqr_config], + ], + output='screen', + arguments=['--ros-args','--log-level','error'], ) - - return [los_node, lqr_node] + return [container] + def generate_launch_description(): diff --git a/auv_setup/launch/dp.launch.py b/auv_setup/launch/dp.launch.py index f178cb1a7..efe5e023a 100644 --- a/auv_setup/launch/dp.launch.py +++ b/auv_setup/launch/dp.launch.py @@ -1,11 +1,10 @@ import os - from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import OpaqueFunction -from launch_ros.actions import ComposableNodeContainer +from launch.actions import OpaqueFunction, DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import ComposableNodeContainer, Node from launch_ros.descriptions import ComposableNode - from auv_setup.launch_arg_common import ( declare_drone_and_namespace_args, resolve_drone_and_namespace, @@ -14,12 +13,7 @@ def launch_setup(context, *args, **kwargs): drone, namespace = resolve_drone_and_namespace(context) - - filter_file_path = os.path.join( - get_package_share_directory("reference_filter_dp"), - "config", - "reference_filter_params.yaml", - ) + controller_type = LaunchConfiguration("controller_type").perform(context).lower() drone_params = os.path.join( get_package_share_directory("auv_setup"), @@ -27,47 +21,104 @@ def launch_setup(context, *args, **kwargs): "robots", f"{drone}.yaml", ) - - adapt_params = os.path.join( - get_package_share_directory("dp_adapt_backs_controller"), + filter_file_path = os.path.join( + get_package_share_directory("reference_filter_dp"), "config", + "reference_filter_params.yaml", f"adapt_params_{drone}.yaml", ) - container = ComposableNodeContainer( - name="dp_container", - namespace=namespace, - package="rclcpp_components", - executable="component_container_mt", - composable_node_descriptions=[ - ComposableNode( - package="reference_filter_dp", - plugin="ReferenceFilterNode", - name="reference_filter_node", - namespace=namespace, - parameters=[filter_file_path, drone_params], - extra_arguments=[{"use_intra_process_comms": True}], - ), - ComposableNode( - package="dp_adapt_backs_controller", - plugin="DPAdaptBacksControllerNode", - name="dp_adapt_backs_controller_node", - namespace=namespace, - parameters=[adapt_params, drone_params], - extra_arguments=[{"use_intra_process_comms": True}], - ), - ], - output="screen", - arguments=["--ros-args", "--log-level", "error"], - ) + nodes = [] + + if controller_type == "adaptive": + adapt_params = os.path.join( + get_package_share_directory("dp_adapt_backs_controller"), + "config", + f"adapt_params_{drone}.yaml", + ) + container = ComposableNodeContainer( + name="dp_container", + namespace=namespace, + package="rclcpp_components", + executable="component_container_mt", + composable_node_descriptions=[ + ComposableNode( + package="reference_filter_dp", + plugin="ReferenceFilterNode", + name="reference_filter_node", + namespace=namespace, + parameters=[filter_file_path, drone_params], + extra_arguments=[{"use_intra_process_comms": True}], + ), + ComposableNode( + package="dp_adapt_backs_controller", + plugin="DPAdaptBacksControllerNode", + name="dp_adapt_backs_controller_node", + namespace=namespace, + parameters=[adapt_params, drone_params], + extra_arguments=[{"use_intra_process_comms": True}], + ), + ], + output="screen", + arguments=["--ros-args", "--log-level", "error"], + ) + nodes.append(container) - return [container] + elif controller_type == "pid": + pid_params = os.path.join( + get_package_share_directory("pid_controller_dp"), + "config", + "pid_params.yaml", + ) + filter_quat_file_path = os.path.join( + get_package_share_directory("reference_filter_dp_quat"), + "config", + "reference_filter_params.yaml", + ) + container = ComposableNodeContainer( + name="dp_container", + namespace=namespace, + package="rclcpp_components", + executable="component_container_mt", + composable_node_descriptions=[ + ComposableNode( + package="reference_filter_dp_quat", + plugin="ReferenceFilterNode", + name="reference_filter_node", + namespace=namespace, + parameters=[filter_quat_file_path, drone_params], + extra_arguments=[{"use_intra_process_comms": True}], + ), + ComposableNode( + package="pid_controller_dp", + plugin="PIDControllerNode", + name="pid_controller_node", + namespace=namespace, + parameters=[pid_params, drone_params], + extra_arguments=[{"use_intra_process_comms": True}], + ), + ], + output="screen", + ) + nodes.append(container) + else: + raise ValueError( + f"Unknown controller_type '{controller_type}'. " + f"Expected 'adaptive' or 'pid'." + ) + + return nodes def generate_launch_description(): return LaunchDescription( declare_drone_and_namespace_args() + [ + DeclareLaunchArgument( + "controller_type", + default_value="adaptive", + description="Controller to use: 'adaptive' or 'pid'", + ), OpaqueFunction(function=launch_setup), ] ) diff --git a/auv_setup/launch/dp_quat.launch.py b/auv_setup/launch/dp_quat.launch.py new file mode 100644 index 000000000..dd77414ae --- /dev/null +++ b/auv_setup/launch/dp_quat.launch.py @@ -0,0 +1,60 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import OpaqueFunction +from launch_ros.actions import Node + +from auv_setup.launch_arg_common import ( + declare_drone_and_namespace_args, + resolve_drone_and_namespace, +) + + +def launch_setup(context, *args, **kwargs): + drone, namespace = resolve_drone_and_namespace(context) + + filter_config = os.path.join( + get_package_share_directory("reference_filter_dp_quat"), + "config", + "reference_filter_params.yaml", + ) + + drone_params = os.path.join( + get_package_share_directory("auv_setup"), + "config", + "robots", + f"{drone}.yaml", + ) + + adapt_params = os.path.join( + get_package_share_directory("dp_adapt_backs_controller_quat"), + "config", + f"adapt_params_{drone}.yaml", + ) + + return [ + Node( + package="reference_filter_dp_quat", + executable="reference_filter_dp_quat_node", + name="reference_filter_node", + namespace=namespace, + parameters=[filter_config, drone_params], + output="screen", + ), + Node( + package="dp_adapt_backs_controller_quat", + executable="dp_adapt_backs_controller_quat_node", + name="dp_adapt_backs_controller_node", + namespace=namespace, + parameters=[adapt_params, drone_params], + output="screen", + ), + ] + + +def generate_launch_description(): + return LaunchDescription( + declare_drone_and_namespace_args() + + [OpaqueFunction(function=launch_setup)] + ) diff --git a/auv_setup/launch/nucleus_odom_transformer.launch.py b/auv_setup/launch/nucleus_odom_transformer.launch.py new file mode 100644 index 000000000..a1aa0bd31 --- /dev/null +++ b/auv_setup/launch/nucleus_odom_transformer.launch.py @@ -0,0 +1,122 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription, OpaqueFunction +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node + +from auv_setup.launch_arg_common import ( + declare_drone_and_namespace_args, + resolve_drone_and_namespace, +) + + +def launch_setup(context, *args, **kwargs): + drone, namespace = resolve_drone_and_namespace(context) + + drone_params = os.path.join( + get_package_share_directory("auv_setup"), + "config", + "robots", + f"{drone}.yaml", + ) + + drone_description_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join( + get_package_share_directory("auv_setup"), + "launch", + "drone_description.launch.py", + ) + ), + launch_arguments={ + "drone": drone, + "namespace": namespace, + }.items(), + ) + + nortek_nucleus_ros_interface_node = Node( + package="nortek_nucleus_ros_interface", + executable="nortek_nucleus_ros_interface_node", + name="nortek_nucleus_ros_interface_node", + namespace=namespace, + parameters=[ + { + "frame_id": f"{namespace}/nucleus_frame", + "connection_params.remote_ip": "10.0.0.42", + "connection_params.data_remote_port": 9000, + "connection_params.password": "", + "reset_pose_on_start": True, + "enable_imu": True, + "enable_ins_odom": True, + "enable_dvl": True, + "enable_pressure": True, + "enable_altimeter": True, + "enable_magnetometer": False, + "enable_ins_twist": False, + "enable_ins_position": False, + "enable_ins_pose": False, + "imu_data_raw_pub_topic": f"/{namespace}/imu/data_raw", + "imu_data_pub_topic": f"/{namespace}/imu/data", + "ins_pub_topic": f"/{namespace}/nucleus/odom", + "dvl_pub_topic": f"/{namespace}/nucleus/dvl", + "pressure_pub_topic": f"/{namespace}/nucleus/pressure", + "altimeter_pub_topic": f"/{namespace}/nucleus/altitude", + "magnetometer_pub_topic": f"/{namespace}/imu/mag", + "ins_twist_pub_topic": f"/{namespace}/nucleus/ins/twist", + "ins_position_pub_topic": f"/{namespace}/nucleus/ins/position", + "ins_pose_pub_topic": f"/{namespace}/nucleus/ins/pose", + "imu_settings.freq": 125, + "ahrs_settings.freq": 10, + "ahrs_settings.mode": 0, + "bottom_track_settings.mode": 2, + "bottom_track_settings.velocity_range": 5, + "bottom_track_settings.enable_watertrack": False, + "altimeter_settings.power_level": 0, # 0=default + "magnetometer_settings.freq": 75, + "magnetometer_settings.mode": 0, + "instrument_settings.rotxy": 180.0, + "instrument_settings.rotyz": 0.0, + "instrument_settings.rotxz": 0.0, + }, + drone_params, + ], + output="screen", + ) + + odom_transformer_node = Node( + package="odom_transformer", + executable="odom_transformer_node", + name="odom_transformer_node", + namespace=namespace, + parameters=[ + { + "sensor_frame": "dvl_link", + "publish_tf": True, + "publish_pose": True, + "publish_twist": True, + "rotate_yaw_180": True, + "topics.input": f"/{namespace}/nucleus/odom", + "topics.output": f"/{namespace}/odom", + "topics.pose": f"/{namespace}/pose", + "topics.twist": f"/{namespace}/twist", + }, + drone_params, + {"frame_prefix": namespace}, + ], + output="screen", + ) + + return [ + drone_description_launch, + nortek_nucleus_ros_interface_node, + odom_transformer_node, + ] + + +def generate_launch_description(): + return LaunchDescription( + declare_drone_and_namespace_args() + + [OpaqueFunction(function=launch_setup)] + ) \ No newline at end of file diff --git a/auv_setup/launch/state_estimation.launch.py b/auv_setup/launch/state_estimation.launch.py new file mode 100644 index 000000000..a7520cb90 --- /dev/null +++ b/auv_setup/launch/state_estimation.launch.py @@ -0,0 +1,187 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription, OpaqueFunction +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + +from auv_setup.launch_arg_common import ( + declare_drone_and_namespace_args, + resolve_drone_and_namespace, +) + + +def launch_setup(context, *args, **kwargs): + drone, namespace = resolve_drone_and_namespace(context) + + drone_params = os.path.join( + get_package_share_directory("auv_setup"), + "config", + "robots", + f"{drone}.yaml", + ) + + drone_description_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join( + get_package_share_directory("auv_setup"), + "launch", + "drone_description.launch.py", + ) + ), + launch_arguments={ + "drone": drone, + "namespace": namespace, + }.items(), + ) + + container = ComposableNodeContainer( + name="eskf_container", + namespace=namespace, + package="rclcpp_components", + executable="component_container_mt", + composable_node_descriptions=[ + ComposableNode( + package="eskf", + plugin="ESKFNode", + name="eskf_node", + namespace=namespace, + parameters=[ + drone_params, + { + "diag_Q_std": [ + 0.05, 0.05, 0.1, + 0.01, 0.01, 0.02, + 0.001, 0.001, 0.001, + 0.0001, 0.0001, 0.0001 + ], + + "diag_p_init": [ + 1.0, 1.0, 1.0, + 0.5, 0.5, 0.5, + 0.1, 0.1, 0.1, + 0.001, 0.001, 0.001, + 0.001, 0.001, 0.001 + ], + + "transform.imu_frame_r": [ + -1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, -1.0 + ], + "transform.imu_frame_t": [ + 0.0, 0.0, 0.0 + ], + + "transform.dvl_frame_r": [ + 0.0, -1.0, 0.0, + 1.0, 0.0, 0.0, + 0.0, 0.0, 1.0 + ], + "transform.dvl_frame_t": [ + 0.4, 0.0, 0.2 + ], + + "transform.depth_frame_t": [ + 0.0, 0.0, 0.0 + ], + + "use_tf_transforms": True, + "publish_tf": True, + "publish_pose": True, + "publish_twist": True, + "publish_rate_ms": 5, + "add_gravity_to_imu": True, + "frame_prefix": namespace, + "initial_gyro_bias": [0.0, 0.0, 0.0], + "initial_accel_bias": [0.0, 0.0, -0.05] + }, + ], + extra_arguments=[{"use_intra_process_comms": True}], + ), + ComposableNode( + package="mru_ros_interface", + plugin="MruRosInterface", + name="mru_ros_interface_node", + namespace=namespace, + parameters=[ + { + "imu_pub_topic": f"/{namespace}/imu/data_raw", + "frame_id": f"/{namespace}/imu_link", + "connection_params.remote_ip": "10.0.1.20", # MRU IP + "connection_params.data_remote_port": 7550, + "connection_params.data_local_port": 7551, + "connection_params.control_local_port": 7552, + "mru_settings.channel": "UDP1", + "mru_settings.port": 7551, + "mru_settings.ip_addr": "10.0.0.69", # Host computer IP + "mru_settings.format": "MRUBIN", + "mru_settings.interval": 5, + "mru_settings.token": 21, + } + ], + extra_arguments=[{"use_intra_process_comms": True}], + ), + ComposableNode( + package="nortek_nucleus_ros_interface", + plugin="NortekNucleusRosInterface", + name="nortek_nucleus_ros_interface_node", + namespace=namespace, + parameters=[ + { + "frame_id": f"/{namespace}/nucleus_frame", + "connection_params.remote_ip": "10.0.0.42", + "connection_params.data_remote_port": 9000, + "connection_params.password": "", + "reset_pose_on_start": True, + "enable_imu": False, + "enable_ins_odom": False, + "enable_dvl": True, + "enable_pressure": True, + "enable_magnetometer": False, + "enable_ins_twist": False, + "enable_ins_position": False, + "enable_ins_pose": False, + "imu_data_raw_pub_topic": f"/{namespace}/nucleus/imu/data_raw", + "imu_data_pub_topic": f"/{namespace}/imu/data", + "ins_pub_topic": f"/{namespace}/nucleus/odom", + "dvl_pub_topic": f"/{namespace}/nucleus/dvl", + "pressure_pub_topic": f"/{namespace}/nucleus/pressure", + "magnetometer_pub_topic": f"/{namespace}/imu/mag", + "ins_twist_pub_topic": f"/{namespace}/nucleus/ins/twist", + "ins_position_pub_topic": f"/{namespace}/nucleus/ins/position", + "ins_pose_pub_topic": f"/{namespace}/nucleus/ins/pose", + "imu_settings.freq": 125, + "ahrs_settings.freq": 10, + "ahrs_settings.mode": 0, + "bottom_track_settings.mode": 2, + "bottom_track_settings.velocity_range": 5, + "bottom_track_settings.enable_watertrack": False, + "fast_pressure_settings.enable": True, + "fast_pressure_settings.sampling_rate": 16, + "magnetometer_settings.freq": 75, + "magnetometer_settings.mode": 0, + "instrument_settings.rotxy": 180.0, + "instrument_settings.rotyz": 0.0, + "instrument_settings.rotxz": 0.0, + } + ], + extra_arguments=[{"use_intra_process_comms": True}], + ), + ], + output="screen", + arguments=["--ros-args", "--log-level", "error"], + ) + + return [drone_description_launch, container] + + +def generate_launch_description(): + return LaunchDescription( + declare_drone_and_namespace_args() + + [ + OpaqueFunction(function=launch_setup), + ] + ) \ No newline at end of file diff --git a/auv_setup/launch/thruster.launch.py b/auv_setup/launch/thruster.launch.py index 3d49274c8..20c87869f 100755 --- a/auv_setup/launch/thruster.launch.py +++ b/auv_setup/launch/thruster.launch.py @@ -2,10 +2,12 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import OpaqueFunction, SetEnvironmentVariable +from launch.actions import DeclareLaunchArgument, OpaqueFunction, SetEnvironmentVariable from launch_ros.actions import ComposableNodeContainer +from launch.substitutions import LaunchConfiguration from launch_ros.descriptions import ComposableNode + from auv_setup.launch_arg_common import ( declare_drone_and_namespace_args, resolve_drone_and_namespace, @@ -14,6 +16,8 @@ def launch_setup(context, *args, **kwargs): drone, namespace = resolve_drone_and_namespace(context) + solver_type = LaunchConfiguration("solver_type").perform(context) + drone_params = os.path.join( get_package_share_directory("auv_setup"), @@ -39,7 +43,10 @@ def launch_setup(context, *args, **kwargs): plugin="ThrustAllocator", name="thrust_allocator_auv_node", namespace=namespace, - parameters=[drone_params], + parameters=[ + drone_params, + {"propulsion.solver_type": solver_type}, + ], extra_arguments=[{"use_intra_process_comms": True}], ), ComposableNode( @@ -78,5 +85,12 @@ def generate_launch_description() -> LaunchDescription: return LaunchDescription( [set_env_var] + declare_drone_and_namespace_args() - + [OpaqueFunction(function=launch_setup)] + + [ + DeclareLaunchArgument( + "solver_type", + default_value="qp", + description="Thrust allocator solver type (available: pseudoinverse, qp)", + ), + OpaqueFunction(function=launch_setup) + ] ) diff --git a/control/dp_adapt_backs_controller/config/adapt_params_nautilus.yaml b/control/dp_adapt_backs_controller/config/adapt_params_nautilus.yaml index 65b639ed7..049309462 100644 --- a/control/dp_adapt_backs_controller/config/adapt_params_nautilus.yaml +++ b/control/dp_adapt_backs_controller/config/adapt_params_nautilus.yaml @@ -1,7 +1,7 @@ /**: ros__parameters: - K1 : [15.0, 15.0, 15.0, 1.5, 6.0, 6.0] # Outer loop tuning parameters - K2 : [30.0, 30.0, 30.0, 2.0, 8.5, 8.5] # Inner loop tuning parameters - r_b_bg : [0.0, 0.0, 0.01] # Vector from body centre to centre of gravity - adapt_gain : [0.6, 0.15, 0.6, 0.15, 0.6, 0.15, 0.6, 0.15, 0.6, 0.15, 0.6, 0.15] # Tuning parameters for linear and nonlinear damping - d_gain : [0.8, 0.8, 0.8, 0.8, 0.8, 0.8] # Tuning parameters for the unmodeled disturbances and uncertanties + K1 : [0.1, 0.1, 10.0, 0.1, 0.1, 10.0] # Outer loop tuning parameters + K2 : [10.0, 10.0, 250.0, 10.0, 10.0, 250.0] # Inner loop tuning parameters + r_b_bg : [0.01, 0.0, 0.015] # Vector from body centre to centre of gravity + adapt_gain : [0.1, 0.05, 0.1, 0.05, 0.1, 0.05, 0.1, 0.05, 0.1, 0.05, 0.1, 0.05] # Tuning parameters for linear and nonlinear damping + d_gain : [0.1, 0.1, 0.1, 0.1, 0.1, 0.1] # Tuning parameters for the unmodeled disturbances and uncertanties diff --git a/control/dp_adapt_backs_controller/config/adapt_params_nautilus_sim.yaml b/control/dp_adapt_backs_controller/config/adapt_params_nautilus_sim.yaml new file mode 100644 index 000000000..907f90868 --- /dev/null +++ b/control/dp_adapt_backs_controller/config/adapt_params_nautilus_sim.yaml @@ -0,0 +1,7 @@ +/**: + ros__parameters: + K1 : [5.0, 5.0, 9.5, 1.5, 5.0, 9.5] # Outer loop tuning parameters + K2 : [25.0, 25.0, 40.0, 25.0, 25.0, 40.0] # Inner loop tuning parameters + r_b_bg : [0.00, 0.0, 0.015] # Vector from body centre to centre of gravity + adapt_gain : [0.4, 0.2, 0.4, 0.2, 0.4, 0.2, 0.4, 0.2, 0.4, 0.2, 0.4, 0.2] # Tuning parameters for linear and nonlinear damping + d_gain : [0.2, 0.2, 0.2, 0.2, 0.2, 0.2] # Tuning parameters for the unmodeled disturbances and uncertanties diff --git a/control/dp_adapt_backs_controller/config/adapt_params_orca.yaml b/control/dp_adapt_backs_controller/config/adapt_params_orca.yaml deleted file mode 100644 index e128131b2..000000000 --- a/control/dp_adapt_backs_controller/config/adapt_params_orca.yaml +++ /dev/null @@ -1,7 +0,0 @@ -/**: - ros__parameters: - K1 : [10.5, 10.5, 13.5, 0.0, 4.0, 4.0] # Outer loop tuning parameters - K2 : [20.5, 20.5, 20.5, 0.0, 5.5, 5.5] # Inner loop tuning parameters - r_b_bg : [0.01, 0.0, 0.02] # Vector from body centre to centre of gravity - adapt_gain : [0.4, 0.1, 0.4, 0.1, 0.4, 0.1, 0.4, 0.1, 0.4, 0.1, 0.4, 0.1] # Tuning parameters for linear and nonlinear damping - d_gain : [0.6, 0.6, 0.6, 0.6, 0.6, 0.6] # Tuning parameters for the unmodeled disturbances and uncertanties diff --git a/control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/dp_adapt_backs_controller.hpp b/control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/dp_adapt_backs_controller.hpp index a26969cee..fd87d998b 100644 --- a/control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/dp_adapt_backs_controller.hpp +++ b/control/dp_adapt_backs_controller/include/dp_adapt_backs_controller/dp_adapt_backs_controller.hpp @@ -15,6 +15,7 @@ struct DPAdaptParams { Eigen::Vector3d r_b_bg = Eigen::Vector3d::Zero(); Eigen::Vector3d I_b = Eigen::Vector3d::Zero(); Eigen::Matrix6d mass_matrix = Eigen::Matrix6d::Zero(); + Eigen::Vector6d tau_max = Eigen::Vector6d::Ones(); double mass{}; }; @@ -54,6 +55,7 @@ class DPAdaptBacksController { Eigen::Vector6d d_est_; Eigen::Matrix3d I_b_; Eigen::Matrix6d mass_matrix_; + Eigen::Vector6d tau_max_; double m_{}; double dt_{}; }; diff --git a/control/dp_adapt_backs_controller/src/dp_adapt_backs_controller.cpp b/control/dp_adapt_backs_controller/src/dp_adapt_backs_controller.cpp index e009d7f90..54ae228f1 100644 --- a/control/dp_adapt_backs_controller/src/dp_adapt_backs_controller.cpp +++ b/control/dp_adapt_backs_controller/src/dp_adapt_backs_controller.cpp @@ -21,6 +21,7 @@ DPAdaptBacksController::DPAdaptBacksController( d_est_(Eigen::Vector6d::Zero()), I_b_(dp_adapt_params.I_b.asDiagonal().toDenseMatrix()), mass_matrix_(dp_adapt_params.mass_matrix), + tau_max_(dp_adapt_params.tau_max), m_(dp_adapt_params.mass), dt_(0.01) {} @@ -49,7 +50,7 @@ Eigen::Vector6d DPAdaptBacksController::calculate_tau(const PoseEuler& pose, (pose.as_j_matrix().transpose() * z_1) - (K2_ * z_2) - F_est - d_est_; - tau = tau.cwiseMax(-100.0).cwiseMin(100.0); + tau = tau.cwiseMax(-tau_max_).cwiseMin(tau_max_); adapt_param_ += adapt_param_dot * dt_; d_est_ += d_est_dot * dt_; adapt_param_ = adapt_param_.cwiseMax(-10.0).cwiseMin(10.0); diff --git a/control/dp_adapt_backs_controller/src/dp_adapt_backs_controller_ros.cpp b/control/dp_adapt_backs_controller/src/dp_adapt_backs_controller_ros.cpp index 8e6c884ce..f6be06b64 100644 --- a/control/dp_adapt_backs_controller/src/dp_adapt_backs_controller_ros.cpp +++ b/control/dp_adapt_backs_controller/src/dp_adapt_backs_controller_ros.cpp @@ -40,9 +40,9 @@ void DPAdaptBacksControllerNode::set_subscribers_and_publisher() { vortex::utils::qos_profiles::sensor_data_profile(1)}; const auto qos_reliable{vortex::utils::qos_profiles::reliable_profile(1)}; - this->declare_parameter("topics.guidance.dp"); + this->declare_parameter("topics.guidance.dp_rpy"); std::string dp_reference_topic = - this->get_parameter("topics.guidance.dp").as_string(); + this->get_parameter("topics.guidance.dp_rpy").as_string(); guidance_sub_ = this->create_subscription( dp_reference_topic, qos_sensor_data, @@ -177,6 +177,17 @@ void DPAdaptBacksControllerNode::set_adap_params() { this->declare_parameter>("K2"); this->declare_parameter>("r_b_bg"); this->declare_parameter>("physical.mass_matrix"); + this->declare_parameter>("physical.center_of_mass"); + this->declare_parameter>( + "propulsion.thrusters.thruster_force_direction"); + this->declare_parameter>( + "propulsion.thrusters.thruster_position"); + this->declare_parameter("propulsion.thrusters.num"); + this->declare_parameter("propulsion.dimensions.num"); + this->declare_parameter( + "propulsion.thrusters.constraints.min_force"); + this->declare_parameter( + "propulsion.thrusters.constraints.max_force"); std::vector adapt_param_vec = this->get_parameter("adapt_gain").as_double_array(); @@ -204,6 +215,54 @@ void DPAdaptBacksControllerNode::set_adap_params() { Eigen::Vector3d I_b_eigen(mass_matrix(3, 3), mass_matrix(4, 4), mass_matrix(5, 5)); + // Compute per-DOF max wrench from the thruster configuration + int num_thrusters = + this->get_parameter("propulsion.thrusters.num").as_int(); + int num_dims = this->get_parameter("propulsion.dimensions.num").as_int(); + double min_force = + this->get_parameter("propulsion.thrusters.constraints.min_force") + .as_double(); + double max_force = + this->get_parameter("propulsion.thrusters.constraints.max_force") + .as_double(); + + Eigen::Vector3d center_of_mass = Eigen::Map( + this->get_parameter("physical.center_of_mass") + .as_double_array() + .data()); + + auto dir_vec = + this->get_parameter("propulsion.thrusters.thruster_force_direction") + .as_double_array(); + auto pos_vec = this->get_parameter("propulsion.thrusters.thruster_position") + .as_double_array(); + + Eigen::MatrixXd thruster_dir = + Eigen::Map>( + dir_vec.data(), num_dims, num_thrusters); + Eigen::MatrixXd thruster_pos = + Eigen::Map>( + pos_vec.data(), num_dims, num_thrusters); + + Eigen::MatrixXd T = Eigen::MatrixXd::Zero(6, num_thrusters); + for (int i = 0; i < num_thrusters; i++) { + Eigen::Vector3d pos = thruster_pos.col(i) - center_of_mass; + Eigen::Vector3d F = thruster_dir.col(i); + T.block<3, 1>(0, i) = F; + T.block<3, 1>(3, i) = pos.cross(F); + } + + Eigen::Vector6d tau_max; + for (int i = 0; i < 6; i++) { + double w = 0.0; + for (int j = 0; j < num_thrusters; j++) { + w += (T(i, j) > 0) ? T(i, j) * max_force : T(i, j) * min_force; + } + tau_max(i) = w; + } + DPAdaptParams dp_adapt_params; dp_adapt_params.adapt_param = adapt_param_eigen; dp_adapt_params.d_gain = d_gain_eigen; @@ -212,6 +271,7 @@ void DPAdaptBacksControllerNode::set_adap_params() { dp_adapt_params.r_b_bg = r_b_bg_eigen; dp_adapt_params.I_b = I_b_eigen; dp_adapt_params.mass_matrix = mass_matrix; + dp_adapt_params.tau_max = tau_max; dp_adapt_params.mass = mass; dp_adapt_backs_controller_ = @@ -231,13 +291,12 @@ void DPAdaptBacksControllerNode::publish_tau() { geometry_msgs::msg::WrenchStamped tau_msg; tau_msg.header.stamp = this->now(); tau_msg.header.frame_id = "base_link"; - tau_msg.wrench.force.x = tau(0); - tau_msg.wrench.force.y = tau(1); + tau_msg.wrench.force.x = -tau(0); + tau_msg.wrench.force.y = -tau(1); tau_msg.wrench.force.z = tau(2); // comment out if roll control is not needed tau_msg.wrench.torque.x = tau(3); - tau_msg.wrench.torque.y = tau(4); tau_msg.wrench.torque.z = tau(5); diff --git a/control/dp_adapt_backs_controller_quat/CMakeLists.txt b/control/dp_adapt_backs_controller_quat/CMakeLists.txt new file mode 100644 index 000000000..0fac2659e --- /dev/null +++ b/control/dp_adapt_backs_controller_quat/CMakeLists.txt @@ -0,0 +1,79 @@ +cmake_minimum_required(VERSION 3.8) +project(dp_adapt_backs_controller_quat) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 20) +endif() + +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(vortex_utils REQUIRED) +find_package(vortex_utils_ros REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(tf2 REQUIRED) +find_package(vortex_msgs REQUIRED) +find_package(fmt REQUIRED) +find_package(spdlog REQUIRED) + + +include_directories(include) + +set(LIB_NAME "${PROJECT_NAME}_component") + +add_library(${LIB_NAME} SHARED + src/dp_adapt_backs_controller.cpp + src/dp_adapt_backs_controller_ros.cpp + src/dp_adapt_backs_controller_utils.cpp) + +ament_target_dependencies(${LIB_NAME} PUBLIC + rclcpp + rclcpp_components + geometry_msgs + nav_msgs + Eigen3 + tf2 + fmt + spdlog + vortex_msgs + vortex_utils + vortex_utils_ros +) + +rclcpp_components_register_node( + ${LIB_NAME} + PLUGIN "DPAdaptBacksControllerNode" + EXECUTABLE ${PROJECT_NAME}_node +) + +ament_export_targets(export_${LIB_NAME}) + +install(TARGETS ${LIB_NAME} + EXPORT export_${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(test) +endif() + +ament_package() diff --git a/control/dp_adapt_backs_controller_quat/README.md b/control/dp_adapt_backs_controller_quat/README.md new file mode 100644 index 000000000..b856f5372 --- /dev/null +++ b/control/dp_adapt_backs_controller_quat/README.md @@ -0,0 +1,334 @@ +## DP Adaptive Backstepping Controller — Quaternion Formulation + +This package implements a dynamic positioning (DP) adaptive backstepping controller for the Nautilus AUV. It uses a **unit-quaternion error-state formulation** for the attitude kinematics, which eliminates the gimbal-lock singularity that occurs at ±90° pitch in the Euler-angle version. The adaptive terms estimate the linear and quadratic damping online and compensate for unmodelled disturbances, in the same spirit as a model reference adaptive controller. Stability and convergence are proven via a composite Lyapunov function. + +### Overview +- Uses the backstepping control method for position and orientation control +- Replaces the Euler-angle Jacobian $J(\eta)$ with the **quaternion error-state Jacobian** $J_e(\eta)$ (called `L` in the code), which is square, smooth, and invertible for all attitude errors smaller than 180° +- Includes adaptive terms to estimate linear and nonlinear (quadratic) damping and external disturbances + +### Model for AUV + +**Kinematics (error-state form):** + +```math +\dot{z}_1 = J_e(\eta)\,\nu +``` + +where the error state $z_1 \in \mathbb{R}^6$ is defined below, and the error-state Jacobian is: + +```math +J_e(\eta) = +\begin{bmatrix} +R(q) & 0_{3\times 3} \\ +0_{3\times 3} & T_e(q_e) +\end{bmatrix}, \quad +T_e(q_e) = \eta_e I_3 + S(\varepsilon_e) +``` + +Here $R(q) \in SO(3)$ is the rotation matrix from NED to body, $q_e = q_d^* \otimes q = \begin{bmatrix}\eta_e \\ \varepsilon_e\end{bmatrix}$ is the error quaternion, and $S(\cdot)$ is the skew-symmetric (cross-product) matrix. + +**Dynamics (Newton–Euler, body frame):** + +```math +M\dot{\nu} + C(\nu)\,\nu - F(\nu,\Theta^\star) = \tau + d^\star +``` + +- $\nu \in \mathbb{R}^6$: body-fixed velocity (linear and angular) +- $M$: constant, symmetric, positive-definite mass-inertia matrix +- $C(\nu)$: Coriolis and centripetal matrix +- $F(\nu, \Theta^\star) = Y(\nu)\Theta^\star$: damping in regressor form (linear and quadratic per DOF) +- $\tau$: control wrench +- $d^\star$: lumped disturbances and unmodelled dynamics + +**Notation note:** The symbol $\eta_e$ denotes the scalar part of the error quaternion (not the pose vector $\eta$); this follows Fossen's standard overloading. + +### File overview + +1. **dp_adapt_backs_controller.cpp/hpp** + - Core controller implementation: computes `L`, `L_inv`, `L_dot`, the error state $z_1$, $z_2$, $\alpha$, $\dot\alpha$, and the full control wrench $\tau$. + - Integrates the adaptive parameters online. + +2. **dp_adapt_backs_controller_utils.cpp/hpp** + - Utility functions: `calculate_L_inv`, `calculate_R_dot`, `calculate_Q_dot`, `calculate_L_dot`, `calculate_coriolis`, `calculate_Y_v`. + +3. **dp_adapt_backs_controller_ros.cpp/hpp** + - ROS 2 node wrapper: subscribes to odometry, killswitch, and reference topics; publishes thrust commands. + +4. **adapt_params_nautilus.yaml / adapt_params_nautilus_sim.yaml** + - Tunable controller parameters (`K1`, `K2`, `adapt_gain`, `d_gain`, `r_b_bg`, `time_step`). + +5. **CMakeLists.txt** + - Build configuration, ROS 2 dependencies, executable generation, and installation setup. + +### Tuning Parameters +- **K1**: Outer loop gain matrix (position and orientation errors $z_1$). +- **K2**: Inner loop gain matrix (velocity error $z_2$). +- **adapt\_gain**: Diagonal adaptation rate for the 12 damping parameters ($\Gamma_\theta$). +- **d\_gain**: Diagonal adaptation rate for the 6 disturbance estimates ($\Gamma_d$). +- **r\_b\_bg**: Vector from body origin to centre of gravity (used in the Coriolis matrix). + +## Backstepping Controller + +### Error state and backstepping variables + +The orientation error quaternion is formed by left-multiplication with the desired quaternion conjugate: + +```math +q_e = q_d^* \otimes q = \begin{bmatrix} \eta_e \\ \varepsilon_e \end{bmatrix} +``` + +The tracking error in $\mathbb{R}^6$ is then: + +```math +z_1 = \begin{bmatrix} p - p_d \\ 2\varepsilon_e \end{bmatrix} +``` + +The factor of 2 on the vector part of the error quaternion ensures $\dot{z}_{1,\text{ori}} = T_e(q_e)\,\omega$ and makes the LFC derivative tractable. The velocity error is: + +```math +z_2 = \nu - \alpha +``` + +where $\alpha$ is the virtual control law defined below. + +### Adaptive parameters + +```math +\tilde{\Theta} = \hat{\Theta} - \Theta^\star, \qquad \tilde{d} = \hat{d} - d^\star +``` + +where: +- $\Theta^\star$ and $d^\star$ are the (unknown) true parameters +- $\hat{\Theta}$ and $\hat{d}$ are online estimates +- $\tilde{\Theta}$ and $\tilde{d}$ are the estimation errors + +### Proof of control law + +#### Step 1 — Outer loop (position and attitude) + +Define the LFC: + +```math +V_1 = \frac{1}{2} z_1^\top z_1 +``` + +which is positive definite, radially unbounded, and satisfies $V_1(0) = 0$. For a constant setpoint ($\dot{\eta}_d = 0$): + +```math +\dot{V}_1 = z_1^\top \dot{z}_1 = z_1^\top J_e(\eta)\,\nu +``` + +Treating $\nu$ as a virtual input (Khalil §14.3) and splitting $\nu = \alpha + z_2$: + +```math +\dot{V}_1 = z_1^\top J_e\,\alpha + z_1^\top J_e\,z_2 +``` + +Choose the virtual control law: + +```math +\boxed{ +\alpha = -J_e(\eta)^{-1} K_1\, z_1, \quad K_1 = K_1^\top > 0 +} +``` + +Then $z_1^\top J_e\,\alpha = -z_1^\top K_1 z_1 < 0$ and: + +```math +\dot{V}_1 = -z_1^\top K_1 z_1 + z_1^\top J_e\, z_2 +``` + +The cross term $z_1^\top J_e z_2$ will be cancelled in Step 2. + +#### Step 2 — Inner loop (velocity) + +Augment the LFC with the inertia-weighted velocity term (Fossen 2021, §12.1): + +```math +V_2 = \frac{1}{2} z_2^\top M\, z_2, \quad M = M^\top > 0,\; \dot{M} = 0 +``` + +Differentiating and substituting the dynamics: + +```math +\dot{V}_2 = z_2^\top M\,(\dot{\nu} - \dot{\alpha}) = z_2^\top\bigl(\tau - C(\nu)\nu + Y(\nu)\Theta^\star + d^\star - M\dot{\alpha}\bigr) +``` + +**Cross-term cancellation.** The scalar $z_1^\top J_e z_2 = z_2^\top J_e^\top z_1$. If the control law contains $-J_e^\top z_1$, then in $\dot{V}_1 + \dot{V}_2$: + +```math +z_1^\top J_e\, z_2 + z_2^\top(-J_e^\top z_1) = z_2^\top J_e^\top z_1 - z_2^\top J_e^\top z_1 = 0 +``` + +The cross terms cancel exactly, independent of the structure of $J_e$. + +#### Adaptive extension + +Since $\Theta^\star$ and $d^\star$ are unknown, form the composite LFC: + +```math +V = V_1 + V_2 + \frac{1}{2}\tilde{\Theta}^\top \Gamma^{-1}_{\theta}\,\tilde{\Theta} + \frac{1}{2}\tilde{d}^\top \Gamma^{-1}_{d}\,\tilde{d} +``` + +Assuming $\dot{\Theta}^\star = 0$ and $\dot{d}^\star = 0$ (static true parameters): + +```math +\dot{V} = \dot{V}_1 + z_2^\top M(\dot{\nu} - \dot{\alpha}) + \tilde{\Theta}^\top \Gamma^{-1}_{\theta}\,\dot{\hat{\Theta}} + \tilde{d}^\top \Gamma^{-1}_{d}\,\dot{\hat{d}} +``` + +Substituting the control law: + +```math +\tau = -J_e^\top z_1 - K_2\, z_2 + M\dot{\alpha} + C(\nu)\nu - Y(\nu)\hat{\Theta} - \hat{d} +``` + +and collecting terms (cross terms cancel, $C(\nu)\nu$ cancels, $M\dot\alpha$ cancels): + +```math +\dot{V} = -z_1^\top K_1 z_1 - z_2^\top K_2 z_2 ++ \tilde{\Theta}^\top\!\left(\Gamma_\theta^{-1}\dot{\hat{\Theta}} - Y(\nu)^\top z_2\right) ++ \tilde{d}^\top\!\left(\Gamma_d^{-1}\dot{\hat{d}} - z_2\right) +``` + +From this we can separate the adaptive terms: + +```math +\tilde{\Theta}^\top\!\left(\Gamma_\theta^{-1}\dot{\hat{\Theta}} - Y(\nu)^\top z_2\right) = \tilde{\Theta}^\top \Gamma_\theta^{-1}\!\left(\dot{\hat{\Theta}} - \Gamma_\theta Y(\nu)^\top z_2\right) +``` + +```math +\tilde{d}^\top\!\left(\Gamma_d^{-1}\dot{\hat{d}} - z_2\right) = \tilde{d}^\top \Gamma_d^{-1}\!\left(\dot{\hat{d}} - \Gamma_d z_2\right) +``` + +Choosing the update laws to zero these brackets: + +```math +\boxed{ +\dot{\hat{\Theta}} = \Gamma_{\theta}\, Y(\nu)^\top z_2 +} +``` + +```math +\boxed{ +\dot{\hat{d}} = \Gamma_{d}\, z_2 +} +``` + +This gives the final Lyapunov derivative: + +```math +\dot{V} = -z_1^\top K_1 z_1 - z_2^\top K_2 z_2 < 0, \quad \forall\,(z_1,z_2) \neq 0 +``` + +Global asymptotic stability of $z_1 = 0$, $z_2 = 0$ follows from LaSalle's invariance principle (Khalil §4.2), with parameter estimates remaining bounded by the adaptive law structure. + +### Full control law + +```math +\boxed{ +\tau = -J_e^\top z_1 - K_2\, z_2 + M\dot{\alpha} + C(\nu)\nu - Y(\nu)\hat{\Theta} - \hat{d} +} +``` + +### Controller gains + +**$K_1$** is the outer loop gain (position and orientation errors): + +```math +K_1 = +\begin{bmatrix} +k_{1,1} & & & & & \\ +& k_{1,2} & & & & \\ +& & k_{1,3} & & & \\ +& & & k_{1,4} & & \\ +& & & & k_{1,5} & \\ +& & & & & k_{1,6} +\end{bmatrix} +``` + +**$K_2$** is the inner loop gain (velocity errors): + +```math +K_2 = +\begin{bmatrix} +k_{2,1} & & & & & \\ +& k_{2,2} & & & & \\ +& & k_{2,3} & & & \\ +& & & k_{2,4} & & \\ +& & & & k_{2,5} & \\ +& & & & & k_{2,6} +\end{bmatrix} +``` + +### Adaptive parameters and functions + +The damping regressor $Y(\nu) \in \mathbb{R}^{6 \times 12}$ captures one linear and one quadratic term per DOF: + +```math +Y(\nu) = +\begin{bmatrix} +\nu_1 & \nu_1|\nu_1| & 0 & 0 & \cdots & 0 & 0 \\ +0 & 0 & \nu_2 & \nu_2|\nu_2| & \cdots & 0 & 0 \\ +\vdots & & & & \ddots & & \vdots \\ +0 & 0 & 0 & 0 & \cdots & \nu_6 & \nu_6|\nu_6| +\end{bmatrix} +``` + +The parameter vector $\hat{\Theta} \in \mathbb{R}^{12}$ is: + +```math +\hat{\Theta} = +\begin{bmatrix} +\alpha_1 & \beta_1 & \alpha_2 & \beta_2 & \alpha_3 & \beta_3 & \alpha_4 & \beta_4 & \alpha_5 & \beta_5 & \alpha_6 & \beta_6 +\end{bmatrix}^\top +``` + +where $\alpha_i$ and $\beta_i$ are the estimated linear and quadratic damping coefficients for DOF $i$. The adaptation gain $\Gamma_\theta$ is a $12 \times 12$ positive-definite diagonal matrix. + +The disturbance estimate $\hat{d} \in \mathbb{R}^6$ has one component per DOF, adapted with the $6 \times 6$ diagonal gain $\Gamma_d$. + +### Important implementation detail: computing $\dot{\alpha}$ + +The control law requires $\dot{\alpha}$, the time derivative of the virtual control. Since $\alpha = -J_e(\eta)^{-1}K_1 z_1$, applying the matrix identity $\tfrac{d}{dt}(A^{-1}) = -A^{-1}\dot{A}A^{-1}$: + +```math +\dot{\alpha} = J_e^{-1}\dot{J}_e J_e^{-1} K_1 z_1 - J_e^{-1} K_1 J_e\,\nu +``` + +The block structure $J_e = \begin{bmatrix} R & 0 \\ 0 & T_e \end{bmatrix}$ gives: + +```math +\dot{J}_e = +\begin{bmatrix} +\dot{R} & 0_{3\times 3} \\ +0_{3\times 3} & \dot{T}_e +\end{bmatrix} +``` + +**$\dot{R}$** uses the standard identity: + +```math +\dot{R} = R\,S(\omega) +``` + +**$\dot{T}_e$** follows from differentiating $T_e = \eta_e I + S(\varepsilon_e)$ using the quaternion kinematic equations $\dot{\eta}_e = -\tfrac{1}{2}\varepsilon_e^\top\omega$ and $\dot{\varepsilon}_e = \tfrac{1}{2}T_e\omega$: + +```math +\dot{T}_e = \tfrac{1}{2}\bigl(S(T_e\,\omega) - (\varepsilon_e^\top\omega)\,I_3\bigr) +``` + +This compact closed form — compared to the lengthy trigonometric expression required for $\dot{T}$ in the Euler-angle version — is a key practical advantage of the quaternion parameterisation. + +## Launch + +To run the controller, use the ROS 2 launch file: +```bash +ros2 launch dp_adapt_backs_controller_quat dp_adapt_backs_controller_quat.launch.py +``` + +Remember to `colcon build` and `source install/setup.bash` first. + +Two configuration files are provided: +- `adapt_params_nautilus.yaml` — tuned for the physical Nautilus AUV +- `adapt_params_nautilus_sim.yaml` — tuned for simulation diff --git a/control/dp_adapt_backs_controller_quat/config/adapt_params_nautilus.yaml b/control/dp_adapt_backs_controller_quat/config/adapt_params_nautilus.yaml new file mode 100644 index 000000000..a1d0221ac --- /dev/null +++ b/control/dp_adapt_backs_controller_quat/config/adapt_params_nautilus.yaml @@ -0,0 +1,8 @@ +/**: + ros__parameters: + K1 : [-0.55, -0.7, 2.5, 1.4, 2.0, 0.9] # Outer loop tuning parameters + K2 : [125.0, 120.0, 100.0, 60.0, 60.0, 60.0] # Inner loop tuning parameters + r_b_bg : [0.00, 0.0, 0.010] # Vector from body centre to centre of gravity + adapt_gain : [0.32, 0.11, 0.32, 0.11, 0.32, 0.11, 0.32, 0.11, 0.32, 0.11, 0.32, 0.11] # Tuning parameters for linear and nonlinear damping + d_gain : [0.13, 0.13, 0.13, 0.13, 0.13, 0.13] # Tuning parameters for the unmodeled disturbances and uncertanties + time_step: 10 #Controller timestep in milliseconds (ms) diff --git a/control/dp_adapt_backs_controller_quat/config/adapt_params_nautilus_sim.yaml b/control/dp_adapt_backs_controller_quat/config/adapt_params_nautilus_sim.yaml new file mode 100644 index 000000000..ff7710bfb --- /dev/null +++ b/control/dp_adapt_backs_controller_quat/config/adapt_params_nautilus_sim.yaml @@ -0,0 +1,8 @@ +/**: + ros__parameters: + K1 : [5.0, 5.0, 9.5, 1.5, 5.0, 9.5] # Outer loop tuning parameters + K2 : [25.0, 25.0, 40.0, 25.0, 25.0, 40.0] # Inner loop tuning parameters + r_b_bg : [0.00, 0.0, 0.015] # Vector from body centre to centre of gravity + adapt_gain : [0.4, 0.2, 0.4, 0.2, 0.4, 0.2, 0.4, 0.2, 0.4, 0.2, 0.4, 0.2] # Tuning parameters for linear and nonlinear damping + d_gain : [0.2, 0.2, 0.2, 0.2, 0.2, 0.2] # Tuning parameters for the unmodeled disturbances and uncertanties + time_step: 10 #Controller timestep in milliseconds (ms) diff --git a/control/dp_adapt_backs_controller_quat/include/dp_adapt_backs_controller_quat/dp_adapt_backs_controller.hpp b/control/dp_adapt_backs_controller_quat/include/dp_adapt_backs_controller_quat/dp_adapt_backs_controller.hpp new file mode 100644 index 000000000..eb74f17ff --- /dev/null +++ b/control/dp_adapt_backs_controller_quat/include/dp_adapt_backs_controller_quat/dp_adapt_backs_controller.hpp @@ -0,0 +1,69 @@ +#ifndef DP_ADAPT_BACKS_CONTROLLER__DP_ADAPT_BACKS_CONTROLLER_HPP_ +#define DP_ADAPT_BACKS_CONTROLLER__DP_ADAPT_BACKS_CONTROLLER_HPP_ + +#include +#include +#include "dp_adapt_backs_controller_quat/typedefs.hpp" + +namespace vortex::control { + +struct DPAdaptParams { + Eigen::Vector12d adapt_param = Eigen::Vector12d::Zero(); + Eigen::Vector6d d_gain = Eigen::Vector6d::Zero(); + Eigen::Vector6d K1 = Eigen::Vector6d::Zero(); + Eigen::Vector6d K2 = Eigen::Vector6d::Zero(); + Eigen::Vector3d r_b_bg = Eigen::Vector3d::Zero(); + Eigen::Vector3d inertia_matrix_body = Eigen::Vector3d::Zero(); + Eigen::Matrix6d mass_intertia_matrix = Eigen::Matrix6d::Zero(); + Eigen::Vector6d tau_max = Eigen::Vector6d::Ones(); + double mass{}; +}; + +class DPAdaptBacksController { + public: + explicit DPAdaptBacksController(const DPAdaptParams& dp_adapt_params); + + // @brief Calculates the control input tau found in the backstepping proof. + // Utilizes error state to avoid 7x6 non invertible J matrix. The + // approximation of quaternion error -> euler angle error is used, and + // therefore we explicitly assume small pertrubations + // + // @param pose: 7D vector containing the vehicle pose [x, y, z, qw, qx, qy, + // qz] + // @param pose_d: 7D vector containing the desired vehicle pose [x, y, z, + // qw, qx, qy, qz] + // @param twist: 6D vector containing the vehicle velocity [u, v, w, p, q, + // r] + // @return 6D vector containing the control input tau [X, Y, Z, K, M, N] + Eigen::Vector6d calculate_tau(const vortex::utils::types::Pose& pose, + const vortex::utils::types::Pose& pose_d, + const vortex::utils::types::Twist& twist); + + // @brief Reset the adaptive parameters + void reset_adap_param(); + + // @brief Reset the disturbance estimate + void reset_d_est(); + + // @brief Set the time step + // @param dt: Time step + void set_time_step(const double dt); + + private: + Eigen::Matrix6d K1_; + Eigen::Matrix6d K2_; + Eigen::Vector3d r_b_bg_; + Eigen::Matrix12d adapt_gain_; + Eigen::Matrix6d d_gain_; + Eigen::Vector12d adapt_param_; + Eigen::Vector6d d_est_; + Eigen::Matrix3d inertia_matrix_body_; + Eigen::Matrix6d mass_intertia_matrix_; + Eigen::Vector6d tau_max_; + double m_{}; + double dt_{}; +}; + +} // namespace vortex::control + +#endif // DP_ADAPT_BACKS_CONTROLLER__DP_ADAPT_BACKS_CONTROLLER_HPP_ diff --git a/control/dp_adapt_backs_controller_quat/include/dp_adapt_backs_controller_quat/dp_adapt_backs_controller_ros.hpp b/control/dp_adapt_backs_controller_quat/include/dp_adapt_backs_controller_quat/dp_adapt_backs_controller_ros.hpp new file mode 100644 index 000000000..b3ad6e6e0 --- /dev/null +++ b/control/dp_adapt_backs_controller_quat/include/dp_adapt_backs_controller_quat/dp_adapt_backs_controller_ros.hpp @@ -0,0 +1,112 @@ +#ifndef DP_ADAPT_BACKS_CONTROLLER__DP_ADAPT_BACKS_CONTROLLER_ROS_HPP_ +#define DP_ADAPT_BACKS_CONTROLLER__DP_ADAPT_BACKS_CONTROLLER_ROS_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "dp_adapt_backs_controller_quat/dp_adapt_backs_controller.hpp" +#include "dp_adapt_backs_controller_quat/typedefs.hpp" +#include "typedefs.hpp" + +namespace vortex::control { + +// @brief Class for the DP Adaptive Backstepping controller node +class DPAdaptBacksControllerNode : public rclcpp::Node { + public: + explicit DPAdaptBacksControllerNode( + const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); + + private: + // @brief Client for the GetOperationMode service + rclcpp::Client::SharedPtr + get_operation_mode_client_; + + // @brief Callback function for the killswitch topic + // @param msg: Bool message containing the killswitch status + void killswitch_callback(const std_msgs::msg::Bool::SharedPtr msg); + + // @brief Callback function for the software mode topic + // @param msg: String message containing the software mode + void operation_mode_callback( + const vortex_msgs::msg::OperationMode::SharedPtr msg); + + // @brief Callback function for the pose topic + // @param msg: PoseWithCovarianceStamped message containing the AUV pose + void pose_callback( + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); + + // @brief Callback function for the twist topic + // @param msg: TwistWithCovarianceStamped message containing the AUV speed + void twist_callback( + const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg); + + // @brief Callback function for the control input tau publish + void publish_tau(); + + // @brief set the DP Adaptive Backstepping controller parameters + void set_adap_params(); + + // @brief Set the subscriber and publisher for the node + void set_subscribers_and_publisher(); + + // @brief Initialize the operation mode by calling the GetOperationMode + // service + void initialize_operation_mode(); + + // @brief Callback function for the guidance topic + // @param msg: ReferenceFilter message containing the desired vehicle pose + // and velocity + void guidance_callback( + const vortex_msgs::msg::ReferenceFilterQuat::SharedPtr msg); + + rclcpp::Subscription::SharedPtr killswitch_sub_{}; + + rclcpp::Subscription::SharedPtr + operation_mode_sub_{}; + + rclcpp::Subscription< + geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_sub_{}; + + rclcpp::Subscription< + geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr twist_sub_{}; + + rclcpp::Subscription::SharedPtr + guidance_sub_{}; + + rclcpp::Publisher::SharedPtr tau_pub_{}; + + rclcpp::TimerBase::SharedPtr tau_pub_timer_{}; + + std::chrono::milliseconds time_step_{}; + + vortex::utils::types::Pose pose_; + + vortex::utils::types::Pose pose_d_; + + vortex::utils::types::Twist twist_; + + std::unique_ptr dp_adapt_backs_controller_{}; + + bool killswitch_on_{true}; + + vortex::utils::types::Mode operation_mode_{ + vortex::utils::types::Mode::manual}; +}; + +} // namespace vortex::control + +#endif // DP_ADAPT_BACKS_CONTROLLER__DP_ADAPT_BACKS_CONTROLLER_ROS_HPP_ diff --git a/control/dp_adapt_backs_controller_quat/include/dp_adapt_backs_controller_quat/dp_adapt_backs_controller_utils.hpp b/control/dp_adapt_backs_controller_quat/include/dp_adapt_backs_controller_quat/dp_adapt_backs_controller_utils.hpp new file mode 100644 index 000000000..7ef767045 --- /dev/null +++ b/control/dp_adapt_backs_controller_quat/include/dp_adapt_backs_controller_quat/dp_adapt_backs_controller_utils.hpp @@ -0,0 +1,53 @@ +#ifndef DP_ADAPT_BACKS_CONTROLLER__DP_ADAPT_BACKS_CONTROLLER_UTILS_HPP_ +#define DP_ADAPT_BACKS_CONTROLLER__DP_ADAPT_BACKS_CONTROLLER_UTILS_HPP_ + +#include +#include "dp_adapt_backs_controller_quat/typedefs.hpp" +#include "typedefs.hpp" + +namespace vortex::control { + +// @brief Calculate the derivative of the rotation matrix +// @param pose: 7D vector containing the vehicle pose [x, y, z, qw, qx, qy, qz] +// @param twist: 6D vector containing the vehicle velocity [u, v, w, p, q, r] +// @return 3x3 derivative of the rotation matrix +Eigen::Matrix3d calculate_R_dot(const vortex::utils::types::Pose& pose, + const vortex::utils::types::Twist& twist); + +// @brief Calculate the derivative of the transformation matrix +// @param pose: 6D vector containing the vehicle pose [x, y, z, qw, qx, qy, qz] +// @param twist: 6D vector containing the vehicle velocity [u, v, w, p, q, r] +// @return 3x3 derivative of the transformation matrix +Eigen::Matrix3d calculate_Q_dot(const vortex::utils::types::Pose& pose, + const vortex::utils::types::Twist& twist); + +// @brief Calculate the pseudo-inverse of the Jacobian matrix +// @param pose: 7D vector containing the vehicle pose [x, y, z, qw, qx, qy, qz] +// @return 6x6 pseudo-inverse Jacobian matrix +Eigen::Matrix6d calculate_L_inv(const vortex::utils::types::Pose& pose); + +// @brief calculate the derivative of the Jacobian matrix +// @param pose: 7D vector containing the vehicle pose [x, y, z, qw, qx, qy, qz] +// @param twist: 6D vector containing the vehicle velocity [u, v, w, p, q, r] +Eigen::Matrix6d calculate_L_dot(const vortex::utils::types::Pose& pose, + const vortex::utils::types::Twist& twist); + +// @brief Calculate the coriolis matrix +// @param m: mass of the vehicle +// @param r_b_bg: 3D vector of the body frame to the center of gravity +// @param twist: 6D vector containing linear and angular velocity of the vehicle +// @param inertia_matrix_body : 3D matrix containing the inertia matrix +// @return 6x6 coriolis matrix +Eigen::Matrix6d calculate_coriolis(const double mass, + const Eigen::Vector3d& r_b_bg, + const vortex::utils::types::Twist& twist, + const Eigen::Matrix3d& inertia_matrix_body); + +// @brief Calculate the damping matrix for the adaptive backstepping controller +// @param twist: 6D vector containing the vehicle velocity [u, v, w, p, q, r] +// @return 6x6 damping matrix +Eigen::Matrix6x12d calculate_Y_v(const vortex::utils::types::Twist& twist); + +} // namespace vortex::control + +#endif // DP_ADAPT_BACKS_CONTROLLER__DP_ADAPT_BACKS_CONTROLLER_UTILS_HPP_ diff --git a/control/dp_adapt_backs_controller_quat/include/dp_adapt_backs_controller_quat/typedefs.hpp b/control/dp_adapt_backs_controller_quat/include/dp_adapt_backs_controller_quat/typedefs.hpp new file mode 100644 index 000000000..49f908072 --- /dev/null +++ b/control/dp_adapt_backs_controller_quat/include/dp_adapt_backs_controller_quat/typedefs.hpp @@ -0,0 +1,22 @@ +/** + * @file typedefs.hpp + * @brief Contains the Eigen typedefs for the controller. + */ + +#ifndef DP_ADAPT_BACKS_CONTROLLER__TYPEDEFS_HPP_ +#define DP_ADAPT_BACKS_CONTROLLER__TYPEDEFS_HPP_ + +#include + +namespace Eigen { + +typedef Eigen::Matrix Vector6d; +typedef Eigen::Matrix Vector12d; +typedef Eigen::Matrix Matrix6d; +typedef Eigen::Matrix Matrix6x12d; +typedef Eigen::Matrix Matrix12x6d; +typedef Eigen::Matrix Matrix12d; + +} // namespace Eigen + +#endif // DP_ADAPT_BACKS_CONTROLLER__TYPEDEFS_HPP_ diff --git a/control/dp_adapt_backs_controller_quat/launch/dp_adapt_backs_controller_quat.launch.py b/control/dp_adapt_backs_controller_quat/launch/dp_adapt_backs_controller_quat.launch.py new file mode 100644 index 000000000..8350bd019 --- /dev/null +++ b/control/dp_adapt_backs_controller_quat/launch/dp_adapt_backs_controller_quat.launch.py @@ -0,0 +1,45 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import OpaqueFunction +from launch_ros.actions import Node + +from auv_setup.launch_arg_common import ( + declare_drone_and_namespace_args, + resolve_drone_and_namespace, +) + + +def launch_setup(context, *args, **kwargs): + drone, namespace = resolve_drone_and_namespace(context) + + adapt_params = os.path.join( + get_package_share_directory("dp_adapt_backs_controller_quat"), + "config", + f"adapt_params_{drone}.yaml", + ) + + drone_params = os.path.join( + get_package_share_directory("auv_setup"), + "config", + "robots", + f"{drone}.yaml", + ) + + return [ + Node( + package="dp_adapt_backs_controller_quat", + executable="dp_adapt_backs_controller_quat_node", + name="dp_adapt_backs_controller_node", + namespace=namespace, + parameters=[adapt_params, drone_params], + output="screen", + ) + ] + + +def generate_launch_description(): + return LaunchDescription( + declare_drone_and_namespace_args() + [OpaqueFunction(function=launch_setup)] + ) diff --git a/control/dp_adapt_backs_controller_quat/package.xml b/control/dp_adapt_backs_controller_quat/package.xml new file mode 100644 index 000000000..a2d7afe43 --- /dev/null +++ b/control/dp_adapt_backs_controller_quat/package.xml @@ -0,0 +1,25 @@ + + + + dp_adapt_backs_controller_quat + 1.0.0 + Adaptive backstepping controller for DP + Cyprian Osinski + MIT + + ament_cmake + + rclcpp + geometry_msgs + nav_msgs + eigen + tf2 + vortex_msgs + vortex_utils + vortex_utils_ros + yaml-cpp + + + ament_cmake + + diff --git a/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller.cpp b/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller.cpp new file mode 100644 index 000000000..46c0c8aa3 --- /dev/null +++ b/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller.cpp @@ -0,0 +1,114 @@ +#include "dp_adapt_backs_controller_quat/dp_adapt_backs_controller.hpp" +#include +#include +#include +#include +#include "dp_adapt_backs_controller_quat/dp_adapt_backs_controller_utils.hpp" +#include "dp_adapt_backs_controller_quat/typedefs.hpp" + +namespace vortex::control { + +using vortex::utils::types::Pose; +using vortex::utils::types::Twist; + +DPAdaptBacksController::DPAdaptBacksController( + const DPAdaptParams& dp_adapt_params) + : K1_(dp_adapt_params.K1.asDiagonal().toDenseMatrix()), + K2_(dp_adapt_params.K2.asDiagonal().toDenseMatrix()), + r_b_bg_(dp_adapt_params.r_b_bg), + adapt_gain_(dp_adapt_params.adapt_param.asDiagonal().toDenseMatrix()), + d_gain_(dp_adapt_params.d_gain.asDiagonal().toDenseMatrix()), + adapt_param_(Eigen::Vector12d::Zero()), + d_est_(Eigen::Vector6d::Zero()), + inertia_matrix_body_( + dp_adapt_params.inertia_matrix_body.asDiagonal().toDenseMatrix()), + mass_intertia_matrix_(dp_adapt_params.mass_intertia_matrix), + tau_max_(dp_adapt_params.tau_max), + m_(dp_adapt_params.mass), + dt_(0.01) {} + +Eigen::Vector6d DPAdaptBacksController::calculate_tau(const Pose& pose, + const Pose& pose_d, + const Twist& twist) { + Eigen::Vector3d pos_error = pose.pos_vector() - pose_d.pos_vector(); + + // Error quaternion q_e = q_d^{-1} * q (rotation from desired to current). + // z_1_ori = 2*eps_e, so d/dt(z_1_ori) = (qw_e*I + S(eps_e)) * omega. + // This requires building L with the error quaternion, not q_current, + // otherwise the Lyapunov cross-terms don't cancel and orientation diverges. + Eigen::Quaterniond q_e = + pose_d.ori_quaternion().conjugate() * pose.ori_quaternion(); + if (q_e.w() < 0.0) + q_e.coeffs() = -q_e.coeffs(); + const Eigen::Vector3d eps_e = q_e.vec(); + const double qw_e = q_e.w(); + const Eigen::Vector3d quat_error = 2.0 * eps_e; + + Eigen::Vector6d z_1; + z_1 << pos_error, quat_error; + + // L: [R(q_current), 0; 0, qw_e*I + S(eps_e)] + const Eigen::Matrix3d R = pose.as_rotation_matrix(); + const Eigen::Matrix3d Q_e = + qw_e * Eigen::Matrix3d::Identity() + + vortex::utils::math::get_skew_symmetric_matrix(eps_e); + Eigen::Matrix6d L = Eigen::Matrix6d::Zero(); + L.topLeftCorner<3, 3>() = R; + L.bottomRightCorner<3, 3>() = Q_e; + + // L_inv with singularity guard + Eigen::Matrix6d L_inv; + if (std::abs(L.determinant()) < 1e-8) { + spdlog::error("L is singular"); + L_inv = L.completeOrthogonalDecomposition().pseudoInverse(); + } else { + L_inv = L.inverse(); + } + + // L_dot: R_dot = R*S(omega), Q_e_dot via quaternion kinematics on q_e + // qw_e_dot = -0.5 * eps_e^T * omega + // eps_e_dot = 0.5 * Q_e * omega (standard quat kinematics) + const Eigen::Vector3d omega = twist.to_vector().tail<3>(); + const Eigen::Matrix3d Q_e_dot = + (-0.5 * eps_e.dot(omega)) * Eigen::Matrix3d::Identity() + + vortex::utils::math::get_skew_symmetric_matrix(0.5 * Q_e * omega); + Eigen::Matrix6d L_dot = Eigen::Matrix6d::Zero(); + L_dot.topLeftCorner<3, 3>() = calculate_R_dot(pose, twist); + L_dot.bottomRightCorner<3, 3>() = Q_e_dot; + + Eigen::Matrix6d C = + calculate_coriolis(m_, r_b_bg_, twist, inertia_matrix_body_); + Eigen::Vector6d alpha = -L_inv * K1_ * z_1; + Eigen::Vector6d z_2 = twist.to_vector() - alpha; + Eigen::Vector6d alpha_dot = ((L_inv * L_dot * L_inv) * K1_ * z_1) - + (L_inv * K1_ * L * twist.to_vector()); + Eigen::Matrix6x12d Y_v = calculate_Y_v(twist); + Eigen::Vector12d adapt_param_dot = adapt_gain_ * Y_v.transpose() * z_2; + Eigen::Vector6d d_est_dot = d_gain_ * z_2; + Eigen::Vector6d F_est = Y_v * adapt_param_; + Eigen::Vector6d tau = (mass_intertia_matrix_ * alpha_dot) + + (C * twist.to_vector()) - (L.transpose() * z_1) - + (K2_ * z_2) - F_est - d_est_; + + tau = tau.cwiseMax(-tau_max_).cwiseMin(tau_max_); + adapt_param_ += adapt_param_dot * dt_; + d_est_ += d_est_dot * dt_; + adapt_param_ = adapt_param_.cwiseMax(-15.0).cwiseMin(15.0); + d_est_ = d_est_.cwiseMax(-15.0).cwiseMin(15.0); + + return tau; +} + +void DPAdaptBacksController::reset_adap_param() { + adapt_param_.setZero(); +} + +void DPAdaptBacksController::reset_d_est() { + d_est_.setZero(); +} + +void DPAdaptBacksController::set_time_step(const double dt) { + dt_ = dt; +} + +} // namespace vortex::control diff --git a/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller_ros.cpp b/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller_ros.cpp new file mode 100644 index 000000000..226998b92 --- /dev/null +++ b/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller_ros.cpp @@ -0,0 +1,349 @@ +#include "dp_adapt_backs_controller_quat/dp_adapt_backs_controller_ros.hpp" +#include +#include +#include +#include +#include +#include +#include +#include "dp_adapt_backs_controller_quat/dp_adapt_backs_controller_utils.hpp" +#include "dp_adapt_backs_controller_quat/typedefs.hpp" + +constexpr std::string_view start_message = R"( + ██████╗ ██╗ ██╗ █████╗ ████████╗███████╗██████╗ ███╗ ██╗██╗ ██████╗ ███╗ ██╗ ██████╗ ██████╗ +██╔═══██╗██║ ██║██╔══██╗╚══██╔══╝██╔════╝██╔══██╗████╗ ██║██║██╔═══██╗████╗ ██║ ██╔══██╗██╔══██╗ +██║ ██║██║ ██║███████║ ██║ █████╗ ██████╔╝██╔██╗ ██║██║██║ ██║██╔██╗ ██║ ██║ ██║██████╔╝ +██║▄▄ ██║██║ ██║██╔══██║ ██║ ██╔══╝ ██╔══██╗██║╚██╗██║██║██║ ██║██║╚██╗██║ ██║ ██║██╔═══╝ +╚██████╔╝╚██████╔╝██║ ██║ ██║ ███████╗██║ ██║██║ ╚████║██║╚██████╔╝██║ ╚████║ ██████╔╝██║ + ╚══▀▀═╝ ╚═════╝ ╚═╝ ╚═╝ ╚═╝ ╚══════╝╚═╝ ╚═╝╚═╝ ╚═══╝╚═╝ ╚═════╝ ╚═╝ ╚═══╝ ╚═════╝ ╚═╝ + +)"; + +namespace vortex::control { + +DPAdaptBacksControllerNode::DPAdaptBacksControllerNode( + const rclcpp::NodeOptions& options) + : Node("dp_adapt_backs_controller_node", options) { + this->declare_parameter("time_step"); + int time_step = static_cast(this->get_parameter("time_step").as_int()); + time_step_ = std::chrono::milliseconds(time_step); + + set_subscribers_and_publisher(); + initialize_operation_mode(); + + tau_pub_timer_ = this->create_wall_timer( + time_step_, std::bind(&DPAdaptBacksControllerNode::publish_tau, this)); + set_adap_params(); + + spdlog::info(start_message); +} + +void DPAdaptBacksControllerNode::set_subscribers_and_publisher() { + const auto qos_sensor_data{ + vortex::utils::qos_profiles::sensor_data_profile(1)}; + const auto qos_reliable{vortex::utils::qos_profiles::reliable_profile(1)}; + + this->declare_parameter("topics.guidance.dp_quat"); + std::string dp_reference_topic = + this->get_parameter("topics.guidance.dp_quat").as_string(); + guidance_sub_ = + this->create_subscription( + dp_reference_topic, qos_sensor_data, + std::bind(&DPAdaptBacksControllerNode::guidance_callback, this, + std::placeholders::_1)); + + this->declare_parameter("topics.pose"); + std::string pose_topic = this->get_parameter("topics.pose").as_string(); + pose_sub_ = this->create_subscription< + geometry_msgs::msg::PoseWithCovarianceStamped>( + pose_topic, qos_sensor_data, + std::bind(&DPAdaptBacksControllerNode::pose_callback, this, + std::placeholders::_1)); + + this->declare_parameter("topics.twist"); + std::string twist_topic = this->get_parameter("topics.twist").as_string(); + twist_sub_ = this->create_subscription< + geometry_msgs::msg::TwistWithCovarianceStamped>( + twist_topic, qos_sensor_data, + std::bind(&DPAdaptBacksControllerNode::twist_callback, this, + std::placeholders::_1)); + + this->declare_parameter("topics.killswitch"); + std::string software_kill_switch_topic = + this->get_parameter("topics.killswitch").as_string(); + killswitch_sub_ = this->create_subscription( + software_kill_switch_topic, qos_reliable, + std::bind(&DPAdaptBacksControllerNode::killswitch_callback, this, + std::placeholders::_1)); + + this->declare_parameter("topics.operation_mode"); + std::string software_operation_mode_topic = + this->get_parameter("topics.operation_mode").as_string(); + operation_mode_sub_ = + this->create_subscription( + software_operation_mode_topic, qos_reliable, + std::bind(&DPAdaptBacksControllerNode::operation_mode_callback, + this, std::placeholders::_1)); + + this->declare_parameter("topics.wrench_input"); + std::string control_topic = + this->get_parameter("topics.wrench_input").as_string(); + tau_pub_ = this->create_publisher( + control_topic, qos_sensor_data); +} + +void DPAdaptBacksControllerNode::initialize_operation_mode() { + this->declare_parameter("services.get_operation_mode"); + std::string get_operation_mode_service = + this->get_parameter("services.get_operation_mode").as_string(); + + get_operation_mode_client_ = + this->create_client( + get_operation_mode_service); + + while (!get_operation_mode_client_->wait_for_service( + std::chrono::seconds(1))) { + spdlog::warn("Waiting for GetOperationMode service to be available..."); + } + + auto request = + std::make_shared(); + get_operation_mode_client_->async_send_request( + request, + [this](rclcpp::Client::SharedFuture + future) { + try { + auto response = future.get(); + operation_mode_ = + vortex::utils::ros_conversions::convert_from_ros( + response->current_operation_mode); + killswitch_on_ = response->killswitch_status; + spdlog::info( + "Initial operation mode: {} | Killswitch status: {}", + vortex::utils::types::mode_to_string(operation_mode_), + killswitch_on_ ? "on" : "off"); + } catch (const std::exception& e) { + spdlog::error("Failed to get initial operation mode: {}", + e.what()); + killswitch_on_ = true; + } + }); +} + +void DPAdaptBacksControllerNode::killswitch_callback( + const std_msgs::msg::Bool::SharedPtr msg) { + killswitch_on_ = msg->data; + dp_adapt_backs_controller_->reset_adap_param(); + dp_adapt_backs_controller_->reset_d_est(); + spdlog::info("Killswitch: {}", killswitch_on_ ? "on" : "off"); +} + +void DPAdaptBacksControllerNode::operation_mode_callback( + const vortex_msgs::msg::OperationMode::SharedPtr msg) { + operation_mode_ = vortex::utils::ros_conversions::convert_from_ros(*msg); + spdlog::info("Operation mode: {}", + vortex::utils::types::mode_to_string(operation_mode_)); + + if (operation_mode_ == vortex::utils::types::Mode::autonomous || + operation_mode_ == vortex::utils::types::Mode::reference) { + pose_d_ = pose_; + } +} + +void DPAdaptBacksControllerNode::pose_callback( + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) { + pose_.x = msg->pose.pose.position.x; + pose_.y = msg->pose.pose.position.y; + pose_.z = msg->pose.pose.position.z; + + const auto& o = msg->pose.pose.orientation; + pose_.qw = o.w; + pose_.qx = o.x; + pose_.qy = o.y; + pose_.qz = o.z; +} + +void DPAdaptBacksControllerNode::twist_callback( + const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) { + twist_.u = msg->twist.twist.linear.x; + twist_.v = msg->twist.twist.linear.y; + twist_.w = msg->twist.twist.linear.z; + twist_.p = msg->twist.twist.angular.x; + twist_.q = msg->twist.twist.angular.y; + twist_.r = msg->twist.twist.angular.z; +} + +void DPAdaptBacksControllerNode::set_adap_params() { + this->declare_parameter>("adapt_gain"); + this->declare_parameter>("d_gain"); + this->declare_parameter>("K1"); + this->declare_parameter>("K2"); + this->declare_parameter>("r_b_bg"); + this->declare_parameter>("physical.mass_matrix"); + this->declare_parameter>("physical.center_of_mass"); + this->declare_parameter>( + "propulsion.thrusters.thruster_force_direction"); + this->declare_parameter>( + "propulsion.thrusters.thruster_position"); + this->declare_parameter("propulsion.thrusters.num"); + this->declare_parameter("propulsion.dimensions.num"); + this->declare_parameter( + "propulsion.thrusters.constraints.min_force"); + this->declare_parameter( + "propulsion.thrusters.constraints.max_force"); + + std::vector adapt_param_vec = + this->get_parameter("adapt_gain").as_double_array(); + std::vector d_gain_vec = + this->get_parameter("d_gain").as_double_array(); + std::vector K1_vec = this->get_parameter("K1").as_double_array(); + std::vector K2_vec = this->get_parameter("K2").as_double_array(); + std::vector r_b_bg_vec = + this->get_parameter("r_b_bg").as_double_array(); + std::vector mass_intertia_matrix_vec = + this->get_parameter("physical.mass_matrix").as_double_array(); + + Eigen::Vector12d adapt_param_eigen = + Eigen::Map(adapt_param_vec.data()); + Eigen::Vector6d d_gain_eigen = + Eigen::Map(d_gain_vec.data()); + Eigen::Vector6d K1_eigen = Eigen::Map(K1_vec.data()); + Eigen::Vector6d K2_eigen = Eigen::Map(K2_vec.data()); + Eigen::Vector3d r_b_bg_eigen = + Eigen::Map(r_b_bg_vec.data()); + Eigen::Matrix6d mass_intertia_matrix = + Eigen::Map(mass_intertia_matrix_vec.data()); + + double mass = mass_intertia_matrix(0, 0); + Eigen::Vector3d inertia_matrix_body_eigen(mass_intertia_matrix(3, 3), + mass_intertia_matrix(4, 4), + mass_intertia_matrix(5, 5)); + + // Compute per-DOF max wrench from the thruster configuration + int num_thrusters = + this->get_parameter("propulsion.thrusters.num").as_int(); + int num_dims = this->get_parameter("propulsion.dimensions.num").as_int(); + double min_force = + this->get_parameter("propulsion.thrusters.constraints.min_force") + .as_double(); + double max_force = + this->get_parameter("propulsion.thrusters.constraints.max_force") + .as_double(); + + Eigen::Vector3d center_of_mass = Eigen::Map( + this->get_parameter("physical.center_of_mass") + .as_double_array() + .data()); + + auto dir_vec = + this->get_parameter("propulsion.thrusters.thruster_force_direction") + .as_double_array(); + auto pos_vec = + this->get_parameter("propulsion.thrusters.thruster_position") + .as_double_array(); + + Eigen::MatrixXd thruster_dir = + Eigen::Map>( + dir_vec.data(), num_dims, num_thrusters); + Eigen::MatrixXd thruster_pos = + Eigen::Map>( + pos_vec.data(), num_dims, num_thrusters); + + Eigen::MatrixXd T = Eigen::MatrixXd::Zero(6, num_thrusters); + for (int i = 0; i < num_thrusters; i++) { + Eigen::Vector3d pos = thruster_pos.col(i) - center_of_mass; + Eigen::Vector3d F = thruster_dir.col(i); + T.block<3, 1>(0, i) = F; + T.block<3, 1>(3, i) = pos.cross(F); + } + + Eigen::Vector6d tau_max; + for (int i = 0; i < 6; i++) { + double w = 0.0; + for (int j = 0; j < num_thrusters; j++) { + w += (T(i, j) > 0) ? T(i, j) * max_force : T(i, j) * min_force; + } + tau_max(i) = w; + } + + DPAdaptParams dp_adapt_params; + dp_adapt_params.adapt_param = adapt_param_eigen; + dp_adapt_params.d_gain = d_gain_eigen; + dp_adapt_params.K1 = K1_eigen; + dp_adapt_params.K2 = K2_eigen; + dp_adapt_params.r_b_bg = r_b_bg_eigen; + dp_adapt_params.inertia_matrix_body = inertia_matrix_body_eigen; + dp_adapt_params.mass_intertia_matrix = mass_intertia_matrix; + dp_adapt_params.tau_max = tau_max; + dp_adapt_params.mass = mass; + + dp_adapt_backs_controller_ = + std::make_unique(dp_adapt_params); +} + +void DPAdaptBacksControllerNode::publish_tau() { + if (killswitch_on_ || + operation_mode_ == vortex::utils::types::Mode::manual) { + return; + } + + Eigen::Vector6d tau = + dp_adapt_backs_controller_->calculate_tau(pose_, pose_d_, twist_); + + geometry_msgs::msg::WrenchStamped tau_msg; + tau_msg.header.stamp = this->now(); + tau_msg.header.frame_id = "base_link"; + tau_msg.wrench.force.x = tau(0); + tau_msg.wrench.force.y = tau(1); + tau_msg.wrench.force.z = tau(2); + + // comment out if roll control is not needed + tau_msg.wrench.torque.x = tau(3); + tau_msg.wrench.torque.y = tau(4); + tau_msg.wrench.torque.z = tau(5); + + tau_pub_->publish(tau_msg); +} + +void DPAdaptBacksControllerNode::guidance_callback( + const vortex_msgs::msg::ReferenceFilterQuat::SharedPtr msg) { + pose_d_.x = msg->x; + pose_d_.y = msg->y; + pose_d_.z = msg->z; + + pose_d_.qw = msg->qw; + pose_d_.qx = msg->qx; + pose_d_.qy = msg->qy; + pose_d_.qz = msg->qz; +} + +RCLCPP_COMPONENTS_REGISTER_NODE(DPAdaptBacksControllerNode) + +} // namespace vortex::control + +// ⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣀⣀⣠⡤⣦⢦⠖⡶⠲⢦⣤⢤⣤⣤⣀⢀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀ +// ⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⣠⡴⢶⠛⡍⢎⠱⣈⠒⢌⡘⠰⠉⢆⠰⢉⠔⠢⡉⢝⢫⠳⢦⣄⡀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀ +// ⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⣠⡶⢏⠳⡘⠢⢉⠔⡈⠔⠠⠈⠄⠠⠁⠌⡀⠂⠌⠠⠁⠌⡐⢂⠉⢆⠩⡝⢳⣦⣀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀ +// ⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⣴⡞⢫⠔⡉⠆⡁⠢⢁⣂⡐⡈⠄⢁⠈⠄⣁⡤⠄⣁⣠⠁⡈⠄⠐⠠⠉⠠⠁⡌⢡⠊⡝⢷⣄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀ +// ⠀⠀⠀⠀⠀⠀⠀⠀⣠⡾⢣⠎⡡⢊⠴⣅⣶⣽⣷⣾⣿⣶⠌⠀⠄⢺⢾⣿⣿⣿⣷⣾⣿⣤⣧⡰⠈⠠⢁⠐⢂⠑⠌⡸⠸⣷⣤⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀ +// ⠀⠀⠀⠀⠀⠀⠀⣸⢯⣑⠣⣌⠱⢨⣿⣿⣿⣿⡿⠟⠋⠁⡀⠌⠐⠠⢁⠚⡹⢛⡿⢿⣿⣿⣿⠯⢀⠁⠂⠌⠠⢈⠂⣁⠣⡘⠽⣦⡀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀ +// ⠀⠀⠀⠀⠀⠀⢴⡟⡥⢊⠵⣠⠣⢍⠺⢟⠩⣁⠒⠨⢀⠂⠀⠄⠡⡁⢂⠠⠀⠄⢠⠉⠄⡊⢄⠂⡄⠊⠄⢃⠤⢁⠂⢄⠂⠥⡙⣚⢷⡀⠀⠀⠀⠀⠀⠀⠀⠀⠀ +// ⠀⠀⠀⠀⠀⢀⡿⡜⠴⣉⠖⣡⢚⣬⢳⡉⠖⣀⠃⣁⠢⠐⡈⠠⢁⠒⡄⢂⠡⠊⠄⡘⠤⠑⡌⢒⠤⢃⠜⣀⠂⢆⠘⡠⠘⡀⢆⢡⢻⣧⠀⠀⠀⠀⠀⠀⠀⠀⠀ +// ⠀⠀⠀⠀⠀⢸⣛⣌⠳⡌⢎⡵⣫⣎⠧⡘⠡⠀⠂⢀⠂⠱⢠⢁⠊⡴⣘⠢⠁⠌⠀⠐⠠⢁⠘⡄⠫⡜⠲⣄⠩⢄⠢⠁⠆⠡⠌⡂⢎⡽⣆⠀⠀⠀⠀⠀⠀⠀⠀ +// ⠀⠀⠀⠀⠀⢸⡷⢌⠳⡌⢧⣿⢿⡜⣢⣱⣤⡧⣐⣢⡀⢃⢢⠁⢎⡶⣡⢆⣵⠴⠧⢦⣁⣂⠰⣈⠱⣘⠳⣬⡓⢌⠢⡉⢌⠡⡘⢄⢣⢚⣿⠀⠀⠀⠀⠀⠀⠀⠀ +// ⠀⠀⠀⠀⠀⢹⡞⣌⠳⡌⢧⣿⣿⣽⢷⣫⣔⡠⢀⢀⠈⠉⢲⣭⣰⣟⣯⣹⣔⣢⢡⠀⡀⠌⠙⠞⣳⠿⢷⣷⣝⠦⡑⠨⠄⡃⠔⡈⠦⣙⡾⡆⠀⠀⠀⠀⠀⠀⠀ +// ⠀⠀⠀⠀⠀⢸⡿⢤⠓⣌⢣⢻⣹⣿⣻⣿⣿⣿⣶⠢⣌⢡⠂⡴⢻⡿⣷⣿⣿⣿⣷⣳⠴⡈⡜⡰⣄⢻⣜⣿⢳⡡⢊⡑⢌⠰⠡⡘⡰⣡⢿⡇⠀⠀⠀⠀⠀⠀⠀ +// ⠀⠀⠀⠀⠀⠸⣟⢦⢋⡔⢣⡙⣷⡻⣿⣿⣿⣿⣿⣷⣎⢦⣙⢶⢏⠻⣿⣿⣿⣿⣿⡿⢧⡹⣴⢳⣽⣳⡿⢡⠗⡠⢃⠔⡈⢆⠱⣐⠱⡬⢿⡇⠀⠀⠀⠀⠀⠀⠀ +// ⠀⠀⠀⠀⠀⠀⢻⣎⠖⡬⢑⡜⢲⢻⣮⠻⣿⣿⣻⣷⣾⡻⢞⠧⢎⠛⣮⣛⠿⣽⣻⣽⣯⣿⣾⣿⠿⢋⡴⢋⡒⢡⠊⡔⣁⠊⡔⠤⣛⠼⣿⠀⠀⠀⠀⠀⠀⠀⠀ +// ⠀⠀⠀⠀⠀⠀⠘⢯⣞⠰⡡⠎⢥⢋⡞⡿⢶⡞⣭⡝⣲⡙⢎⡱⢊⡱⠄⣍⠛⢦⢋⠭⣉⡍⣡⠔⣎⠣⡜⡡⠘⡄⢣⠐⡄⢣⠘⢦⢭⣿⡟⠀⠀⠀⠀⠀⠀⠀⠀ +// ⠀⠀⠀⠀⠀⣠⣤⡸⣿⢢⡑⠎⣆⢣⠚⣭⢣⡛⡴⡙⢆⠹⢠⠑⠢⢄⠃⠄⣉⠂⠍⠒⡡⠜⠤⢋⠔⡃⢆⠡⢃⡘⢄⠣⡘⢤⢋⡼⣺⣿⠉⣀⣤⣄⠀⠀⠀⠀⠀ +// ⠀⠀⠀⠀⣤⡗⠀⣉⣻⣧⣙⠲⡐⢎⡱⢂⠇⣎⠱⢡⠊Life is pain,⡈⢂⠡⠌⡐⡈⠤⢁⠰⡈⢆⠱⣊⠼⣼⣿⣿⣛⣉⠐⠾⣄⠀⠀⠀⠀ +// ⠀⠀⠀⡼⠃⠀⠀⠙⣿⡘⢿⣵⠿⡼⣴⣩⣚Existence is meaningless⢢⣑⣎⡱⢬⣻⣿⠇⣸⡿⠁⠀⠀⠙⣆⠀⠀⠀ +// ⠀⠀⣰⡗⠀⠀⠀⠀⠘⣧⡀⠙⠻⣷⣾⣭⣿⣹⢏⡻⢩⢛⠛⡛⢛⠻⠛⠟⡛⠟⢛⠛⡛⠹⢛⠛⣙⡋⣏⣙⣩⣭⣭⣴⣼⣿⠟⠁⠈⣿⠁⠀⠀⠀⠀⢾⡄⠀⠀ +// ⠀⢠⠻⠀⢀⣠⣄⠀⠀⣻⠀⠀⠀⠀⠛⠿⣿⣿⣿⣷⣷⣮⣵⣌⣦⡱⣌⣲⡰⣌⣦⣱⣬⣷⣾⣿⣿⣿⡿⣿⢿⣿⣿⣿⠛⠁⠀⢀⢸⡇⠀⠀⣀⣀⠀⠘⡿⡀⠀ +// ⠀⣧⠇⠀⣴⠟⠈⠀⠀⣿⡄⠀⠀⠀⠀⠀⠈⠙⠻⢿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⢿⣻⢯⣷⣯⣿⣽⡿⠟⠉⠀⠀⠀⠀⠀⣘⣿⠀⢸⠉⠿⣄⠀⠰⣡⠀ +// ⠸⡌⠀⢹⡇⠀⠀⣏⠀⣽⡀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠉⠙⠛⠛⠿⠿⠿⢽⢿⡾⠷⠿⠿⠿⠟⠛⠋⠉⠀⠀⠀⠀⠀⠀⠀⠀⠀⢸⣇⠀⠨⠀⠀⢹⠇⠀⢏⠂ +// ⠀⡇⠀⣻⡆⠀⠀⠉⠒⠋⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠈⠓⠒⠃⠀⠀⢸⣟⠀⣸⠀ +// ⠀⠣⠴⠞⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠘⠷⠄⠆⠀ diff --git a/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller_utils.cpp b/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller_utils.cpp new file mode 100644 index 000000000..52cfc81ed --- /dev/null +++ b/control/dp_adapt_backs_controller_quat/src/dp_adapt_backs_controller_utils.cpp @@ -0,0 +1,107 @@ +#include "dp_adapt_backs_controller_quat/dp_adapt_backs_controller_utils.hpp" +#include +#include +#include +#include +#include +#include "dp_adapt_backs_controller_quat/typedefs.hpp" + +namespace vortex::control { + +Eigen::Matrix6d calculate_L_inv(const vortex::utils::types::Pose& pose) { + Eigen::Matrix6d L = Eigen::Matrix6d::Zero(); + L.topLeftCorner<3, 3>() = pose.as_rotation_matrix(); + L.bottomRightCorner<3, 3>() = pose.as_transformation_matrix().bottomRows<3>(); + + constexpr double tolerance = 1e-8; + + if (std::abs(L.determinant()) < tolerance) { + spdlog::error("L is singular"); + + // Moore-Penrose pseudoinverse in case of near singular matrix, better + // result for smaller singular values + return L.completeOrthogonalDecomposition().pseudoInverse(); + } + + return L.inverse(); +} + +Eigen::Matrix3d calculate_R_dot(const vortex::utils::types::Pose& pose, + const vortex::utils::types::Twist& twist) { + return pose.as_rotation_matrix() * + vortex::utils::math::get_skew_symmetric_matrix( + twist.to_vector().tail(3)); +} + +Eigen::Matrix3d calculate_Q_dot(const vortex::utils::types::Pose& pose, + const vortex::utils::types::Twist& twist) { + Eigen::Vector3d omega = twist.to_vector().tail(3); + Eigen::Matrix3d Q_tilde = pose.as_transformation_matrix().bottomRows<3>(); + Eigen::Vector3d eps = pose.ori_quaternion().vec(); + Eigen::Matrix3d eta_dot_term = 0.5 * eps.dot(omega) * Eigen::Matrix3d::Identity(); + Eigen::Matrix3d eps_dot_term = + vortex::utils::math::get_skew_symmetric_matrix(Q_tilde * omega); + return 0.5 * (eps_dot_term - eta_dot_term); +} + +Eigen::Matrix6d calculate_L_dot(const vortex::utils::types::Pose& pose, + const vortex::utils::types::Twist& twist) { + Eigen::Matrix3d R_dot = calculate_R_dot(pose, twist); + Eigen::Matrix3d Q_dot = calculate_Q_dot(pose, twist); + + Eigen::Matrix6d L_dot = Eigen::Matrix6d::Zero(); + L_dot.topLeftCorner<3, 3>() = R_dot; + L_dot.bottomRightCorner<3, 3>() = Q_dot; + + return L_dot; +} + +Eigen::Matrix6d calculate_coriolis(const double mass, + const Eigen::Vector3d& r_b_bg, + const vortex::utils::types::Twist& twist, + const Eigen::Matrix3d& inertia_matrix_body) { + using vortex::utils::math::get_skew_symmetric_matrix; + Eigen::Vector3d linear_speed = twist.to_vector().head(3); + Eigen::Vector3d angular_speed = twist.to_vector().tail(3); + Eigen::Matrix6d C; + C.topLeftCorner<3, 3>() = + mass * vortex::utils::math::get_skew_symmetric_matrix(linear_speed); + C.topRightCorner<3, 3>() = -mass * + get_skew_symmetric_matrix(angular_speed) * + get_skew_symmetric_matrix(r_b_bg); + C.bottomLeftCorner<3, 3>() = mass * + get_skew_symmetric_matrix(angular_speed) * + get_skew_symmetric_matrix(r_b_bg); + ; + C.bottomRightCorner<3, 3>() = + get_skew_symmetric_matrix(inertia_matrix_body * angular_speed); + + return C; +} + +Eigen::Matrix6x12d calculate_Y_v(const vortex::utils::types::Twist& twist) { + Eigen::Matrix6x12d Y_v; + Y_v.setZero(); + + Y_v(0, 0) = twist.u; + Y_v(0, 1) = twist.u * std::abs(twist.u); + + Y_v(1, 2) = twist.v; + Y_v(1, 3) = twist.v * std::abs(twist.v); + + Y_v(2, 4) = twist.w; + Y_v(2, 5) = twist.w * std::abs(twist.w); + + Y_v(3, 6) = twist.p; + Y_v(3, 7) = twist.p * std::abs(twist.p); + + Y_v(4, 8) = twist.q; + Y_v(4, 9) = twist.q * std::abs(twist.q); + + Y_v(5, 10) = twist.r; + Y_v(5, 11) = twist.r * std::abs(twist.r); + + return Y_v; +} + +} // namespace vortex::control diff --git a/control/dp_adapt_backs_controller_quat/test/CMakeLists.txt b/control/dp_adapt_backs_controller_quat/test/CMakeLists.txt new file mode 100644 index 000000000..5b97abfbf --- /dev/null +++ b/control/dp_adapt_backs_controller_quat/test/CMakeLists.txt @@ -0,0 +1,28 @@ +cmake_minimum_required(VERSION 3.8) + +find_package(GTest REQUIRED) +find_package(yaml-cpp REQUIRED) +include(GoogleTest) + +set(TEST_BINARY_NAME ${PROJECT_NAME}_test) +add_executable( + ${TEST_BINARY_NAME} + dp_adapt_backs_controller_tests.cpp +) + +target_compile_definitions(${TEST_BINARY_NAME} PRIVATE + DRONE_YAML_PATH="${CMAKE_CURRENT_SOURCE_DIR}/../../../auv_setup/config/robots/nautilus.yaml" + CONTROLLER_YAML_PATH="${CMAKE_CURRENT_SOURCE_DIR}/../config/adapt_params_nautilus.yaml" +) + +target_link_libraries( + ${TEST_BINARY_NAME} + PRIVATE + ${LIB_NAME} + yaml-cpp + GTest::GTest +) + +ament_target_dependencies(${TEST_BINARY_NAME} PUBLIC Eigen3 tf2) + +gtest_discover_tests(${TEST_BINARY_NAME}) diff --git a/control/dp_adapt_backs_controller_quat/test/dp_adapt_backs_controller_tests.cpp b/control/dp_adapt_backs_controller_quat/test/dp_adapt_backs_controller_tests.cpp new file mode 100644 index 000000000..2e19489ac --- /dev/null +++ b/control/dp_adapt_backs_controller_quat/test/dp_adapt_backs_controller_tests.cpp @@ -0,0 +1,477 @@ +#include +#include + +#include + +#include "dp_adapt_backs_controller_quat/dp_adapt_backs_controller.hpp" +#include "dp_adapt_backs_controller_quat/dp_adapt_backs_controller_utils.hpp" +#include "dp_adapt_backs_controller_quat/typedefs.hpp" + +namespace vortex::control { + +using vortex::utils::types::Pose; +using vortex::utils::types::PoseEuler; +using vortex::utils::types::Twist; + +DPAdaptParams load_dp_adapt_params(const std::string& drone_yaml_path, + const std::string& controller_yaml_path) { + YAML::Node drone_params = + YAML::LoadFile(drone_yaml_path)["/**"]["ros__parameters"]; + YAML::Node controller_params = + YAML::LoadFile(controller_yaml_path)["/**"]["ros__parameters"]; + + auto K1_vec = controller_params["K1"].as>(); + auto K2_vec = controller_params["K2"].as>(); + auto adapt_gain_vec = + controller_params["adapt_gain"].as>(); + auto d_gain_vec = controller_params["d_gain"].as>(); + auto r_b_bg_vec = controller_params["r_b_bg"].as>(); + + auto mass_matrix_vec = + drone_params["physical"]["mass_matrix"].as>(); + + Eigen::Matrix6d mass_matrix = + Eigen::Map(mass_matrix_vec.data()); + + DPAdaptParams params; + params.K1 = Eigen::Map(K1_vec.data()); + params.K2 = Eigen::Map(K2_vec.data()); + params.adapt_param = Eigen::Map(adapt_gain_vec.data()); + params.d_gain = Eigen::Map(d_gain_vec.data()); + params.r_b_bg = Eigen::Map(r_b_bg_vec.data()); + params.mass_intertia_matrix = mass_matrix; + params.mass = mass_matrix(0, 0); + params.inertia_matrix_body = Eigen::Vector3d(mass_matrix(3, 3), mass_matrix(4, 4), + mass_matrix(5, 5)); + return params; +} + +class DPAdaptBacksControllerTests : public ::testing::Test { + protected: + DPAdaptBacksControllerTests() + : dp_adapt_backs_controller_{ + load_dp_adapt_params(DRONE_YAML_PATH, CONTROLLER_YAML_PATH)} {} + + Pose generate_current_pose(const double north_pos, + const double east_pos, + const double down_pos, + const double roll_angle, + const double pitch_angle, + const double yaw_angle) { + return PoseEuler{north_pos, east_pos, down_pos, + roll_angle, pitch_angle, yaw_angle} + .as_pose(); + } + + Pose generate_reference_pose(const double north_pos, + const double east_pos, + const double down_pos, + const double roll_angle, + const double pitch_angle, + const double yaw_angle) { + return PoseEuler{north_pos, east_pos, down_pos, + roll_angle, pitch_angle, yaw_angle} + .as_pose(); + } + + Twist generate_current_velocity(const double surge_vel, + const double sway_vel, + const double heave_vel, + const double roll_rate, + const double pitch_rate, + const double yaw_rate) { + return {surge_vel, sway_vel, heave_vel, + roll_rate, pitch_rate, yaw_rate}; + } + + DPAdaptBacksController dp_adapt_backs_controller_; +}; + +/* +Test that negative north error only (in body) gives positive surge command only. +*/ + +TEST_F(DPAdaptBacksControllerTests, + T01_neg_north_error_with_zero_heading_gives_surge_only_command) { + Pose pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Pose pose_d{generate_reference_pose(10.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Twist twist{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Eigen::Vector6d tau{ + dp_adapt_backs_controller_.calculate_tau(pose, pose_d, twist)}; + EXPECT_GT(tau[0], 0.0); + EXPECT_NEAR(tau[1], 0.0, 0.01); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that negative north error with positive heading gives a positive surge +command and negative sway command. +*/ + +TEST_F( + DPAdaptBacksControllerTests, + T02_neg_north_error_with_positive_heading_gives_pos_surge_and_neg_sway_command) { + Pose pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 1.5)}; + Pose pose_d{generate_reference_pose(10.0, 0.0, 0.0, 0.0, 0.0, 1.5)}; + Twist twist{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Eigen::Vector6d tau{ + dp_adapt_backs_controller_.calculate_tau(pose, pose_d, twist)}; + EXPECT_GT(tau[0], 0.0); + EXPECT_LT(tau[1], 0.0); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that negative north error with negative heading gives a positive surge +command and positive sway command. +*/ + +TEST_F( + DPAdaptBacksControllerTests, + T03_neg_north_error_with_negative_heading_gives_pos_surge_and_pos_sway_command) { + Pose pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, -1.5)}; + Pose pose_d{generate_reference_pose(10.0, 0.0, 0.0, 0.0, 0.0, -1.5)}; + Twist twist{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Eigen::Vector6d tau{ + dp_adapt_backs_controller_.calculate_tau(pose, pose_d, twist)}; + EXPECT_GT(tau[0], 0.0); + EXPECT_GT(tau[1], 0.0); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that negative down error with zero roll and pitch gives a positive heave +command. +*/ + +TEST_F( + DPAdaptBacksControllerTests, + T04_neg_down_error_with_zero_roll_and_pitch_gives_positive_heave_command) { + Pose pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Pose pose_d{generate_reference_pose(0.0, 0.0, 2.0, 0.0, 0.0, 0.0)}; + Twist twist{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Eigen::Vector6d tau{ + dp_adapt_backs_controller_.calculate_tau(pose, pose_d, twist)}; + EXPECT_NEAR(tau[0], 0.0, 0.01); + EXPECT_NEAR(tau[1], 0.0, 0.01); + EXPECT_GT(tau[2], 0.0); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that negative down error with zero roll and negative pitch gives a positive +heave and positive surge command. +*/ + +TEST_F( + DPAdaptBacksControllerTests, + T05_neg_down_error_with_zero_roll_and_neg_pitch_gives_positive_heave_and_positive_surge_command) { + Pose pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, -0.5, 0.0)}; + Pose pose_d{generate_reference_pose(0.0, 0.0, 2.0, 0.0, -0.5, 0.0)}; + Twist twist{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Eigen::Vector6d tau{ + dp_adapt_backs_controller_.calculate_tau(pose, pose_d, twist)}; + EXPECT_GT(tau[0], 0.0); + EXPECT_NEAR(tau[1], 0.0, 0.01); + EXPECT_GT(tau[2], 0.0); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that negative down error with zero roll and positive pitch gives a positive +heave and negative surge command. +*/ + +TEST_F( + DPAdaptBacksControllerTests, + T06_neg_down_error_with_zero_roll_and_pos_pitch_gives_positive_heave_and_negative_surge_command) { + Pose pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.5, 0.0)}; + Pose pose_d{generate_reference_pose(0.0, 0.0, 2.0, 0.0, 0.5, 0.0)}; + Twist twist{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Eigen::Vector6d tau{ + dp_adapt_backs_controller_.calculate_tau(pose, pose_d, twist)}; + EXPECT_LT(tau[0], 0.0); + EXPECT_NEAR(tau[1], 0.0, 0.01); + EXPECT_GT(tau[2], 0.0); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that negative east error with zero heading gives a positive sway command. +*/ + +TEST_F(DPAdaptBacksControllerTests, + T07_neg_east_error_with_zero_heading_gives_positive_sway_command) { + Pose pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Pose pose_d{generate_reference_pose(0.0, 10.0, 0.0, 0.0, 0.0, 0.0)}; + Twist twist{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Eigen::Vector6d tau{ + dp_adapt_backs_controller_.calculate_tau(pose, pose_d, twist)}; + EXPECT_NEAR(tau[0], 0.0, 0.01); + EXPECT_GT(tau[1], 0.0); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that positive east error with zero heading gives a negative sway command. +*/ + +TEST_F(DPAdaptBacksControllerTests, + T08_pos_east_error_with_zero_heading_gives_pos_sway_command) { + Pose pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Pose pose_d{generate_reference_pose(0.0, -10.0, 0.0, 0.0, 0.0, 0.0)}; + Twist twist{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Eigen::Vector6d tau{ + dp_adapt_backs_controller_.calculate_tau(pose, pose_d, twist)}; + EXPECT_NEAR(tau[0], 0.0, 0.01); + EXPECT_LT(tau[1], 0.0); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that negative east error with positive heading gives a positive surge and +sway command. +*/ + +TEST_F( + DPAdaptBacksControllerTests, + T09_neg_east_error_with_positive_heading_gives_pos_sway_and_pos_surge_command) { + Pose pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 1.5)}; + Pose pose_d{generate_reference_pose(0.0, 10.0, 0.0, 0.0, 0.0, 1.5)}; + Twist twist{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Eigen::Vector6d tau{ + dp_adapt_backs_controller_.calculate_tau(pose, pose_d, twist)}; + EXPECT_GT(tau[0], 0.0); + EXPECT_GT(tau[1], 0.0); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that negative east error with negative heading gives a negative surge and +positive sway command. +*/ + +TEST_F( + DPAdaptBacksControllerTests, + T10_neg_east_error_with_negative_heading_gives_pos_sway_and_neg_surge_command) { + Pose pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, -1.5)}; + Pose pose_d{generate_reference_pose(0.0, 10.0, 0.0, 0.0, 0.0, -1.5)}; + Twist twist{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Eigen::Vector6d tau{ + dp_adapt_backs_controller_.calculate_tau(pose, pose_d, twist)}; + EXPECT_LT(tau[0], 0.0); + EXPECT_GT(tau[1], 0.0); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that negative roll error gives positive roll command. +*/ + +TEST_F(DPAdaptBacksControllerTests, + T11_neg_roll_error_gives_positive_roll_command) { + Pose pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Pose pose_d{generate_reference_pose(0.0, 0.0, 0.0, 1.0, 0.0, 0.0)}; + Twist twist{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Eigen::Vector6d tau{ + dp_adapt_backs_controller_.calculate_tau(pose, pose_d, twist)}; + EXPECT_NEAR(tau[0], 0.0, 0.01); + EXPECT_NEAR(tau[1], 0.0, 0.01); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_GT(tau[3], 0.0); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that positive roll error gives negative roll command. +*/ + +TEST_F(DPAdaptBacksControllerTests, T12_pos_roll_error_gives_neg_roll_command) { + Pose pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Pose pose_d{generate_reference_pose(0.0, 0.0, 0.0, -1.0, 0.0, 0.0)}; + Twist twist{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Eigen::Vector6d tau{ + dp_adapt_backs_controller_.calculate_tau(pose, pose_d, twist)}; + EXPECT_NEAR(tau[0], 0.0, 0.01); + EXPECT_NEAR(tau[1], 0.0, 0.01); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_LT(tau[3], 0.0); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that negative pitch error gives positive pitch command. +*/ + +TEST_F(DPAdaptBacksControllerTests, + T13_neg_pitch_error_gives_pos_pitch_command) { + Pose pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Pose pose_d{generate_reference_pose(0.0, 0.0, 0.0, 0.0, 1.0, 0.0)}; + Twist twist{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Eigen::Vector6d tau{ + dp_adapt_backs_controller_.calculate_tau(pose, pose_d, twist)}; + EXPECT_NEAR(tau[0], 0.0, 0.01); + EXPECT_NEAR(tau[1], 0.0, 0.01); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_GT(tau[4], 0.0); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that positive pitch error gives negative pitch command. +*/ + +TEST_F(DPAdaptBacksControllerTests, + T14_pos_pitch_error_gives_neg_pitch_command) { + Pose pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Pose pose_d{generate_reference_pose(0.0, 0.0, 0.0, 0.0, -1.0, 0.0)}; + Twist twist{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Eigen::Vector6d tau{ + dp_adapt_backs_controller_.calculate_tau(pose, pose_d, twist)}; + EXPECT_NEAR(tau[0], 0.0, 0.01); + EXPECT_NEAR(tau[1], 0.0, 0.01); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_LT(tau[4], 0.0); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that negative yaw error gives positive yaw command. +*/ + +TEST_F(DPAdaptBacksControllerTests, T15_neg_yaw_error_gives_pos_yaw_command) { + Pose pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Pose pose_d{generate_reference_pose(0.0, 0.0, 0.0, 0.0, 0.0, 1.0)}; + Twist twist{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Eigen::Vector6d tau{ + dp_adapt_backs_controller_.calculate_tau(pose, pose_d, twist)}; + EXPECT_NEAR(tau[0], 0.0, 0.01); + EXPECT_NEAR(tau[1], 0.0, 0.01); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_GT(tau[5], 0.0); +} + +/* +Test that positive yaw error gives negative yaw command. +*/ + +TEST_F(DPAdaptBacksControllerTests, T16_pos_yaw_error_gives_neg_yaw_command) { + Pose pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Pose pose_d{generate_reference_pose(0.0, 0.0, 0.0, 0.0, 0.0, -1.0)}; + Twist twist{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Eigen::Vector6d tau{ + dp_adapt_backs_controller_.calculate_tau(pose, pose_d, twist)}; + EXPECT_NEAR(tau[0], 0.0, 0.01); + EXPECT_NEAR(tau[1], 0.0, 0.01); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_LT(tau[5], 0.0); +} + +/* +Test that positive surge velocity only results in negative surge command +(breaking effect). +*/ + +TEST_F(DPAdaptBacksControllerTests, + T17_pos_surge_vel_gives_negative_surge_command) { + Pose pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Pose pose_d{generate_reference_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Twist twist{generate_current_velocity(1.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Eigen::Vector6d tau{ + dp_adapt_backs_controller_.calculate_tau(pose, pose_d, twist)}; + EXPECT_LT(tau[0], 0.0); + EXPECT_NEAR(tau[1], 0.0, 0.01); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); + // Torque channels may have cross-coupling from off-diagonal mass matrix + // terms in the Coriolis matrix +} + +/* +Test that positive sway velocity only results in negative sway command (breaking +effect). +*/ + +TEST_F(DPAdaptBacksControllerTests, + T18_pos_sway_vel_gives_negative_sway_command) { + Pose pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Pose pose_d{generate_reference_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Twist twist{generate_current_velocity(0.0, 1.0, 0.0, 0.0, 0.0, 0.0)}; + Eigen::Vector6d tau{ + dp_adapt_backs_controller_.calculate_tau(pose, pose_d, twist)}; + EXPECT_NEAR(tau[0], 0.0, 0.01); + EXPECT_LT(tau[1], 0.0); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); + // Torque channels may have cross-coupling from off-diagonal mass matrix + // terms in the Coriolis matrix +} + +/* +Test that positive heave velocity only results in negative heave command +(breaking effect). +*/ + +TEST_F(DPAdaptBacksControllerTests, + T19_pos_heave_vel_gives_negative_heave_command) { + Pose pose{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Pose pose_d{generate_reference_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + Twist twist{generate_current_velocity(0.0, 0.0, 1.0, 0.0, 0.0, 0.0)}; + Eigen::Vector6d tau{ + dp_adapt_backs_controller_.calculate_tau(pose, pose_d, twist)}; + EXPECT_NEAR(tau[0], 0.0, 0.01); + EXPECT_NEAR(tau[1], 0.0, 0.01); + EXPECT_LT(tau[2], 0.0); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); + // Torque channels may have cross-coupling from off-diagonal mass matrix + // terms in the Coriolis matrix +} + +} // namespace vortex::control + +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + + return RUN_ALL_TESTS(); +} diff --git a/control/pid_controller_dp/CMakeLists.txt b/control/pid_controller_dp/CMakeLists.txt index 17db75976..481c7ad31 100644 --- a/control/pid_controller_dp/CMakeLists.txt +++ b/control/pid_controller_dp/CMakeLists.txt @@ -11,38 +11,98 @@ endif() find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) find_package(nav_msgs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(Eigen3 REQUIRED) find_package(tf2 REQUIRED) find_package(vortex_msgs REQUIRED) +find_package(rcl_interfaces REQUIRED) find_package(vortex_utils REQUIRED) +find_package(spdlog REQUIRED) +find_package(fmt REQUIRED) find_package(vortex_utils_ros REQUIRED) include_directories(include) +set(LIB_NAME ${PROJECT_NAME}_lib) -add_executable(pid_controller_node - src/pid_controller_node.cpp - src/pid_controller_ros.cpp +add_library(${LIB_NAME} SHARED src/pid_controller.cpp src/pid_controller_utils.cpp src/pid_controller_conversions.cpp ) -ament_target_dependencies(pid_controller_node +ament_target_dependencies(${LIB_NAME} PUBLIC + rclcpp + geometry_msgs + nav_msgs + Eigen3 + tf2 + vortex_msgs + rcl_interfaces + vortex_utils + vortex_utils_ros + spdlog + fmt +) + + +add_library(pid_controller_component SHARED + src/pid_controller_ros.cpp +) + +ament_target_dependencies(pid_controller_component PUBLIC rclcpp + rclcpp_components geometry_msgs nav_msgs Eigen3 tf2 vortex_msgs + rcl_interfaces vortex_utils vortex_utils_ros + spdlog + fmt +) + +target_link_libraries(pid_controller_component PUBLIC + ${LIB_NAME} + spdlog::spdlog +) + +rclcpp_components_register_node(pid_controller_component + PLUGIN "PIDControllerNode" + EXECUTABLE pid_controller_node_component +) + +add_executable(pid_controller_node + src/pid_controller_node.cpp +) + +target_link_libraries(pid_controller_node PRIVATE + pid_controller_component + spdlog::spdlog ) -install(TARGETS - pid_controller_node - DESTINATION lib/${PROJECT_NAME}) +ament_export_targets(export_${LIB_NAME}) + +install(TARGETS ${LIB_NAME} + EXPORT export_${LIB_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install(TARGETS pid_controller_component + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install(TARGETS pid_controller_node + DESTINATION lib/${PROJECT_NAME} +) install(DIRECTORY config @@ -50,4 +110,9 @@ install(DIRECTORY DESTINATION share/${PROJECT_NAME}/ ) + +if(BUILD_TESTING) + add_subdirectory(test) +endif() + ament_package() diff --git a/control/pid_controller_dp/README.md b/control/pid_controller_dp/README.md index 902fc9fc3..40c470e3e 100644 --- a/control/pid_controller_dp/README.md +++ b/control/pid_controller_dp/README.md @@ -1,7 +1,94 @@ -## PID controller +# PID controller + The PID controller is defined + ```math \tau = -J_{q}^{\dagger}(K_p \tilde{\eta} + K_d \dot{\tilde{\eta}} + K_i \int^t_0 \tilde{\eta}(\tau)d\tau) ``` -where $\tau$ is the control input, $\tilde{\eta} = \eta - \eta_d$ is the pose error, $J_q$ is the quaternion based Jacobian matrix and $K_p$, $K_d$ and $K_i$ are tuning matrices. +where: + +- $\tilde{\eta} = \eta - \eta_d$ is the pose error (7D quaternion representation), +- $J_q$ is the quaternion Jacobian (7×6), and $J_q^{\dagger}$ is its (pseudo-)inverse, +- $K_p$, $K_d$, $K_i$ are 6×6 gain matrices. + +## PID controller (pid_controller_dp) + +This package implements a 6-DOF PID controller that operates on the +6-dimensional control vector +$$\tau = [X, Y, Z, K, M, N]^T$$ +and uses a quaternion-based 7D pose representation for attitude. + +## Build + + +This package is built as part of the workspace. From the workspace root: + +```bash +colcon build --packages-select pid_controller_dp +``` + +To run tests for this package only: + +```bash +colcon test --packages-select pid_controller_dp && colcon test-result --verbose +``` + +## Usage (ROS 2 node) + +The package provides a node `pid_controller_node` that subscribes to pose, +twist and guidance topics and publishes wrench (tau) commands. + + +- `topics.pose` (type: `geometry_msgs/PoseWithCovarianceStamped`) — vehicle pose input +- `topics.twist` (type: `geometry_msgs/TwistWithCovarianceStamped`) — velocity input +- `topics.guidance.dp` (type: `vortex_msgs/ReferenceFilter`) — desired states (pose/vecocity) +- `topics.wrench_input` (type: `geometry_msgs/WrenchStamped`) — output wrench + +Parameters expose PID gains (Kp, Ki, Kd) as per-component values which are +assembled into diagonal gain matrices inside the node. See the node source +(`src/pid_controller_ros.cpp`) for parameter names. + +## Examples + +Start the node (after sourcing workspace): + +1. Run the simulation + + ```bash + ros2 launch stonefish_sim simulation.launch.py scenario:=default + ``` + +2. Run the thrust allocation node: + + ```bash + ros2 launch thrust_allocator_auv thrust_allocator_auv.launch.py + ``` + +3. To move the robot, run the joystick node + + ```bash + ros2 launch stonefish_sim orca_sim.launch.py + ``` + +4. Run the controller + + ```bash + ros2 launch pid_controller_dp pid_controller_dp.launch.py + ``` + +Use the joy stick to move the robot. The key mappings are: + +- B - kill +- Y - autonomous mode (reference model) +- A - manual mode + +Note: When plotting, the axis plotted and actual command might not align since the plotting is based on the joy controller frame (`odom`), whereas the controller works on the robot frame (`body_frame`) + +## Tuning + +The `rqt_reconfigure` can be used to change the controller gains. + +```bash +ros2 run rqt_reconfigure rqt_reconfigure +``` diff --git a/control/pid_controller_dp/config/pid_params.yaml b/control/pid_controller_dp/config/pid_params.yaml index 2a77ffd59..226d6b219 100644 --- a/control/pid_controller_dp/config/pid_params.yaml +++ b/control/pid_controller_dp/config/pid_params.yaml @@ -1,5 +1,20 @@ /**: ros__parameters: - Kp: [70.0, 70.0, 70.0, 12.0, 12.0, 12.0] - Ki: [2.0, 2.0, 2.0, 0.12, 0.12, 0.12] - Kd: [10.0, 10.0, 10.0, 4.0, 5.0, 4.0] + Kp_x: -80.0 + Kp_y: -90.0 + Kp_z: 120.0 + Kp_roll: 35.0 + Kp_pitch: 35.0 + Kp_yaw: 60.0 + Ki_x: 0.00000006 + Ki_y: 0.00000006 + Ki_z: 0.2 + Ki_roll: 0.35 + Ki_pitch: 0.35 + Ki_yaw: 0.005 + Kd_x: 235.0 + Kd_y: 240.0 + Kd_z: 135.0 + Kd_roll: 135.0 + Kd_pitch: 135.0 + Kd_yaw: 125.0 diff --git a/control/pid_controller_dp/config/pid_params_sim.yaml b/control/pid_controller_dp/config/pid_params_sim.yaml new file mode 100644 index 000000000..487640b84 --- /dev/null +++ b/control/pid_controller_dp/config/pid_params_sim.yaml @@ -0,0 +1,20 @@ +/**: + ros__parameters: + Kp_x: 50.0 + Kp_y: 50.0 + Kp_z: 25.0 + Kp_roll: 50.0 + Kp_pitch: 80.0 + Kp_yaw: 50.0 + Ki_x: 0.00 + Ki_y: 0.00 + Ki_z: 0.01 + Ki_roll: 0.07 + Ki_pitch: 0.10 + Ki_yaw: 0.0 + Kd_x: 120.0 + Kd_y: 120.0 + Kd_z: 250.0 + Kd_roll: 90.0 + Kd_pitch: 120.0 + Kd_yaw: 90.0 diff --git a/control/pid_controller_dp/include/pid_controller_dp/pid_controller.hpp b/control/pid_controller_dp/include/pid_controller_dp/pid_controller.hpp index 1017b5bd6..389249311 100644 --- a/control/pid_controller_dp/include/pid_controller_dp/pid_controller.hpp +++ b/control/pid_controller_dp/include/pid_controller_dp/pid_controller.hpp @@ -1,6 +1,7 @@ #ifndef PID_CONTROLLER_DP__PID_CONTROLLER_HPP_ #define PID_CONTROLLER_DP__PID_CONTROLLER_HPP_ +#include #include "pid_controller_dp/typedefs.hpp" class PIDController { @@ -36,11 +37,15 @@ class PIDController { // @param dt: Time step void set_time_step(double dt); + types::Matrix6d get_kp(); + types::Matrix6d get_ki(); + types::Matrix6d get_kd(); + private: types::Matrix6d Kp_; types::Matrix6d Ki_; types::Matrix6d Kd_; - types::Vector7d integral_; + types::Vector6d integral_; double dt_; }; diff --git a/control/pid_controller_dp/include/pid_controller_dp/pid_controller_conversions.hpp b/control/pid_controller_dp/include/pid_controller_dp/pid_controller_conversions.hpp index a28097e9f..5b2ea6dbc 100644 --- a/control/pid_controller_dp/include/pid_controller_dp/pid_controller_conversions.hpp +++ b/control/pid_controller_dp/include/pid_controller_dp/pid_controller_conversions.hpp @@ -3,16 +3,16 @@ #include #include -#include -#include +#include +#include #include #include #include "pid_controller_dp/typedefs.hpp" types::Eta eta_convert_from_ros_to_eigen( - const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); + const geometry_msgs::msg::PoseWithCovariance& msg); types::Nu nu_convert_from_ros_to_eigen( - const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg); + const geometry_msgs::msg::TwistWithCovariance& msg); #endif // PID_CONTROLLER_DP__PID_CONTROLLER_CONVERSIONS_HPP_ diff --git a/control/pid_controller_dp/include/pid_controller_dp/pid_controller_ros.hpp b/control/pid_controller_dp/include/pid_controller_dp/pid_controller_ros.hpp index 9b302d1af..6b09d4fb6 100644 --- a/control/pid_controller_dp/include/pid_controller_dp/pid_controller_ros.hpp +++ b/control/pid_controller_dp/include/pid_controller_dp/pid_controller_ros.hpp @@ -7,6 +7,7 @@ #include #include #include +#include #include #include #include @@ -15,7 +16,7 @@ #include #include #include -#include +#include #include #include "pid_controller_dp/pid_controller.hpp" #include "pid_controller_dp/typedefs.hpp" @@ -23,7 +24,7 @@ // @brief Class for the PID controller node class PIDControllerNode : public rclcpp::Node { public: - PIDControllerNode(); + explicit PIDControllerNode(const rclcpp::NodeOptions & options); private: // @brief Callback function for the killswitch topic @@ -35,16 +36,6 @@ class PIDControllerNode : public rclcpp::Node { void operation_mode_callback( const vortex_msgs::msg::OperationMode::SharedPtr msg); - // @brief Callback function for the pose topic - // @param msg: PoseWithCovarianceStamped message containing the AUV pose - void pose_callback( - const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); - - // @brief Callback function for the twist topic - // @param msg: TwistWithCovarianceStamped message containing the AUV speed - void twist_callback( - const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg); - // @brief Callback function for the tau publisher timer void publish_tau(); @@ -62,7 +53,16 @@ class PIDControllerNode : public rclcpp::Node { // @param msg: ReferenceFilter message containing the desired vehicle pose // and velocity void guidance_callback( - const vortex_msgs::msg::ReferenceFilter::SharedPtr msg); + const vortex_msgs::msg::ReferenceFilterQuat::SharedPtr msg); + + // @brief Callback function for the odometry topic + // @param msg: Odometry message containing the AUV pose and speed + void odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg); + + // @brief Callback function for parameter updates + // @param parameters: vector of parameters to be set + rcl_interfaces::msg::SetParametersResult parametersCallback( + const std::vector& parameters); rclcpp::Client::SharedPtr get_operation_mode_client_; @@ -74,13 +74,9 @@ class PIDControllerNode : public rclcpp::Node { rclcpp::Subscription::SharedPtr operation_mode_sub_; - rclcpp::Subscription< - geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_sub_; - - rclcpp::Subscription< - geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr twist_sub_; + rclcpp::Subscription::SharedPtr odom_sub_; - rclcpp::Subscription::SharedPtr + rclcpp::Subscription::SharedPtr guidance_sub_; rclcpp::Subscription::SharedPtr kp_sub_; @@ -107,6 +103,8 @@ class PIDControllerNode : public rclcpp::Node { vortex::utils::types::Mode operation_mode_{ vortex::utils::types::Mode::manual}; + + OnSetParametersCallbackHandle::SharedPtr callback_handle_; }; #endif // PID_CONTROLLER_DP__PID_CONTROLLER_ROS_HPP_ diff --git a/control/pid_controller_dp/include/pid_controller_dp/pid_controller_utils.hpp b/control/pid_controller_dp/include/pid_controller_dp/pid_controller_utils.hpp index 1bdce214b..442e31813 100644 --- a/control/pid_controller_dp/include/pid_controller_dp/pid_controller_utils.hpp +++ b/control/pid_controller_dp/include/pid_controller_dp/pid_controller_utils.hpp @@ -1,12 +1,15 @@ #ifndef PID_CONTROLLER_DP__PID_CONTROLLER_UTILS_HPP_ #define PID_CONTROLLER_DP__PID_CONTROLLER_UTILS_HPP_ +#include #include #include #include #include #include +#include #include "pid_controller_dp/typedefs.hpp" +#include "typedefs.hpp" // @brief Calculate the sine of an angle in degrees // @param angle: Angle in degrees @@ -20,26 +23,26 @@ double ssa(double angle); // p.34 eq: 2.72 types::Matrix3d calculate_R_quat(const types::Eta& eta); -// @brief Calculate the transformation matrix from a quaternion -// @param q: Quaternion represented as a 4D vector [w, x, y, z] -// @return 4x3 transformation matrix +// @brief Calculate the transformation sub-matrix from a quaternion. +// Returns the bottom 3 rows of the full 4x3 T matrix (rows for qx, qy, qz +// derivatives), giving a 3x3 mapping from body angular velocity to d/dt [qx, +// qy, qz]. // REF: Handbook of Marine Craft Hydrodynamics and Motion Control, Fossen 2021 // p.35 eq: 2.78 -types::Matrix4x3d calculate_T_quat(const types::Eta& eta); +types::Matrix3d calculate_T_quat(const types::Eta& eta); -// @brief Calculate the Jacobian matrix -// @param eta: 7D vector containing the vehicle pose [x, y, z, w, x, y, z] -// @return 7x6 Jacobian matrix -// REF: Handbook of Marine Craft Hydrodynamics and Motion Control, Fossen 2021 -// p.36 eq: 2.83 +// @brief Calculate the 6x6 Jacobian matrix using only the vector part of the +// quaternion. +// J = blockdiag(R, T_33) where T_33 maps body angular velocity to d/dt +// [qx,qy,qz]. +// @param eta: vehicle pose [x, y, z, qw, qx, qy, qz] +// @return 6x6 Jacobian matrix types::J_transformation calculate_J(const types::Eta& eta); -// @brief Calculate the pseudo-inverse of the Jacobian matrix -// @param eta: 7D vector containing the vehicle pose [x, y, z, w, x, y, z] -// @return 6x7 pseudo-inverse Jacobian matrix -// REF: Handbook of Marine Craft Hydrodynamics and Motion Control, Fossen 2021 -// p.34 eq: 2.72 -types::Matrix6x7d calculate_J_sudo_inv(const types::Eta& eta); +// @brief Calculate the inverse of the 6x6 Jacobian matrix. +// @param eta: vehicle pose [x, y, z, qw, qx, qy, qz] +// @return 6x6 inverse Jacobian matrix +types::Matrix6d calculate_J_sudo_inv(const types::Eta& eta); // @brief Calculate the error between the desired and actual vehicle pose // @param eta: 7D vector containing the actual vehicle pose [x, y, z, w, x, y, @@ -60,15 +63,14 @@ Eigen::VectorXd clamp_values(const Eigen::VectorXd& values, double min_val, double max_val); -// @brief Calculate the anti-windup term +// @brief Calculate the anti-windup term. // @param dt: Time step -// @param error: 7D vector containing the error between the desired and -// actual vehicle pose [x, y, z, w, x, y, z] -// @param integral: 7D vector containing the integral term of the PID -// controller [x, y, z, w, x, y, z] -// @return 7D vector containing the anti-windup term -types::Vector7d anti_windup(const double dt, - const types::Eta& error, - const types::Vector7d& integral); +// @param error_body: 6D error in body frame [surge, sway, heave, roll, pitch, +// yaw] +// @param integral: 6D integral term in body frame +// @return 6D anti-windup clamped integral +types::Vector6d anti_windup(const double dt, + const types::Vector6d& error_body, + const types::Vector6d& integral); #endif // PID_CONTROLLER_DP__PID_CONTROLLER_UTILS_HPP_ diff --git a/control/pid_controller_dp/include/pid_controller_dp/typedefs.hpp b/control/pid_controller_dp/include/pid_controller_dp/typedefs.hpp index beeea3174..5b8a65373 100644 --- a/control/pid_controller_dp/include/pid_controller_dp/typedefs.hpp +++ b/control/pid_controller_dp/include/pid_controller_dp/typedefs.hpp @@ -7,50 +7,33 @@ #define PID_CONTROLLER_DP__TYPEDEFS_HPP_ #include +#include namespace types { -typedef Eigen::Matrix Vector3d; -typedef Eigen::Matrix Vector6d; -typedef Eigen::Matrix Vector7d; -typedef Eigen::Matrix Vector4d; -typedef Eigen::Matrix Matrix6d; -typedef Eigen::Matrix Matrix3d; -typedef Eigen::Matrix Matrix4x3d; -typedef Eigen::Matrix Matrix7x6d; -typedef Eigen::Matrix Matrix6x7d; -typedef Eigen::Matrix Matrix7d; -typedef Eigen::Quaterniond Quaterniond; - -struct Eta { - Eigen::Vector3d pos = Eigen::Vector3d::Zero(); - Eigen::Quaterniond ori = Eigen::Quaterniond::Identity(); - - types::Vector7d as_vector() const { - types::Vector7d vec; - vec << pos, ori.w(), ori.x(), ori.y(), ori.z(); - return vec; - } -}; - -struct Nu { - Eigen::Vector3d linear_speed = types::Vector3d::Zero(); - Eigen::Vector3d angular_speed = types::Vector3d::Zero(); - - types::Vector6d as_vector() const { - types::Vector6d vec; - vec << linear_speed, angular_speed; - return vec; - } -}; +using Vector3d = Eigen::Matrix; +using Vector4d = Eigen::Matrix; +using Vector6d = Eigen::Matrix; +using Vector7d = Eigen::Matrix; +using Matrix3d = Eigen::Matrix; +using Matrix4x3d = Eigen::Matrix; +using Matrix6d = Eigen::Matrix; +using Matrix7x6d = Eigen::Matrix; +using Matrix6x7d = Eigen::Matrix; +using Matrix7d = Eigen::Matrix; +using Quaterniond = Eigen::Quaterniond; + +// Alias canonical types from vortex utils +using Eta = ::vortex::utils::types::Pose; +using Nu = ::vortex::utils::types::Twist; struct J_transformation { - Eigen::Matrix3d R = types::Matrix3d::Identity(); - types::Matrix4x3d T = types::Matrix4x3d::Zero(); + Matrix3d R = Matrix3d::Identity(); + Matrix3d T = Matrix3d::Zero(); - types::Matrix7x6d as_matrix() const { - types::Matrix7x6d mat = types::Matrix7x6d::Zero(); + Matrix6d as_matrix() const { + Matrix6d mat = Matrix6d::Zero(); mat.block<3, 3>(0, 0) = R; - mat.block<4, 3>(3, 3) = T; + mat.block<3, 3>(3, 3) = T; return mat; } }; diff --git a/control/pid_controller_dp/package.xml b/control/pid_controller_dp/package.xml index dd13d899e..0ec8c9b80 100644 --- a/control/pid_controller_dp/package.xml +++ b/control/pid_controller_dp/package.xml @@ -10,11 +10,13 @@ ament_cmake rclcpp + rclcpp_components geometry_msgs nav_msgs eigen tf2 vortex_msgs + rcl_interfaces vortex_utils vortex_utils_ros diff --git a/control/pid_controller_dp/src/pid_controller.cpp b/control/pid_controller_dp/src/pid_controller.cpp index 427c7dcdf..2b523248e 100644 --- a/control/pid_controller_dp/src/pid_controller.cpp +++ b/control/pid_controller_dp/src/pid_controller.cpp @@ -1,34 +1,109 @@ #include "pid_controller_dp/pid_controller.hpp" #include "pid_controller_dp/pid_controller_utils.hpp" +void print_eta(const types::Eta& eta) { + spdlog::info("Eta values:"); + auto pos = eta.pos_vector(); + auto ori = eta.ori_quaternion(); + spdlog::info("Position - North: {}, East: {}, Down: {}", pos[0], pos[1], + pos[2]); + spdlog::info("Orientation - w: {}, x: {}, y: {}, z: {}", ori.w(), ori.x(), + ori.y(), ori.z()); +} + +void print_nu(const types::Nu& nu) { + spdlog::info("Nu values:"); + auto v = nu.to_vector(); + spdlog::info("Linear Speed - u: {}, v: {}, w: {}", v(0), v(1), v(2)); + spdlog::info("Angular Speed - p: {}, q: {}, r: {}", v(3), v(4), v(5)); +} + +void print_vect_6d(const types::Vector6d& vec) { + spdlog::info("Vector6d values:"); + for (int i = 0; i < 6; ++i) { + spdlog::info("Element[{}]: {}", i, vec[i]); + } +} + +void print_J_transformation(const types::J_transformation& J) { + spdlog::info("J_transformation:"); + + spdlog::info("R (3x3) elements:"); + for (int i = 0; i < J.R.rows(); ++i) { + for (int j = 0; j < J.R.cols(); ++j) { + spdlog::info("R[{},{}] = {}", i, j, J.R(i, j)); + } + } + + spdlog::info("T (4x3) elements:"); + for (int i = 0; i < J.T.rows(); ++i) { + for (int j = 0; j < J.T.cols(); ++j) { + spdlog::info("T[{},{}] = {}", i, j, J.T(i, j)); + } + } + + spdlog::info("Combined Matrix (7x6) elements:"); + auto M = J.as_matrix(); + for (int i = 0; i < M.rows(); ++i) { + for (int j = 0; j < M.cols(); ++j) { + spdlog::info("M[{},{}] = {}", i, j, M(i, j)); + } + } +} + +void print_Jinv_transformation(const types::Matrix6x7d& J_inv) { + spdlog::info("J_pseudo_inverse (6x7):"); + for (int i = 0; i < J_inv.rows(); ++i) { + std::string row; + row.reserve(128); + row += "["; + for (int j = 0; j < J_inv.cols(); ++j) { + row += std::to_string(J_inv(i, j)); + if (j < J_inv.cols() - 1) + row += ", "; + } + row += "]"; + spdlog::info("{}", row); + } +} + PIDController::PIDController() : Kp_(types::Matrix6d::Identity()), Ki_(types::Matrix6d::Zero()), Kd_(types::Matrix6d::Zero()), - integral_(types::Vector7d::Zero()), + integral_(types::Vector6d::Zero()), dt_(0.01) {} types::Vector6d PIDController::calculate_tau(const types::Eta& eta, const types::Eta& eta_d, const types::Nu& nu, const types::Eta& eta_dot_d) { - types::Eta error = error_eta(eta, eta_d); + types::Eta error = error_eta(eta, eta_d); // calculate eta error - types::Matrix6x7d J_inv = calculate_J_sudo_inv(error); + // Only use the vector part of the quaternion for orientation error. + types::Vector6d error_6; + error_6 << error.x, error.y, error.z, error.qx, error.qy, error.qz; - types::Vector6d nu_d = J_inv * eta_dot_d.as_vector(); + // Desired velocity: also drop qw, keeping [x,y,z,qx,qy,qz] derivatives. + types::Vector6d eta_dot_d_6; + eta_dot_d_6 << eta_dot_d.x, eta_dot_d.y, eta_dot_d.z, eta_dot_d.qx, + eta_dot_d.qy, eta_dot_d.qz; - types::Vector6d error_nu = nu.as_vector() - nu_d; + // 6x6 J inverse: blockdiag(R, T_33)^{-1} + types::Matrix6d J_inv = calculate_J_sudo_inv(eta); - types::Vector6d P = Kp_ * J_inv * error.as_vector(); + types::Vector6d error_body = J_inv * error_6; // error in body frame - types::Vector6d I = Ki_ * J_inv * integral_; + types::Vector6d nu_d = J_inv * eta_dot_d_6; // desired body velocity + types::Vector6d error_nu = nu.to_vector() - nu_d; // velocity error - types::Vector6d D = Kd_ * error_nu; + types::Vector6d P = Kp_ * error_body; // P term + types::Vector6d I = Ki_ * integral_; // I term + types::Vector6d D = Kd_ * error_nu; // D term - types::Vector6d tau = -clamp_values((P + I + D), -80.0, 80.0); + types::Vector6d tau = -clamp_values((P + I + D), -90.0, 90.0); - integral_ = anti_windup(dt_, error, integral_); + integral_ = anti_windup(dt_, error_body, integral_); return tau; } @@ -48,3 +123,13 @@ void PIDController::set_kd(const types::Matrix6d& Kd) { void PIDController::set_time_step(double dt) { this->dt_ = dt; } + +types::Matrix6d PIDController::get_kp() { + return this->Kp_; +} +types::Matrix6d PIDController::get_ki() { + return this->Ki_; +} +types::Matrix6d PIDController::get_kd() { + return this->Kd_; +} diff --git a/control/pid_controller_dp/src/pid_controller_conversions.cpp b/control/pid_controller_dp/src/pid_controller_conversions.cpp index 8f6ff1970..1720752c4 100644 --- a/control/pid_controller_dp/src/pid_controller_conversions.cpp +++ b/control/pid_controller_dp/src/pid_controller_conversions.cpp @@ -5,24 +5,28 @@ #include "pid_controller_dp/typedefs.hpp" types::Eta eta_convert_from_ros_to_eigen( - const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) { + const geometry_msgs::msg::PoseWithCovariance& msg) { types::Eta eta; - eta.pos << msg->pose.pose.position.x, msg->pose.pose.position.y, - msg->pose.pose.position.z; - eta.ori.w() = msg->pose.pose.orientation.w; - eta.ori.x() = msg->pose.pose.orientation.x; - eta.ori.y() = msg->pose.pose.orientation.y; - eta.ori.z() = msg->pose.pose.orientation.z; + eta.x = msg.pose.position.x; + eta.y = msg.pose.position.y; + eta.z = msg.pose.position.z; + eta.qw = msg.pose.orientation.w; + eta.qx = msg.pose.orientation.x; + eta.qy = msg.pose.orientation.y; + eta.qz = msg.pose.orientation.z; return eta; } types::Nu nu_convert_from_ros_to_eigen( - const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) { + const geometry_msgs::msg::TwistWithCovariance& msg) { types::Nu nu; - nu.linear_speed << msg->twist.twist.linear.x, msg->twist.twist.linear.y, - msg->twist.twist.linear.z; - nu.angular_speed << msg->twist.twist.angular.x, msg->twist.twist.angular.y, - msg->twist.twist.angular.z; + nu.u = msg.twist.linear.x; + nu.v = msg.twist.linear.y; + nu.w = msg.twist.linear.z; + nu.p = msg.twist.angular.x; + nu.q = msg.twist.angular.y; + nu.r = msg.twist.angular.z; + return nu; } diff --git a/control/pid_controller_dp/src/pid_controller_node.cpp b/control/pid_controller_dp/src/pid_controller_node.cpp index f9bf44663..433ddf4bf 100644 --- a/control/pid_controller_dp/src/pid_controller_node.cpp +++ b/control/pid_controller_dp/src/pid_controller_node.cpp @@ -2,8 +2,7 @@ int main(int argc, char** argv) { rclcpp::init(argc, argv); - RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Started PID Controller Node"); - rclcpp::spin(std::make_shared()); + rclcpp::spin(std::make_shared(rclcpp::NodeOptions())); rclcpp::shutdown(); return 0; } diff --git a/control/pid_controller_dp/src/pid_controller_ros.cpp b/control/pid_controller_dp/src/pid_controller_ros.cpp index 44a63cc63..797d4020d 100644 --- a/control/pid_controller_dp/src/pid_controller_ros.cpp +++ b/control/pid_controller_dp/src/pid_controller_ros.cpp @@ -1,4 +1,6 @@ #include +#include +#include #include #include #include @@ -7,7 +9,17 @@ #include "pid_controller_dp/pid_controller_utils.hpp" #include "pid_controller_dp/typedefs.hpp" -PIDControllerNode::PIDControllerNode() : Node("pid_controller_node") { +constexpr std::string_view start_message = R"( + ____ ___ ____ ____ _ _ _ + | _ \_ _| _ \ / ___|___ _ __ | |_ _ __ ___ | | | ___ _ __ + | |_) | || | | | | | / _ \| '_ \| __| '__/ _ \| | |/ _ \ '__| + | __/| || |_| | | |__| (_) | | | | |_| | | (_) | | | __/ | + |_| |___|____/ \____\___/|_| |_|\__|_| \___/|_|_|\___|_| + +)"; + +PIDControllerNode::PIDControllerNode(const rclcpp::NodeOptions& options) + : Node("pid_controller_node", options) { time_step_ = std::chrono::milliseconds(10); set_subscribers_and_publisher(); @@ -16,6 +28,11 @@ PIDControllerNode::PIDControllerNode() : Node("pid_controller_node") { tau_pub_timer_ = this->create_wall_timer( time_step_, std::bind(&PIDControllerNode::publish_tau, this)); set_pid_params(); + + callback_handle_ = this->add_on_set_parameters_callback(std::bind( + &PIDControllerNode::parametersCallback, this, std::placeholders::_1)); + + spdlog::info(start_message); } void PIDControllerNode::set_subscribers_and_publisher() { @@ -24,15 +41,12 @@ void PIDControllerNode::set_subscribers_and_publisher() { rclcpp::QoSInitialization(qos_profile.history, 1), qos_profile); const auto qos_reliable{vortex::utils::qos_profiles::reliable_profile(1)}; - this->declare_parameter("topics.guidance.dp"); + this->declare_parameter("topics.guidance.dp_quat"); std::string dp_reference_topic = - this->get_parameter("topics.guidance.dp").as_string(); - - this->declare_parameter("topics.pose"); - std::string pose_topic = this->get_parameter("topics.pose").as_string(); + this->get_parameter("topics.guidance.dp_quat").as_string(); - this->declare_parameter("topics.twist"); - std::string twist_topic = this->get_parameter("topics.twist").as_string(); + this->declare_parameter("topics.odom"); + std::string odom_topic = this->get_parameter("topics.odom").as_string(); this->declare_parameter("topics.killswitch"); std::string software_kill_switch_topic = @@ -56,20 +70,13 @@ void PIDControllerNode::set_subscribers_and_publisher() { std::bind(&PIDControllerNode::operation_mode_callback, this, std::placeholders::_1)); - pose_sub_ = this->create_subscription< - geometry_msgs::msg::PoseWithCovarianceStamped>( - pose_topic, qos_sensor_data, - std::bind(&PIDControllerNode::pose_callback, this, - std::placeholders::_1)); - - twist_sub_ = this->create_subscription< - geometry_msgs::msg::TwistWithCovarianceStamped>( - twist_topic, qos_sensor_data, - std::bind(&PIDControllerNode::twist_callback, this, + odom_sub_ = this->create_subscription( + odom_topic, qos_sensor_data, + std::bind(&PIDControllerNode::odom_callback, this, std::placeholders::_1)); guidance_sub_ = - this->create_subscription( + this->create_subscription( dp_reference_topic, qos_sensor_data, std::bind(&PIDControllerNode::guidance_callback, this, std::placeholders::_1)); @@ -121,14 +128,16 @@ void PIDControllerNode::operation_mode_callback( operation_mode_ = vortex::utils::ros_conversions::convert_from_ros(*msg); } -void PIDControllerNode::pose_callback( - const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) { - eta_ = eta_convert_from_ros_to_eigen(msg); -} - -void PIDControllerNode::twist_callback( - const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) { - nu_ = nu_convert_from_ros_to_eigen(msg); +void PIDControllerNode::odom_callback( + const nav_msgs::msg::Odometry::SharedPtr msg) { + eta_ = eta_convert_from_ros_to_eigen(msg->pose); + if (eta_.qw < 0.0) { + eta_.qw = -eta_.qw; + eta_.qx = -eta_.qx; + eta_.qy = -eta_.qy; + eta_.qz = -eta_.qz; + } + nu_ = nu_convert_from_ros_to_eigen(msg->twist); } void PIDControllerNode::publish_tau() { @@ -154,20 +163,57 @@ void PIDControllerNode::publish_tau() { } void PIDControllerNode::set_pid_params() { - this->declare_parameter>( - "Kp", {1.0, 1.0, 1.0, 1.0, 1.0, 1.0}); - this->declare_parameter>( - "Ki", {0.1, 0.1, 0.1, 0.1, 0.1, 0.1}); - this->declare_parameter>( - "Kd", {0.1, 0.1, 0.1, 0.1, 0.1, 0.1}); - - std::vector Kp_vec = this->get_parameter("Kp").as_double_array(); - std::vector Ki_vec = this->get_parameter("Ki").as_double_array(); - std::vector Kd_vec = this->get_parameter("Kd").as_double_array(); - - types::Matrix6d Kp_eigen = Eigen::Map(Kp_vec.data()); - types::Matrix6d Ki_eigen = Eigen::Map(Ki_vec.data()); - types::Matrix6d Kd_eigen = Eigen::Map(Kd_vec.data()); + this->declare_parameter("Kp_x", 1.0); + this->declare_parameter("Kp_y", 1.0); + this->declare_parameter("Kp_z", 1.0); + this->declare_parameter("Kp_roll", 1.0); + this->declare_parameter("Kp_pitch", 1.0); + this->declare_parameter("Kp_yaw", 1.0); + this->declare_parameter("Ki_x", 0.1); + this->declare_parameter("Ki_y", 0.1); + this->declare_parameter("Ki_z", 0.1); + this->declare_parameter("Ki_roll", 0.1); + this->declare_parameter("Ki_pitch", 0.1); + this->declare_parameter("Ki_yaw", 0.1); + this->declare_parameter("Kd_x", 0.1); + this->declare_parameter("Kd_y", 0.1); + this->declare_parameter("Kd_z", 0.1); + this->declare_parameter("Kd_roll", 0.1); + this->declare_parameter("Kd_pitch", 0.1); + this->declare_parameter("Kd_yaw", 0.1); + + std::vector Kp_vec = { + this->get_parameter("Kp_x").as_double(), + this->get_parameter("Kp_y").as_double(), + this->get_parameter("Kp_z").as_double(), + this->get_parameter("Kp_roll").as_double(), + this->get_parameter("Kp_pitch").as_double(), + this->get_parameter("Kp_yaw").as_double(), + }; + std::vector Ki_vec = { + this->get_parameter("Ki_x").as_double(), + this->get_parameter("Ki_y").as_double(), + this->get_parameter("Ki_z").as_double(), + this->get_parameter("Ki_roll").as_double(), + this->get_parameter("Ki_pitch").as_double(), + this->get_parameter("Ki_yaw").as_double(), + }; + std::vector Kd_vec = { + this->get_parameter("Kd_x").as_double(), + this->get_parameter("Kd_y").as_double(), + this->get_parameter("Kd_z").as_double(), + this->get_parameter("Kd_roll").as_double(), + this->get_parameter("Kd_pitch").as_double(), + this->get_parameter("Kd_yaw").as_double(), + }; + + types::Vector6d Kp_vec_eigen(Kp_vec.data()); + types::Vector6d Ki_vec_eigen(Ki_vec.data()); + types::Vector6d Kd_vec_eigen(Kd_vec.data()); + + types::Matrix6d Kp_eigen = Kp_vec_eigen.asDiagonal().toDenseMatrix(); + types::Matrix6d Ki_eigen = Ki_vec_eigen.asDiagonal().toDenseMatrix(); + types::Matrix6d Kd_eigen = Kd_vec_eigen.asDiagonal().toDenseMatrix(); pid_controller_.set_kp(Kp_eigen); pid_controller_.set_ki(Ki_eigen); @@ -175,14 +221,147 @@ void PIDControllerNode::set_pid_params() { } void PIDControllerNode::guidance_callback( - const vortex_msgs::msg::ReferenceFilter::SharedPtr msg) { - eta_d_.pos << msg->x, msg->y, msg->z; + const vortex_msgs::msg::ReferenceFilterQuat::SharedPtr msg) { + eta_d_.x = msg->x; + eta_d_.y = msg->y; + eta_d_.z = msg->z; + + // Enforce positive hemisphere so eta_d_ and eta_ stay in the same half of + // the double cover, reducing sign-flip errors in the PID error computation. + const double sign = msg->qw >= 0.0 ? 1.0 : -1.0; + eta_d_.qw = sign * msg->qw; + eta_d_.qx = sign * msg->qx; + eta_d_.qy = sign * msg->qy; + eta_d_.qz = sign * msg->qz; + + // Desired velocity feedforward (x/y/z in world frame; roll/pitch/yaw are + // the body-frame angular-velocity components from the reference filter). + eta_dot_d_.x = msg->x_dot; + eta_dot_d_.y = msg->y_dot; + eta_dot_d_.z = msg->z_dot; + eta_dot_d_.qx = msg->roll_dot; + eta_dot_d_.qy = msg->pitch_dot; + eta_dot_d_.qz = msg->yaw_dot; +} - double roll = msg->roll; - double pitch = msg->pitch; - double yaw = msg->yaw; +rcl_interfaces::msg::SetParametersResult PIDControllerNode::parametersCallback( + const std::vector& parameters) { + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + result.reason = "success"; + + bool kp_x_updated = false; + bool kp_y_updated = false; + bool kp_z_updated = false; + bool kp_roll_updated = false; + bool kp_pitch_updated = false; + bool kp_yaw_updated = false; + + bool ki_x_updated = false; + bool ki_y_updated = false; + bool ki_z_updated = false; + bool ki_roll_updated = false; + bool ki_pitch_updated = false; + bool ki_yaw_updated = false; + + bool kd_x_updated = false; + bool kd_y_updated = false; + bool kd_z_updated = false; + bool kd_roll_updated = false; + bool kd_pitch_updated = false; + bool kd_yaw_updated = false; + + types::Vector6d Kp_vec_eigen = pid_controller_.get_kp().diagonal(); + types::Vector6d Ki_vec_eigen = pid_controller_.get_ki().diagonal(); + types::Vector6d Kd_vec_eigen = pid_controller_.get_kd().diagonal(); + + for (const auto& param : parameters) { + if (param.get_name() == "Kp_x") { + Kp_vec_eigen(0) = param.as_double(); + kp_x_updated = true; + } else if (param.get_name() == "Kp_y") { + Kp_vec_eigen(1) = param.as_double(); + kp_y_updated = true; + } else if (param.get_name() == "Kp_z") { + Kp_vec_eigen(2) = param.as_double(); + kp_z_updated = true; + } else if (param.get_name() == "Kp_roll") { + Kp_vec_eigen(3) = param.as_double(); + kp_roll_updated = true; + } else if (param.get_name() == "Kp_pitch") { + Kp_vec_eigen(4) = param.as_double(); + kp_pitch_updated = true; + } else if (param.get_name() == "Kp_yaw") { + Kp_vec_eigen(5) = param.as_double(); + kp_yaw_updated = true; + } else if (param.get_name() == "Ki_x") { + Ki_vec_eigen(0) = param.as_double(); + ki_x_updated = true; + } else if (param.get_name() == "Ki_y") { + Ki_vec_eigen(1) = param.as_double(); + ki_y_updated = true; + } else if (param.get_name() == "Ki_z") { + Ki_vec_eigen(2) = param.as_double(); + ki_z_updated = true; + } else if (param.get_name() == "Ki_roll") { + Ki_vec_eigen(3) = param.as_double(); + ki_roll_updated = true; + } else if (param.get_name() == "Ki_pitch") { + Ki_vec_eigen(4) = param.as_double(); + ki_pitch_updated = true; + } else if (param.get_name() == "Ki_yaw") { + Ki_vec_eigen(5) = param.as_double(); + ki_yaw_updated = true; + } else if (param.get_name() == "Kd_x") { + Kd_vec_eigen(0) = param.as_double(); + kd_x_updated = true; + } else if (param.get_name() == "Kd_y") { + Kd_vec_eigen(1) = param.as_double(); + kd_y_updated = true; + } else if (param.get_name() == "Kd_z") { + Kd_vec_eigen(2) = param.as_double(); + kd_z_updated = true; + } else if (param.get_name() == "Kd_roll") { + Kd_vec_eigen(3) = param.as_double(); + kd_roll_updated = true; + } else if (param.get_name() == "Kd_pitch") { + Kd_vec_eigen(4) = param.as_double(); + kd_pitch_updated = true; + } else if (param.get_name() == "Kd_yaw") { + Kd_vec_eigen(5) = param.as_double(); + kd_yaw_updated = true; + } + } - eta_d_.ori = Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX()) * - Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()); + // Only set the gains if the parameter update was successful + if (result.successful) { + if (kp_x_updated || kp_y_updated || kp_z_updated || kp_roll_updated || + kp_pitch_updated || kp_yaw_updated) { + types::Matrix6d Kp_eigen = + Kp_vec_eigen.asDiagonal().toDenseMatrix(); + pid_controller_.set_kp(Kp_eigen); + } + if (ki_x_updated || ki_y_updated || ki_z_updated || ki_roll_updated || + ki_pitch_updated || ki_yaw_updated) { + types::Matrix6d Ki_eigen = + Ki_vec_eigen.asDiagonal().toDenseMatrix(); + pid_controller_.set_ki(Ki_eigen); + } + if (kd_x_updated || kd_y_updated || kd_z_updated || kd_roll_updated || + kd_pitch_updated || kd_yaw_updated) { + types::Matrix6d Kd_eigen = + Kd_vec_eigen.asDiagonal().toDenseMatrix(); + pid_controller_.set_kd(Kd_eigen); + } + } + + // print + for (const auto& param : parameters) { + RCLCPP_INFO(this->get_logger(), "%s", param.get_name().c_str()); + RCLCPP_INFO(this->get_logger(), "%s", param.get_type_name().c_str()); + RCLCPP_INFO(this->get_logger(), "%s", param.value_to_string().c_str()); + } + return result; } + +RCLCPP_COMPONENTS_REGISTER_NODE(PIDControllerNode) diff --git a/control/pid_controller_dp/src/pid_controller_utils.cpp b/control/pid_controller_dp/src/pid_controller_utils.cpp index 246e15500..f48aa7796 100644 --- a/control/pid_controller_dp/src/pid_controller_utils.cpp +++ b/control/pid_controller_dp/src/pid_controller_utils.cpp @@ -1,75 +1,49 @@ #include "pid_controller_dp/pid_controller_utils.hpp" #include +#include +#include #include "pid_controller_dp/pid_controller_conversions.hpp" #include "pid_controller_dp/typedefs.hpp" types::Matrix3d calculate_R_quat(const types::Eta& eta) { - return eta.ori.normalized().toRotationMatrix(); + return eta.as_rotation_matrix(); } -types::Matrix4x3d calculate_T_quat(const types::Eta& eta) { - types::Quaterniond quaternion_norm = eta.ori.normalized(); - - double w = quaternion_norm.w(); - double x = quaternion_norm.x(); - double y = quaternion_norm.y(); - double z = quaternion_norm.z(); - - types::Matrix4x3d transformation_matrix; - - transformation_matrix << -x, -y, -z, w, -z, y, z, w, -x, -y, x, w; - - return transformation_matrix * 0.5; +types::Matrix3d calculate_T_quat(const types::Eta& eta) { + // Full T is 4x3; rows 1-3 map body angular velocity to d/dt[qx, qy, qz]. + return eta.as_transformation_matrix().bottomRows<3>(); } -types::Matrix6x7d calculate_J_sudo_inv(const types::Eta& eta) { - types::Eta eta_norm; - - eta_norm.pos = eta.pos; - eta_norm.ori = eta.ori; - - types::Matrix3d R = calculate_R_quat(eta_norm); - types::Matrix4x3d T = calculate_T_quat(eta_norm); - - types::J_transformation J; - J.R = R; - J.T = T; - - types::Matrix6x7d J_transpose = J.as_matrix().transpose(); - types::Matrix6x7d J_pseudo_inv = - (J_transpose * J.as_matrix()).inverse() * J_transpose; - - return J_pseudo_inv; +types::Matrix6d calculate_J_sudo_inv(const types::Eta& eta) { + types::Matrix3d R = calculate_R_quat(eta); + types::Matrix6d J_inv = types::Matrix6d::Zero(); + J_inv.topLeftCorner<3, 3>() = R.transpose(); + J_inv.bottomRightCorner<3, 3>() = types::Matrix3d::Identity(); + return J_inv; } types::Eta error_eta(const types::Eta& eta, const types::Eta& eta_d) { - types::Eta eta_error; - - eta_error.pos = eta.pos - eta_d.pos; - eta_error.ori = eta_d.ori.conjugate() * eta.ori; - - eta_error.ori = eta_error.ori.normalized(); - - return eta_error; + types::Eta error = eta - eta_d; + // Enforce shortest path: q and -q represent the same rotation, but only + // qw >= 0 gives the correct sign for the vector part used as error signal. + if (error.qw < 0.0) { + error.qw = -error.qw; + error.qx = -error.qx; + error.qy = -error.qy; + error.qz = -error.qz; + } + return error; } Eigen::VectorXd clamp_values(const Eigen::VectorXd& values, double min_val, double max_val) { - return values.cwiseMax(min_val).cwiseMin(max_val); + return vortex::utils::math::clamp_values(values, min_val, max_val); } -types::Vector7d anti_windup(const double dt, - const types::Eta& error, - const types::Vector7d& integral) { - types::Eta error_norm; - - error_norm.pos = error.pos; - error_norm.ori = error.ori; - - types::Vector7d integral_anti_windup = - integral + (error_norm.as_vector() * dt); - - integral_anti_windup = clamp_values(integral_anti_windup, -80.0, 80.0); - return integral_anti_windup; +types::Vector6d anti_windup(const double dt, + const types::Vector6d& error_body, + const types::Vector6d& integral) { + return vortex::utils::math::anti_windup(dt, error_body, integral, -50.0, + 50.0); } diff --git a/control/pid_controller_dp/test/CMakeLists.txt b/control/pid_controller_dp/test/CMakeLists.txt new file mode 100644 index 000000000..d47998914 --- /dev/null +++ b/control/pid_controller_dp/test/CMakeLists.txt @@ -0,0 +1,22 @@ +cmake_minimum_required(VERSION 3.8) + +find_package(GTest REQUIRED) +include(GoogleTest) + +set(TEST_BINARY_NAME ${PROJECT_NAME}_test) +add_executable( + ${TEST_BINARY_NAME} + pid_controller_tests.cpp +) + +target_link_libraries( + ${TEST_BINARY_NAME} + PRIVATE + ${LIB_NAME} + GTest::GTest + spdlog::spdlog +) + +ament_target_dependencies(${TEST_BINARY_NAME} PUBLIC Eigen3 tf2 vortex_utils) + +gtest_discover_tests(${TEST_BINARY_NAME}) diff --git a/control/pid_controller_dp/test/pid_controller_tests.cpp b/control/pid_controller_dp/test/pid_controller_tests.cpp new file mode 100644 index 000000000..18b2a7e6f --- /dev/null +++ b/control/pid_controller_dp/test/pid_controller_tests.cpp @@ -0,0 +1,522 @@ +#include +#include + +#include +#include +#include "pid_controller_dp/pid_controller.hpp" +#include "pid_controller_dp/pid_controller_utils.hpp" +#include "pid_controller_dp/typedefs.hpp" + +void print_tau(const types::Vector6d& tau) { + spdlog::info("Tau values:"); + spdlog::info("Surge: {}", tau[0]); + spdlog::info("Sway: {}", tau[1]); + spdlog::info("Heave: {}", tau[2]); + spdlog::info("Roll: {}", tau[3]); + spdlog::info("Pitch: {}", tau[4]); + spdlog::info("Yaw: {}", tau[5]); +} + +class PIDControllerTests : public ::testing::Test { + protected: + PIDControllerTests() : pid_controller_() { + // Set PID gains for testing + types::Matrix6d Kp = types::Matrix6d::Identity() * 10.0; + types::Matrix6d Ki = types::Matrix6d::Identity() * 0.5; + types::Matrix6d Kd = types::Matrix6d::Identity() * 2.0; + + pid_controller_.set_kp(Kp); + pid_controller_.set_ki(Ki); + pid_controller_.set_kd(Kd); + } + + types::Eta generate_current_pose(const double north_pos, + const double east_pos, + const double down_pos, + const double roll_angle, + const double pitch_angle, + const double yaw_angle) { + types::Eta current_pose; + current_pose.x = north_pos; + current_pose.y = east_pos; + current_pose.z = down_pos; + Eigen::Quaterniond q = vortex::utils::math::euler_to_quat( + roll_angle, pitch_angle, yaw_angle); + current_pose.qw = q.w(); + current_pose.qx = q.x(); + current_pose.qy = q.y(); + current_pose.qz = q.z(); + return current_pose; + } + + types::Eta generate_reference_pose(const double north_pos, + const double east_pos, + const double down_pos, + const double roll_angle, + const double pitch_angle, + const double yaw_angle) { + types::Eta reference_pose; + reference_pose.x = north_pos; + reference_pose.y = east_pos; + reference_pose.z = down_pos; + Eigen::Quaterniond q = vortex::utils::math::euler_to_quat( + roll_angle, pitch_angle, yaw_angle); + reference_pose.qw = q.w(); + reference_pose.qx = q.x(); + reference_pose.qy = q.y(); + reference_pose.qz = q.z(); + return reference_pose; + } + + types::Nu generate_current_velocity(const double surge_vel, + const double sway_vel, + const double heave_vel, + const double roll_rate, + const double pitch_rate, + const double yaw_rate) { + types::Nu current_velocity; + current_velocity.u = surge_vel; + current_velocity.v = sway_vel; + current_velocity.w = heave_vel; + current_velocity.p = roll_rate; + current_velocity.q = pitch_rate; + current_velocity.r = yaw_rate; + return current_velocity; + } + + PIDController pid_controller_; +}; + +/* +Test that negative north error only (in body) gives positive surge command only. +*/ + +TEST_F(PIDControllerTests, + T01_neg_north_error_with_zero_heading_gives_surge_only_command) { + types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Eta eta_d{generate_reference_pose(10.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + + types::Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Vector6d tau{ + pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)}; + print_tau(tau); + + EXPECT_GT(tau[0], 0.0); + EXPECT_NEAR(tau[1], 0.0, 0.01); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that negative north error with positive heading gives a positive surge +command and negative sway command. +*/ + +TEST_F( + PIDControllerTests, + T02_neg_north_error_with_positive_heading_gives_pos_surge_and_neg_sway_command) { + types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 1.5)}; + types::Eta eta_d{generate_reference_pose(10.0, 0.0, 0.0, 0.0, 0.0, 1.5)}; + types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + + types::Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Vector6d tau{ + pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)}; + print_tau(tau); + EXPECT_GT(tau[0], 0.0); + EXPECT_LT(tau[1], 0.0); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that negative north error with negative heading gives a positive surge +command and positive sway command. +*/ + +TEST_F( + PIDControllerTests, + T03_neg_north_error_with_negative_heading_gives_pos_surge_and_pos_sway_command) { + types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, -1.5)}; + types::Eta eta_d{generate_reference_pose(10.0, 0.0, 0.0, 0.0, 0.0, -1.5)}; + types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + + types::Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Vector6d tau{ + pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)}; + print_tau(tau); + + EXPECT_GT(tau[0], 0.0); + EXPECT_GT(tau[1], 0.0); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that negative down error with zero roll and pitch gives a positive heave +command. +*/ + +TEST_F( + PIDControllerTests, + T04_neg_down_error_with_zero_roll_and_pitch_gives_positive_heave_command) { + types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Eta eta_d{generate_reference_pose(0.0, 0.0, 2.0, 0.0, 0.0, 0.0)}; + types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + + types::Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Vector6d tau{ + pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)}; + print_tau(tau); + EXPECT_NEAR(tau[1], 0.0, 0.01); + EXPECT_GT(tau[2], 0.0); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that negative down error with zero roll and negative pitch gives a positive +heave and positive surge command. +*/ + +TEST_F( + PIDControllerTests, + T05_neg_down_error_with_zero_roll_and_neg_pitch_gives_positive_heave_and_positive_surge_command) { + types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, -0.5, 0.0)}; + types::Eta eta_d{generate_reference_pose(0.0, 0.0, 2.0, 0.0, -0.5, 0.0)}; + types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + + types::Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Vector6d tau{ + pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)}; + print_tau(tau); + + EXPECT_GT(tau[0], 0.0); + EXPECT_NEAR(tau[1], 0.0, 0.01); + EXPECT_GT(tau[2], 0.0); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that negative down error with zero roll and positive pitch gives a positive +heave and negative surge command. +*/ + +TEST_F( + PIDControllerTests, + T06_neg_down_error_with_zero_roll_and_pos_pitch_gives_positive_heave_and_negative_surge_command) { + types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.5, 0.0)}; + types::Eta eta_d{generate_reference_pose(0.0, 0.0, 2.0, 0.0, 0.5, 0.0)}; + types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + + types::Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Vector6d tau{ + pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)}; + print_tau(tau); + + EXPECT_LT(tau[0], 0.0); + EXPECT_NEAR(tau[1], 0.0, 0.01); + EXPECT_GT(tau[2], 0.0); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that negative east error with zero heading gives a positive sway command. +*/ + +TEST_F(PIDControllerTests, + T07_neg_east_error_with_zero_heading_gives_positive_sway_command) { + types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Eta eta_d{generate_reference_pose(0.0, 10.0, 0.0, 0.0, 0.0, 0.0)}; + types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + + types::Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Vector6d tau{ + pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)}; + print_tau(tau); + + EXPECT_NEAR(tau[0], 0.0, 0.01); + EXPECT_GT(tau[1], 0.0); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that positive east error with zero heading gives a negative sway command. +*/ + +TEST_F(PIDControllerTests, + T08_pos_east_error_with_zero_heading_gives_pos_sway_command) { + types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Eta eta_d{generate_reference_pose(0.0, -10.0, 0.0, 0.0, 0.0, 0.0)}; + types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + + types::Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Vector6d tau{ + pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)}; + print_tau(tau); + + EXPECT_NEAR(tau[0], 0.0, 0.01); + EXPECT_LT(tau[1], 0.0); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that negative east error with positive heading gives a positive surge and +sway command. +*/ + +TEST_F( + PIDControllerTests, + T09_neg_east_error_with_positive_heading_gives_pos_sway_and_pos_surge_command) { + types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 1.5)}; + types::Eta eta_d{generate_reference_pose(0.0, 10.0, 0.0, 0.0, 0.0, 1.5)}; + types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + + types::Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Vector6d tau{ + pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)}; + EXPECT_GT(tau[0], 0.0); + EXPECT_GT(tau[1], 0.0); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that negative east error with negative heading gives a negative surge and +positive sway command. +*/ + +TEST_F( + PIDControllerTests, + T10_neg_east_error_with_negative_heading_gives_pos_sway_and_neg_surge_command) { + types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, -1.5)}; + types::Eta eta_d{generate_reference_pose(0.0, 10.0, 0.0, 0.0, 0.0, -1.5)}; + types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + + types::Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Vector6d tau{ + pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)}; + print_tau(tau); + + EXPECT_LT(tau[0], 0.0); + EXPECT_GT(tau[1], 0.0); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that negative roll error gives positive roll command. +*/ + +TEST_F(PIDControllerTests, T11_neg_roll_error_gives_positive_roll_command) { + types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Eta eta_d{generate_reference_pose(0.0, 0.0, 0.0, 1.0, 0.0, 0.0)}; + types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + + types::Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Vector6d tau{ + pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)}; + print_tau(tau); + + EXPECT_NEAR(tau[0], 0.0, 0.01); + EXPECT_NEAR(tau[1], 0.0, 0.01); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_GT(tau[3], 0.0); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that positive roll error gives negative roll command. +*/ + +TEST_F(PIDControllerTests, T12_pos_roll_error_gives_neg_roll_command) { + types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Eta eta_d{generate_reference_pose(0.0, 0.0, 0.0, -1.0, 0.0, 0.0)}; + types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + + types::Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Vector6d tau{ + pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)}; + EXPECT_NEAR(tau[0], 0.0, 0.01); + EXPECT_NEAR(tau[1], 0.0, 0.01); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_LT(tau[3], 0.0); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that negative pitch error gives positive pitch command. +*/ + +TEST_F(PIDControllerTests, T13_neg_pitch_error_gives_pos_pitch_command) { + types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Eta eta_d{generate_reference_pose(0.0, 0.0, 0.0, 0.0, 1.0, 0.0)}; + types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + + types::Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Vector6d tau{ + pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)}; + EXPECT_NEAR(tau[0], 0.0, 0.01); + EXPECT_NEAR(tau[1], 0.0, 0.01); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_GT(tau[4], 0.0); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that positive pitch error gives negative pitch command. +*/ + +TEST_F(PIDControllerTests, T14_pos_pitch_error_gives_neg_pitch_command) { + types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Eta eta_d{generate_reference_pose(0.0, 0.0, 0.0, 0.0, -1.0, 0.0)}; + types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + + types::Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Vector6d tau{ + pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)}; + EXPECT_NEAR(tau[0], 0.0, 0.01); + EXPECT_NEAR(tau[1], 0.0, 0.01); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_LT(tau[4], 0.0); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that negative yaw error gives positive yaw command. +*/ + +TEST_F(PIDControllerTests, T15_neg_yaw_error_gives_pos_yaw_command) { + types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Eta eta_d{generate_reference_pose(0.0, 0.0, 0.0, 0.0, 0.0, 1.0)}; + types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + + types::Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Vector6d tau{ + pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)}; + print_tau(tau); + + EXPECT_NEAR(tau[0], 0.0, 0.01); + EXPECT_NEAR(tau[1], 0.0, 0.01); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_GT(tau[5], 0.0); +} + +/* +Test that positive yaw error gives negative yaw command. +*/ + +TEST_F(PIDControllerTests, T16_pos_yaw_error_gives_neg_yaw_command) { + types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Eta eta_d{generate_reference_pose(0.0, 0.0, 0.0, 0.0, 0.0, -1.0)}; + types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + + types::Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Vector6d tau{ + pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)}; + print_tau(tau); + + EXPECT_NEAR(tau[0], 0.0, 0.01); + EXPECT_NEAR(tau[1], 0.0, 0.01); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_LT(tau[5], 0.0); +} + +/* +Test that positive surge velocity only results in negative surge command +(breaking effect). +*/ + +TEST_F(PIDControllerTests, T17_pos_surge_vel_gives_negative_surge_command) { + types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Eta eta_d{generate_reference_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + + types::Nu nu{generate_current_velocity(1.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Vector6d tau{ + pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)}; + EXPECT_LT(tau[0], 0.0); + EXPECT_NEAR(tau[1], 0.0, 0.01); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that positive sway velocity only results in negative sway command (breaking +effect). +*/ + +TEST_F(PIDControllerTests, T18_pos_sway_vel_gives_negative_sway_command) { + types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Eta eta_d{generate_reference_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + + types::Nu nu{generate_current_velocity(0.0, 1.0, 0.0, 0.0, 0.0, 0.0)}; + types::Vector6d tau{ + pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)}; + EXPECT_NEAR(tau[0], 0.0, 0.01); + EXPECT_LT(tau[1], 0.0); + EXPECT_NEAR(tau[2], 0.0, 0.01); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +/* +Test that positive heave velocity only results in negative heave command +(breaking effect). +*/ + +TEST_F(PIDControllerTests, T19_pos_heave_vel_gives_negative_heave_command) { + types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Eta eta_d{generate_reference_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)}; + + types::Nu nu{generate_current_velocity(0.0, 0.0, 1.0, 0.0, 0.0, 0.0)}; + types::Vector6d tau{ + pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)}; + EXPECT_NEAR(tau[0], 0.0, 0.01); + EXPECT_NEAR(tau[1], 0.0, 0.01); + EXPECT_LT(tau[2], 0.0); + EXPECT_NEAR(tau[3], 0.0, 0.01); + EXPECT_NEAR(tau[4], 0.0, 0.01); + EXPECT_NEAR(tau[5], 0.0, 0.01); +} + +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + + return RUN_ALL_TESTS(); +} diff --git a/control/pid_controller_dp_euler/src/pid_controller_ros.cpp b/control/pid_controller_dp_euler/src/pid_controller_ros.cpp index 2b456647f..93a727d11 100644 --- a/control/pid_controller_dp_euler/src/pid_controller_ros.cpp +++ b/control/pid_controller_dp_euler/src/pid_controller_ros.cpp @@ -20,9 +20,9 @@ PIDControllerNode::PIDControllerNode() : Node("pid_controller_euler_node") { } void PIDControllerNode::set_subscribers_and_publisher() { - this->declare_parameter("topics.guidance.dp"); + this->declare_parameter("topics.guidance.dp_rpy"); std::string dp_reference_topic = - this->get_parameter("topics.guidance.dp").as_string(); + this->get_parameter("topics.guidance.dp_rpy").as_string(); this->declare_parameter("topics.pose"); std::string pose_topic = this->get_parameter("topics.pose").as_string(); diff --git a/control/velocity_controller/CMakeLists.txt b/control/velocity_controller/CMakeLists.txt new file mode 100644 index 000000000..d3c5b5426 --- /dev/null +++ b/control/velocity_controller/CMakeLists.txt @@ -0,0 +1,84 @@ +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_components REQUIRED) +find_package(rclcpp_lifecycle 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 +) + +#target_include_directories(velocity_controller_node PUBLIC +# $ +# $ +#) + +set(LIB_NAME velocity_controller_component) +add_library(${LIB_NAME} SHARED + src/velocity_controller.cpp + src/PID_setup.cpp + src/LQR_setup.cpp + src/utilities.cpp + src/ct_instantiations.cpp +) +ament_target_dependencies(${LIB_NAME} + rclcpp + rclcpp_components + rclcpp_lifecycle + lifecycle_msgs + std_msgs + vortex_msgs + geometry_msgs + nav_msgs + vortex_utils +) +#target_include_directories(${LIB_NAME} PRIVATE casadi Eigen3) +target_link_libraries(${LIB_NAME} Eigen3::Eigen casadi::casadi ct_optcon ct_core) + +rclcpp_components_register_node(${LIB_NAME} + PLUGIN "Velocity_node" + EXECUTABLE velocity_controller_node +) + +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() diff --git a/control/velocity_controller/README.md b/control/velocity_controller/README.md new file mode 100644 index 000000000..25556cb17 --- /dev/null +++ b/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 automaticly 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 + +| 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 | Size | 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 +``` + +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. Remeber to intialize correctly, either in 'on_configure' or in constructor, add the appropriate parameters, and update alle 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. \ No newline at end of file diff --git a/control/velocity_controller/config/nautilus_params.yaml b/control/velocity_controller/config/nautilus_params.yaml new file mode 100644 index 000000000..becd032bb --- /dev/null +++ b/control/velocity_controller/config/nautilus_params.yaml @@ -0,0 +1,24 @@ +/**: + ros__parameters: + + PID_params: + surge: [300.0,10.0,5.0] + pitch: [60.0,8.0,12.0] + yaw: [10.0,1.0,5.0] + + LQR_params: + Q: [200.0,32.84,32.84,15.0,15.0,100.0,32.84,32.84] + R: [0.02,3.1,3.10] + dampening_matrix_low: [23.0,0.0,0.0,0.0,0.0,0.0, 0.0,46.0,0.0,0.0,0.0,0.0, 0.0,0.0,46.0,0.0,0.0,0.0, 0.0,0.0,0.0,46.0,0.0,0.0, 0.0,0.0,0.0,0.0,46.0,0.0, 0.0,0.0,0.0,0.0,0.0,46.0] + dampening_matrix_high: [1.0,0.0,0.0,0.0,0.0,0.0, 0.0,1.0,0.0,0.0,0.0,0.0, 0.0,0.0,1.0,0.0,0.0,0.0, 0.0,0.0,0.0,1.0,0.0,0.0, 0.0,0.0,0.0,0.0,1.0,0.0, 0.0,0.0,0.0,0.0,0.0,1.0] + + + Settings: #Settings for the controller + controller_type: 2 #1 PID 2 LQR + auto_start: false #0 for no, 1 for yes + anti_overshoot: false + reset_on_new_ref: true + odometry_dropout_guard: true + publish_rate: 100 #ms + max_force: 99.5 + diff --git a/control/velocity_controller/config/orca_params.yaml b/control/velocity_controller/config/orca_params.yaml new file mode 100644 index 000000000..9a111d51a --- /dev/null +++ b/control/velocity_controller/config/orca_params.yaml @@ -0,0 +1,27 @@ +/**: + ros__parameters: + + PID_params: + surge: [300.0,10.0,5.0] + pitch: [60.0,8.0,12.0] + yaw: [10.0,1.0,5.0] + + LQR_params: + Q: [200.0,32.84,32.84,15.0,15.0,100.0,32.84,32.84] + R: [0.02,3.1,3.10] + dampening_matrix_low: [23.0,0.0,0.0,0.0,0.0,0.0, 0.0,46.0,0.0,0.0,0.0,0.0, 0.0,0.0,46.0,0.0,0.0,0.0, 0.0,0.0,0.0,46.0,0.0,0.0, 0.0,0.0,0.0,0.0,46.0,0.0, 0.0,0.0,0.0,0.0,0.0,46.0] + dampening_matrix_high: [1.0,0.0,0.0,0.0,0.0,0.0, 0.0,1.0,0.0,0.0,0.0,0.0, 0.0,0.0,1.0,0.0,0.0,0.0, 0.0,0.0,0.0,1.0,0.0,0.0, 0.0,0.0,0.0,0.0,1.0,0.0, 0.0,0.0,0.0,0.0,0.0,1.0] + + + Settings: #Settings for the controller + controller_type: 2 #1 PID 2 LQR + auto_start: false #0 for no, 1 for yes + anti_overshoot: false + reset_on_new_ref: true + odometry_dropout_guard: true + publish_rate: 100 #ms + max_force: 99.5 + + + + diff --git a/control/velocity_controller/include/velocity_controller/LQR_setup.hpp b/control/velocity_controller/include/velocity_controller/LQR_setup.hpp new file mode 100644 index 000000000..2b4adfadd --- /dev/null +++ b/control/velocity_controller/include/velocity_controller/LQR_setup.hpp @@ -0,0 +1,47 @@ +#pragma once + +#include +#include +#include +#include +#include "velocity_controller/utilities.hpp" + +class LQRController{ + + public: + LQRController(); + bool set_matrices(std::vector Q_,std::vector R_,std::vector inertia_matrix, double max_force,std::vector D_low); + void reset_controller(int nr=0); + bool calculate_thrust(State states, Guidance_data guidance_values); + bool set_interval(double interval); + Eigen::Vector get_thrust(); + private: + Eigen::Matrix linearize(State states); + Eigen::Matrix coriolis(const State& s); + + + std::tuple saturate (double value, bool windup, double limit); + double anti_windup(double error, double integral_sum, bool windup); + Eigen::Vector saturate_input(Eigen::Vector u); + + Eigen::Vector update_error(const Guidance_data& error, const State& state); + + double interval_; + double integral_error_surge; double integral_error_pitch; double integral_error_yaw; + bool surge_windup; bool pitch_windup; bool yaw_windup; + Eigen::Matrix Q; Eigen::Matrix R; Eigen::Matrix B; + Eigen::Matrix D; + double max_force, mass, Ixx, Iyy,Izz; + + Eigen::Matrix inertia_matrix_inv; + Eigen::Matrix state_weight_matrix; + Eigen::Matrix3d input_weight_matrix; + Eigen::Matrix augmented_system_matrix; + Eigen::Matrix augmented_input_matrix; + Eigen::Vector u; + + + + +}; + diff --git a/control/velocity_controller/include/velocity_controller/PID_setup.hpp b/control/velocity_controller/include/velocity_controller/PID_setup.hpp new file mode 100644 index 000000000..8168c90a0 --- /dev/null +++ b/control/velocity_controller/include/velocity_controller/PID_setup.hpp @@ -0,0 +1,28 @@ +#pragma once + +#include +#include +#include +#include + +class PID_controller { + public: + //PID_controller(double Kp=0, double Ki=0, double Kd=0, double max_output=0, double min_output=0, double dt=0); + //PID_controller(double k_p, double k_i, double k_d) : PID_controller(k_p, k_i, k_d, 100.0, -100.0) {}; + bool calculate_thrust(double error); + bool calculate_thrust(double error, double error_d); + void reset_controller(); + double get_output(); + bool set_output_limits(double min_output, double max_output); + bool set_parameters(double k_p,double k_i, double k_d, double dt); + bool set_parameters(std::vector& params,double dt); + bool set_dt(double dt); + private: + double Kp_, Ki_, Kd_, dt_; + double integral=0; + double previous_error=0; + double output=0; + double max_output_, min_output_; + bool init=false; +}; + diff --git a/control/velocity_controller/include/velocity_controller/utilities.hpp b/control/velocity_controller/include/velocity_controller/utilities.hpp new file mode 100644 index 000000000..70512f5cf --- /dev/null +++ b/control/velocity_controller/include/velocity_controller/utilities.hpp @@ -0,0 +1,43 @@ +#pragma once +#include +#include +#include +#include "std_msgs/msg/float64_multi_array.hpp" +#include "vortex_msgs/msg/los_guidance.hpp" +#include +#include + + +struct angle{ + double phit=0.0; + double thetat=0.0; + double psit=0.0; +}; +angle quaternion_to_euler_angle(double w, double x, double y, double z); +geometry_msgs::msg::Quaternion euler_angle_to_quaternion(double roll, double pitch, double yaw); + +class State{ + public: + double surge=0.0, sway=0.0, heave=0.0, roll_rate=0.0, pitch_rate=0.0, yaw_rate=0.0; //roll_rate=0.0, pitch_rate=0.0, yaw_rate=0.0; + double roll=0.0, pitch=0.0, yaw=0.0; //phi, theta, psi + double w=0.0, x=0.0,y=0.0,z=0.0; + State(double surge=0,double pitch=0, double yaw=0):surge{surge}, pitch{pitch},yaw{yaw}{}; + + State operator=(int n){if (n){surge=0.0,sway=0.0,heave=0.0,roll_rate=0.0,pitch_rate=0.0,yaw_rate=0.0,roll=0.0,pitch=0.0,yaw=0.0;} return *this;}; + State operator=(nav_msgs::msg::Odometry::SharedPtr rhs); + angle get_angle(); +}; +class Guidance_data{ + public: + double surge=0.0; double pitch=0.0; double yaw=0.0; + Guidance_data(double surge, double pitch, double yaw):surge{surge},pitch{pitch}, yaw{yaw} {}; + Guidance_data():surge{0.0}, pitch{0.0}, yaw{0.0} {}; + Guidance_data& operator=(const vortex_msgs::msg::LOSGuidance::SharedPtr& msg); +}; + +Eigen::Vector3d body_rates_to_euler_rates(double roll, double pitch,double p, double q, double r); +angle angle_NED_to_body( double roll_des, double pitch_des, double yaw_des, double roll, double pitch, double yaw); +angle angle_NED_to_body(angle desired, angle state); +inline angle angle_NED_to_body(angle desired, angle state){return angle_NED_to_body(desired.phit, desired.thetat, desired.psit, state.phit, state.thetat, state.psit);} + + diff --git a/control/velocity_controller/include/velocity_controller/velocity_controller.hpp b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp new file mode 100644 index 000000000..58a783b89 --- /dev/null +++ b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp @@ -0,0 +1,109 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include "velocity_controller/PID_setup.hpp" +#include "LQR_setup.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "vortex_msgs/msg/los_guidance.hpp" +#include +#include + + + +class Velocity_node : public rclcpp_lifecycle::LifecycleNode{ + public: + explicit Velocity_node(const rclcpp::NodeOptions& options); + Velocity_node(const Velocity_node&)=delete; //no copy constructor + Velocity_node& operator=(const Velocity_node&) = delete; //no copy assignment + //TODO: decide if i one should be allowed to move or transfer ownership if the class + //Different initializatin functions + void get_new_parameters(); + + //Timer functions + void calc_thrust(); + + //Callback functions + void guidance_callback(const vortex_msgs::msg::LOSGuidance::SharedPtr msg_ptr); + void odometry_callback(const nav_msgs::msg::Odometry::SharedPtr msg_ptr); + + //Publisher instance + rclcpp::Publisher::SharedPtr publisher_thrust; + + //Timer instance + rclcpp::TimerBase::SharedPtr timer_calculation; + rclcpp::TimerBase::SharedPtr startup_timer_; + //Subscriber instance + rclcpp::Subscription::SharedPtr subscriber_Odometry; + rclcpp::Subscription::SharedPtr subscriber_guidance; + //rclcpp::Subscription::SharedPtr subscriber_killswitch; + + //Variables for topics + + std::string topic_thrust; + std::string topic_guidance; + std::string topic_killswitch; + std::string topic_odometry; + + //Variables for timers + int publish_rate; + double max_force; + + //Stored wrenches values + vortex_msgs::msg::LOSGuidance reference_in; + Guidance_data guidance_values; + State current_state; + geometry_msgs::msg::WrenchStamped thrust_out; + + + int controller_type; //1 PID, 2 LQR + + //PID controllers + PID_controller PID_surge; + std::vector surge_params; + PID_controller PID_yaw; + std::vector yaw_params; + PID_controller PID_pitch; + std::vector pitch_params; + + + //LQR Controller + LQRController lqr_controller; + //LQRparameters lqr_parameters; + std::vector Q; + std::vector R; + //std::vector Qi; + //std::vector Ri; + std::vector inertia_matrix; + std::vector dampening_matrix_low; + std::vector dampening_matrix_high; + + std::atomic_bool should_exit_{false}; + //VC settings + bool reset_on_new_ref; + bool anti_overshoot; + bool auto_start; + bool odometry_dropout_guard; + int publish_counter=0; + bool first_start=true; + + //States + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure(const rclcpp_lifecycle::State &) override; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_cleanup(const rclcpp_lifecycle::State &) override; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; + + void reset_controllers(int nr=0); + rclcpp::QoS pub_QoS; + rclcpp::QoS sub_QoS; + + +}; + + diff --git a/control/velocity_controller/launch/VCnTest.launch.py b/control/velocity_controller/launch/VCnTest.launch.py new file mode 100644 index 000000000..44ff3a8a4 --- /dev/null +++ b/control/velocity_controller/launch/VCnTest.launch.py @@ -0,0 +1,74 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +import os +from launch.actions import IncludeLaunchDescription, TimerAction +from launch.actions import IncludeLaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.launch_description_sources import PythonLaunchDescriptionSource +from ament_index_python.packages import get_package_share_directory +from launch.substitutions import LaunchConfiguration +from auv_setup.launch_arg_common import ( + declare_drone_and_namespace_args, + resolve_drone_and_namespace, +) +from launch.actions import OpaqueFunction + +def launch_setup(context,*args,**kwargs): + drone, namespace=resolve_drone_and_namespace(context) + global_share = get_package_share_directory('auv_setup') + config_path_global = os.path.join(global_share,'config','robots',f'{drone}.yaml') + common_launch_args = { + "drone": drone, + "namespace": namespace, + }.items() + + stonefish_dir = get_package_share_directory('stonefish_sim') + + stonefish_sim = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(stonefish_dir, 'launch', 'simulation.launch.py') + ), + launch_arguments={'rendering_quality': 'low','rendering':'true'}.items(), + ) + orca_sim = TimerAction( + period=12.0, + actions=[ + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(stonefish_dir, 'launch', 'drone_sim.launch.py') + ) + ) + ] + ) + operation_mode_manager_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join( + get_package_share_directory("operation_mode_manager"), + "launch", + "operation_mode_manager.launch.py", + ) + ), + launch_arguments=common_launch_args, + ) + + node_name_arg = DeclareLaunchArgument( + 'node_name_1', default_value='test_VC_node', + description='Name of the test VC node' + ) + test_VC_name = LaunchConfiguration('node_name_1') + + return [ + stonefish_sim, + orca_sim, + node_name_arg, + #operation_mode_manager_launch, + Node(package='velocity_controller', + executable='test_VC_node', + name=test_VC_name, + namespace=namespace, + output='screen', + parameters=[config_path_global]) + ] + +def generate_launch_description(): + return LaunchDescription(declare_drone_and_namespace_args()+[OpaqueFunction(function=launch_setup)]) \ No newline at end of file diff --git a/control/velocity_controller/launch/velocity_controller.launch.py b/control/velocity_controller/launch/velocity_controller.launch.py new file mode 100644 index 000000000..00ad9cb81 --- /dev/null +++ b/control/velocity_controller/launch/velocity_controller.launch.py @@ -0,0 +1,33 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +import os +#from launch.actions import IncludeLaunchDescription +from launch.actions import DeclareLaunchArgument +#from launch.launch_description_sources import PythonLaunchDescriptionSource +from ament_index_python.packages import get_package_share_directory +from launch.substitutions import LaunchConfiguration +from auv_setup.launch_arg_common import ( + declare_drone_and_namespace_args, + resolve_drone_and_namespace, +) +from launch.actions import OpaqueFunction + + +def launch_setup(context,*args,**kwargs): + drone, namespace=resolve_drone_and_namespace(context) + + pkg_share = get_package_share_directory('velocity_controller') + global_share = get_package_share_directory('auv_setup') + config_path_local = os.path.join(pkg_share, 'config', f'{drone}_params.yaml') + config_path_global = os.path.join(global_share,'config','robots',f"{drone}.yaml") + + return [ + Node(package='velocity_controller', + executable='velocity_controller_node', + name="velocity_controller_node", + namespace=namespace, + output='screen', + parameters=[config_path_local,config_path_global]) + ] +def generate_launch_description(): + return LaunchDescription(declare_drone_and_namespace_args()+[OpaqueFunction(function=launch_setup)]) \ No newline at end of file diff --git a/control/velocity_controller/package.xml b/control/velocity_controller/package.xml new file mode 100644 index 000000000..dd2cf0912 --- /dev/null +++ b/control/velocity_controller/package.xml @@ -0,0 +1,32 @@ + + + + velocity_controller + 0.0.0 + TODO: Package description + henrik + TODO: License declaration + + ament_cmake + + rclcpp + std_msgs + vortex_msgs + geometry_msgs + nav_msgs + + + vortex_utils + rclcpp_components + rclcpp_lifecycle + lifecycle_msgs + auv_setup + + ament_cmake_gtest + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/control/velocity_controller/src/LQR_setup.cpp b/control/velocity_controller/src/LQR_setup.cpp new file mode 100644 index 000000000..f6314cac6 --- /dev/null +++ b/control/velocity_controller/src/LQR_setup.cpp @@ -0,0 +1,195 @@ +#include "velocity_controller/LQR_setup.hpp" +#include "rclcpp/rclcpp.hpp" +#include +#include +#include +#include +#include +#include +//#include +//#include +//#include +//#include +#include "velocity_controller/PID_setup.hpp" +#include "velocity_controller/utilities.hpp" +#include +//#include +#include "vortex/utils/math.hpp" +#include "ct/optcon/lqr/LQR.hpp" + + +LQRController::LQRController(){ + Q.setZero(); + R.setZero(); + B.setZero(); + D.setZero(); + inertia_matrix_inv.setZero(); +}; +bool LQRController::set_matrices(std::vector Q_,std::vector R_,std::vector inertia_matrix_,double max_force_, std::vector D_low){ + if (Q_.size()!=8){ + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),"The Q matrix has the wrong amount of elements"); + return 0; + } + if(R_.size()!=3){ + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),"The R matrix has the wrong amount of elements"); + return 0; + } + if(inertia_matrix_.size()!=36){ + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),"The M matrix has the wrong amount of elements"); + return 0; + } + if (max_force_<0){ + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),"The max_force need to be >0"); + return 0; + } + max_force=max_force_; + Q.diagonal() = Eigen::Map(Q_.data(), Q_.size()); + R.diagonal() = Eigen::Map(R_.data(), R_.size()); + Ixx=inertia_matrix_.at(6*3+3); Iyy=inertia_matrix_.at(4*6+4); Izz=inertia_matrix_.at(5*6+5); mass=inertia_matrix_.at(0); + + Eigen::Matrix inertia_matrix = Eigen::Map>(inertia_matrix_.data(),6,6); + D=Eigen::Map>(D_low.data(),6,6); + inertia_matrix_inv=inertia_matrix.inverse(); + + Eigen::MatrixB_t=inertia_matrix_inv*(Eigen::Matrix()<<1,0,0, 0,0,0, 0,0,0, 0,0,0, 0,1,0, 0,0,1).finished(); + Eigen::Matrix B_m=Eigen::Matrix::Zero(); + B_m.block<6,3>(0,0)=B_t; + std::vector> swaplines{{1,7},{2,8},{3,4},{4,5}}; + for (long unsigned int i=0;i(0,0)=B_m.block<5,3>(0,0); + reset_controller(); + return 1; +} + +std::tuple LQRController::saturate (double value, bool windup, double limit){ + if (abs(value) > limit){ + windup=true; + value = limit * (value/abs(value)); + } + else { + windup=false; + } + return {windup,value}; +} + + + +double LQRController::anti_windup(double error, double integral_sum, bool windup){ + if (!windup){ + integral_sum += error*interval_; + } + return integral_sum; +} + + + +Eigen::Matrix LQRController::linearize(State s){ + Eigen::Matrix D_=Eigen::Matrix::Zero(); + + D_=-inertia_matrix_inv*D; //Assuming linear dampening for now + + Eigen::Matrix C=coriolis(s); + + D_-=inertia_matrix_inv*C; //To avoid unneccessary allocation + + Eigen::Matrix T=Eigen::Matrix::Identity(); + Eigen::Matrix A; + A.block<6,6>(0,0)=D_; + A.block<3,3>(0,6)=A.block<3,3>(6,0)=A.block<3,3>(6,6)=Eigen::Matrix3d::Zero(); + A.block<3,3>(6,3)=T; + std::vector> swaplines{{1,7},{2,8},{3,4},{4,5}}; + for (long unsigned int i=0;i ret; + ret.setZero(); + ret.block<5,5>(0,0)=A.block<5,5>(0,0); + ret.block<3,3>(5,0)=Eigen::Matrix3d::Identity(); + + return ret; +}; +Eigen::Vector LQRController::update_error(const Guidance_data& error, const State& state){ + double surge_error = error.surge; + double pitch_error = error.pitch; + double yaw_error = error.yaw; + + integral_error_surge = anti_windup(surge_error, integral_error_surge, surge_windup); + integral_error_pitch = anti_windup(pitch_error, integral_error_pitch, pitch_windup); + integral_error_yaw = anti_windup(yaw_error, integral_error_yaw, yaw_windup); + + Eigen::Vector state_error= {surge_error, pitch_error, yaw_error, -state.pitch_rate, -state.yaw_rate, integral_error_surge, integral_error_pitch, integral_error_yaw}; + return state_error; +} +Eigen::Vector LQRController::saturate_input(Eigen::Vector u){ + double force_x, torque_y, torque_z; + std::tie(surge_windup, force_x) = saturate(u[0], surge_windup, max_force); + std::tie(pitch_windup, torque_y) = saturate(u[1], pitch_windup, max_force); + std::tie(yaw_windup, torque_z) = saturate(u[2], yaw_windup, max_force); + return {force_x, torque_y, torque_z}; +} +bool LQRController::calculate_thrust(State state, Guidance_data error){ + ct::optcon::LQR<8,3> lqr; + Eigen::Matrix K_l; + bool INFO= lqr.compute(Q,R,linearize(state),B,K_l,true,false); + if(INFO==0)return false; + Eigen::Matrix state_error = update_error(error, state); + u=saturate_input( (K_l*state_error)); + return true; +} +void LQRController::reset_controller(int nr){ + if(nr==0||nr==1){ + integral_error_surge=0.0; + surge_windup=false; + } + if(nr==0||nr==2){ + integral_error_pitch=0.0; + pitch_windup=false; + } + if(nr==0||nr==3){ + integral_error_yaw=0.0; + yaw_windup=false; + } + + return; +} +bool LQRController::set_interval(double interval){ + if(interval<=0)return false; + interval_=interval; + return true; +} + +Eigen::Vector LQRController::get_thrust(){ + return u; +} + +//TODO: double check the matrices here +Eigen::Matrix LQRController::coriolis(const State& s){ + double u = s.surge; + double v = s.sway; + double w = s.heave; + double p = s.roll_rate; + double q = s.pitch_rate; + double r = s.yaw_rate; + Eigen::Matrix C = Eigen::Matrix::Zero(); + + // Top-right block (translational-rotational coupling) + C(0,4) = mass * w; C(0,5) = -mass * v; + C(1,3) = -mass * w; C(1,5) = mass * u; + C(2,3) = mass * v; C(2,4) = -mass * u; + + // Bottom-left block (rotational-translational coupling) + C(3,1) = mass * w; C(3,2) = -mass * v; + C(4,0) = -mass* w; C(4,2) = mass * u; + C(5,0) = mass * v; C(5,1) = -mass * u; + + // Bottom-right block (rotational-rotational coupling) + C(3,4) = Izz * r; C(3,5) = -Iyy * q; + C(4,3) = -Izz * r; C(4,5) = Ixx * p; + C(5,3) = Iyy * q; C(5,4) = -Ixx * p; + + return C; +} \ No newline at end of file diff --git a/control/velocity_controller/src/PID_setup.cpp b/control/velocity_controller/src/PID_setup.cpp new file mode 100644 index 000000000..3b8102214 --- /dev/null +++ b/control/velocity_controller/src/PID_setup.cpp @@ -0,0 +1,86 @@ +#include "velocity_controller/PID_setup.hpp" + +/* +PID_controller::PID_controller( double Kp, double Ki, double Kd, double max_output, double min_output, double dt):Kp_(Kp), Ki_(Ki), Kd_(Kd), max_output_(max_output), min_output_(min_output), dt_(dt) { + integral = 0.0; + previous_error = 0.0; +};*/ +//TODO: kanskje forbedre integrasjon og derivasjons beregningene +//TODO: check for more errors, f.example Nan or very high intergral +bool PID_controller::calculate_thrust(double error){ + if(!init)return false; + //Saturation + if (output>max_output_){ + output = max_output_; + } + else if (output < min_output_){ + output = min_output_; + } + else{ + integral+=error*dt_; //anti-wind up + } + //P + I + D + output=Kp_*error+Ki_*integral + Kd_ * (error - previous_error) / dt_; + previous_error = error; + + return true; +}; +bool PID_controller::calculate_thrust(double error, double error_d){ + if(!init)return false; + //Saturation + if (output>max_output_){ + output = max_output_; + } + else if (output < min_output_){ + output = min_output_; + } + else{ + integral+=error*dt_; //anti-wind up + } + //P + I + D + output=Kp_*error+Ki_*integral + Kd_ * error_d; + previous_error = error; + + return true; +} +void PID_controller::reset_controller(){ + integral = 0.0; + previous_error = 0.0; + output=0.0; +} + +double PID_controller::get_output(){ + return output; +}; + +bool PID_controller::set_output_limits(double min_output, double max_output){ + if (max_output& params,double dt){ + return set_parameters(params.at(0),params.at(1),params.at(2), dt); + +}; + diff --git a/control/velocity_controller/src/ct_instantiations.cpp b/control/velocity_controller/src/ct_instantiations.cpp new file mode 100644 index 000000000..124a25e99 --- /dev/null +++ b/control/velocity_controller/src/ct_instantiations.cpp @@ -0,0 +1,16 @@ +// src/ct_instantiations.cpp +// This file exists ONLY to emit Control Toolbox symbols + +//#include +#include +//#define CT_OPTCON_ENABLE_LQR +//#define CT_CORE_ENABLE_CARE + +//#include +//#include + +//#include +//#include + +template class ct::optcon::LQR<8, 3>; +template class ct::optcon::CARE<8, 3>; \ No newline at end of file diff --git a/control/velocity_controller/src/utilities.cpp b/control/velocity_controller/src/utilities.cpp new file mode 100644 index 000000000..69242e7e0 --- /dev/null +++ b/control/velocity_controller/src/utilities.cpp @@ -0,0 +1,143 @@ +#include "velocity_controller/utilities.hpp" +#include "Eigen/Dense" +#include +#include +#include +#include + +angle quaternion_to_euler_angle(double w, double x, double y, double z){ + double ysqr = y * y; + + double t0 = +2.0 * (w * x + y * z); + double t1 = +1.0 - 2.0 * (x * x + ysqr); + double phi = std::atan2(t0, t1); + + double t2 = +2.0 * (w * y - z * x); + t2 = t2 > 1.0 ? 1.0 : t2; + t2 = t2 < -1.0 ? -1.0 : t2; + double theta = std::asin(t2); + + double t3 = +2.0 * (w * z + x * y); + double t4 = +1.0 - 2.0 * (ysqr + z * z); + double psi = std::atan2(t3, t4); + + return {phi, theta, psi}; +}; +/* +angle NED_to_BODY(const angle &a,const State &s){ + //TODO tests for illegal angles maybe + Eigen::Vector3d q; + q<pose.pose.orientation.w; + x=rhs->pose.pose.orientation.x; + y=rhs->pose.pose.orientation.y; + z=rhs->pose.pose.orientation.z; + + auto [r,p,y_]=quaternion_to_euler_angle(w, x, y, z); + roll=r; + pitch=p; + yaw=y_; + + //angular velocity + roll_rate=rhs->twist.twist.angular.x; + pitch_rate=rhs->twist.twist.angular.y; + yaw_rate=rhs->twist.twist.angular.z; + //velocity + surge = rhs->twist.twist.linear.x; + sway = rhs->twist.twist.linear.y; + heave = rhs->twist.twist.linear.z; + + return (*this); + +} + +geometry_msgs::msg::Quaternion euler_angle_to_quaternion(double roll, double pitch, double yaw){ + double cy = cos(yaw * 0.5); + double sy = sin(yaw * 0.5); + double cp = cos(pitch * 0.5); + double sp = sin(pitch * 0.5); + double cr = cos(roll * 0.5); + double sr = sin(roll * 0.5); + + geometry_msgs::msg::Quaternion q; + q.w = cr * cp * cy + sr * sp * sy; + q.x = sr * cp * cy - cr * sp * sy; + q.y = cr * sp * cy + sr * cp * sy; + q.z = cr * cp * sy - sr * sp * cy; + + return q; +} + +angle angle_NED_to_body( double roll_des, double pitch_des, double yaw_des,double roll, double pitch, double yaw) +{ + double cr = std::cos(roll), sr = std::sin(roll); + double cp = std::cos(pitch), sp = std::sin(pitch); + double cy = std::cos(yaw), sy = std::sin(yaw); + + // R_current: NED to body for current attitude + Eigen::Matrix3d R_current; + R_current << cp*cy, cp*sy, -sp, + sr*sp*cy - cr*sy, sr*sp*sy + cr*cy, sr*cp, + cr*sp*cy + sr*sy, cr*sp*sy - sr*cy, cr*cp; + + double cr_d = std::cos(roll_des), sr_d = std::sin(roll_des); + double cp_d = std::cos(pitch_des), sp_d = std::sin(pitch_des); + double cy_d = std::cos(yaw_des), sy_d = std::sin(yaw_des); + + // R_desired: NED to body for desired attitude + Eigen::Matrix3d R_desired; + R_desired << cp_d*cy_d, cp_d*sy_d, -sp_d, + sr_d*sp_d*cy_d - cr_d*sy_d, sr_d*sp_d*sy_d + cr_d*cy_d, sr_d*cp_d, + cr_d*sp_d*cy_d + sr_d*sy_d, cr_d*sp_d*sy_d - sr_d*cy_d, cr_d*cp_d; + + // Error rotation matrix: how much to rotate in body to reach desired + // R_error = R_desired * R_current^T + Eigen::Matrix3d R_error = R_desired * R_current.transpose(); + + // Extract euler angles from R_error — this gives the error in body frame + double pitch_err = std::asin(-R_error(2, 0)); + double roll_err = std::atan2(R_error(2, 1), R_error(2, 2)); + double yaw_err = std::atan2(R_error(1, 0), R_error(0, 0)); + + return {roll_err, pitch_err, yaw_err}; +} + +angle State::get_angle(){ + return {roll,pitch,yaw}; +} +Guidance_data& Guidance_data::operator=(const vortex_msgs::msg::LOSGuidance::SharedPtr& msg){ + surge=msg->surge; + pitch=msg->pitch; + yaw=msg->yaw; + return *this; +} diff --git a/control/velocity_controller/src/velocity_controller.cpp b/control/velocity_controller/src/velocity_controller.cpp new file mode 100644 index 000000000..de3903618 --- /dev/null +++ b/control/velocity_controller/src/velocity_controller.cpp @@ -0,0 +1,264 @@ +#include "velocity_controller/velocity_controller.hpp" +#include +#include "velocity_controller/PID_setup.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "vortex/utils/math.hpp" +#include "velocity_controller/utilities.hpp" +#include + + + +Velocity_node::Velocity_node(const rclcpp::NodeOptions& options) : rclcpp_lifecycle::LifecycleNode("velocity_controller_lifecycle", options), lqr_controller(), pub_QoS(10), sub_QoS(10) +{ + get_new_parameters(); + pub_QoS.keep_last(10).reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT).durability(RMW_QOS_POLICY_DURABILITY_VOLATILE); + sub_QoS.keep_last(10).reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT).durability(RMW_QOS_POLICY_DURABILITY_VOLATILE); + + //PID initialization + PID_surge.set_output_limits(-max_force, max_force); + PID_pitch.set_output_limits(-max_force, max_force); + PID_yaw.set_output_limits(-max_force, max_force); + PID_surge.set_parameters(surge_params,publish_rate/1000.0); + PID_pitch.set_parameters(pitch_params,publish_rate/1000.0); + PID_yaw.set_parameters(yaw_params,publish_rate/1000.0); + + //LQR + if(!lqr_controller.set_matrices(Q,R,inertia_matrix,max_force,dampening_matrix_low)||!lqr_controller.set_interval(static_cast(publish_rate)/1000)){ + controller_type=1; + RCLCPP_INFO(this->get_logger(),"Switching to PID"); + }; + //Automaticly start in activate if auto_start is true + if(auto_start){ + startup_timer_=create_wall_timer(std::chrono::milliseconds(0), [this](){startup_timer_->cancel(); trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE);}); + } + RCLCPP_INFO(this->get_logger(), "Velocity control node has been started."); + + return; +} + + +void Velocity_node::calc_thrust() +{ + if (odometry_dropout_guard){ + publish_counter++; + if(publish_counter>=100){ + reset_controllers(); + RCLCPP_WARN(this->get_logger(),"Odometry dropout, no thrust"); + return; + } + } + //TODO: Do I need ssa here? + angle ref_in_body=angle_NED_to_body({0,vortex::utils::math::ssa(guidance_values.pitch),vortex::utils::math::ssa(guidance_values.yaw)},current_state.get_angle()); + Guidance_data error={guidance_values.surge-current_state.surge,-ref_in_body.thetat,-ref_in_body.psit}; + + if(anti_overshoot){ + if (abs(error.yaw)get_logger(),"Switching to PID"); + } + else{ + Eigen::Vector3d u=lqr_controller.get_thrust(); + thrust_out.wrench.force.x=u[0]; + thrust_out.wrench.torque.y=u[1]; + thrust_out.wrench.torque.z=u[2]; + } + break; + } + default:{ + RCLCPP_ERROR(this->get_logger(),"Unknown controller set, switching to PID"); + controller_type=1; + calc_thrust(); + return; + } + } + publisher_thrust->publish(thrust_out); + return; +} + + + +//Callback functions +void Velocity_node::guidance_callback(const vortex_msgs::msg::LOSGuidance::SharedPtr msg_ptr){ + if(reset_on_new_ref){ //On big step changes, reset the controllers to avoid big overshoots + if(abs(msg_ptr->surge-guidance_values.surge)>=0.1) reset_controllers(1); + if (abs(msg_ptr->pitch-guidance_values.pitch)>std::numbers::pi/4)reset_controllers(2); + if (abs(msg_ptr->yaw-guidance_values.yaw)declare_parameter("topics.wrench_input"); + topic_thrust = this->get_parameter("topics.wrench_input").as_string(); + this->declare_parameter("topics.guidance.los"); + topic_guidance = this->get_parameter("topics.guidance.los").as_string(); + this->declare_parameter("topics.odom"); + topic_odometry = this->get_parameter("topics.odom").as_string(); + + //Settings + this->declare_parameter("Settings.max_force"); + max_force = this->get_parameter("Settings.max_force").as_double(); + this->declare_parameter("Settings.publish_rate"); + publish_rate = this->get_parameter("Settings.publish_rate").as_int(); + this->declare_parameter("Settings.controller_type"); + controller_type=this->get_parameter("Settings.controller_type").as_int(); + this->declare_parameters("Settings", {{"auto_start", false}, {"reset_on_new_ref", true}, {"anti_overshoot", true},{"odometry_dropout_guard", true}}); + auto settings = this->get_parameters({"Settings.auto_start", "Settings.reset_on_new_ref", "Settings.anti_overshoot", "Settings.odometry_dropout_guard"}); + auto_start = settings[0].as_bool(); + reset_on_new_ref = settings[1].as_bool(); + anti_overshoot = settings[2].as_bool(); + odometry_dropout_guard = settings[3].as_bool(); + + //PID Params + this->declare_parameter>("PID_params.surge"); + surge_params=this->get_parameter("PID_params.surge").as_double_array(); + this->declare_parameter>("PID_params.pitch"); + pitch_params=this->get_parameter("PID_params.pitch").as_double_array(); + this->declare_parameter>("PID_params.yaw"); + yaw_params=this->get_parameter("PID_params.yaw").as_double_array(); + + //LQR Parameters + + this->declare_parameter>("LQR_params.Q"); + Q=this->get_parameter("LQR_params.Q").as_double_array(); + this->declare_parameter>("LQR_params.R"); + R=this->get_parameter("LQR_params.R").as_double_array(); + this->declare_parameter>("physical.mass_matrix"); + inertia_matrix=this->get_parameter("physical.mass_matrix").as_double_array(); + + //D + this->declare_parameter>("dampening_matrix_low"); + this->declare_parameter>("dampening_matrix_high"); + this->dampening_matrix_low=this->get_parameter("dampening_matrix_low").as_double_array(); + this->dampening_matrix_high=this->get_parameter("dampening_matrix_high").as_double_array(); + + + +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Velocity_node::on_configure(const rclcpp_lifecycle::State &){ + RCLCPP_INFO(get_logger(), "Configure VC"); + + // Publishers + publisher_thrust = create_publisher(topic_thrust, pub_QoS); + + //Subscribers + subscriber_Odometry = this->create_subscription( topic_odometry,sub_QoS, std::bind(&Velocity_node::odometry_callback,this,std::placeholders::_1)); + subscriber_guidance = this->create_subscription( topic_guidance,sub_QoS,std::bind(&Velocity_node::guidance_callback,this, std::placeholders::_1)); + //Timer + if(first_start&&auto_start){ + startup_timer_=create_wall_timer(std::chrono::milliseconds(0),[this](){startup_timer_->cancel(); trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE);}); + } + first_start=false; + return CallbackReturn::SUCCESS; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +Velocity_node::on_activate(const rclcpp_lifecycle::State & state) +{ + RCLCPP_INFO(get_logger(), "Activating..."); + timer_calculation = this->create_wall_timer(std::chrono::milliseconds(publish_rate), std::bind(&Velocity_node::calc_thrust, this)); + auto ret = LifecycleNode::on_activate(state); + + return ret; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +Velocity_node::on_deactivate(const rclcpp_lifecycle::State & state) +{ + RCLCPP_INFO(get_logger(), "Deactivating..."); + auto ret = LifecycleNode::on_deactivate(state); + timer_calculation.reset(); + reset_controllers(); + return ret; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +Velocity_node::on_cleanup(const rclcpp_lifecycle::State &) +{ + RCLCPP_INFO(get_logger(), "Cleaning up..."); + timer_calculation.reset(); + publisher_thrust.reset(); + subscriber_guidance.reset(); + subscriber_Odometry.reset(); + return CallbackReturn::SUCCESS; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +Velocity_node::on_shutdown(const rclcpp_lifecycle::State & state) +{ + RCLCPP_INFO(get_logger(), "Shutting down from state %s", state.label().c_str()); + if(timer_calculation) timer_calculation->cancel(); + timer_calculation.reset(); + publisher_thrust.reset(); + subscriber_guidance.reset(); + subscriber_Odometry.reset(); + should_exit_=true; + return CallbackReturn::SUCCESS; +} + +void Velocity_node::reset_controllers(int nr){ + switch (nr) { + case 0: + PID_pitch.reset_controller(); + PID_surge.reset_controller(); + PID_yaw.reset_controller(); + lqr_controller.reset_controller(); + break; + case 1: + PID_surge.reset_controller(); + lqr_controller.reset_controller(1); + break; + + case 2: + PID_pitch.reset_controller(); + lqr_controller.reset_controller(2); + break; + + case 3: + PID_yaw.reset_controller(); + lqr_controller.reset_controller(3); + break; + } +} + +RCLCPP_COMPONENTS_REGISTER_NODE(Velocity_node) + + diff --git a/control/velocity_controller/tests/CMakeLists.txt b/control/velocity_controller/tests/CMakeLists.txt new file mode 100644 index 000000000..0d579c4f3 --- /dev/null +++ b/control/velocity_controller/tests/CMakeLists.txt @@ -0,0 +1,107 @@ +cmake_minimum_required(VERSION 3.8) +include(GoogleTest) +find_package(ament_cmake_gtest REQUIRED) +#find_package(GTest REQUIRED) +find_package(yaml-cpp REQUIRED) + #find_package(tf2 REQUIRED) + +#Unit test +set(TEST_BINARY_NAME PID_test) + +ament_add_gtest(${TEST_BINARY_NAME} +test_PID.cpp +../src/utilities.cpp +../src/ct_instantiations.cpp +../src/PID_setup.cpp +) + +ament_add_gtest(LQR_test +test_LQR.cpp +../src/utilities.cpp +../src/ct_instantiations.cpp +../src/LQR_setup.cpp +) + +#To DO need to clean up the include directories/dependencies and such +target_include_directories(${TEST_BINARY_NAME} PUBLIC +$ +$ +${EIGEN3_INCLUDE_DIR} +) +target_include_directories(LQR_test PUBLIC +$ +$ +${EIGEN3_INCLUDE_DIR} +) #Need to fix +ament_target_dependencies(${TEST_BINARY_NAME} +rclcpp +std_msgs +vortex_msgs +geometry_msgs +nav_msgs +vortex_utils +) +ament_target_dependencies(LQR_test + rclcpp + vortex_utils + std_msgs + geometry_msgs + vortex_msgs + nav_msgs +) +target_compile_definitions(LQR_test PRIVATE +YAML_PATH="${PROJECT_SOURCE_DIR}/config/parameters.yaml") + +target_link_libraries(${TEST_BINARY_NAME} +Eigen3::Eigen +ct_optcon +ct_core +) +target_link_libraries(LQR_test + Eigen3::Eigen + ct_optcon + ct_core + yaml-cpp +) + + +#System tests + + +target_link_libraries( + ${TEST_BINARY_NAME} + ${LIB_NAME} + #yaml-cpp + #GTest::GTest + #spdlog::spdlog + Eigen3::Eigen + ct_optcon + ct_core +) +add_executable(test_VC_node +test_VC.cpp +../src/utilities.cpp +) + +target_include_directories(test_VC_node PUBLIC +$ +$ +${EIGEN3_INCLUDE_DIR} +) + + +target_link_libraries(test_VC_node Eigen3::Eigen ct_optcon ct_core) + +ament_target_dependencies(test_VC_node +rclcpp +rclcpp_lifecycle +std_msgs +vortex_msgs +geometry_msgs +nav_msgs +vortex_utils +) + +install(TARGETS test_VC_node +DESTINATION lib/${PROJECT_NAME} +) \ No newline at end of file diff --git a/control/velocity_controller/tests/test_LQR.cpp b/control/velocity_controller/tests/test_LQR.cpp new file mode 100644 index 000000000..f2355ab44 --- /dev/null +++ b/control/velocity_controller/tests/test_LQR.cpp @@ -0,0 +1,103 @@ +#include +#include +#include +#include +#include "velocity_controller/utilities.hpp" +#include + +class LQR_test : public ::testing::Test{ + protected: + double delta=0.0001; + static inline YAML::Node cfg; + LQRController controller; + static void SetUpTestSuite() { + try { + cfg = YAML::LoadFile(YAML_PATH); + } + catch (const YAML::Exception& e) { + FAIL() << "Failed to load YAML from '" << YAML_PATH << "': " << e.what(); + } + + }; + void SetUp() override{ + controller.set_matrices(cfg["/**"]["ros__parameters"]["LQR_params"]["Q"].as>(),cfg["/**"]["ros__parameters"]["LQR_params"]["R"].as>(),cfg["/**"]["ros__parameters"]["inertia_matrix"].as>(),cfg["/**"]["ros__parameters"]["max_force"].as(),cfg["/**"]["ros__parameters"]["dampening_matrix_low"].as>()); + controller.reset_controller(); + controller.set_interval(0.01); + } + void TearDown() override{ + + } +}; +/* +TEST(LQR,setup){ + LQRController controller; + controller.set_interval(1); + YAML::Node cfg; + ASSERT_NO_THROW(cfg=YAML::LoadFile(YAML_PATH)); + controller.set_matrices(cfg["LQR"]["Q"],cfg["LQR"]["Q"],cfg["LQR"]["Q"],cfg["LQR"]["Q"],100); +}; +*/ +TEST_F(LQR_test,wrong_setup){ + //LQRController controller; + std::vector eight={1,2,3,4,5,6,7,8}; + std::vector six={1,2,3,4,5,6}; + std::vector thirty_six={1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20,21,22,23,24,25,26,27,28,29,30,31,32,33,34,35,36}; + std::vector three={1,2,3}; + EXPECT_TRUE(controller.set_matrices(eight,three,thirty_six,100,thirty_six)); + EXPECT_FALSE(controller.set_matrices(eight,eight,thirty_six,100,thirty_six)); + EXPECT_FALSE(controller.set_matrices(three,three,thirty_six,100,thirty_six)); + EXPECT_FALSE(controller.set_matrices(eight,three,eight,100,thirty_six)); + EXPECT_FALSE(controller.set_matrices(eight,three,thirty_six,100,eight)); + EXPECT_FALSE(controller.set_matrices(eight,three,thirty_six,100,eight)); + EXPECT_FALSE(controller.set_matrices(eight,three,thirty_six,-100,thirty_six)); +}; +/* +TEST_F(LQR_test,solve){ + State states{1,1,1,2,2,2,1,2,1}; + Guidance_data value{1,3,2}; + Eigen::Vector result=controller.calculate_thrust(states,value); + EXPECT_NEAR(result(0),0,delta); + EXPECT_NEAR(result(1),0,delta); + EXPECT_NEAR(result(2),0,delta); +};*/ +TEST_F(LQR_test,Direction){ + Guidance_data value; + State state{}; + value.surge=0.2; + controller.calculate_thrust(state,value); + Eigen::Vector result=controller.get_thrust(); + EXPECT_TRUE(result(0)>0); + +} +TEST_F(LQR_test,zero_input){ + State states{}; + states.surge=1; + states.yaw=0.2; + states.pitch=0.3; + Guidance_data value; + value.surge=1.0; + value.yaw=0.2; + value.pitch=0.3; + controller.calculate_thrust(states,value); + Eigen::Vector result=controller.get_thrust(); + EXPECT_NEAR(result(0),0,delta); + EXPECT_NEAR(result(1),0,delta); + EXPECT_NEAR(result(2),0,delta); + controller.reset_controller(); + states.surge=0; + states.pitch=0; + states.yaw=0; + value.surge=0; + value.pitch=0; + value.yaw=0; + controller.calculate_thrust(states, value); + result=controller.get_thrust(); + EXPECT_NEAR(result(0),0,delta); + EXPECT_NEAR(result(1),0,delta); + EXPECT_NEAR(result(2),0,delta); +} + +int main(int argc,char** argv){ + testing::InitGoogleTest(&argc,argv); + return RUN_ALL_TESTS(); +} diff --git a/control/velocity_controller/tests/test_PID.cpp b/control/velocity_controller/tests/test_PID.cpp new file mode 100644 index 000000000..0d449c15f --- /dev/null +++ b/control/velocity_controller/tests/test_PID.cpp @@ -0,0 +1,111 @@ +#include +//#include +//#include +#include +//#include + +class PID_test : public ::testing::Test{ + protected: + double delta=0.0001; + PID_controller PID; + void SetUp() override{ + PID.set_parameters(0,0,0,1); + PID.reset_controller(); + } + void TearDown() override{ + + } + +}; +/* +class Node_test : public ::testing:Test{ + protected: + static void SetUpTestSuite(){ + int argc=0; + char ** argv==nullptr; + rclcpp::init(); + } + static void TearDownTestSuite(){ + rclcpp::shutdown(); + } + +}; + + +*/ +TEST_F(PID_test,reset_controller){ + PID.set_parameters(0,1,0,100); + PID.calculate_thrust(100); + PID.set_parameters(0,1,0,1); + PID.calculate_thrust(0); + PID.reset_controller(); + SCOPED_TRACE("Scenario: reset"); + EXPECT_NEAR(PID.get_output(),0,delta); + PID.calculate_thrust(0); + SCOPED_TRACE("Scenario: reset2"); + EXPECT_NEAR(PID.get_output(),0,delta); +} +TEST_F(PID_test,P){ + PID.set_parameters(1,0,0,1); + EXPECT_NEAR(PID.calculate_thrust(1),1,delta); + EXPECT_NEAR(PID.calculate_thrust(2),2,delta); + PID.set_parameters(1.2,0,0,1); + EXPECT_NEAR(PID.calculate_thrust(-2.2),-2.64,delta); + EXPECT_NEAR(PID.calculate_thrust(-1.5),-1.8,delta); +} +TEST_F(PID_test,I){ + PID.set_parameters(0,1.1,0,1); + PID.calculate_thrust(1); + EXPECT_NEAR(PID.get_output(),0,delta); + PID.calculate_thrust(1); + EXPECT_NEAR(PID.get_output(),1.1,delta); + PID.calculate_thrust(-1); + EXPECT_NEAR(PID.get_output(),2.2,delta); + EXPECT_NEAR(PID.calculate_thrust(0),1.1,delta); + PID.set_output_limits(-101,101); + PID.reset_controller(); + PID.set_parameters(1,1,0,10); + EXPECT_NEAR(PID.calculate_thrust(1000),101,delta); + PID.set_parameters(1,1,0,1); + EXPECT_NEAR(PID.calculate_thrust(0),0,delta); + PID.reset_controller(); + EXPECT_NEAR(PID.calculate_thrust(-10000),-101,delta); + PID.calculate_thrust(-50); + EXPECT_NEAR(PID.calculate_thrust(1),-49,delta); +} +TEST_F(PID_test,D){ + +} +TEST_F(PID_test,illegal_inputs){ + double temp=PID.get_output(); + PID.set_parameters(1,1,0,0); + + EXPECT_FALSE(PID.calculate_thrust(1)); + EXPECT_NEAR(PID.get_output(),temp,delta); + EXPECT_FALSE(PID.set_output_limits(1,-1)); + PID.set_parameters(1,1,0,-1); + + EXPECT_FALSE(PID.calculate_thrust(1)); +} +/* +TEST(PID,BASIC){ + PID_controller foo (1,1,0); + ASSERT_NO_THROW(foo.set_output_limits(-10,100)); + ASSERT_NO_THROW(foo.calculate_thrust(1,1)); + EXPECT_TRUE(foo.get_output()>0); + EXPECT_NEAR(foo.get_output(),1,0.01); + ASSERT_NO_THROW(foo.calculate_thrust(1,1)); + EXPECT_NEAR(foo.get_output(),2,0.01); + ASSERT_NO_THROW(foo.calculate_thrust(1000000,1)); + EXPECT_EQ(foo.get_output(),100); + ASSERT_NO_THROW(foo.calculate_thrust(-100000,1)); + EXPECT_EQ(foo.get_output(),-10); +} +*/ + + + +int main(int argc,char** argv){ + testing::InitGoogleTest(&argc,argv); + return RUN_ALL_TESTS(); +} \ No newline at end of file diff --git a/control/velocity_controller/tests/test_VC.cpp b/control/velocity_controller/tests/test_VC.cpp new file mode 100644 index 000000000..9e32436f7 --- /dev/null +++ b/control/velocity_controller/tests/test_VC.cpp @@ -0,0 +1,70 @@ +#include +#include +#include +#include +#include +#include +#include "test_VC.hpp" +#include +#include +#include +#include "vortex_msgs/msg/los_guidance.hpp" +#include "velocity_controller/utilities.hpp" + +//Denne noden er kun for å teste velocity_controller noden + +test_VC::test_VC() : Node("test_VC_node") +{ + this->declare_parameter("topics.guidance.los"); + this->declare_parameter("topics.odom"); + this->topic_guidance=this->get_parameter("topics.guidance.los").as_string(); + this->topic_odometry=this->get_parameter("topics.odom").as_string(); + rclcpp::QoS pub_QoS(10); + pub_QoS.keep_last(10).reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT).durability(RMW_QOS_POLICY_DURABILITY_VOLATILE); + + publisher_guidance = this->create_publisher(topic_guidance, pub_QoS); + publisher_state = this->create_publisher("/state", pub_QoS); + + rclcpp::QoS sub_QoS(10); + sub_QoS.keep_last(10).reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT).durability(RMW_QOS_POLICY_DURABILITY_VOLATILE); + subscription_state = this->create_subscription( + topic_odometry,sub_QoS, + std::bind(&test_VC::odometry_callback,this,std::placeholders::_1)); + timer_ = this->create_wall_timer( + std::chrono::milliseconds(200), + std::bind(&test_VC::send_guidance, this)); + clock_ = this->get_clock(); + RCLCPP_INFO(this->get_logger(), "Test_VC node has been started"); + + +} + +void test_VC::send_guidance() +{ + time1+=0.2; + reference_msg.yaw=std::numbers::pi*sin(time1*std::numbers::pi/9); + //reference_msg.pitch=0.3*sin(time1*std::numbers::pi/9); + reference_msg.surge=0.20;reference_msg.pitch=-0.4;//reference_msg.yaw=0.0; //Surge, pitch, yaw + publisher_guidance->publish(reference_msg); +} + +void test_VC::odometry_callback(const nav_msgs::msg::Odometry::SharedPtr msg_ptr){ + vortex_msgs::msg::LOSGuidance msg; + angle temp=quaternion_to_euler_angle(msg_ptr->pose.pose.orientation.w, msg_ptr->pose.pose.orientation.x, msg_ptr->pose.pose.orientation.y, msg_ptr->pose.pose.orientation.z); + msg.set__pitch(temp.thetat); + msg.set__yaw(temp.psit); + msg.set__surge(msg_ptr->twist.twist.linear.x); + publisher_state->publish(msg); + +} +int main(int argc, char const *argv[]) +{ + + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} + + + \ No newline at end of file diff --git a/control/velocity_controller/tests/test_VC.hpp b/control/velocity_controller/tests/test_VC.hpp new file mode 100644 index 000000000..d5270b509 --- /dev/null +++ b/control/velocity_controller/tests/test_VC.hpp @@ -0,0 +1,43 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include "vortex_msgs/msg/los_guidance.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "velocity_controller/utilities.hpp" + +class test_VC : public rclcpp::Node{ + public: + test_VC(); + //Callback functions + void send_guidance(); + void odometry_callback(const nav_msgs::msg::Odometry::SharedPtr msg_ptr); + + //Variables + + //Subscribers and publishers + rclcpp::Publisher::SharedPtr publisher_guidance; + rclcpp::Publisher::SharedPtr publisher_state; + rclcpp::Subscription::SharedPtr subscription_state; + //Timers + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Clock::SharedPtr clock_; + //Messages + vortex_msgs::msg::LOSGuidance reference_msg; + + //Topics + std::string topic_guidance; + std::string topic_state="/state"; + std::string topic_odometry; + + + //MSGS + double time1=0; +}; + +geometry_msgs::msg::Quaternion euler_angle_to_quaternion(double roll, double pitch, double yaw); \ No newline at end of file diff --git a/control/velocity_controller_lqr/config/param_velocity_controller_lqr.yaml b/control/velocity_controller_lqr/config/param_velocity_controller_lqr.yaml index 727c1c71e..c3afbdabe 100644 --- a/control/velocity_controller_lqr/config/param_velocity_controller_lqr.yaml +++ b/control/velocity_controller_lqr/config/param_velocity_controller_lqr.yaml @@ -1,23 +1,23 @@ /**: ros__parameters: - dt: 0.1 + dt: 0.05 LQR_params: - q_surge: 75 - q_pitch: 175 - q_yaw: 175 + q_surge: 40 + q_pitch: 80 + q_yaw: 80 - r_surge: 0.3 - r_pitch: 0.4 - r_yaw: 0.4 + r_surge: 0.4 + r_pitch: 0.6 + r_yaw: 0.6 - i_surge: 0.3 - i_pitch: 0.4 - i_yaw: 0.3 + i_surge: 0.2 + i_pitch: 0.2 + i_yaw: 0.2 i_weight: 0.5 - inertia_matrix: [30.0, 0.6, 0.0, 0.6, 1.629, 0.0, 0.0, 0.0, 1.729] + inertia_matrix: [53.7, 0.0, 0.0, 0.0, 23.1128, 0.1025, 0.0, 0.1025, 26.23998] #Clamp parameter - max_force: 99.5 + max_force: 80 diff --git a/control/velocity_controller_lqr/scripts/velocity_controller_lqr_node.py b/control/velocity_controller_lqr/scripts/velocity_controller_lqr_node.py index c2b7722c0..ccd36a0aa 100755 --- a/control/velocity_controller_lqr/scripts/velocity_controller_lqr_node.py +++ b/control/velocity_controller_lqr/scripts/velocity_controller_lqr_node.py @@ -305,13 +305,12 @@ def main(args=None): if __name__ == "__main__": main() - -# ⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀ +⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀ # ⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⣀⣠⢴⣤⣠⣤⢤⣂⣔⣲⣒⣖⡺⢯⣝⡿⣿⣿⣿⣷⣶⣶⣢⢦⣤⣄⣀⣀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀ # ⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣰⣯⣿⣾⣿⣶⣺⣯⡿⣓⣽⢞⡸⣻⢏⣋⠌⣛⣭⣿⢟⣿⣛⣿⢷⣿⣿⣿⡟⣿⣻⣵⣲⢢⣄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀ # ⠀⠀⠀⠀⠀⠀⢀⣀⣤⡴⠲⠶⢦⠤⢤⡤⠿⠿⠿⠿⣿⣽⣿⣽⣷⣿⢽⣾⣷⣭⡞⣩⡐⠏⡋⣽⡬⣭⠏⢍⣞⢿⣽⣿⣷⢿⣿⣿⡿⠾⣿⢶⡶⣤⣀⠀⠀⠀⠀⠀⠀⠀⠀ # ⠀⠀⠀⠀⣤⣖⠯⡙⢢⡁⠄⢢⡤⢠⢀⣸⠀⣄⡠⣀⣀⣦⡄⣠⢀⡃⠽⣽⠬⠍⣿⣿⣤⣥⣷⣿⣿⣿⣾⡍⠛⢌⠈⢫⣍⢋⣍⡁⠹⢍⠈⣳⢎⠴⠟⠻⢧⣄⠀⠀⠀⠀⠀ -# ⠀⠀⣠⣾⣿⣿⣿⣯⡔⠆⠠⠈⣿⣿⠾⡾⠿⣶⠿⡟⣻⡛⣭⢙⠍⢩ANDERS ER GOATED⣤⣥⣩⣶⣟⣻⠧⣻⠶⢤⢰⡱⣬⣤⣌⣑⠞⠲⠓⠭⡀⠀⠀⠀ +# ⠀⠀⣠⣾⣿⣿⣿⣯⡔⠆⠠⠈⣿⣿⠾⡾⠿⣶⠿⡟⣻⡛⣭⢙⠍⢩ANDERS ER GOATED⣶⣟⣻⠧⣻⠶⢤⢰⡱⣬⣤⣌⣑⠞⠲⠓⠭⡀⠀⠀⠀ # ⠀⠐⣿⣿⣿⢟⡋⢈⢤⣤⣷⢿⣿⠒⢜⣁⡱⡧⢿⣹⣷⣿⡿⣷⠌⣢⣟⢱⢽⡨⢊⠴⡉⢉⡿⣯⢿⣏⢹⠏⣯⣩⣙⠾⢿⣳⣶⢻⣟⣿⠧⢀⠋⠟⣿⡧⠠⠄⡤⠈⢠⠀⠀ # ⠀⠀⠘⠻⠿⠶⠶⠿⠛⣹⣟⡞⠸⣬⠐⠙⢍⢉⣔⠪⠟⡛⠫⢵⣾⣣⣼⣽⢈⠔⡅⣜⡽⢯⢞⡕⡠⠓⣡⢚⣷⣷⣿⣳⡄⠢⠉⠛⢿⣲⢿⣶⢿⣬⣾⣛⠳⣼⡮⠳⡂⠒⠀ # ⠀⠀⠀⠀⠀⠀⠀⠀⢠⠏⡁⢉⣀⣑⣆⡐⠊⣅⡕⢦⣀⣱⡶⢫⣨⢟⠽⠕⣇⢶⣵⣋⢝⣉⣋⠜⠉⠉⡯⠛⣿⣿⣿⣾⣳⠠⠤⠪⠁⠊⠉⠻⣟⣾⣿⣿⣟⣧⣧⢸⠂⠠⢠ diff --git a/guidance/los_guidance/CMakeLists.txt b/guidance/los_guidance/CMakeLists.txt index 90b51de3a..f8e7e7b4c 100644 --- a/guidance/los_guidance/CMakeLists.txt +++ b/guidance/los_guidance/CMakeLists.txt @@ -12,25 +12,34 @@ endif() find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) +find_package(rclcpp_components REQUIRED) find_package(vortex_msgs REQUIRED) find_package(vortex_utils_ros REQUIRED) find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) find_package(Eigen3 REQUIRED) find_package(spdlog REQUIRED) find_package(fmt REQUIRED) +find_package(yaml-cpp REQUIRED) + include_directories(include) set(LIB_NAME los_guidance_lib) add_library(${LIB_NAME} SHARED - src/los_guidance.cpp + src/lib/proportional_los.cpp + src/lib/integral_los.cpp + src/lib/adaptive_los.cpp src/los_guidance_ros.cpp + src/lib/vector_field_los.cpp ) ament_target_dependencies(${LIB_NAME} rclcpp rclcpp_action + rclcpp_components geometry_msgs + nav_msgs vortex_msgs vortex_utils_ros Eigen3 @@ -38,11 +47,14 @@ ament_target_dependencies(${LIB_NAME} fmt ) -add_executable(los_guidance_node - src/los_guidance_node.cpp +target_link_libraries(${LIB_NAME} + yaml-cpp ) -target_link_libraries(los_guidance_node ${LIB_NAME}) +rclcpp_components_register_node(${LIB_NAME} + PLUGIN "vortex::guidance::los::LosGuidanceNode" + EXECUTABLE los_guidance_node +) install(TARGETS ${LIB_NAME} @@ -52,14 +64,9 @@ install(TARGETS RUNTIME DESTINATION bin ) -install(TARGETS - los_guidance_node - DESTINATION lib/${PROJECT_NAME} -) - install( DIRECTORY include/ - DESTINATION include + DESTINATION include/ ) install(DIRECTORY @@ -68,6 +75,11 @@ install(DIRECTORY DESTINATION share/${PROJECT_NAME}/ ) +install(PROGRAMS + scripts/test_scenarios.py + DESTINATION share/${PROJECT_NAME}/scripts +) + if(BUILD_TESTING) add_subdirectory(test) endif() diff --git a/guidance/los_guidance/README.md b/guidance/los_guidance/README.md index 33c616c59..b482a74e6 100644 --- a/guidance/los_guidance/README.md +++ b/guidance/los_guidance/README.md @@ -1,22 +1,183 @@ -## ALSO Guidance Law for 3D Path Following +# 3D LOS Guidance Library -The guidance law gives calculates the desired heading angle $\psi_d$ and desired pitch angle $\theta_d$. The crab angles $\beta_c$ and $\alpha_c$ are estimated adaptively. The guidance law looks like +This package implements several **Line-of-Sight (LOS) guidance algorithms** for **3D path following**. + +The guidance system computes the **desired yaw** $\psi_d$ and **desired pitch** $\theta_d$ that allow a vehicle to follow a path between waypoints. + +The node receives + +- vehicle pose +- waypoint information + +and computes guidance references based on the selected LOS algorithm. The resulting guidance commands consist of + +- desired yaw +- desired pitch +- desired surge velocity. + +The vehicle surge speed is kept **constant during path following** and is defined in the configuration file using the parameter `u_desired`. + +--- + +# Implemented LOS Methods + +The library supports four LOS guidance algorithms. + +| Mode | Method | +|-----|------| +| 0 | Proportional LOS | +| 1 | Integral LOS | +| 2 | Adaptive LOS | +| 3 | Vector Field LOS | + +The guidance method can be preconfigured in the package configuration file before startup, and changed during runtime using a ROS service. + +--- + + +# Sending a LOS Goal + +A waypoint can be sent to the guidance node using the action interface: + +``` +ros2 action send_goal /drone_name/los_guidance \ +vortex_msgs/action/LOSGuidance \ +"{goal: {header: {frame_id: world_ned}, point: {x: 0.0, y: 0.0, z: 0.0}}}" +``` + +This command instructs the guidance node to start following a path toward the waypoint. + +--- + +# Switching LOS Method + +The active LOS guidance method can be configured in advance through the package config file, and it can also be changed during runtime. + +At runtime, the LOS guidance method can be switched with: + +``` +ros2 service call /drone_name/set_los_mode \ +vortex_msgs/srv/SetLosMode "{mode: X}" +``` + +Where + +| X | Method | +|---|---| +| 0 | Proportional LOS | +| 1 | Integral LOS | +| 2 | Adaptive LOS | +| 3 | Vector Field LOS | + +Example: + +``` +ros2 service call /drone_name/set_los_mode vortex_msgs/srv/SetLosMode "{mode: 2}" +``` + +This switches the guidance system to **Adaptive LOS**. + +--- +## Path Geometry + +The path between two waypoints defines the **path direction angles** used by the guidance law. ```math -\psi_d = \pi_h - \hat{\beta}_c - \tan^{-1}\left(\frac{y_e^p}{\Delta_h}\right) +\pi_h = +\text{atan2}(y_{i+1}^n - y_i^n, x_{i+1}^n - x_i^n) +``` + +```math +\pi_v = +\text{atan2} +\left( +-(z_{i+1}^n - z_i^n), +\sqrt{(x_{i+1}^n - x_i^n)^2 + +(y_{i+1}^n - y_i^n)^2} +\right) +``` + +where + +- $\pi_h$ is the **horizontal path angle (azimuth angle)** +- $\pi_v$ is the **vertical path angle (elevation angle)** + +These angles define the **direction of the current path segment** between two waypoints. + +Additionally + +- $P_i^n = (x_i^n, y_i^n, z_i^n)$ is the previous waypoint in the north-east-down (NED) frame +- $P_{i+1}^n = (x_{i+1}^n, y_{i+1}^n, z_{i+1}^n)$ is the next waypoint. + +--- + +## Path Frame Errors + +The along-, cross- and vertical-track errors in the path-tangential frame are found by + +```math +\begin{bmatrix} +x_e^p \\ +y_e^p \\ +z_e^p +\end{bmatrix} += +\mathbf{R}_{y,\pi_v}^\top +\mathbf{R}_{z,\pi_h}^\top +\left( +\begin{bmatrix} +x^n \\ +y^n \\ +z^n +\end{bmatrix} +- +\begin{bmatrix} +x_i^n \\ +y_i^n \\ +z_i^n +\end{bmatrix} +\right) +``` + +where + +- $x_e^p$ is the along-track error +- $y_e^p$ is the cross-track error +- $z_e^p$ is the vertical-track error + +and +- $P^n = (x^n, y^n, z^n)$ is the current position of the vehicle. + +--- + +## Adaptive LOS (ALSO) + +Adaptive LOS estimates **crab angles caused by disturbances** such as ocean currents or wind. + +```math +\psi_d = \pi_h - \hat{\beta}_c - \tan^{-1}\left(\frac{y_e^p}{\Delta_h}\right) ``` ```math -\dot{\hat{\beta}}_c = \gamma_h \frac{\Delta_h}{\sqrt{\Delta_h^2 + (y_e^p)^2}} y_e^p +\dot{\hat{\beta}}_c = +\gamma_h +\frac{\Delta_h}{\sqrt{\Delta_h^2 + (y_e^p)^2}} +y_e^p ``` ```math -\theta_d = \pi_v + \hat{\alpha}_c + \tan^{-1}\left(\frac{z_e^p}{\Delta_v}\right) +\theta_d = +\pi_v + +\hat{\alpha}_c + +\tan^{-1}\left(\frac{z_e^p}{\Delta_v}\right) ``` ```math -\dot{\hat{\alpha}}_c = \gamma_v \frac{\Delta_v}{\sqrt{\Delta_v^2 + (z_e^p)^2}} z_e^p +\dot{\hat{\alpha}}_c = +\gamma_v +\frac{\Delta_v}{\sqrt{\Delta_v^2 + (z_e^p)^2}} +z_e^p ``` where @@ -27,27 +188,211 @@ where - $y_e^p$ is the cross-track error - $z_e^p$ is the vertical-track error -The azimuth angle $\pi_v$ and the elevation angle $\pi_h$ can be found by +The terms + +- $\hat{\beta}_c$ is the **estimated horizontal crab angle** +- $\hat{\alpha}_c$ is the **estimated vertical crab angle** + +These angles represent the **estimated disturbance-induced deviation** between the vehicle heading and the actual direction of motion. +They allow the guidance system to **compensate for disturbances such as currents or wind**. + +Adaptive LOS is generally the **most robust method** and works well for + +- curved trajectories +- long paths +- environments with disturbances. + +--- + +## Proportional LOS (PLOS) + +Proportional LOS is the simplest LOS guidance law. ```math -\pi_h = \text{atan2}(y_{i+1}^n - y_i^n, x_{i+1}^n, - x_i^n) +\psi_d = +\pi_h - +\tan^{-1}\left(\frac{y_e^p}{\Delta_h}\right) ``` + ```math -\pi_v = \text{atan2}(-(z_{i+1}^n - z_i^n), \sqrt{(x_{i+1}^n - x_i^n)^2 + (y_{i+1}^n - y_i^n)^2}) +\theta_d = +\pi_v + +\tan^{-1}\left(\frac{z_e^p}{\Delta_v}\right) ``` -where $P_i^n = (x_i^n, y_i^n, z_i^n)$ is the previous waypoint in the north-east-down frame and $P_{i+1}^n = (x_{i+1}^n, y_{i+1}^n, z_{i+1}^n)$ is the next waypoint in north-east-down frame. +where -The along-, cross- and vertical-track errors in the path-tangential frame are found by +- $\Delta_h$ horizontal lookahead distance +- $\Delta_v$ vertical lookahead distance + +The lookahead distances determine **how aggressively the vehicle corrects path errors**. + +- small values → aggressive corrections +- large values → smoother but slower convergence + +### Use case + +PLOS works best for + +- simple waypoint following +- environments with minimal disturbances. + +However, it may suffer from **steady-state tracking error** when disturbances are present. + +--- + +## Integral LOS (ILOS) + +Integral LOS adds integral action to remove steady-state error. ```math -\begin{bmatrix} -x_e^p \\ y_e^p \\ z_e^p -\end{bmatrix} = \mathbf{R}_{y, \pi_v}^\top \mathbf{R}_{z, \pi_h}^\top \left( \begin{bmatrix} -x^n \\ y^n \\ z^n -\end{bmatrix} - \begin{bmatrix} -x_i^n \\ y_i^n \\ z_i^n -\end{bmatrix} -\right) +u_h = +k_{p,h}y_e^p + +k_{i,h}\int y_e^p dt ``` -where $P^n = (x^n, y^n, z^n)$ is the current position of the drone. + +```math +u_v = +k_{p,v}z_e^p + +k_{i,v}\int z_e^p dt +``` + +```math +\psi_d = +\pi_h - +\tan^{-1}(u_h) +``` + +```math +\theta_d = +\pi_v + +\tan^{-1}(u_v) +``` + +where + +- $k_{p,h}$ horizontal proportional gain +- $k_{p,v}$ vertical proportional gain +- $k_{i,h}$ horizontal integral gain +- $k_{i,v}$ vertical integral gain + +The integral term allows the controller to **eliminate steady-state cross-track errors** caused by constant disturbances. + +### Use case + +ILOS works well when there are **persistent disturbances**, such as + +- steady ocean currents +- constant wind disturbances. + +--- + +## Vector Field LOS (VF-LOS) + +Vector Field LOS generates a **bounded approach angle** toward the path. + +```math +\psi_d = \pi_h - \chi_h +``` + +```math +\chi_h = +\psi_{max} +\frac{2}{\pi} +\tan^{-1}(k_p y_e^p) +``` + +where + +- $\psi_{max}$ maximum allowed approach angle +- $k_p$ proportional gain controlling path convergence + +The bounded approach angle prevents excessively aggressive heading changes. + +### Use case + +VF-LOS works best for + +- long straight path following +- corridor tracking +- inspection missions along pipelines or cables. + +However it performs worse when the path contains **sharp turns or rapidly changing path segments**. + +--- + +## Testing with `guidance_test.launch.py` + +To test the entire los system from just one place a simple, low-effort launch script has been implemented. The file is named `los_guidance/launch/guidance_test.launch.py` It launches the necessary nodes for simulation and autopilot and lets you choose a test scenario through the `test_scenario` argument.The sceraios that have been implemented are: + +| Scenario | Use | +|----------|------| +| Circle | ends the drone in a circular path | +| Square |sends the drone through a square waypoint pattern| +| opposite_point | sends the drone toward a waypoint and then toward a waypoint in the opposite direction (180 degrees) | + +Example of use: + +``` +ros2 launch los_guidance guidance_test.launch.py test_scenario:=circle +``` + +--- + + +## ROS Interfaces + +| Interface | Name | Type | Message-Type | +|----------|------|------|---------| +| Action Server | `/drone_name/los_guidance` | Goal input | `vortex_msgs/action/LOSGuidance` | +| Subscriber | `/drone_name/pose` | Vehicle pose | `geometry_msgs/PoseWithCovarianceStamped` | +| Subscriber | `/drone_name/odom` | Vehicle velocity | `nav_msgs/Odometry` | +| Publisher | `/drone_name/guidance/los` | Guidance reference (yaw, pitch, surge) | `vortex_msgs/LOSGuidance` | +| Publisher | `/los_debug` | LOS debug output | `vortex_msgs/LOSGuidance` | +| Publisher | `/state_debug` | Vehicle state debug | `vortex_msgs/LOSGuidance` | + +--- + +## Guidance Node Architecture +The LOS guidance node computes reference commands for the vehicle based on +the current vehicle state and the active waypoint. + +The process works as follows + +1. The node receives the **vehicle pose**. +2. The current **path segment** is defined between two waypoints. +3. The **path geometry** is computed to determine the path direction. +4. The **cross-track and vertical-track errors** are calculated in the path frame. +5. The selected **LOS guidance algorithm** computes desired yaw and pitch. +6. The node publishes the resulting **guidance reference**. + +### Data Flow + +``` +Vehicle Pose + Waypoint + │ + ▼ + Path Geometry + (π_h , π_v angles) + │ + ▼ + Path Frame Errors + (x_e^p , y_e^p , z_e^p) + │ + ▼ + LOS Algorithm + (PLOS / ILOS / ALSO / VF-LOS) + │ + ▼ + Guidance Output + (ψ_d , θ_d , u_desired) +``` + +The resulting guidance command is published as a `vortex_msgs/LOSGuidance` +message containing + +- desired yaw +- desired pitch +- desired surge velocity. + +--- diff --git a/guidance/los_guidance/config/guidance_params.yaml b/guidance/los_guidance/config/guidance_params.yaml index a580cf57c..16c66b470 100644 --- a/guidance/los_guidance/config/guidance_params.yaml +++ b/guidance/los_guidance/config/guidance_params.yaml @@ -1,10 +1,54 @@ -/**: - ros__parameters: - los: - lookahead_distance_h: 1.5 - lookahead_distance_v: 0.6 - gamma_h: 0.05 - gamma_v: 0.1 - time_step: 0.01 - u_desired: 0.3 - goal_reached_tol: 1.0 +#/** +# * @file guidance_params.yaml +# * @brief Configuration parameters for the LOS guidance node. +# * +# * This file contains the tunable parameters for all supported Line-Of-Sight +# * (LOS) guidance methods, together with common guidance settings shared across +# * the node. +# * +# * Parameter descriptions: +# * - `lookahead_distance_h`: Horizontal lookahead distance used to define the +# * LOS reference point in the horizontal plane. +# * - `lookahead_distance_v`: Vertical lookahead distance used to define the +# * LOS reference point in the vertical plane. +# * - `gamma_h`: Adaptive update gain for horizontal LOS adaptation. +# * - `gamma_v`: Adaptive update gain for vertical LOS adaptation. +# * - `k_p_h`: Proportional gain in the horizontal plane. +# * - `k_p_v`: Proportional gain in the vertical plane. +# * - `k_i_h`: Integral gain in the horizontal plane. +# * - `k_i_v`: Integral gain in the vertical plane. +# * - `max_approach_angle_h`: Maximum allowed horizontal approach angle for +# * vector field LOS guidance. +# * - `max_approach_angle_v`: Maximum allowed vertical approach angle for +# * vector field LOS guidance. +# * - +# */ +adaptive_los: + lookahead_distance_h: 2.0 + lookahead_distance_v: 1.4 + adaptation_gain_h: 0.005 + adaptation_gain_v: 0.005 + +prop_los: + lookahead_distance_h: 0.74 + lookahead_distance_v: 0.8 + +integer_los: + proportional_gain_h: 0.5 + proportional_gain_v: 0.5 + integral_gain_h: 0.1 + integral_gain_v: 0.1 + +vector_field_los: + max_approach_angle_h: 1.0 + max_approach_angle_v: 1.0 + proportional_gain_h: 1.5 + proportional_gain_v: 0.9 + +common: + active_los_method: 2 + u_desired: 0.3 + goal_reached_tol: 0.20 + max_pitch_angle: 0.96 + missed_goal_distance_margin: 1.0 + missed_goal_timeout: 10.0 diff --git a/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp b/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp new file mode 100644 index 000000000..68cffec71 --- /dev/null +++ b/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp @@ -0,0 +1,128 @@ +/** + * @file adaptive_los.hpp + * @brief Defines the AdaptiveLOSGuidance class and its parameter structure for + * the adaptive LOS guidance algorithm. + */ + +#ifndef LOS_GUIDANCE__LIB__ADAPTIVE_LOS_HPP_ +#define LOS_GUIDANCE__LIB__ADAPTIVE_LOS_HPP_ + +#include +#include +#include + +#include "los_guidance/lib/types.hpp" + +namespace vortex::guidance::los { + +/** + * @brief Parameters for the Adaptive LOS guidance algorithm. + * + * This structure stores the tuning parameters used by the adaptive LOS + * controller for horizontal and vertical path-following. + */ +struct AdaptiveLosParams { + double lookahead_distance_h{}; + double lookahead_distance_v{}; + double adaptation_gain_h{}; + double adaptation_gain_v{}; + double time_step{}; // in milliseconds +}; + +/** + * @brief Implements the adaptive LOS guidance algorithm. + * + * This class computes desired heading and pitch commands based on the current + * vehicle position and the active path segment. + */ +class AdaptiveLOSGuidance { + public: + /** + * @brief Constructs an AdaptiveLOSGuidance object. + * @param params Parameters for the adaptive LOS guidance algorithm. + */ + explicit AdaptiveLOSGuidance(const AdaptiveLosParams& params); + + /** + * @brief Destroys the AdaptiveLOSGuidance object. + */ + ~AdaptiveLOSGuidance() = default; + + /** + * @brief Calculates the desired LOS guidance outputs. + * @param inputs Input values required for adaptive LOS computation. + * @return types::Outputs The desired heading and pitch commands. + */ + types::Outputs calculate_outputs(const types::Inputs& inputs); + + /** + * @brief resets the adaptive values when new-segment starts + */ + void reset(); + + private: + /** + * @brief Updates the path heading and path pitch angles from the active + * path segment. + * @param inputs Input values containing the previous and next path points. + */ + void update_angles(const types::Inputs& inputs); + + /** + * @brief Calculates the cross-track error in the path-fixed reference + * frame. + * @param inputs Input values containing the current vehicle position and + * path information. + * @return types::CrossTrackError The calculated horizontal and vertical + * cross-track errors. + */ + const types::CrossTrackError calculate_crosstrack_error( + const types::Inputs& inputs); + + /** + * @brief Updates the adaptive estimates based on the current cross-track + * error. + * @param cross_track_error The current cross-track error in the path frame. + */ + void update_adaptive_estimates( + const types::CrossTrackError& cross_track_error); + + /** + * @brief Parameters used by the adaptive LOS guidance algorithm. + */ + AdaptiveLosParams params_{}; + + /** + * @brief Rotation matrix for the path pitch rotation about the y-axis. + */ + Eigen::Matrix3d rotation_y_ = Eigen::Matrix3d::Zero(); + + /** + * @brief Rotation matrix for the path heading rotation about the z-axis. + */ + Eigen::Matrix3d rotation_z_ = Eigen::Matrix3d::Zero(); + + /** + * @brief Stores the horizontal path angle. + */ + double path_heading_{0.0}; + + /** + * @brief Stores the vertical path angle. + */ + double path_pitch_{0.0}; + + /** + * @brief Stores the horizontal adaptive estimate. + */ + double beta_c_hat_{0.0}; + + /** + * @brief Stores the vertical adaptive estimate. + */ + double alpha_c_hat_{0.0}; +}; + +} // namespace vortex::guidance::los + +#endif // LOS_GUIDANCE__LIB__ADAPTIVE_LOS_HPP_ diff --git a/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp b/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp new file mode 100644 index 000000000..2bf8d951c --- /dev/null +++ b/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp @@ -0,0 +1,112 @@ +/** + * @file integral_los.hpp + * @brief Defines the IntegralLOSGuidance class and parameter structure for the + * integral LOS guidance algorithm. + */ +#ifndef LOS_GUIDANCE__LIB__INTEGRAL_LOS_HPP_ +#define LOS_GUIDANCE__LIB__INTEGRAL_LOS_HPP_ + +#include +#include +#include + +#include "los_guidance/lib/types.hpp" + +namespace vortex::guidance::los { + +/** + * @brief Parameters for the Integral LOS guidance algorithm. + */ +struct IntegralLosParams { + double proportional_gain_h{}; + double proportional_gain_v{}; + double integral_gain_h{}; + double integral_gain_v{}; + double time_step{}; // in milliseconds +}; + +/** + * @brief Implements the integral LOS guidance algorithm. + * + * This class computes desired heading and pitch commands using proportional and + * integral cross-track error feedback. + */ +class IntegralLOSGuidance { + public: + /** + * @brief Constructs an IntegralLOSGuidance object. + * @param params Parameters for the integral LOS guidance algorithm. + */ + explicit IntegralLOSGuidance(const IntegralLosParams& params); + + /** + * @brief Destroys the IntegralLOSGuidance object. + */ + ~IntegralLOSGuidance() = default; + + /** + * @brief Calculates the desired LOS guidance outputs. + * @param inputs Input values required for integral LOS computation. + * @return types::Outputs The desired heading and pitch commands. + */ + types::Outputs calculate_outputs(const types::Inputs& inputs); + + private: + /** + * @brief Updates the path heading and path pitch angles from the active + * path segment. + * @param inputs Input values containing the previous and next path points. + */ + void update_angles(const types::Inputs& inputs); + + /** + * @brief Calculates the cross-track error in the path-fixed reference + * frame. + * @param inputs Input values containing the current vehicle position and + * path information. + * @return types::CrossTrackError The calculated horizontal and vertical + * cross-track errors. + */ + types::CrossTrackError calculate_crosstrack_error( + const types::Inputs& inputs); + + /** + * @brief Parameters used by the integral LOS guidance algorithm. + */ + IntegralLosParams m_params{}; + + /** + * @brief Stores the integral of the horizontal cross-track error. + */ + double integrated_horizontal_error_{}; + + /** + * @brief Stores the integral of the vertical cross-track error. + */ + double integrated_vertical_error_{}; + + /** + * @brief Stores the horizontal path angle. + */ + double path_heading_{}; + + /** + * @brief Stores the vertical path angle. + */ + double path_pitch_{}; + + /** + * @brief Rotation representation for the path pitch angle about the y-axis. + */ + Eigen::AngleAxisd rotation_y_{0.0, Eigen::Vector3d::UnitY()}; + + /** + * @brief Rotation representation for the path heading angle about the + * z-axis. + */ + Eigen::AngleAxisd rotation_z_{0.0, Eigen::Vector3d::UnitZ()}; +}; + +} // namespace vortex::guidance::los + +#endif // LOS_GUIDANCE__LIB__INTEGRAL_LOS_HPP_ diff --git a/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp b/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp new file mode 100644 index 000000000..cf81bc774 --- /dev/null +++ b/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp @@ -0,0 +1,99 @@ +/** + * @file proportional_los.hpp + * @brief Defines the ProportionalLOSGuidance class and parameter structure for + * the proportional LOS guidance algorithm. + */ +#ifndef LOS_GUIDANCE__LIB__PROPORTIONAL_LOS_HPP_ +#define LOS_GUIDANCE__LIB__PROPORTIONAL_LOS_HPP_ + +#include +#include +#include + +#include "los_guidance/lib/types.hpp" + +namespace vortex::guidance::los { + +/** + * @brief Parameters for the proportional LOS guidance algorithm. + */ +struct ProportionalLosParams { + double lookahead_distance_h{}; + double lookahead_distance_v{}; +}; + +/** + * @brief Implements the proportional LOS guidance algorithm. + * + * This class computes desired heading and pitch commands using proportional + * cross-track error feedback. + */ +class ProportionalLOSGuidance { + public: + /** + * @brief Constructs a ProportionalLOSGuidance object. + * @param params Parameters for the proportional LOS guidance algorithm. + */ + explicit ProportionalLOSGuidance(const ProportionalLosParams& params); + + /** + * @brief Destroys the ProportionalLOSGuidance object. + */ + ~ProportionalLOSGuidance() = default; + + /** + * @brief Calculates the desired LOS guidance outputs. + * @param inputs Input values required for proportional LOS computation. + * @return types::Outputs The desired heading and pitch commands. + */ + types::Outputs calculate_outputs(const types::Inputs& inputs); + + private: + /** + * @brief Updates the path heading and path pitch angles from the active + * path segment. + * @param inputs Input values containing the previous and next path points. + */ + void update_angles(const types::Inputs& inputs); + + /** + * @brief Calculates the cross-track error in the path-fixed reference + * frame. + * @param inputs Input values containing the current vehicle position and + * path information. + * @return types::CrossTrackError The calculated horizontal and vertical + * cross-track errors. + */ + types::CrossTrackError calculate_crosstrack_error( + const types::Inputs& inputs) const; + + /** + * @brief Parameters used by the proportional LOS guidance algorithm. + */ + ProportionalLosParams m_params{}; + + /** + * @brief Stores the horizontal path angle. + */ + double path_heading_{0.0}; + + /** + * @brief Stores the vertical path angle. + */ + double path_pitch_{0.0}; + + /** + * @brief Rotation representation for the path pitch angle about the y-axis. + */ + Eigen::AngleAxisd rotation_y_{0.0, Eigen::Vector3d::UnitY()}; + + /** + * @brief Rotation representation for the path heading angle about the + * z-axis. + */ + Eigen::AngleAxisd rotation_z_{0.0, Eigen::Vector3d::UnitZ()}; +}; + +} // namespace vortex::guidance::los + +#endif // LOS_GUIDANCE__LIB__PROPORTIONAL_LOS_HPP_ diff --git a/guidance/los_guidance/include/los_guidance/lib/types.hpp b/guidance/los_guidance/include/los_guidance/lib/types.hpp new file mode 100644 index 000000000..76f8fa296 --- /dev/null +++ b/guidance/los_guidance/include/los_guidance/lib/types.hpp @@ -0,0 +1,97 @@ +/** + * @file types.hpp + * @brief Defines shared data types used by the LOS guidance algorithms. + */ +#ifndef LOS_GUIDANCE__LIB__TYPES_HPP_ +#define LOS_GUIDANCE__LIB__TYPES_HPP_ + +#include +#include +#include +#include +#include + +namespace vortex::guidance::los::types { + +/** + * @brief Represents a 3D point. + * + * This struct is used to store waypoint positions and vehicle position data + * in Cartesian coordinates. + */ +struct Point { + double x{}; + double y{}; + double z{}; + + /** + * @brief Subtracts another point from this point. + * @param other The point to subtract. + * @return Point The resulting difference vector as a point. + */ + Point operator-(const Point& other) const { + return Point{x - other.x, y - other.y, z - other.z}; + } + + /** + * @brief Converts the point to an Eigen 3D vector. + * @return Eigen::Vector3d The point represented as an Eigen vector. + */ + Eigen::Vector3d as_vector() const { return Eigen::Vector3d(x, y, z); } + + /** + * @brief Creates a Point from a ROS point message. + * @param msg ROS point message. + * @return Point The converted point. + */ + static Point point_from_ros(const geometry_msgs::msg::Point& msg) { + return Point{msg.x, msg.y, msg.z}; + } +}; + +/** + * @brief Represents cross-track error in the path-fixed reference frame. + * + * The values describe the position error relative to the active path segment + * in the transformed coordinate frame. + */ +struct CrossTrackError { + double x_e{}; + double y_e{}; + double z_e{}; + + /** + * @brief Creates a CrossTrackError from an Eigen vector. + * @param vector Eigen vector containing cross-track error components. + * @return CrossTrackError The converted cross-track error. + */ + inline static CrossTrackError from_vector(const Eigen::Vector3d& vector) { + return CrossTrackError{vector.x(), vector.y(), vector.z()}; + } +}; + +/** + * @brief Stores the LOS guidance outputs. + */ +struct Outputs { + double psi_d{}; + double theta_d{}; +}; + +/** + * @brief Stores the inputs required by the LOS guidance algorithms. + */ +struct Inputs { + Point prev_point{}; + Point next_point{}; + Point current_position{}; +}; + +/** + * @brief Enumerates the available LOS guidance methods. + */ +enum class ActiveLosMethod { PROPORTIONAL, INTEGRAL, ADAPTIVE, VECTOR_FIELD }; + +} // namespace vortex::guidance::los::types + +#endif // LOS_GUIDANCE__LIB__TYPES_HPP_ diff --git a/guidance/los_guidance/include/los_guidance/lib/vector_field_los.hpp b/guidance/los_guidance/include/los_guidance/lib/vector_field_los.hpp new file mode 100644 index 000000000..6f4ec133e --- /dev/null +++ b/guidance/los_guidance/include/los_guidance/lib/vector_field_los.hpp @@ -0,0 +1,103 @@ +/** + * @file vector_field_los.hpp + * @brief Defines the VectorFieldLOSGuidance class and parameter structure for + * the vector field LOS guidance algorithm. + */ +#ifndef LOS_GUIDANCE__LIB__VECTOR_FIELD_LOS_HPP_ +#define LOS_GUIDANCE__LIB__VECTOR_FIELD_LOS_HPP_ + +#include +#include +#include + +#include "los_guidance/lib/types.hpp" + +namespace vortex::guidance::los { + +/** + * @brief Parameters for the vector field LOS guidance algorithm. + */ +struct VectorFieldLosParams { + double max_approach_angle_h{}; + double max_approach_angle_v{}; + double proportional_gain_h{}; + double proportional_gain_v{}; + double time_step{}; // in milliseconds +}; + +/** + * @brief Implements the vector field LOS guidance algorithm. + * + * This class computes desired heading and pitch commands using vector field + * path-following logic based on the current vehicle position and active path + * segment. + */ +class VectorFieldLOSGuidance { + public: + /** + * @brief Constructs a VectorFieldLOSGuidance object. + * @param params Parameters for the vector field LOS guidance algorithm. + */ + explicit VectorFieldLOSGuidance(const VectorFieldLosParams& params); + + /** + * @brief Destroys the VectorFieldLOSGuidance object. + */ + ~VectorFieldLOSGuidance() = default; + + /** + * @brief Calculates the desired LOS guidance outputs. + * @param inputs Input values required for vector field LOS computation. + * @return types::Outputs The desired heading and pitch commands. + */ + types::Outputs calculate_outputs(const types::Inputs& inputs); + + private: + /** + * @brief Updates the path heading and path pitch angles from the active + * path segment. + * @param inputs Input values containing the previous and next path points. + */ + void update_angles(const types::Inputs& inputs); + + /** + * @brief Calculates the cross-track error in the path-fixed reference + * frame. + * @param inputs Input values containing the current vehicle position and + * path information. + * @return types::CrossTrackError The calculated horizontal and vertical + * cross-track errors. + */ + types::CrossTrackError calculate_crosstrack_error( + const types::Inputs& inputs) const; + + /** + * @brief Parameters used by the vector field LOS guidance algorithm. + */ + VectorFieldLosParams m_params{}; + + /** + * @brief Stores the horizontal path angle. + */ + double path_heading_{0.0}; + + /** + * @brief Stores the vertical path angle. + */ + double path_pitch_{0.0}; + + /** + * @brief Rotation representation for the path pitch angle about the y-axis. + */ + Eigen::AngleAxisd rotation_y_{0.0, Eigen::Vector3d::UnitY()}; + + /** + * @brief Rotation representation for the path heading angle about the + * z-axis. + */ + Eigen::AngleAxisd rotation_z_{0.0, Eigen::Vector3d::UnitZ()}; +}; + +} // namespace vortex::guidance::los + +#endif // LOS_GUIDANCE__LIB__VECTOR_FIELD_LOS_HPP_ diff --git a/guidance/los_guidance/include/los_guidance/los_guidance.hpp b/guidance/los_guidance/include/los_guidance/los_guidance.hpp deleted file mode 100644 index 71ddfdb11..000000000 --- a/guidance/los_guidance/include/los_guidance/los_guidance.hpp +++ /dev/null @@ -1,77 +0,0 @@ -#ifndef LOS_GUIDANCE__LOS_GUIDANCE_HPP_ -#define LOS_GUIDANCE__LOS_GUIDANCE_HPP_ - -#include - -namespace vortex::guidance { - -namespace LOS { - -struct Point { - double x{}; - double y{}; - double z{}; - - Point operator-(const Point& other) const { - return Point{x - other.x, y - other.y, z - other.z}; - } - - Eigen::Vector3d as_vector() const { return Eigen::Vector3d(x, y, z); } -}; - -struct CrossTrackError { - double x_e{}; - double y_e{}; - double z_e{}; - - inline static CrossTrackError from_vector(const Eigen::Vector3d& vector) { - return CrossTrackError{vector.x(), vector.y(), vector.z()}; - } -}; - -struct Params { - double lookahead_distance_h{}; - double lookahead_distance_v{}; - double gamma_h{}; - double gamma_v{}; - double time_step{}; -}; - -} // namespace LOS - -/** - * @brief Adaptive Line-of-Sight (LOS) guidance algorithm based on slide 113 - * in "Fossen 2024 Lecture on 2D and 3D path-following control". - */ -class AdaptiveLOSGuidance { - public: - explicit AdaptiveLOSGuidance(const LOS::Params& params); - ~AdaptiveLOSGuidance() = default; - - void update_angles(const LOS::Point& prev_point, - const LOS::Point& next_point); - - LOS::CrossTrackError calculate_crosstrack_error( - const LOS::Point& prev_point, - const LOS::Point& current_position) const; - - double calculate_psi_d(const double& y_e) const; - - double calculate_theta_d(const double& z_e) const; - - void update_adaptive_estimates( - const LOS::CrossTrackError& crosstrack_error); - - private: - LOS::Params params_; - Eigen::Matrix3d rotation_y_ = Eigen::Matrix3d::Zero(); - Eigen::Matrix3d rotation_z_ = Eigen::Matrix3d::Zero(); - double pi_h_{}; - double pi_v_{}; - double beta_c_hat_{}; - double alpha_c_hat_{}; -}; - -} // namespace vortex::guidance - -#endif // LOS_GUIDANCE__LOS_GUIDANCE_HPP_ diff --git a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp index 623dac5da..698507d3e 100644 --- a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp +++ b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp @@ -1,119 +1,252 @@ +/** + * @file los_guidance_ros.hpp + * @brief The LosGuidanceNode class initializes ROS interfaces, loads + * configuration parameters, and runs the LOS guidance node. + */ #ifndef LOS_GUIDANCE__LOS_GUIDANCE_ROS_HPP_ #define LOS_GUIDANCE__LOS_GUIDANCE_ROS_HPP_ +#include #include #include #include -#include -#include +#include #include #include +#include #include +#include #include #include -#include "los_guidance.hpp" +#include + +#include +#include +#include -namespace vortex::guidance { +#include "los_guidance/lib/adaptive_los.hpp" +#include "los_guidance/lib/integral_los.hpp" +#include "los_guidance/lib/proportional_los.hpp" +#include "los_guidance/lib/types.hpp" +#include "los_guidance/lib/vector_field_los.hpp" -class LOSGuidanceNode : public rclcpp::Node { +namespace vortex::guidance::los { + +/** + * @brief The LosGuidanceNode class initializes ROS interfaces, loads LOS + * guidance parameters, and manages path-following execution. + */ +class LosGuidanceNode : public rclcpp::Node { public: - LOSGuidanceNode(); + /** + * @brief Constructs a LosGuidanceNode object. + * @param options ROS node options used when creating the node. + */ + explicit LosGuidanceNode(const rclcpp::NodeOptions& options); private: - // @brief Set the subscribers and publishers + /** + * @brief Sets up the ROS subscribers and publishers used by the node. + */ void set_subscribers_and_publisher(); - // @brief Set the action server + /** + * @brief Sets up the LOS guidance action server. + */ void set_action_server(); - // @brief Set the adaptive LOS guidance parameters - void set_adaptive_los_guidance(); - - // @brief Callback for the waypoint topic - // @param msg The reference message + /** + * @brief Sets up the service server used for changing LOS guidance mode. + */ + void set_service_server(); + + /** + * @brief Initializes the adaptive LOS guidance module from configuration. + * @param config YAML configuration node containing adaptive LOS parameters. + */ + void set_adaptive_los_guidance(YAML::Node config); + + /** + * @brief Initializes the proportional LOS guidance module from + * configuration. + * @param config YAML configuration node containing proportional LOS + * parameters. + */ + void set_proportional_los_guidance(YAML::Node config); + + /** + * @brief Initializes the integral LOS guidance module from configuration. + * @param config YAML configuration node containing integral LOS parameters. + */ + void set_integral_los_guidance(YAML::Node config); + + /** + * @brief Initializes the vector field LOS guidance module from + * configuration. + * @param config YAML configuration node containing vector field LOS + * parameters. + */ + void set_vector_field_guidance(YAML::Node config); + + /** + * @brief Loads the LOS guidance YAML configuration file. + * @param yaml_file_path Path to the YAML configuration file. + * @return YAML::Node Parsed YAML configuration. + */ + YAML::Node get_los_config(std::string yaml_file_path); + + /** + * @brief Parses common guidance parameters shared by all LOS methods. + * @param common_config YAML node containing common guidance parameters. + */ + void parse_common_config(YAML::Node common_config); + + /** + * @brief Callback for receiving waypoint updates. + * @param msg Received waypoint message. + */ void waypoint_callback( const geometry_msgs::msg::PointStamped::SharedPtr msg); - // @brief Callback for the pose topic - // @param msg The pose message + /** + * @brief Callback for receiving pose updates. + * @param msg Received pose message. + */ void pose_callback( const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); - // @brief Handle the goal request - // @param uuid The goal UUID - // @param goal The goal message - // @return The goal response + /** + * @brief Callback for receiving odometry updates. + * @param msg Received odometry message. + */ + void odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg); + + /** + * @brief Handles an incoming LOS guidance action goal request. + * @param uuid Unique identifier for the received goal. + * @param goal Requested LOS guidance goal. + * @return rclcpp_action::GoalResponse Response indicating whether the goal + * is accepted. + */ rclcpp_action::GoalResponse handle_goal( const rclcpp_action::GoalUUID& uuid, std::shared_ptr goal); - // @brief Handle the cancel request - // @param goal_handle The goal handle - // @return The cancel response + /** + * @brief Handles cancellation of an active LOS guidance goal. + * @param goal_handle Handle to the goal being cancelled. + * @return rclcpp_action::CancelResponse Response indicating whether + * cancellation is accepted. + */ rclcpp_action::CancelResponse handle_cancel( const std::shared_ptr< rclcpp_action::ServerGoalHandle> goal_handle); - // @brief Handle the accepted request - // @param goal_handle The goal handle + /** + * @brief Handles an accepted LOS guidance goal. + * @param goal_handle Handle to the accepted goal. + */ void handle_accepted(const std::shared_ptr> goal_handle); - // @brief Execute the goal - // @param goal_handle The goal handle + /** + * @brief Executes the LOS guidance action. + * @param goal_handle Handle to the active LOS guidance goal. + */ void execute(const std::shared_ptr> goal_handle); - // @brief Fill the lost waypoints - // @param goal The goal message - void fill_los_waypoints( - const geometry_msgs::msg::PointStamped& los_waypoint); + /** + * @brief Service callback for changing the active LOS guidance method. + * @param request Service request containing the desired LOS mode. + * @param response Service response indicating whether the mode change + * succeeded. + */ + void set_los_mode( + const std::shared_ptr request, + std::shared_ptr response); + + /** + * @brief Publishes debug information about the current vehicle state. + * @param current_pose Current vehicle pose used for debug publishing. + */ + void publish_state_debug( + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr + current_pose); + + /** + * @brief Fills a LOS guidance reference message from computed outputs and + * inputs. + * @param output Calculated LOS guidance outputs. + * @param inputs Current LOS guidance inputs. + * @return vortex_msgs::msg::LOSGuidance Populated LOS guidance reference + * message. + */ + vortex_msgs::msg::LOSGuidance fill_los_reference(types::Outputs output); + + /** + * @brief Checks if the given LOS guidance goal is feasible based on the + * provided inputs. + * @param inputs Current LOS guidance inputs. + * @return true if the goal is feasible, false otherwise. + */ + bool is_goal_feasible( + const types::Inputs& inputs, + std::shared_ptr goal); + + /** + * @brief Checks if the LOS guidance goal has been missed based on the + * provided inputs. + * @param inputs Current LOS guidance inputs. + * @return true if the goal is missed, false otherwise. + */ + bool is_goal_missed(const types::Inputs& inputs); - vortex_msgs::msg::LOSGuidance fill_los_reference(); + bool has_active_segment_{false}; rclcpp_action::Server::SharedPtr action_server_; - + rclcpp::Service::SharedPtr los_mode_service_; rclcpp::Publisher::SharedPtr reference_pub_; - + rclcpp::Publisher::SharedPtr los_debug_pub_; + rclcpp::Publisher::SharedPtr + state_debug_pub_; rclcpp::Subscription::SharedPtr waypoint_sub_; - rclcpp::Subscription< geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_sub_; - + rclcpp::Subscription::SharedPtr odom_sub_; rclcpp::TimerBase::SharedPtr reference_pub_timer_; + rclcpp::CallbackGroup::SharedPtr cb_group_; std::chrono::milliseconds time_step_; - std::mutex mutex_; rclcpp_action::GoalUUID preempted_goal_id_; - std::shared_ptr< rclcpp_action::ServerGoalHandle> goal_handle_; - rclcpp::CallbackGroup::SharedPtr cb_group_; - - LOS::Point eta_; - - LOS::Point last_point_; - - LOS::Point next_point_; - - std::unique_ptr adaptive_los_guidance_; - - double yaw_d_{}; + types::Inputs path_inputs_{}; + double u_desired_{}; + double goal_reached_tol_{}; + double max_pitch_angle_{}; + types::ActiveLosMethod method_{}; - double pitch_d_{}; + double nearest_been_to_goal_{std::numeric_limits::max()}; + double time_since_nearest_goal_{}; + double missed_goal_distance_margin_{}; + double missed_goal_timeout_{}; - double u_desired_{}; + std::unique_ptr adaptive_los_{}; + std::unique_ptr integral_los_{}; + std::unique_ptr proportional_los_{}; + std::unique_ptr vector_field_los_{}; - double goal_reached_tol_{}; + nav_msgs::msg::Odometry::SharedPtr debug_current_odom_{}; }; -} // namespace vortex::guidance +} // namespace vortex::guidance::los #endif // LOS_GUIDANCE__LOS_GUIDANCE_ROS_HPP_ diff --git a/guidance/los_guidance/launch/guidance_test.launch.py b/guidance/los_guidance/launch/guidance_test.launch.py new file mode 100644 index 000000000..9ce95fb64 --- /dev/null +++ b/guidance/los_guidance/launch/guidance_test.launch.py @@ -0,0 +1,133 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + ExecuteProcess, + IncludeLaunchDescription, + OpaqueFunction, + TimerAction, +) +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration + +from auv_setup.launch_arg_common import ( + declare_drone_and_namespace_args, + resolve_drone_and_namespace, +) + + +def launch_setup(context, *args, **kwargs): + drone, namespace = resolve_drone_and_namespace(context) + test_scenario = LaunchConfiguration("test_scenario").perform(context) + use_keyboard_joy = LaunchConfiguration("use_keyboard_joy") + + stonefish_dir = get_package_share_directory("stonefish_sim") + los_guidance_dir = get_package_share_directory("los_guidance") + keyboard_joy_dir = get_package_share_directory("keyboard_joy") + velocity_controller_dir = get_package_share_directory("velocity_controller") + utility_dir = get_package_share_directory("vortex_utility_nodes") + + stonefish_sim = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(stonefish_dir, "launch", "simulation.launch.py") + ), + launch_arguments={ + "drone": drone, + "scenario": "nautilus_no_gpu", + "rendering": "true", + }.items(), + ) + + los_guidance_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(los_guidance_dir, "launch", "los_guidance.launch.py") + ), + launch_arguments={ + "drone": drone, + "namespace": namespace, + }.items(), + ) + + keyboard_joy = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(keyboard_joy_dir, "launch", "keyboard_joy_node.launch.py") + ), + condition=IfCondition(use_keyboard_joy), + ) + + velocity_controller_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join( + velocity_controller_dir, "launch", "velocity_controller.launch.py" + ) + ), + launch_arguments={ + "drone": drone, + "namespace": namespace, + }.items(), + ) + + drone_sim = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(stonefish_dir, "launch", "drone_sim.launch.py") + ), + launch_arguments={ + "drone": drone, + }.items(), + ) + + utility_node = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(utility_dir, "launch", "message_publisher.launch.py") + ) + ) + + run_test_scenario = TimerAction( + period=20.0, + actions=[ + ExecuteProcess( + cmd=[ + "python3", + os.path.join(los_guidance_dir, "scripts", "test_scenarios.py"), + "--ros-args", + "-p", + f"drone:={drone}", + "-p", + f"test_scenario:={test_scenario}", + ], + output="screen", + ) + ], + ) + + return [ + stonefish_sim, + keyboard_joy, + los_guidance_launch, + velocity_controller_launch, + drone_sim, + utility_node, + run_test_scenario, + ] + + +def generate_launch_description(): + return LaunchDescription( + declare_drone_and_namespace_args(default_drone="nautilus") + + [ + DeclareLaunchArgument( + "test_scenario", + default_value="4_corner", + description="Scenario to run: 4_corner, circle, test_pitch, opposite_point", + ), + DeclareLaunchArgument( + "use_keyboard_joy", + default_value="true", + description="Launch keyboard joy node", + ), + OpaqueFunction(function=launch_setup), + ] + ) diff --git a/guidance/los_guidance/launch/los_guidance.launch.py b/guidance/los_guidance/launch/los_guidance.launch.py index fb1749a3f..6ffefc6df 100644 --- a/guidance/los_guidance/launch/los_guidance.launch.py +++ b/guidance/los_guidance/launch/los_guidance.launch.py @@ -33,7 +33,13 @@ def launch_setup(context, *args, **kwargs): executable="los_guidance_node", name="los_guidance_node", namespace=namespace, - parameters=[drone_params, los_config], + parameters=[ + drone_params, + { + "los_config_file": los_config, + "time_step": 0.1, + }, + ], output="screen", ) ] diff --git a/guidance/los_guidance/package.xml b/guidance/los_guidance/package.xml index 5ff6218fe..450afeae7 100644 --- a/guidance/los_guidance/package.xml +++ b/guidance/los_guidance/package.xml @@ -11,11 +11,13 @@ rclcpp rclcpp_action + rclcpp_components geometry_msgs vortex_msgs + nav_msgs vortex_utils_ros eigen - + yaml-cpp ament_cmake diff --git a/guidance/los_guidance/scripts/test_scenarios.py b/guidance/los_guidance/scripts/test_scenarios.py new file mode 100644 index 000000000..9b8a82a8c --- /dev/null +++ b/guidance/los_guidance/scripts/test_scenarios.py @@ -0,0 +1,150 @@ +import math + +import rclpy +from rclpy.action import ActionClient +from rclpy.node import Node +from std_msgs.msg import Header +from vortex_msgs.action import LOSGuidance + + +class WaypointTest(Node): + def __init__(self): + super().__init__("waypoint_test_client") + + self.declare_parameter("test_scenario", "4_corner") + self.declare_parameter("drone", "nautilus") + + self.test_scenario = self.get_parameter("test_scenario").value + self.drone = self.get_parameter("drone").value + + self._action_client = ActionClient( + self, + LOSGuidance, + f"/{self.drone}/los_guidance", + ) + + self.depth = 2.5 + self.square_size = 10.0 + + self.circle_radius = 8.0 + self.circle_points = 16 + self.circle_center_x = 0.0 + self.circle_center_y = 0.0 + + self.waypoints = self.generate_waypoints(self.test_scenario) + self.current_index = 0 + + self.get_logger().info(f"Starting test scenario: {self.test_scenario}") + self.get_logger().info(f"Using drone namespace: {self.drone}") + self.get_logger().info(f"Number of waypoints: {len(self.waypoints)}") + + self.send_next_goal() + + def generate_waypoints(self, test_scenario): + if test_scenario == "4_corner": + s = self.square_size + d = self.depth + return [ + (s, 0.0, d), + (s, s, d), + (0.0, s, d), + (0.0, 0.0, d), + ] + + elif test_scenario == "circle": + d = self.depth + waypoints = [] + + for i in range(self.circle_points): + theta = 2.0 * math.pi * i / self.circle_points + x = self.circle_center_x + self.circle_radius * math.cos(theta) + y = self.circle_center_y + self.circle_radius * math.sin(theta) + waypoints.append((x, y, d)) + + waypoints.append(waypoints[0]) + return waypoints + + elif test_scenario == "test_pitch": + # 0 = water surface, do not go above + # this test scenario has no seabed, so z can be however we need. + # Keep all depths safely between these + return [ + (3.0, 0.0, 1.0), # slight up + (6.0, 0.0, 2.0), # slight down + (9.0, 0.0, 1.0), # up again + (12.0, 0.0, 2.0), # down again + ] + + elif test_scenario == "opposite_point": + # Go to one point, then the exact opposite point + return [ + (6.0, 4.0, self.depth), + (-6.0, -4.0, self.depth), + ] + + else: + self.get_logger().warn( + f"Unknown test_scenario '{test_scenario}', defaulting to 4_corner" + ) + return self.generate_waypoints("4_corner") + + def send_next_goal(self): + if self.current_index >= len(self.waypoints): + self.get_logger().info(f"{self.test_scenario} test completed!") + rclpy.shutdown() + return + + self._action_client.wait_for_server() + + goal_msg = LOSGuidance.Goal() + + header = Header() + header.frame_id = "world_ned" + goal_msg.goal.header = header + + x, y, z = self.waypoints[self.current_index] + goal_msg.goal.point.x = float(x) + goal_msg.goal.point.y = float(y) + goal_msg.goal.point.z = float(z) + + self.get_logger().info( + f"Sending waypoint {self.current_index + 1}/{len(self.waypoints)}: " + f"x={x:.2f}, y={y:.2f}, z={z:.2f}" + ) + + self._send_goal_future = self._action_client.send_goal_async( + goal_msg, + feedback_callback=self.feedback_callback, + ) + self._send_goal_future.add_done_callback(self.goal_response_callback) + + def goal_response_callback(self, future): + goal_handle = future.result() + + if not goal_handle.accepted: + self.get_logger().error("Goal rejected") + rclpy.shutdown() + return + + self.get_logger().info("Goal accepted") + + self._get_result_future = goal_handle.get_result_async() + self._get_result_future.add_done_callback(self.result_callback) + + def result_callback(self, future): + self.get_logger().info("Waypoint reached") + self.current_index += 1 + self.send_next_goal() + + def feedback_callback(self, feedback_msg): + pass + + +def main(args=None): + rclpy.init(args=args) + node = WaypointTest() + rclpy.spin(node) + + +if __name__ == "__main__": + main() diff --git a/guidance/los_guidance/src/lib/adaptive_los.cpp b/guidance/los_guidance/src/lib/adaptive_los.cpp new file mode 100644 index 000000000..c3ad64fc5 --- /dev/null +++ b/guidance/los_guidance/src/lib/adaptive_los.cpp @@ -0,0 +1,85 @@ +#include + +#include "los_guidance/lib/types.hpp" + +namespace vortex::guidance::los { + +AdaptiveLOSGuidance::AdaptiveLOSGuidance(const AdaptiveLosParams& params) + : params_{params} { + if (params.lookahead_distance_h <= 0.0 || + params.lookahead_distance_v <= 0.0 || params.adaptation_gain_h <= 0.0 || + params.adaptation_gain_v <= 0.0 || params.time_step <= 0.0) { + throw std::invalid_argument( + "AdaptiveLOSGuidance: all params must be > 0"); + } +} + +void AdaptiveLOSGuidance::reset() { + beta_c_hat_ = 0.0; + alpha_c_hat_ = 0.0; +} + +void AdaptiveLOSGuidance::update_angles(const types::Inputs& inputs) { + const double dx = inputs.next_point.x - inputs.prev_point.x; + const double dy = inputs.next_point.y - inputs.prev_point.y; + const double dz = inputs.next_point.z - inputs.prev_point.z; + + path_heading_ = std::atan2(dy, dx); + path_pitch_ = std::atan2(-dz, std::sqrt(dx * dx + dy * dy)); + + rotation_y_ = Eigen::AngleAxisd(path_pitch_, Eigen::Vector3d::UnitY()); + rotation_z_ = Eigen::AngleAxisd(path_heading_, Eigen::Vector3d::UnitZ()); +} + +const types::CrossTrackError AdaptiveLOSGuidance::calculate_crosstrack_error( + const types::Inputs& inputs) { + const types::Point difference = inputs.current_position - inputs.prev_point; + const Eigen::Vector3d difference_vector = difference.as_vector(); + + const Eigen::Vector3d cross_track_error = + rotation_y_.transpose() * rotation_z_.transpose() * difference_vector; + + return types::CrossTrackError::from_vector(cross_track_error); +} + +void AdaptiveLOSGuidance::update_adaptive_estimates( + const types::CrossTrackError& cross_track_error) { + const double denom_h = + std::sqrt(params_.lookahead_distance_h * params_.lookahead_distance_h + + cross_track_error.y_e * cross_track_error.y_e); + const double denom_v = + std::sqrt(params_.lookahead_distance_v * params_.lookahead_distance_v + + cross_track_error.z_e * cross_track_error.z_e); + + const double beta_dot = params_.adaptation_gain_h * + (params_.lookahead_distance_h / denom_h) * + cross_track_error.y_e; + const double alpha_dot = params_.adaptation_gain_v * + (params_.lookahead_distance_v / denom_v) * + cross_track_error.z_e; + + beta_c_hat_ += beta_dot * params_.time_step; + alpha_c_hat_ += alpha_dot * params_.time_step; +} + +types::Outputs AdaptiveLOSGuidance::calculate_outputs( + const types::Inputs& inputs) { + update_angles(inputs); + + const types::CrossTrackError cross_track_error = + calculate_crosstrack_error(inputs); + + update_adaptive_estimates(cross_track_error); + + const double desired_yaw = + path_heading_ - beta_c_hat_ - + std::atan(cross_track_error.y_e / params_.lookahead_distance_h); + + const double desired_pitch = + path_pitch_ + alpha_c_hat_ + + std::atan(cross_track_error.z_e / params_.lookahead_distance_v); + + return types::Outputs{desired_yaw, desired_pitch}; +} + +} // namespace vortex::guidance::los diff --git a/guidance/los_guidance/src/lib/integral_los.cpp b/guidance/los_guidance/src/lib/integral_los.cpp new file mode 100644 index 000000000..9add664c5 --- /dev/null +++ b/guidance/los_guidance/src/lib/integral_los.cpp @@ -0,0 +1,64 @@ +#include + +namespace vortex::guidance::los { + +// Constructor +IntegralLOSGuidance::IntegralLOSGuidance(const IntegralLosParams& params) + : m_params(params) { + if (params.proportional_gain_h <= 0.0 || + params.proportional_gain_v <= 0.0 || params.integral_gain_h <= 0.0 || + params.integral_gain_v <= 0.0 || params.time_step <= 0.0) { + throw std::invalid_argument( + "IntegralLOSGuidance: all params must be > 0"); + } +} + +// Angle Update +void IntegralLOSGuidance::update_angles(const types::Inputs& inputs) { + const types::Point difference = inputs.next_point - inputs.prev_point; + + path_heading_ = std::atan2(difference.y, difference.x); + path_pitch_ = std::atan2( + -difference.z, + std::sqrt(difference.x * difference.x + difference.y * difference.y)); + + rotation_y_ = Eigen::AngleAxisd(path_pitch_, Eigen::Vector3d::UnitY()); + rotation_z_ = Eigen::AngleAxisd(path_heading_, Eigen::Vector3d::UnitZ()); +} + +// Cross-Track Error Calculation +types::CrossTrackError IntegralLOSGuidance::calculate_crosstrack_error( + const types::Inputs& inputs) { + const Eigen::Vector3d diff_vec = + (inputs.current_position - inputs.prev_point).as_vector(); + + const Eigen::Vector3d path_frame_error = + rotation_y_.toRotationMatrix().transpose() * + rotation_z_.toRotationMatrix().transpose() * diff_vec; + + return types::CrossTrackError::from_vector(path_frame_error); +} + +// Output Calculation +types::Outputs IntegralLOSGuidance::calculate_outputs( + const types::Inputs& inputs) { + update_angles(inputs); + + const types::CrossTrackError cross_track_error = + calculate_crosstrack_error(inputs); + + integrated_horizontal_error_ += cross_track_error.y_e * m_params.time_step; + integrated_vertical_error_ += cross_track_error.z_e * m_params.time_step; + + const double u_h = m_params.proportional_gain_h * cross_track_error.y_e + + m_params.integral_gain_h * integrated_horizontal_error_; + const double u_v = m_params.proportional_gain_v * cross_track_error.z_e + + m_params.integral_gain_v * integrated_vertical_error_; + + const double desired_yaw = path_heading_ - std::atan(u_h); + const double desired_pitch = path_pitch_ + std::atan(u_v); + + return types::Outputs{desired_yaw, desired_pitch}; +} + +} // namespace vortex::guidance::los diff --git a/guidance/los_guidance/src/lib/proportional_los.cpp b/guidance/los_guidance/src/lib/proportional_los.cpp new file mode 100644 index 000000000..4cb0a2386 --- /dev/null +++ b/guidance/los_guidance/src/lib/proportional_los.cpp @@ -0,0 +1,61 @@ +#include + +namespace vortex::guidance::los { + +// Constructor +ProportionalLOSGuidance::ProportionalLOSGuidance( + const ProportionalLosParams& params) + : m_params(params) { + if (params.lookahead_distance_h <= 0.0 || + params.lookahead_distance_v <= 0.0) { + throw std::invalid_argument( + "ProportionalLOSGuidance: all params must be > 0"); + } +} + +// Angle Update +void ProportionalLOSGuidance::update_angles(const types::Inputs& inputs) { + const types::Point difference = inputs.next_point - inputs.prev_point; + + path_heading_ = std::atan2(difference.y, difference.x); + path_pitch_ = std::atan2( + -difference.z, + std::sqrt(difference.x * difference.x + difference.y * difference.y)); + + rotation_y_ = Eigen::AngleAxisd(path_pitch_, Eigen::Vector3d::UnitY()); + rotation_z_ = Eigen::AngleAxisd(path_heading_, Eigen::Vector3d::UnitZ()); +} + +// Cross-Track Error Calculation +types::CrossTrackError ProportionalLOSGuidance::calculate_crosstrack_error( + const types::Inputs& inputs) const { + const Eigen::Vector3d diff_vec = + (inputs.current_position - inputs.prev_point).as_vector(); + + const Eigen::Vector3d path_frame_error = + rotation_y_.toRotationMatrix().transpose() * + rotation_z_.toRotationMatrix().transpose() * diff_vec; + + return types::CrossTrackError::from_vector(path_frame_error); +} + +// Output Calculation +types::Outputs ProportionalLOSGuidance::calculate_outputs( + const types::Inputs& inputs) { + update_angles(inputs); + + const types::CrossTrackError cross_track_error = + calculate_crosstrack_error(inputs); + + const double k_p_h = 1.0 / m_params.lookahead_distance_h; + const double k_p_v = 1.0 / m_params.lookahead_distance_v; + + const double desired_yaw = + path_heading_ - std::atan(k_p_h * cross_track_error.y_e); + const double desired_pitch = + path_pitch_ + std::atan(k_p_v * cross_track_error.z_e); + + return types::Outputs{desired_yaw, desired_pitch}; +} + +} // namespace vortex::guidance::los diff --git a/guidance/los_guidance/src/lib/vector_field_los.cpp b/guidance/los_guidance/src/lib/vector_field_los.cpp new file mode 100644 index 000000000..ca1e9d267 --- /dev/null +++ b/guidance/los_guidance/src/lib/vector_field_los.cpp @@ -0,0 +1,66 @@ +#include + +namespace vortex::guidance::los { + +// Constructor +VectorFieldLOSGuidance::VectorFieldLOSGuidance( + const VectorFieldLosParams& params) + : m_params(params) { + if (params.max_approach_angle_h <= 0.0 || + params.max_approach_angle_v <= 0.0 || + params.proportional_gain_h <= 0.0 || + params.proportional_gain_v <= 0.0 || params.time_step <= 0.0) { + throw std::invalid_argument( + "VectorFieldLOSGuidance: all params must be > 0"); + } +} + +// Angle Update +void VectorFieldLOSGuidance::update_angles(const types::Inputs& inputs) { + const types::Point difference = inputs.next_point - inputs.prev_point; + + path_heading_ = std::atan2(difference.y, difference.x); + path_pitch_ = std::atan2( + -difference.z, + std::sqrt(difference.x * difference.x + difference.y * difference.y)); + + rotation_y_ = Eigen::AngleAxisd(path_pitch_, Eigen::Vector3d::UnitY()); + rotation_z_ = Eigen::AngleAxisd(path_heading_, Eigen::Vector3d::UnitZ()); +} + +// Cross-Track Error Calculation +types::CrossTrackError VectorFieldLOSGuidance::calculate_crosstrack_error( + const types::Inputs& inputs) const { + const Eigen::Vector3d diff_vec = + (inputs.current_position - inputs.prev_point).as_vector(); + + const Eigen::Vector3d path_frame_error = + rotation_y_.toRotationMatrix().transpose() * + rotation_z_.toRotationMatrix().transpose() * diff_vec; + + return types::CrossTrackError::from_vector(path_frame_error); +} + +// Output Calculation +types::Outputs VectorFieldLOSGuidance::calculate_outputs( + const types::Inputs& inputs) { + update_angles(inputs); + + const types::CrossTrackError cross_track_error = + calculate_crosstrack_error(inputs); + + const double approach_h = + m_params.max_approach_angle_h * (2.0 / M_PI) * + std::atan(m_params.proportional_gain_h * cross_track_error.y_e); + + const double approach_v = + m_params.max_approach_angle_v * (2.0 / M_PI) * + std::atan(m_params.proportional_gain_v * cross_track_error.z_e); + + const double desired_yaw = path_heading_ - approach_h; + const double desired_pitch = path_pitch_ - approach_v; + + return types::Outputs{desired_yaw, desired_pitch}; +} + +} // namespace vortex::guidance::los diff --git a/guidance/los_guidance/src/los_guidance.cpp b/guidance/los_guidance/src/los_guidance.cpp deleted file mode 100644 index 3171d5442..000000000 --- a/guidance/los_guidance/src/los_guidance.cpp +++ /dev/null @@ -1,59 +0,0 @@ -#include "los_guidance/los_guidance.hpp" - -namespace vortex::guidance { - -AdaptiveLOSGuidance::AdaptiveLOSGuidance(const LOS::Params& params) - : params_(params) {} - -void AdaptiveLOSGuidance::update_angles(const LOS::Point& prev_point, - const LOS::Point& next_point) { - const LOS::Point difference = next_point - prev_point; - - pi_h_ = atan2(difference.y, difference.x); - pi_v_ = atan2(-difference.z, sqrt(difference.x * difference.x + - difference.y * difference.y)); - - rotation_y_ = Eigen::AngleAxisd(pi_v_, Eigen::Vector3d::UnitY()); - rotation_z_ = Eigen::AngleAxisd(pi_h_, Eigen::Vector3d::UnitZ()); -} - -LOS::CrossTrackError AdaptiveLOSGuidance::calculate_crosstrack_error( - const LOS::Point& prev_point, - const LOS::Point& current_position) const { - const LOS::Point difference = current_position - prev_point; - const Eigen::Vector3d difference_vector = difference.as_vector(); - - const Eigen::Vector3d cross_track_error = - rotation_y_.transpose() * rotation_z_.transpose() * difference_vector; - - return LOS::CrossTrackError::from_vector(cross_track_error); -} - -double AdaptiveLOSGuidance::calculate_psi_d(const double& y_e) const { - return pi_h_ - beta_c_hat_ - atan(y_e / params_.lookahead_distance_h); -} - -double AdaptiveLOSGuidance::calculate_theta_d(const double& z_e) const { - return pi_v_ + alpha_c_hat_ + atan(z_e / params_.lookahead_distance_v); -} - -void AdaptiveLOSGuidance::update_adaptive_estimates( - const LOS::CrossTrackError& crosstrack_error) { - double beta_c_hat_dot = - params_.gamma_h * - (params_.lookahead_distance_h / - sqrt(params_.lookahead_distance_h * params_.lookahead_distance_h + - crosstrack_error.y_e * crosstrack_error.y_e)) * - crosstrack_error.y_e; - double alpha_c_hat_dot = - params_.gamma_v * - (params_.lookahead_distance_v / - sqrt(params_.lookahead_distance_v * params_.lookahead_distance_v + - crosstrack_error.z_e * crosstrack_error.z_e)) * - crosstrack_error.z_e; - - beta_c_hat_ += beta_c_hat_dot * params_.time_step; - alpha_c_hat_ += alpha_c_hat_dot * params_.time_step; -} - -} // namespace vortex::guidance diff --git a/guidance/los_guidance/src/los_guidance_node.cpp b/guidance/los_guidance/src/los_guidance_node.cpp deleted file mode 100644 index 931a57ded..000000000 --- a/guidance/los_guidance/src/los_guidance_node.cpp +++ /dev/null @@ -1,14 +0,0 @@ -#include -#include "los_guidance/los_guidance_ros.hpp" - -int main(int argc, char** argv) { - rclcpp::init(argc, argv); - - auto node = std::make_shared(); - rclcpp::executors::MultiThreadedExecutor executor; - executor.add_node(node); - - executor.spin(); - - return 0; -} diff --git a/guidance/los_guidance/src/los_guidance_ros.cpp b/guidance/los_guidance/src/los_guidance_ros.cpp index dcb8d309d..9b7007a66 100644 --- a/guidance/los_guidance/src/los_guidance_ros.cpp +++ b/guidance/los_guidance/src/los_guidance_ros.cpp @@ -1,131 +1,276 @@ #include "los_guidance/los_guidance_ros.hpp" +#include #include +#include +#include #include -const auto start_message = R"( - _ ___ ____ ____ _ _ - | | / _ \/ ___| / ___|_ _(_) __| | __ _ _ __ ___ ___ - | | | | | \___ \ | | _| | | | |/ _` |/ _` | '_ \ / __/ _ \ - | |__| |_| |___) | | |_| | |_| | | (_| | (_| | | | | (_| __/ - |_____\___/|____/ \____|\__,_|_|\__,_|\__,_|_| |_|\___\___| +#include "los_guidance/lib/types.hpp" + +#ifdef NDEBUG +constexpr bool debug = false; +#else +constexpr bool debug = true; +#endif +const auto start_message = R"( +██╗ ██████╗ ███████╗ ██████╗ ██╗ ██╗██╗██████╗ █████╗ ███╗ ██╗ ██████╗███████╗ +██║ ██╔═══██╗██╔════╝ ██╔════╝ ██║ ██║██║██╔══██╗██╔══██╗████╗ ██║██╔════╝██╔════╝ +██║ ██║ ██║███████╗ ██║ ███╗██║ ██║██║██║ ██║███████║██╔██╗ ██║██║ █████╗ +██║ ██║ ██║╚════██║ ██║ ██║██║ ██║██║██║ ██║██╔══██║██║╚██╗██║██║ ██╔══╝ +███████╗╚██████╔╝███████║ ╚██████╔╝╚██████╔╝██║██████╔╝██║ ██║██║ ╚████║╚██████╗███████╗ +╚══════╝ ╚═════╝ ╚══════╝ ╚═════╝ ╚═════╝ ╚═╝╚═════╝ ╚═╝ ╚═╝╚═╝ ╚═══╝ ╚═════╝╚══════╝ )"; -namespace vortex::guidance { +namespace vortex::guidance::los { -LOSGuidanceNode::LOSGuidanceNode() : Node("los_guidance_node") { - time_step_ = std::chrono::milliseconds(10); +// Constructor +LosGuidanceNode::LosGuidanceNode(const rclcpp::NodeOptions& options) + : Node("los_guidance_node", options) { + double time_step_s = this->declare_parameter("time_step"); + time_step_ = std::chrono::milliseconds(static_cast( + time_step_s * 1000)); // Convert seconds to milliseconds - set_subscribers_and_publisher(); + const std::string yaml_path = + this->declare_parameter("los_config_file"); - set_action_server(); + YAML::Node config = get_los_config(yaml_path); - set_adaptive_los_guidance(); + parse_common_config(config["common"]); + set_subscribers_and_publisher(); + set_action_server(); + set_service_server(); + set_adaptive_los_guidance(config); + set_proportional_los_guidance(config); + set_integral_los_guidance(config); + set_vector_field_guidance(config); spdlog::info(start_message); } -void LOSGuidanceNode::set_subscribers_and_publisher() { +void LosGuidanceNode::set_subscribers_and_publisher() { this->declare_parameter("topics.pose"); this->declare_parameter("topics.guidance.los"); this->declare_parameter("topics.waypoint"); + this->declare_parameter("topics.odom"); std::string pose_topic = this->get_parameter("topics.pose").as_string(); std::string guidance_topic = this->get_parameter("topics.guidance.los").as_string(); std::string waypoint_topic = this->get_parameter("topics.waypoint").as_string(); + std::string odom_topic = this->get_parameter("topics.odom").as_string(); auto qos_sensor_data = vortex::utils::qos_profiles::sensor_data_profile(1); reference_pub_ = this->create_publisher( guidance_topic, qos_sensor_data); + state_debug_pub_ = this->create_publisher( + "state_debug", qos_sensor_data); + waypoint_sub_ = this->create_subscription( waypoint_topic, qos_sensor_data, - std::bind(&LOSGuidanceNode::waypoint_callback, this, + std::bind(&LosGuidanceNode::waypoint_callback, this, std::placeholders::_1)); pose_sub_ = this->create_subscription< geometry_msgs::msg::PoseWithCovarianceStamped>( pose_topic, qos_sensor_data, - std::bind(&LOSGuidanceNode::pose_callback, this, + std::bind(&LosGuidanceNode::pose_callback, this, + std::placeholders::_1)); + + odom_sub_ = this->create_subscription( + odom_topic, qos_sensor_data, + std::bind(&LosGuidanceNode::odom_callback, this, std::placeholders::_1)); } -void LOSGuidanceNode::set_action_server() { +void LosGuidanceNode::set_action_server() { this->declare_parameter("action_servers.los"); std::string action_server_name = this->get_parameter("action_servers.los").as_string(); + cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); action_server_ = rclcpp_action::create_server( this, action_server_name, - std::bind(&LOSGuidanceNode::handle_goal, this, + std::bind(&LosGuidanceNode::handle_goal, this, std::placeholders::_1, std::placeholders::_2), - std::bind(&LOSGuidanceNode::handle_cancel, this, + std::bind(&LosGuidanceNode::handle_cancel, this, std::placeholders::_1), - std::bind(&LOSGuidanceNode::handle_accepted, this, + std::bind(&LosGuidanceNode::handle_accepted, this, std::placeholders::_1), rcl_action_server_get_default_options(), cb_group_); } -void LOSGuidanceNode::set_adaptive_los_guidance() { - this->declare_parameter("los.lookahead_distance_h"); - this->declare_parameter("los.lookahead_distance_v"); - this->declare_parameter("los.gamma_h"); - this->declare_parameter("los.gamma_v"); - this->declare_parameter("los.time_step"); - this->declare_parameter("los.u_desired"); - this->declare_parameter("los.goal_reached_tol"); +void LosGuidanceNode::set_service_server() { + this->declare_parameter("services.los_mode", "set_los_mode"); + std::string service_name = + this->get_parameter("services.los_mode").as_string(); - LOS::Params params; - params.lookahead_distance_h = - this->get_parameter("los.lookahead_distance_h").as_double(); - params.lookahead_distance_v = - this->get_parameter("los.lookahead_distance_v").as_double(); - params.gamma_h = this->get_parameter("los.gamma_h").as_double(); - params.gamma_v = this->get_parameter("los.gamma_v").as_double(); - params.time_step = this->get_parameter("los.time_step").as_double(); + los_mode_service_ = this->create_service( + service_name, std::bind(&LosGuidanceNode::set_los_mode, this, + std::placeholders::_1, std::placeholders::_2)); +} + +void LosGuidanceNode::set_adaptive_los_guidance(YAML::Node config) { + auto adaptive_los_config = config["adaptive_los"]; + auto params = AdaptiveLosParams{}; + + try { + params.lookahead_distance_h = + adaptive_los_config["lookahead_distance_h"].as(); + params.lookahead_distance_v = + adaptive_los_config["lookahead_distance_v"].as(); + params.adaptation_gain_h = + adaptive_los_config["adaptation_gain_h"].as(); + params.adaptation_gain_v = + adaptive_los_config["adaptation_gain_v"].as(); + params.time_step = static_cast(time_step_.count()) / 1000.0; + + adaptive_los_ = std::make_unique(params); + } catch (const YAML::Exception& e) { + throw std::runtime_error( + std::string("Failed to load adaptive_los parameters: ") + e.what()); + } +} - adaptive_los_guidance_ = std::make_unique(params); +void LosGuidanceNode::set_proportional_los_guidance(YAML::Node config) { + auto proportional_los_config = config["prop_los"]; + auto params = ProportionalLosParams{}; + + try { + params.lookahead_distance_h = + proportional_los_config["lookahead_distance_h"].as(); + params.lookahead_distance_v = + proportional_los_config["lookahead_distance_v"].as(); + + proportional_los_ = std::make_unique(params); + } catch (const YAML::Exception& e) { + throw std::runtime_error( + std::string("Failed to load proportional_los parameters: ") + + e.what()); + } +} + +void LosGuidanceNode::set_integral_los_guidance(YAML::Node config) { + auto integral_los_config = config["integer_los"]; + auto params = IntegralLosParams{}; + + try { + params.proportional_gain_h = + integral_los_config["proportional_gain_h"].as(); + params.proportional_gain_v = + integral_los_config["proportional_gain_v"].as(); + params.integral_gain_h = + integral_los_config["integral_gain_h"].as(); + params.integral_gain_v = + integral_los_config["integral_gain_v"].as(); + params.time_step = static_cast(time_step_.count()) / 1000.0; + + integral_los_ = std::make_unique(params); + } catch (const YAML::Exception& e) { + throw std::runtime_error( + std::string("Failed to load integral_los parameters: ") + e.what()); + } +} + +void LosGuidanceNode::set_vector_field_guidance(YAML::Node config) { + auto vector_field_config = config["vector_field_los"]; + auto params = VectorFieldLosParams{}; + + try { + params.max_approach_angle_h = + vector_field_config["max_approach_angle_h"].as(); + params.max_approach_angle_v = + vector_field_config["max_approach_angle_v"].as(); + params.proportional_gain_h = + vector_field_config["proportional_gain_h"].as(); + params.proportional_gain_v = + vector_field_config["proportional_gain_v"].as(); + params.time_step = static_cast(time_step_.count()) / 1000.0; + + vector_field_los_ = std::make_unique(params); + } catch (const YAML::Exception& e) { + throw std::runtime_error( + std::string("Failed to load vector_field_los parameters: ") + + e.what()); + } } -void LOSGuidanceNode::waypoint_callback( - const geometry_msgs::msg::PointStamped::SharedPtr los_waypoint) { - fill_los_waypoints(*los_waypoint); - adaptive_los_guidance_->update_angles(last_point_, next_point_); - spdlog::info("Received waypoint"); +void LosGuidanceNode::waypoint_callback( + const geometry_msgs::msg::PointStamped::SharedPtr wp_msg) { + std::unique_lock lock(mutex_); + + const auto new_wp = types::Point::point_from_ros(wp_msg->point); + + if (!has_active_segment_) { + path_inputs_.prev_point = path_inputs_.current_position; + path_inputs_.next_point = new_wp; + has_active_segment_ = true; + } else { + path_inputs_.prev_point = path_inputs_.next_point; + path_inputs_.next_point = new_wp; + } + + lock.unlock(); + + spdlog::info("Received waypoint: ({}, {}, {})", new_wp.x, new_wp.y, + new_wp.z); } -void LOSGuidanceNode::pose_callback( +void LosGuidanceNode::pose_callback( const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr current_pose) { - eta_.x = current_pose->pose.pose.position.x; - eta_.y = current_pose->pose.pose.position.y; - eta_.z = current_pose->pose.pose.position.z; + std::unique_lock lock(mutex_); + path_inputs_.current_position = + types::Point::point_from_ros(current_pose->pose.pose.position); + lock.unlock(); } -rclcpp_action::GoalResponse LOSGuidanceNode::handle_goal( +void LosGuidanceNode::odom_callback( + const nav_msgs::msg::Odometry::SharedPtr msg) { + std::unique_lock lock(mutex_); + debug_current_odom_ = msg; + lock.unlock(); +} + +rclcpp_action::GoalResponse LosGuidanceNode::handle_goal( const rclcpp_action::GoalUUID&, std::shared_ptr goal) { - (void)goal; + types::Inputs inputs_copy; + { - std::lock_guard lock(mutex_); - if (goal_handle_) { - if (goal_handle_->is_active()) { - spdlog::info("Aborting current goal and accepting new goal"); - preempted_goal_id_ = goal_handle_->get_goal_id(); - } + std::unique_lock lock(mutex_); + inputs_copy = path_inputs_; + lock.unlock(); + } + + if (!is_goal_feasible(inputs_copy, goal)) { + RCLCPP_WARN(this->get_logger(), + "Rejected goal request: waypoint is not reachable with " + "current pitch limit"); + return rclcpp_action::GoalResponse::REJECT; + } + + { + std::unique_lock lock(mutex_); + if (goal_handle_ && goal_handle_->is_active()) { + RCLCPP_INFO(this->get_logger(), + "Aborting current goal and accepting new goal"); + preempted_goal_id_ = goal_handle_->get_goal_id(); } + lock.unlock(); } - spdlog::info("Accepted goal request"); + + RCLCPP_INFO(this->get_logger(), "Accepted goal request"); return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; } -rclcpp_action::CancelResponse LOSGuidanceNode::handle_cancel( +rclcpp_action::CancelResponse LosGuidanceNode::handle_cancel( const std::shared_ptr< rclcpp_action::ServerGoalHandle> goal_handle) { @@ -134,67 +279,157 @@ rclcpp_action::CancelResponse LOSGuidanceNode::handle_cancel( return rclcpp_action::CancelResponse::ACCEPT; } -void LOSGuidanceNode::handle_accepted( +void LosGuidanceNode::handle_accepted( const std::shared_ptr< rclcpp_action::ServerGoalHandle> goal_handle) { - execute(goal_handle); + std::thread{[this, goal_handle]() { execute(goal_handle); }}.detach(); } -void LOSGuidanceNode::fill_los_waypoints( - const geometry_msgs::msg::PointStamped& los_waypoint) { - last_point_.x = eta_.x; - last_point_.y = eta_.y; - last_point_.z = eta_.z; +void LosGuidanceNode::set_los_mode( + const std::shared_ptr request, + std::shared_ptr response) { + { + std::unique_lock lock(mutex_); + method_ = static_cast(request->mode); + lock.unlock(); + } - next_point_.x = los_waypoint.point.x; - next_point_.y = los_waypoint.point.y; - next_point_.z = los_waypoint.point.z; + spdlog::info("LOS mode set to {}", static_cast(request->mode)); + response->success = true; } -vortex_msgs::msg::LOSGuidance LOSGuidanceNode::fill_los_reference() { +vortex_msgs::msg::LOSGuidance LosGuidanceNode::fill_los_reference( + types::Outputs outputs) { vortex_msgs::msg::LOSGuidance reference_msg; - reference_msg.pitch = pitch_d_; - reference_msg.yaw = yaw_d_; - reference_msg.surge = u_desired_; + const double clamped_pitch = + std::clamp(outputs.theta_d, -max_pitch_angle_, max_pitch_angle_); + reference_msg.pitch = clamped_pitch; + reference_msg.yaw = outputs.psi_d; + reference_msg.surge = u_desired_; return reference_msg; } -void LOSGuidanceNode::execute( +bool LosGuidanceNode::is_goal_feasible( + const types::Inputs& inputs, + std::shared_ptr goal) { + const auto& current_position = inputs.current_position; + const auto& goal_point = goal->goal.point; + + const double dx = goal_point.x - current_position.x; + const double dy = goal_point.y - current_position.y; + const double dz = goal_point.z - current_position.z; + + const double horizontal_distance = std::sqrt(dx * dx + dy * dy); + const double required_pitch = std::atan2(-dz, horizontal_distance); + + return std::abs(required_pitch) <= max_pitch_angle_; +} + +bool LosGuidanceNode::is_goal_missed(const types::Inputs& inputs) { + const double distance_to_goal = + (inputs.current_position - inputs.next_point).as_vector().norm(); + + const double dt = static_cast(time_step_.count()) / 1000.0; + + if (distance_to_goal < nearest_been_to_goal_) { + nearest_been_to_goal_ = distance_to_goal; + time_since_nearest_goal_ = 0.0; + return false; + } + + if (distance_to_goal > + nearest_been_to_goal_ + missed_goal_distance_margin_) { + time_since_nearest_goal_ += dt; + } else { + time_since_nearest_goal_ = 0.0; + } + + return time_since_nearest_goal_ >= missed_goal_timeout_; +} + +YAML::Node LosGuidanceNode::get_los_config(std::string yaml_file_path) { + try { + YAML::Node config = YAML::LoadFile(yaml_file_path); + return config; + } catch (const YAML::Exception& e) { + throw std::runtime_error( + std::string("Failed to load LOS config file '") + yaml_file_path + + "': " + e.what()); + } +} + +void LosGuidanceNode::parse_common_config(YAML::Node common_config) { + try { + std::unique_lock lock(mutex_); + u_desired_ = common_config["u_desired"].as(); + max_pitch_angle_ = common_config["max_pitch_angle"].as(); + goal_reached_tol_ = common_config["goal_reached_tol"].as(); + missed_goal_timeout_ = + common_config["missed_goal_timeout"].as(); + missed_goal_distance_margin_ = + common_config["missed_goal_distance_margin"].as(); + + method_ = static_cast( + common_config["active_los_method"].as()); + + lock.unlock(); + } catch (const YAML::Exception& e) { + throw std::runtime_error( + std::string("Failed to load common parameters: ") + e.what()); + } +} + +void LosGuidanceNode::execute( const std::shared_ptr< rclcpp_action::ServerGoalHandle> goal_handle) { { - std::lock_guard lock(mutex_); + std::unique_lock lock(mutex_); this->goal_handle_ = goal_handle; + lock.unlock(); } - u_desired_ = this->get_parameter("los.u_desired").as_double(); - goal_reached_tol_ = this->get_parameter("los.goal_reached_tol").as_double(); - spdlog::info("Executing goal"); const geometry_msgs::msg::PointStamped los_waypoint = goal_handle->get_goal()->goal; - fill_los_waypoints(los_waypoint); + const auto new_wp = types::Point::point_from_ros(los_waypoint.point); + + { + std::unique_lock lock(mutex_); + if (!has_active_segment_) { + path_inputs_.prev_point = path_inputs_.current_position; + path_inputs_.next_point = new_wp; + has_active_segment_ = true; + } else { + path_inputs_.prev_point = path_inputs_.next_point; + path_inputs_.next_point = new_wp; + } + lock.unlock(); + } - adaptive_los_guidance_->update_angles(last_point_, next_point_); + adaptive_los_->reset(); auto result = std::make_shared(); + nearest_been_to_goal_ = std::numeric_limits::infinity(); + time_since_nearest_goal_ = 0.0; rclcpp::Rate loop_rate(1000.0 / time_step_.count()); while (rclcpp::ok()) { { - std::lock_guard lock(mutex_); + std::unique_lock lock(mutex_); if (goal_handle->get_goal_id() == preempted_goal_id_) { result->success = false; goal_handle->abort(result); return; } + lock.unlock(); } + if (goal_handle->is_canceling()) { result->success = false; goal_handle->canceled(result); @@ -202,33 +437,88 @@ void LOSGuidanceNode::execute( return; } - LOS::CrossTrackError errors = - adaptive_los_guidance_->calculate_crosstrack_error(last_point_, - eta_); + types::Inputs inputs_copy; + types::ActiveLosMethod method_copy; + nav_msgs::msg::Odometry::SharedPtr odom_copy; + double goal_reached_tol_copy; - yaw_d_ = adaptive_los_guidance_->calculate_psi_d(errors.y_e); - pitch_d_ = adaptive_los_guidance_->calculate_theta_d(errors.z_e); + { + std::unique_lock lock(mutex_); + inputs_copy = path_inputs_; + method_copy = method_; + odom_copy = debug_current_odom_; + goal_reached_tol_copy = goal_reached_tol_; + lock.unlock(); + } - adaptive_los_guidance_->update_adaptive_estimates(errors); + if (is_goal_missed(inputs_copy)) { + result->success = false; + goal_handle->abort(result); + spdlog::info("Aborting goal: waypoint missed"); + return; + } + + types::Outputs outputs; + + switch (method_copy) { + case types::ActiveLosMethod::ADAPTIVE: + outputs = adaptive_los_->calculate_outputs(inputs_copy); + break; + case types::ActiveLosMethod::PROPORTIONAL: + outputs = proportional_los_->calculate_outputs(inputs_copy); + break; + case types::ActiveLosMethod::INTEGRAL: + outputs = integral_los_->calculate_outputs(inputs_copy); + break; + case types::ActiveLosMethod::VECTOR_FIELD: + outputs = vector_field_los_->calculate_outputs(inputs_copy); + break; + default: + spdlog::error("Invalid LOS method selected"); + result->success = false; + goal_handle->abort(result); + return; + } + + auto reference_msg = std::make_unique( + fill_los_reference(outputs)); + + if ((inputs_copy.current_position - inputs_copy.next_point) + .as_vector() + .norm() < goal_reached_tol_copy) { + reference_msg->pitch = 0.0; + reference_msg->surge = 0.0; - if ((eta_ - next_point_).as_vector().norm() < goal_reached_tol_) { result->success = true; goal_handle->succeed(result); - u_desired_ = 0.0; - auto final_reference_msg = - std::make_unique( - fill_los_reference()); - reference_pub_->publish(std::move(final_reference_msg)); spdlog::info("Goal reached"); return; } - auto reference_msg = std::make_unique( - fill_los_reference()); reference_pub_->publish(std::move(reference_msg)); + if (debug && odom_copy) { + const auto& v = odom_copy->twist.twist.linear; + double surge = std::sqrt(v.x * v.x + v.y * v.y + v.z * v.z); + + vortex_msgs::msg::LOSGuidance state_debug_msg; + Eigen::Vector3d euler = vortex::utils::math::quat_to_euler( + Eigen::Quaterniond(odom_copy->pose.pose.orientation.w, + odom_copy->pose.pose.orientation.x, + odom_copy->pose.pose.orientation.y, + odom_copy->pose.pose.orientation.z)); + + state_debug_msg.pitch = euler.y(); + state_debug_msg.yaw = euler.z(); + state_debug_msg.surge = surge; + + state_debug_pub_->publish(state_debug_msg); + } + loop_rate.sleep(); } } -} // namespace vortex::guidance +} // namespace vortex::guidance::los + +RCLCPP_COMPONENTS_REGISTER_NODE(vortex::guidance::los::LosGuidanceNode) diff --git a/guidance/los_guidance/test/CMakeLists.txt b/guidance/los_guidance/test/CMakeLists.txt index 69c0dfa52..c06410f19 100644 --- a/guidance/los_guidance/test/CMakeLists.txt +++ b/guidance/los_guidance/test/CMakeLists.txt @@ -6,13 +6,22 @@ include(GoogleTest) set(TEST_BINARY_NAME ${PROJECT_NAME}_test) add_executable( ${TEST_BINARY_NAME} - test_los_guidance.cpp + test_main.cpp + adaptive_los_test.cpp + proportional_los_test.cpp + integral_los_test.cpp + vector_field_los_test.cpp + los_invalid_params_test.cpp + ) -target_link_libraries( - ${TEST_BINARY_NAME} +target_link_libraries(${TEST_BINARY_NAME} PRIVATE ${LIB_NAME} + yaml-cpp + Eigen3::Eigen + spdlog::spdlog + fmt::fmt GTest::GTest ) diff --git a/guidance/los_guidance/test/adaptive_los_test.cpp b/guidance/los_guidance/test/adaptive_los_test.cpp new file mode 100644 index 000000000..afd707604 --- /dev/null +++ b/guidance/los_guidance/test/adaptive_los_test.cpp @@ -0,0 +1,107 @@ +#include "los_guidance/lib/adaptive_los.hpp" +#include + +namespace vortex::guidance::los { + +class AdaptiveLosTest : public ::testing::Test { + protected: + AdaptiveLosTest() : los_{get_params()} {} + + AdaptiveLosParams get_params() { + AdaptiveLosParams p; + p.lookahead_distance_h = 0.9; + p.lookahead_distance_v = 1.4; + p.adaptation_gain_h = 0.03; + p.adaptation_gain_v = 0.02; + p.time_step = 0.01; + return p; + } + + AdaptiveLOSGuidance los_; + const double tol = 1e-9; +}; + +// Test commanded angles when drone is to the right of the track +TEST_F(AdaptiveLosTest, T01_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.5, 0.0}; + + const types::Outputs O = los_.calculate_outputs(inputs); + + // Heading cmd should be between -pi/2 and 0 + EXPECT_LT(O.psi_d, 0.0); + EXPECT_GT(O.psi_d, -1.57); + + // Pitch cmd should be zero + EXPECT_NEAR(O.theta_d, 0.0, tol); +} + +// Test commanded angles when drone is to the left of the track +TEST_F(AdaptiveLosTest, T02_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, -0.5, 0.0}; + + const types::Outputs O = los_.calculate_outputs(inputs); + + // Heading cmd should be between 0 and pi/2 + EXPECT_GT(O.psi_d, 0.0); + EXPECT_LT(O.psi_d, 1.57); + // Pitch cmd should be zero + EXPECT_NEAR(O.theta_d, 0.0, tol); +} + +// Test commanded angles when drone is under the track +TEST_F(AdaptiveLosTest, T03_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.0, 0.5}; + + const types::Outputs O = los_.calculate_outputs(inputs); + + // Heading cmd should be 0 + EXPECT_NEAR(O.psi_d, 0.0, tol); + // Pitch cmd should be between 0 and pi/2 + EXPECT_GT(O.theta_d, 0.0); + EXPECT_LT(O.theta_d, 1.57); +} + +// Test commanded angles when drone is above the track +TEST_F(AdaptiveLosTest, T04_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.0, -0.5}; + + const types::Outputs O = los_.calculate_outputs(inputs); + + // Heading cmd should be 0 + EXPECT_NEAR(O.psi_d, 0.0, tol); + // Pitch cmd should be between -pi/2 and 0 + EXPECT_LT(O.theta_d, 0.0); + EXPECT_GT(O.theta_d, -1.57); +} + +// Test commanded angles when drone is above and to the right of the track + +TEST_F(AdaptiveLosTest, T05_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.5, -0.5}; + + const types::Outputs O = los_.calculate_outputs(inputs); + + // Heading cmd should be between -pi/2 and 0 + EXPECT_LT(O.psi_d, 0.0); + EXPECT_GT(O.psi_d, -1.57); + // Pitch cmd should be between -pi/2 and 0 + EXPECT_LT(O.theta_d, 0.0); + EXPECT_GT(O.theta_d, -1.57); +} + +} // namespace vortex::guidance::los diff --git a/guidance/los_guidance/test/integral_los_test.cpp b/guidance/los_guidance/test/integral_los_test.cpp new file mode 100644 index 000000000..7e06559f4 --- /dev/null +++ b/guidance/los_guidance/test/integral_los_test.cpp @@ -0,0 +1,108 @@ +#include "los_guidance/lib/integral_los.hpp" +#include +#include "los_guidance/lib/adaptive_los.hpp" + +namespace vortex::guidance::los { + +class IntegralLosTest : public ::testing::Test { + protected: + IntegralLosTest() : Ilos_{get_params()} {} + + IntegralLosParams get_params() { + IntegralLosParams params; + params.integral_gain_h = 0.5; + params.integral_gain_v = 0.5; + params.proportional_gain_h = 0.1; + params.proportional_gain_v = 0.1; + params.time_step = 0.01; + return params; + } + + IntegralLOSGuidance Ilos_; + const double tol = 1e-9; +}; + +// Test commanded angles when drone is to the right of the track +TEST_F(IntegralLosTest, T01_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.5, 0.0}; + + const types::Outputs O = Ilos_.calculate_outputs(inputs); + + // Heading cmd should be between -pi/2 and 0 + EXPECT_LT(O.psi_d, 0.0); + EXPECT_GT(O.psi_d, -1.57); + + // Pitch cmd should be zero + EXPECT_NEAR(O.theta_d, 0.0, tol); +} + +// Test commanded angles when drone is to the left of the track +TEST_F(IntegralLosTest, T02_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, -0.5, 0.0}; + + const types::Outputs O = Ilos_.calculate_outputs(inputs); + + // Heading cmd should be between 0 and pi/2 + EXPECT_GT(O.psi_d, 0.0); + EXPECT_LT(O.psi_d, 1.57); + // Pitch cmd should be zero + EXPECT_NEAR(O.theta_d, 0.0, tol); +} + +// Test commanded angles when drone is under the track +TEST_F(IntegralLosTest, T03_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.0, 0.5}; + + const types::Outputs O = Ilos_.calculate_outputs(inputs); + + // Heading cmd should be 0 + EXPECT_NEAR(O.psi_d, 0.0, tol); + // Pitch cmd should be between 0 and pi/2 + EXPECT_GT(O.theta_d, 0.0); + EXPECT_LT(O.theta_d, 1.57); +} + +// Test commanded angles when drone is above the track +TEST_F(IntegralLosTest, T04_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.0, -0.5}; + + const types::Outputs O = Ilos_.calculate_outputs(inputs); + + // Heading cmd should be 0 + EXPECT_NEAR(O.psi_d, 0.0, tol); + // Pitch cmd should be between -pi/2 and 0 + EXPECT_LT(O.theta_d, 0.0); + EXPECT_GT(O.theta_d, -1.57); +} + +// Test commanded angles when drone is above and to the right of the track + +TEST_F(IntegralLosTest, T05_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.5, -0.5}; + + const types::Outputs O = Ilos_.calculate_outputs(inputs); + + // Heading cmd should be between -pi/2 and 0 + EXPECT_LT(O.psi_d, 0.0); + EXPECT_GT(O.psi_d, -1.57); + // Pitch cmd should be between -pi/2 and 0 + EXPECT_LT(O.theta_d, 0.0); + EXPECT_GT(O.theta_d, -1.57); +} + +} // namespace vortex::guidance::los diff --git a/guidance/los_guidance/test/los_invalid_params_test.cpp b/guidance/los_guidance/test/los_invalid_params_test.cpp new file mode 100644 index 000000000..4473c1c66 --- /dev/null +++ b/guidance/los_guidance/test/los_invalid_params_test.cpp @@ -0,0 +1,56 @@ +#include +#include + +#include "los_guidance/lib/adaptive_los.hpp" +#include "los_guidance/lib/integral_los.hpp" +#include "los_guidance/lib/proportional_los.hpp" +#include "los_guidance/lib/vector_field_los.hpp" + +namespace vortex::guidance::los { + +TEST(LosInvalidParamsTest, AdaptiveLosRejectsNegativeLookaheadDistance) { + AdaptiveLosParams params; + params.lookahead_distance_h = -0.9; + params.lookahead_distance_v = 1.4; + params.adaptation_gain_h = 0.03; + params.adaptation_gain_v = 0.02; + params.time_step = 0.01; + + EXPECT_THROW( + { AdaptiveLOSGuidance guidance(params); }, std::invalid_argument); +} + +TEST(LosInvalidParamsTest, ProportionalLosRejectsZeroLookaheadDistance) { + ProportionalLosParams params; + params.lookahead_distance_h = 0.0; + params.lookahead_distance_v = 0.8; + + EXPECT_THROW( + { ProportionalLOSGuidance guidance(params); }, std::invalid_argument); +} + +TEST(LosInvalidParamsTest, IntegralLosRejectsZeroTimeStep) { + IntegralLosParams params; + params.proportional_gain_h = 0.5; + params.proportional_gain_v = 0.5; + params.integral_gain_h = 0.1; + params.integral_gain_v = 0.1; + params.time_step = 0.0; + + EXPECT_THROW( + { IntegralLOSGuidance guidance(params); }, std::invalid_argument); +} + +TEST(LosInvalidParamsTest, VectorFieldLosRejectsNegativeApproachAngle) { + VectorFieldLosParams params; + params.max_approach_angle_h = -1.0; + params.max_approach_angle_v = 1.0; + params.proportional_gain_h = 1.5; + params.proportional_gain_v = 0.9; + params.time_step = 0.01; + + EXPECT_THROW( + { VectorFieldLOSGuidance guidance(params); }, std::invalid_argument); +} + +} // namespace vortex::guidance::los diff --git a/guidance/los_guidance/test/proportional_los_test.cpp b/guidance/los_guidance/test/proportional_los_test.cpp new file mode 100644 index 000000000..311d7d23a --- /dev/null +++ b/guidance/los_guidance/test/proportional_los_test.cpp @@ -0,0 +1,104 @@ +#include "los_guidance/lib/proportional_los.hpp" +#include + +namespace vortex::guidance::los { + +class ProportionalLosTest : public ::testing::Test { + protected: + ProportionalLosTest() : Plos_{get_params()} {} + + ProportionalLosParams get_params() { + ProportionalLosParams params; + params.lookahead_distance_h = 0.74; + params.lookahead_distance_v = 0.8; + return params; + } + + ProportionalLOSGuidance Plos_; + const double tol = 1e-9; +}; + +// Test commanded angles when drone is to the right of the track +TEST_F(ProportionalLosTest, T01_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.5, 0.0}; + + const types::Outputs O = Plos_.calculate_outputs(inputs); + + // Heading cmd should be between -pi/2 and 0 + EXPECT_LT(O.psi_d, 0.0); + EXPECT_GT(O.psi_d, -1.57); + + // Pitch cmd should be zero + EXPECT_NEAR(O.theta_d, 0.0, tol); +} + +// Test commanded angles when drone is to the left of the track +TEST_F(ProportionalLosTest, T02_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, -0.5, 0.0}; + + const types::Outputs O = Plos_.calculate_outputs(inputs); + + // Heading cmd should be between 0 and pi/2 + EXPECT_GT(O.psi_d, 0.0); + EXPECT_LT(O.psi_d, 1.57); + // Pitch cmd should be zero + EXPECT_NEAR(O.theta_d, 0.0, tol); +} + +// Test commanded angles when drone is under the track +TEST_F(ProportionalLosTest, T03_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.0, 0.5}; + + const types::Outputs O = Plos_.calculate_outputs(inputs); + + // Heading cmd should be 0 + EXPECT_NEAR(O.psi_d, 0.0, tol); + // Pitch cmd should be between 0 and pi/2 + EXPECT_GT(O.theta_d, 0.0); + EXPECT_LT(O.theta_d, 1.57); +} + +// Test commanded angles when drone is above the track +TEST_F(ProportionalLosTest, T04_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.0, -0.5}; + + const types::Outputs O = Plos_.calculate_outputs(inputs); + + // Heading cmd should be 0 + EXPECT_NEAR(O.psi_d, 0.0, tol); + // Pitch cmd should be between -pi/2 and 0 + EXPECT_LT(O.theta_d, 0.0); + EXPECT_GT(O.theta_d, -1.57); +} + +// Test commanded angles when drone is above and to the right of the track + +TEST_F(ProportionalLosTest, T05_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.5, -0.5}; + + const types::Outputs O = Plos_.calculate_outputs(inputs); + + // Heading cmd should be between -pi/2 and 0 + EXPECT_LT(O.psi_d, 0.0); + EXPECT_GT(O.psi_d, -1.57); + // Pitch cmd should be between -pi/2 and 0 + EXPECT_LT(O.theta_d, 0.0); + EXPECT_GT(O.theta_d, -1.57); +} + +} // namespace vortex::guidance::los diff --git a/guidance/los_guidance/test/test_los_guidance.cpp b/guidance/los_guidance/test/test_los_guidance.cpp deleted file mode 100644 index 78b3d4fe1..000000000 --- a/guidance/los_guidance/test/test_los_guidance.cpp +++ /dev/null @@ -1,182 +0,0 @@ -#include - -#include "los_guidance/los_guidance.hpp" - -namespace vortex::guidance { - -class LOSTests : public ::testing::Test { - protected: - LOSTests() : los_guidance_{get_los_params()} {} - - LOS::Params get_los_params() { - LOS::Params params; - params.lookahead_distance_h = 1.0; - params.lookahead_distance_v = 1.0; - params.gamma_h = 1.0; - params.gamma_v = 1.0; - params.time_step = 0.01; - return params; - } - - AdaptiveLOSGuidance los_guidance_; - const double tol = 1e-9; -}; - -TEST_F(LOSTests, T01_test_cross_track_error_on_track) { - LOS::Point prev{0.0, 0.0, 0.0}; - LOS::Point next{1.0, 0.0, 0.0}; - los_guidance_.update_angles(prev, next); - LOS::Point current{0.0, 0.0, 0.0}; - LOS::CrossTrackError e = - los_guidance_.calculate_crosstrack_error(prev, current); - - EXPECT_NEAR(e.x_e, 0.0, tol); - EXPECT_NEAR(e.y_e, 0.0, tol); - EXPECT_NEAR(e.z_e, 0.0, tol); -} - -TEST_F(LOSTests, T02_test_cross_track_error_right_off_track) { - LOS::Point prev{0.0, 0.0, 0.0}; - LOS::Point next{1.0, 0.0, 0.0}; - los_guidance_.update_angles(prev, next); - LOS::Point current{0.0, 0.5, 0.0}; - LOS::CrossTrackError e = - los_guidance_.calculate_crosstrack_error(prev, current); - - EXPECT_NEAR(e.x_e, 0.0, tol); - EXPECT_NEAR(e.y_e, 0.5, tol); - EXPECT_NEAR(e.z_e, 0.0, tol); -} - -TEST_F(LOSTests, T03_test_cross_track_error_left_off_track) { - LOS::Point prev{0.0, 0.0, 0.0}; - LOS::Point next{1.0, 0.0, 0.0}; - los_guidance_.update_angles(prev, next); - LOS::Point current{0.0, -0.5, 0.0}; - LOS::CrossTrackError e = - los_guidance_.calculate_crosstrack_error(prev, current); - - EXPECT_NEAR(e.x_e, 0.0, tol); - EXPECT_NEAR(e.y_e, -0.5, tol); - EXPECT_NEAR(e.z_e, 0.0, tol); -} - -TEST_F(LOSTests, T04_test_cross_track_error_under_track) { - LOS::Point prev{0.0, 0.0, 0.0}; - LOS::Point next{1.0, 0.0, 0.0}; - los_guidance_.update_angles(prev, next); - LOS::Point current{0.0, 0.0, 0.5}; - LOS::CrossTrackError e = - los_guidance_.calculate_crosstrack_error(prev, current); - - EXPECT_NEAR(e.x_e, 0.0, tol); - EXPECT_NEAR(e.y_e, 0.0, tol); - EXPECT_NEAR(e.z_e, 0.5, tol); -} - -TEST_F(LOSTests, T05_test_cross_track_error_over_track) { - LOS::Point prev{0.0, 0.0, 0.0}; - LOS::Point next{1.0, 0.0, 0.0}; - los_guidance_.update_angles(prev, next); - LOS::Point current{0.0, 0.0, -0.5}; - LOS::CrossTrackError e = - los_guidance_.calculate_crosstrack_error(prev, current); - - EXPECT_NEAR(e.x_e, 0.0, tol); - EXPECT_NEAR(e.y_e, 0.0, tol); - EXPECT_NEAR(e.z_e, -0.5, tol); -} - -// Test commanded angles when drone is to the right of the track -TEST_F(LOSTests, T06_test_commanded_angles) { - LOS::Point prev{0.0, 0.0, 0.0}; - LOS::Point next{1.0, 0.0, 0.0}; - los_guidance_.update_angles(prev, next); - LOS::Point current{0.0, 0.5, 0.0}; - LOS::CrossTrackError e = - los_guidance_.calculate_crosstrack_error(prev, current); - double psi_d{los_guidance_.calculate_psi_d(e.y_e)}; - double theta_d{los_guidance_.calculate_theta_d(e.z_e)}; - // Heading cmd should be between -pi/2 and 0 - EXPECT_LT(psi_d, 0.0); - EXPECT_GT(psi_d, -1.57); - // Pitch cmd should be zero - EXPECT_NEAR(theta_d, 0.0, tol); -} - -// Test commanded angles when drone is to the left of the track -TEST_F(LOSTests, T07_test_commanded_angles) { - LOS::Point prev{0.0, 0.0, 0.0}; - LOS::Point next{1.0, 0.0, 0.0}; - los_guidance_.update_angles(prev, next); - LOS::Point current{0.0, -0.5, 0.0}; - LOS::CrossTrackError e = - los_guidance_.calculate_crosstrack_error(prev, current); - double psi_d{los_guidance_.calculate_psi_d(e.y_e)}; - double theta_d{los_guidance_.calculate_theta_d(e.z_e)}; - // Heading cmd should be between 0 and pi/2 - EXPECT_GT(psi_d, 0.0); - EXPECT_LT(psi_d, 1.57); - // Pitch cmd should be zero - EXPECT_NEAR(theta_d, 0.0, tol); -} - -// Test commanded angles when drone is under the track -TEST_F(LOSTests, T08_test_commanded_angles) { - LOS::Point prev{0.0, 0.0, 0.0}; - LOS::Point next{1.0, 0.0, 0.0}; - los_guidance_.update_angles(prev, next); - LOS::Point current{0.0, 0.0, 0.5}; - LOS::CrossTrackError e = - los_guidance_.calculate_crosstrack_error(prev, current); - double psi_d{los_guidance_.calculate_psi_d(e.y_e)}; - double theta_d{los_guidance_.calculate_theta_d(e.z_e)}; - // Heading cmd should be 0 - EXPECT_NEAR(psi_d, 0.0, tol); - // Pitch cmd should be between 0 and pi/2 - EXPECT_GT(theta_d, 0.0); - EXPECT_LT(theta_d, 1.57); -} - -// Test commanded angles when drone is over the track -TEST_F(LOSTests, T09_test_commanded_angles) { - LOS::Point prev{0.0, 0.0, 0.0}; - LOS::Point next{1.0, 0.0, 0.0}; - los_guidance_.update_angles(prev, next); - LOS::Point current{0.0, 0.0, -0.5}; - LOS::CrossTrackError e = - los_guidance_.calculate_crosstrack_error(prev, current); - double psi_d{los_guidance_.calculate_psi_d(e.y_e)}; - double theta_d{los_guidance_.calculate_theta_d(e.z_e)}; - // Heading cmd should be 0 - EXPECT_NEAR(psi_d, 0.0, tol); - // Pitch cmd should be between -pi/2 and 0 - EXPECT_LT(theta_d, 0.0); - EXPECT_GT(theta_d, -1.57); -} - -// Test commanded angles when drone is over and to the right of the track -TEST_F(LOSTests, T10_test_commanded_angles) { - LOS::Point prev{0.0, 0.0, 0.0}; - LOS::Point next{1.0, 0.0, 0.0}; - los_guidance_.update_angles(prev, next); - LOS::Point current{0.0, 0.5, -0.5}; - LOS::CrossTrackError e = - los_guidance_.calculate_crosstrack_error(prev, current); - double psi_d{los_guidance_.calculate_psi_d(e.y_e)}; - double theta_d{los_guidance_.calculate_theta_d(e.z_e)}; - // Heading cmd should be between -pi/2 and 0 - EXPECT_LT(psi_d, 0.0); - EXPECT_GT(psi_d, -1.57); - // Pitch cmd should be between -pi/2 and 0 - EXPECT_LT(theta_d, 0.0); - EXPECT_GT(theta_d, -1.57); -} - -} // namespace vortex::guidance - -int main(int argc, char** argv) { - testing::InitGoogleTest(&argc, argv); - - return RUN_ALL_TESTS(); -} diff --git a/guidance/los_guidance/test/test_main.cpp b/guidance/los_guidance/test/test_main.cpp new file mode 100644 index 000000000..af89a87e6 --- /dev/null +++ b/guidance/los_guidance/test/test_main.cpp @@ -0,0 +1,8 @@ +// test/test_main.cpp +#include + +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + + return RUN_ALL_TESTS(); +} diff --git a/guidance/los_guidance/test/vector_field_los_test.cpp b/guidance/los_guidance/test/vector_field_los_test.cpp new file mode 100644 index 000000000..f5f121920 --- /dev/null +++ b/guidance/los_guidance/test/vector_field_los_test.cpp @@ -0,0 +1,111 @@ +#include "los_guidance/lib/vector_field_los.hpp" +#include +#include "los_guidance/lib/proportional_los.hpp" + +namespace vortex::guidance::los { + +class VectorFieldLosTest : public ::testing::Test { + protected: + VectorFieldLosTest() : Vflos_{get_params()} {} + + VectorFieldLosParams get_params() { + VectorFieldLosParams params; + params.max_approach_angle_h = 1.0; + params.max_approach_angle_v = 1.0; + params.proportional_gain_h = 1.5; + params.proportional_gain_v = 0.9; + params.time_step = 0.01; + return params; + } + + VectorFieldLOSGuidance Vflos_; + const double tol = 1e-9; +}; + +// Test commanded angles when drone is to the right of the track +TEST_F(VectorFieldLosTest, T01_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.5, 0.0}; + + const types::Outputs O = Vflos_.calculate_outputs(inputs); + + // Heading cmd should be between -pi/2 and 0 + EXPECT_LT(O.psi_d, 0.0); + EXPECT_GT(O.psi_d, -1.57); + + // Pitch cmd should be zero + EXPECT_NEAR(O.theta_d, 0.0, tol); +} + +// Test commanded angles when drone is to the left of the track +TEST_F(VectorFieldLosTest, T02_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, -0.5, 0.0}; + + const types::Outputs O = Vflos_.calculate_outputs(inputs); + + // Heading cmd should be between 0 and pi/2 + EXPECT_GT(O.psi_d, 0.0); + EXPECT_LT(O.psi_d, 1.57); + + // Pitch cmd should be zero + EXPECT_NEAR(O.theta_d, 0.0, tol); +} + +// Test commanded angles when drone is under the track +TEST_F(VectorFieldLosTest, T03_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.0, 0.5}; + + const types::Outputs O = Vflos_.calculate_outputs(inputs); + + // Heading cmd should be 0 + EXPECT_NEAR(O.psi_d, 0.0, tol); + + // Pitch cmd should be between 0 and pi/2 + EXPECT_LT(O.theta_d, 0.0); + EXPECT_GT(O.theta_d, -1.57); +} + +// Test commanded angles when drone is above the track +TEST_F(VectorFieldLosTest, T04_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.0, -0.5}; + + const types::Outputs O = Vflos_.calculate_outputs(inputs); + + // Heading cmd should be 0 + EXPECT_NEAR(O.psi_d, 0.0, tol); + + // Pitch cmd should be between -pi/2 and 0 + EXPECT_GT(O.theta_d, 0.0); + EXPECT_LT(O.theta_d, 1.57); +} + +// Test commanded angles when drone is above and to the right of the track +TEST_F(VectorFieldLosTest, T05_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.5, -0.5}; + + const types::Outputs O = Vflos_.calculate_outputs(inputs); + + // Heading cmd should be between -pi/2 and 0 + EXPECT_LT(O.psi_d, 0.0); + EXPECT_GT(O.psi_d, -1.57); + + // Pitch cmd should be between -pi/2 and 0 + EXPECT_GT(O.theta_d, 0.0); + EXPECT_LT(O.theta_d, 1.57); +} + +} // namespace vortex::guidance::los diff --git a/guidance/reference_filter_dp/include/reference_filter_dp/lib/waypoint_follower.hpp b/guidance/reference_filter_dp/include/reference_filter_dp/lib/waypoint_follower.hpp index 121a9f431..826abdc25 100644 --- a/guidance/reference_filter_dp/include/reference_filter_dp/lib/waypoint_follower.hpp +++ b/guidance/reference_filter_dp/include/reference_filter_dp/lib/waypoint_follower.hpp @@ -30,7 +30,7 @@ class WaypointFollower { */ void start(const PoseEuler& pose, const Twist& twist, - const Waypoint& waypoint, + const WaypointEuler& waypoint, double convergence_threshold); /** diff --git a/guidance/reference_filter_dp/include/reference_filter_dp/lib/waypoint_types.hpp b/guidance/reference_filter_dp/include/reference_filter_dp/lib/waypoint_types.hpp index ed8e8e2db..556316764 100644 --- a/guidance/reference_filter_dp/include/reference_filter_dp/lib/waypoint_types.hpp +++ b/guidance/reference_filter_dp/include/reference_filter_dp/lib/waypoint_types.hpp @@ -1,30 +1,18 @@ #ifndef REFERENCE_FILTER_DP__LIB__WAYPOINT_TYPES_HPP_ #define REFERENCE_FILTER_DP__LIB__WAYPOINT_TYPES_HPP_ -#include #include namespace vortex::guidance { using vortex::utils::types::PoseEuler; - -/** - * @brief Determines which degrees of freedom the reference filter controls. - * - * The mode affects both the reference goal computation (via apply_mode_logic) - * and the convergence check (via has_converged). - */ -enum class WaypointMode : uint8_t { - FULL_POSE = 0, ///< Control all 6 DOF. - ONLY_POSITION = 1, ///< Control x, y, z; hold current orientation. - FORWARD_HEADING = 2, ///< Control x, y, z with yaw toward target. - ONLY_ORIENTATION = 3, ///< Control roll, pitch, yaw; hold current position. -}; +using vortex::utils::types::WaypointMode; /** * @brief A target pose with an associated waypoint mode. + * */ -struct Waypoint { +struct WaypointEuler { PoseEuler pose{}; WaypointMode mode = WaypointMode::FULL_POSE; }; diff --git a/guidance/reference_filter_dp/include/reference_filter_dp/ros/reference_filter_ros_utils.hpp b/guidance/reference_filter_dp/include/reference_filter_dp/ros/reference_filter_ros_utils.hpp index 35c8a9927..43d66ce56 100644 --- a/guidance/reference_filter_dp/include/reference_filter_dp/ros/reference_filter_ros_utils.hpp +++ b/guidance/reference_filter_dp/include/reference_filter_dp/ros/reference_filter_ros_utils.hpp @@ -35,9 +35,9 @@ inline WaypointMode waypoint_mode_from_ros( } /// @brief Convert a ROS Waypoint message to an internal Waypoint struct. -inline vortex::guidance::Waypoint waypoint_from_ros( +inline vortex::guidance::WaypointEuler waypoint_from_ros( const vortex_msgs::msg::Waypoint& ros_wp) { - Waypoint wp; + WaypointEuler wp; wp.pose = vortex::utils::ros_conversions::ros_pose_to_pose_euler(ros_wp.pose); wp.mode = waypoint_mode_from_ros(ros_wp.waypoint_mode); diff --git a/guidance/reference_filter_dp/src/lib/waypoint_follower.cpp b/guidance/reference_filter_dp/src/lib/waypoint_follower.cpp index 4da1d7c31..cdf8d7e64 100644 --- a/guidance/reference_filter_dp/src/lib/waypoint_follower.cpp +++ b/guidance/reference_filter_dp/src/lib/waypoint_follower.cpp @@ -11,7 +11,7 @@ WaypointFollower::WaypointFollower(const ReferenceFilterParams& params, void WaypointFollower::start(const PoseEuler& pose, const Twist& twist, - const Waypoint& waypoint, + const WaypointEuler& waypoint, double convergence_threshold) { std::lock_guard lock(mutex_); state_ = compute_initial_state(pose, twist); diff --git a/guidance/reference_filter_dp/src/lib/waypoint_utils.cpp b/guidance/reference_filter_dp/src/lib/waypoint_utils.cpp index fe72ebf13..6d72a27b5 100644 --- a/guidance/reference_filter_dp/src/lib/waypoint_utils.cpp +++ b/guidance/reference_filter_dp/src/lib/waypoint_utils.cpp @@ -1,9 +1,22 @@ #include "reference_filter_dp/lib/waypoint_utils.hpp" #include #include +#include +#include namespace vortex::guidance { +namespace { +using vortex::utils::types::PoseEuler; +using vortex::utils::types::Pose; + +Pose vec_to_pose(const Eigen::Vector6d& v) { + return PoseEuler{.x = v(0), .y = v(1), .z = v(2), + .roll = v(3), .pitch = v(4), .yaw = v(5)} + .as_pose(); +} +} // namespace + Eigen::Vector6d apply_mode_logic(const Eigen::Vector6d& reference_in, WaypointMode mode, const Eigen::Vector6d& current_state) { @@ -22,32 +35,13 @@ Eigen::Vector6d apply_mode_logic(const Eigen::Vector6d& reference_in, break; case WaypointMode::ONLY_POSITION: - reference_out(3) = current_state(3); - reference_out(4) = current_state(4); - reference_out(5) = current_state(5); - break; - - case WaypointMode::FORWARD_HEADING: { - double dx = reference_in(0) - current_state(0); - double dy = reference_in(1) - current_state(1); - double forward_heading = std::atan2(dy, dx); - - reference_out(3) = 0.0; - reference_out(4) = 0.0; - reference_out(5) = vortex::utils::math::ssa(forward_heading); - break; - } - + case WaypointMode::FORWARD_HEADING: case WaypointMode::ONLY_ORIENTATION: - reference_out(0) = current_state(0); - reference_out(1) = current_state(1); - reference_out(2) = current_state(2); - reference_out(3) = - current_state(3) + ssa(reference_in(3) - current_state(3)); - reference_out(4) = - current_state(4) + ssa(reference_in(4) - current_state(4)); - reference_out(5) = - current_state(5) + ssa(reference_in(5) - current_state(5)); + reference_out = vortex::utils::waypoints::compute_waypoint_goal( + vec_to_pose(reference_in), mode, + vec_to_pose(current_state)) + .as_pose_euler() + .to_vector(); break; } @@ -58,29 +52,9 @@ bool has_converged(const Eigen::Vector6d& measured_pose, const Eigen::Vector6d& reference, WaypointMode mode, double convergence_threshold) { - using vortex::utils::math::ssa; - const Eigen::Vector3d ep = measured_pose.head<3>() - reference.head<3>(); - - Eigen::Vector3d ea; - ea(0) = ssa(measured_pose(3) - reference(3)); - ea(1) = ssa(measured_pose(4) - reference(4)); - ea(2) = ssa(measured_pose(5) - reference(5)); - - const double err = [&] { - switch (mode) { - case WaypointMode::ONLY_POSITION: - return ep.norm(); - case WaypointMode::ONLY_ORIENTATION: - return ea.norm(); - case WaypointMode::FORWARD_HEADING: - return std::sqrt(ep.squaredNorm() + ea(2) * ea(2)); - case WaypointMode::FULL_POSE: - default: - return std::sqrt(ep.squaredNorm() + ea.squaredNorm()); - } - }(); - - return err < convergence_threshold; + return vortex::utils::waypoints::has_converged( + vec_to_pose(measured_pose), vec_to_pose(reference), mode, + convergence_threshold); } } // namespace vortex::guidance diff --git a/guidance/reference_filter_dp/src/ros/reference_filter_ros.cpp b/guidance/reference_filter_dp/src/ros/reference_filter_ros.cpp index 152a59aab..6a5aa2182 100644 --- a/guidance/reference_filter_dp/src/ros/reference_filter_ros.cpp +++ b/guidance/reference_filter_dp/src/ros/reference_filter_ros.cpp @@ -42,13 +42,13 @@ ReferenceFilterNode::~ReferenceFilterNode() { void ReferenceFilterNode::set_subscribers_and_publisher() { this->declare_parameter("topics.pose"); this->declare_parameter("topics.twist"); - this->declare_parameter("topics.guidance.dp"); + this->declare_parameter("topics.guidance.dp_rpy"); this->declare_parameter("topics.reference_pose"); std::string pose_topic = this->get_parameter("topics.pose").as_string(); std::string twist_topic = this->get_parameter("topics.twist").as_string(); std::string guidance_topic = - this->get_parameter("topics.guidance.dp").as_string(); + this->get_parameter("topics.guidance.dp_rpy").as_string(); std::string reference_pose_topic = this->get_parameter("topics.reference_pose").as_string(); @@ -215,10 +215,9 @@ void ReferenceFilterNode::execute( return; } - auto reference_msg = - std::make_unique( - fill_reference_msg(filter_state)); - reference_pub_->publish(std::move(reference_msg)); + vortex_msgs::msg::ReferenceFilter reference_msg = + fill_reference_msg(filter_state); + reference_pub_->publish(reference_msg); loop_rate.sleep(); } if (!rclcpp::ok() && goal_handle->is_active()) { diff --git a/guidance/reference_filter_dp/test/CMakeLists.txt b/guidance/reference_filter_dp/test/CMakeLists.txt index 2b8229c0c..9295a4300 100644 --- a/guidance/reference_filter_dp/test/CMakeLists.txt +++ b/guidance/reference_filter_dp/test/CMakeLists.txt @@ -20,24 +20,6 @@ ament_target_dependencies(${TEST_BINARY_NAME} PUBLIC Eigen3) gtest_discover_tests(${TEST_BINARY_NAME}) -# Waypoint utils tests -set(TEST_WAYPOINT_UTILS ${PROJECT_NAME}_test_waypoint_utils) -add_executable( - ${TEST_WAYPOINT_UTILS} - test_waypoint_utils.cpp -) - -target_link_libraries( - ${TEST_WAYPOINT_UTILS} - PRIVATE - ${CORE_LIB_NAME} - GTest::GTest -) - -ament_target_dependencies(${TEST_WAYPOINT_UTILS} PUBLIC Eigen3 vortex_utils) - -gtest_discover_tests(${TEST_WAYPOINT_UTILS}) - # Waypoint follower tests set(TEST_WAYPOINT_FOLLOWER ${PROJECT_NAME}_test_waypoint_follower) add_executable( diff --git a/guidance/reference_filter_dp/test/test_waypoint_follower.cpp b/guidance/reference_filter_dp/test/test_waypoint_follower.cpp index 06e063d0f..1c0c6b6db 100644 --- a/guidance/reference_filter_dp/test/test_waypoint_follower.cpp +++ b/guidance/reference_filter_dp/test/test_waypoint_follower.cpp @@ -20,7 +20,7 @@ class WaypointFollowerTests : public ::testing::Test { TEST_F(WaypointFollowerTests, StartAndStepConverges) { WaypointFollower follower(get_params(), 0.01); - Waypoint wp; + WaypointEuler wp; wp.pose = PoseEuler{1.0, 0.0, 0.0, 0.0, 0.0, 0.0}; wp.mode = WaypointMode::FULL_POSE; @@ -39,7 +39,7 @@ TEST_F(WaypointFollowerTests, StartAndStepConverges) { TEST_F(WaypointFollowerTests, StepDoesNotConvergeWhenFar) { WaypointFollower follower(get_params(), 0.01); - Waypoint wp; + WaypointEuler wp; wp.pose = PoseEuler{10.0, 10.0, 0.0, 0.0, 0.0, 0.0}; wp.mode = WaypointMode::FULL_POSE; @@ -54,7 +54,7 @@ TEST_F(WaypointFollowerTests, StepDoesNotConvergeWhenFar) { TEST_F(WaypointFollowerTests, SetReferenceUpdatesMidSequence) { WaypointFollower follower(get_params(), 0.01); - Waypoint wp; + WaypointEuler wp; wp.pose = PoseEuler{1.0, 0.0, 0.0, 0.0, 0.0, 0.0}; wp.mode = WaypointMode::FULL_POSE; @@ -70,7 +70,7 @@ TEST_F(WaypointFollowerTests, SetReferenceUpdatesMidSequence) { TEST_F(WaypointFollowerTests, SnapStateToReference) { WaypointFollower follower(get_params(), 0.01); - Waypoint wp; + WaypointEuler wp; wp.pose = PoseEuler{3.0, 4.0, 5.0, 0.1, 0.2, 0.3}; wp.mode = WaypointMode::FULL_POSE; @@ -88,7 +88,7 @@ TEST_F(WaypointFollowerTests, SnapStateToReference) { TEST_F(WaypointFollowerTests, StateEvolvesWithStep) { WaypointFollower follower(get_params(), 0.01); - Waypoint wp; + WaypointEuler wp; wp.pose = PoseEuler{1.0, 0.0, 0.0, 0.0, 0.0, 0.0}; wp.mode = WaypointMode::FULL_POSE; diff --git a/guidance/reference_filter_dp/test/test_waypoint_utils.cpp b/guidance/reference_filter_dp/test/test_waypoint_utils.cpp deleted file mode 100644 index 6e9de1b82..000000000 --- a/guidance/reference_filter_dp/test/test_waypoint_utils.cpp +++ /dev/null @@ -1,174 +0,0 @@ -#include -#include -#include "reference_filter_dp/lib/waypoint_utils.hpp" - -namespace vortex::guidance { - -// --- apply_mode_logic tests --- - -TEST(ApplyModeLogic, FullPoseWrapsAnglesToShortestPath) { - Eigen::Vector6d r_in; - r_in << 1.0, 2.0, 3.0, 0.1, 0.2, 0.3; - Eigen::Vector6d state = Eigen::Vector6d::Zero(); - - Eigen::Vector6d result = - apply_mode_logic(r_in, WaypointMode::FULL_POSE, state); - - // Position unchanged - EXPECT_DOUBLE_EQ(result(0), 1.0); - EXPECT_DOUBLE_EQ(result(1), 2.0); - EXPECT_DOUBLE_EQ(result(2), 3.0); - - // Small angles: wrapping has no effect - EXPECT_NEAR(result(3), 0.1, 1e-12); - EXPECT_NEAR(result(4), 0.2, 1e-12); - EXPECT_NEAR(result(5), 0.3, 1e-12); -} - -TEST(ApplyModeLogic, FullPoseTakesShortestRotation) { - // Current yaw ~170°, reference yaw ~-170° => only ~20° apart - // Without wrapping, the filter would see a ~340° difference - Eigen::Vector6d state; - state << 0.0, 0.0, 0.0, 0.0, 0.0, 2.96; // ~170° - Eigen::Vector6d r_in; - r_in << 0.0, 0.0, 0.0, 0.0, 0.0, -2.96; // ~-170° - - Eigen::Vector6d result = - apply_mode_logic(r_in, WaypointMode::FULL_POSE, state); - - // The wrapped reference should be close to the current state + short - // rotation - double yaw_error = result(5) - state(5); - EXPECT_NEAR(yaw_error, -2.96 - 2.96 + 2.0 * M_PI, 1e-12); - EXPECT_LT(std::abs(yaw_error), M_PI + 1e-9); -} - -TEST(ApplyModeLogic, OnlyPositionKeepsOrientationFromState) { - Eigen::Vector6d r_in; - r_in << 1.0, 2.0, 3.0, 0.1, 0.2, 0.3; - Eigen::Vector6d state; - state << 10.0, 20.0, 30.0, 0.4, 0.5, 0.6; - - Eigen::Vector6d result = - apply_mode_logic(r_in, WaypointMode::ONLY_POSITION, state); - - EXPECT_DOUBLE_EQ(result(0), 1.0); - EXPECT_DOUBLE_EQ(result(1), 2.0); - EXPECT_DOUBLE_EQ(result(2), 3.0); - EXPECT_DOUBLE_EQ(result(3), 0.4); - EXPECT_DOUBLE_EQ(result(4), 0.5); - EXPECT_DOUBLE_EQ(result(5), 0.6); -} - -TEST(ApplyModeLogic, ForwardHeadingComputesYawFromDelta) { - Eigen::Vector6d r_in; - r_in << 1.0, 1.0, 0.0, 0.0, 0.0, 0.0; - Eigen::Vector6d state = Eigen::Vector6d::Zero(); - - Eigen::Vector6d result = - apply_mode_logic(r_in, WaypointMode::FORWARD_HEADING, state); - - double expected_yaw = std::atan2(1.0, 1.0); // pi/4 - EXPECT_DOUBLE_EQ(result(0), 1.0); - EXPECT_DOUBLE_EQ(result(1), 1.0); - EXPECT_DOUBLE_EQ(result(3), 0.0); - EXPECT_DOUBLE_EQ(result(4), 0.0); - EXPECT_NEAR(result(5), expected_yaw, 1e-12); -} - -TEST(ApplyModeLogic, OnlyOrientationKeepsPositionFromState) { - Eigen::Vector6d r_in; - r_in << 1.0, 2.0, 3.0, 0.1, 0.2, 0.3; - Eigen::Vector6d state; - state << 10.0, 20.0, 30.0, 0.4, 0.5, 0.6; - - Eigen::Vector6d result = - apply_mode_logic(r_in, WaypointMode::ONLY_ORIENTATION, state); - - EXPECT_DOUBLE_EQ(result(0), 10.0); - EXPECT_DOUBLE_EQ(result(1), 20.0); - EXPECT_DOUBLE_EQ(result(2), 30.0); - - // Small angle differences: wrapping has no effect, result matches input - EXPECT_NEAR(result(3), 0.1, 1e-12); - EXPECT_NEAR(result(4), 0.2, 1e-12); - EXPECT_NEAR(result(5), 0.3, 1e-12); -} - -TEST(ApplyModeLogic, OnlyOrientationTakesShortestRotation) { - Eigen::Vector6d state; - state << 10.0, 20.0, 30.0, 0.0, 0.0, 2.96; - Eigen::Vector6d r_in; - r_in << 1.0, 2.0, 3.0, 0.0, 0.0, -2.96; - - Eigen::Vector6d result = - apply_mode_logic(r_in, WaypointMode::ONLY_ORIENTATION, state); - - // Position from state - EXPECT_DOUBLE_EQ(result(0), 10.0); - EXPECT_DOUBLE_EQ(result(1), 20.0); - EXPECT_DOUBLE_EQ(result(2), 30.0); - - // Yaw should take the shortest path - double yaw_error = result(5) - state(5); - EXPECT_LT(std::abs(yaw_error), M_PI + 1e-9); -} - -// --- has_converged tests --- - -TEST(HasConverged, FullPoseBelowThreshold) { - Eigen::Vector6d measured; - measured << 1.0, 2.0, 3.0, 0.1, 0.2, 0.3; - Eigen::Vector6d reference = measured; - reference(0) += 0.001; - - EXPECT_TRUE( - has_converged(measured, reference, WaypointMode::FULL_POSE, 0.1)); -} - -TEST(HasConverged, FullPoseAboveThreshold) { - Eigen::Vector6d measured = Eigen::Vector6d::Zero(); - Eigen::Vector6d reference; - reference << 1.0, 1.0, 1.0, 0.0, 0.0, 0.0; - - EXPECT_FALSE( - has_converged(measured, reference, WaypointMode::FULL_POSE, 0.1)); -} - -TEST(HasConverged, OnlyPositionIgnoresOrientation) { - Eigen::Vector6d measured; - measured << 1.0, 2.0, 3.0, 0.0, 0.0, 0.0; - Eigen::Vector6d reference; - reference << 1.0, 2.0, 3.0, 1.0, 1.0, 1.0; - - EXPECT_TRUE( - has_converged(measured, reference, WaypointMode::ONLY_POSITION, 0.1)); -} - -TEST(HasConverged, OnlyOrientationIgnoresPosition) { - Eigen::Vector6d measured; - measured << 100.0, 200.0, 300.0, 0.1, 0.2, 0.3; - Eigen::Vector6d reference; - reference << 0.0, 0.0, 0.0, 0.1, 0.2, 0.3; - - EXPECT_TRUE(has_converged(measured, reference, - WaypointMode::ONLY_ORIENTATION, 0.1)); -} - -TEST(HasConverged, ForwardHeadingUsesPositionAndYawOnly) { - Eigen::Vector6d measured; - measured << 1.0, 2.0, 3.0, 0.5, 0.5, 0.1; - Eigen::Vector6d reference; - reference << 1.0, 2.0, 3.0, 0.0, 0.0, 0.1; - - // Roll and pitch differ but should be ignored - EXPECT_TRUE( - has_converged(measured, reference, WaypointMode::FORWARD_HEADING, 0.1)); -} - -} // namespace vortex::guidance - -int main(int argc, char** argv) { - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/guidance/reference_filter_dp_quat/README.md b/guidance/reference_filter_dp_quat/README.md index 1573eb2e3..ada98da02 100644 --- a/guidance/reference_filter_dp_quat/README.md +++ b/guidance/reference_filter_dp_quat/README.md @@ -50,7 +50,7 @@ Each time step performs three operations: 1. **Compute reference error** — The 6D reference $r$ is the error between the waypoint goal and the nominal pose: - $r_{0:3} = p_{goal} - p_{nominal}$ - - $r_{3:6} = q_{\mathrm{err}}(q_{nominal}, q_{goal})$ + - $r_{3:6} = \text{quaternion\_error}(q_{nominal},\ q_{goal})$ The `quaternion_error` function returns $2 \cdot \text{vec}(q_{nominal}^{-1} \otimes q_{goal})$, which for small angles approximates the rotation vector from nominal to goal. diff --git a/guidance/reference_filter_dp_quat/config/reference_filter_params.yaml b/guidance/reference_filter_dp_quat/config/reference_filter_params.yaml index 1aef0ec13..71aad134a 100644 --- a/guidance/reference_filter_dp_quat/config/reference_filter_params.yaml +++ b/guidance/reference_filter_dp_quat/config/reference_filter_params.yaml @@ -3,3 +3,4 @@ zeta: [1., 1., 1., 1., 1., 1.] omega: [0.2, 0.2, 0.2, 0.2, 0.2, 0.2] time_step_ms: 10 + publish_rpy_debug: false diff --git a/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/ros/reference_filter_ros.hpp b/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/ros/reference_filter_ros.hpp index 87df8e7e6..458b9a383 100644 --- a/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/ros/reference_filter_ros.hpp +++ b/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/ros/reference_filter_ros.hpp @@ -9,7 +9,8 @@ #include #include #include -#include +#include +#include #include #include #include "reference_filter_dp_quat/lib/waypoint_follower.hpp" @@ -37,17 +38,17 @@ class ReferenceFilterNode : public rclcpp::Node { rclcpp_action::GoalResponse handle_goal( const rclcpp_action::GoalUUID& uuid, std::shared_ptr< - const vortex_msgs::action::ReferenceFilterQuatWaypoint::Goal> goal); + const vortex_msgs::action::ReferenceFilterWaypoint::Goal> goal); /// @brief Accept all cancel requests. rclcpp_action::CancelResponse handle_cancel( const std::shared_ptr> goal_handle); + vortex_msgs::action::ReferenceFilterWaypoint>> goal_handle); /// @brief Join the old execution thread and spawn a new one for the goal. void handle_accepted( const std::shared_ptr> goal_handle); + vortex_msgs::action::ReferenceFilterWaypoint>> goal_handle); /** * @brief Execute the action goal in a loop until convergence or @@ -56,9 +57,9 @@ class ReferenceFilterNode : public rclcpp::Node { */ void execute( const std::shared_ptr> goal_handle); + vortex_msgs::action::ReferenceFilterWaypoint>> goal_handle); - rclcpp_action::Server:: + rclcpp_action::Server:: SharedPtr action_server_; ReferenceFilterParams filter_params_; @@ -68,6 +69,11 @@ class ReferenceFilterNode : public rclcpp::Node { rclcpp::Publisher::SharedPtr reference_pub_; + rclcpp::Publisher::SharedPtr + rpy_debug_pub_; + + bool publish_rpy_debug_{false}; + rclcpp::Subscription::SharedPtr reference_sub_; diff --git a/guidance/reference_filter_dp_quat/src/lib/waypoint_follower.cpp b/guidance/reference_filter_dp_quat/src/lib/waypoint_follower.cpp index ce58bb3d3..c2766c66c 100644 --- a/guidance/reference_filter_dp_quat/src/lib/waypoint_follower.cpp +++ b/guidance/reference_filter_dp_quat/src/lib/waypoint_follower.cpp @@ -57,7 +57,14 @@ void WaypointFollower::inject_and_reset() { if (angle >= 1e-10) { Eigen::Quaterniond delta_quat( Eigen::AngleAxisd(angle, delta_orientation.normalized())); - nominal_pose_.set_ori(nominal_pose_.ori_quaternion() * delta_quat); + Eigen::Quaterniond q_new = nominal_pose_.ori_quaternion() * delta_quat; + // Enforce positive hemisphere to prevent sign flips in the published + // reference quaternion that would cause the downstream controller to + // see large spurious orientation errors. + if (q_new.w() < 0.0) { + q_new.coeffs() = -q_new.coeffs(); + } + nominal_pose_.set_ori(q_new); state_.segment<3>(3).setZero(); } } diff --git a/guidance/reference_filter_dp_quat/src/ros/reference_filter_ros.cpp b/guidance/reference_filter_dp_quat/src/ros/reference_filter_ros.cpp index a129d2aef..af789f4fd 100644 --- a/guidance/reference_filter_dp_quat/src/ros/reference_filter_ros.cpp +++ b/guidance/reference_filter_dp_quat/src/ros/reference_filter_ros.cpp @@ -57,6 +57,16 @@ void ReferenceFilterNode::set_subscribers_and_publisher() { this->create_publisher( guidance_topic, qos_sensor_data); + publish_rpy_debug_ = this->declare_parameter("publish_rpy_debug"); + if (publish_rpy_debug_) { + std::string rpy_topic = this->declare_parameter( + "topics.guidance.dp_rpy", guidance_topic + "_rpy"); + rpy_debug_pub_ = + this->create_publisher( + rpy_topic, qos_sensor_data); + spdlog::info("RPY debug publisher enabled on topic: {}", rpy_topic); + } + reference_sub_ = this->create_subscription( reference_pose_topic, qos_sensor_data, [this](const geometry_msgs::msg::PoseStamped::SharedPtr msg) { @@ -91,7 +101,7 @@ void ReferenceFilterNode::set_action_server() { this->get_parameter("action_servers.reference_filter").as_string(); action_server_ = rclcpp_action::create_server< - vortex_msgs::action::ReferenceFilterQuatWaypoint>( + vortex_msgs::action::ReferenceFilterWaypoint>( this, action_server_name, [this](const auto& uuid, auto goal) { return handle_goal(uuid, std::move(goal)); @@ -119,7 +129,7 @@ void ReferenceFilterNode::set_refererence_filter() { rclcpp_action::GoalResponse ReferenceFilterNode::handle_goal( const rclcpp_action::GoalUUID& /*uuid*/, std::shared_ptr< - const vortex_msgs::action::ReferenceFilterQuatWaypoint::Goal> + const vortex_msgs::action::ReferenceFilterWaypoint::Goal> /*goal*/) { spdlog::info("Accepted goal request"); return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; @@ -127,14 +137,14 @@ rclcpp_action::GoalResponse ReferenceFilterNode::handle_goal( rclcpp_action::CancelResponse ReferenceFilterNode::handle_cancel( const std::shared_ptr> /*goal_handle*/) { + vortex_msgs::action::ReferenceFilterWaypoint>> /*goal_handle*/) { spdlog::info("Received request to cancel goal"); return rclcpp_action::CancelResponse::ACCEPT; } void ReferenceFilterNode::handle_accepted( const std::shared_ptr> goal_handle) { + vortex_msgs::action::ReferenceFilterWaypoint>> goal_handle) { std::lock_guard lock(execute_mutex_); preempted_ = true; if (execute_thread_.joinable()) { @@ -148,7 +158,7 @@ void ReferenceFilterNode::handle_accepted( void ReferenceFilterNode::execute( const std::shared_ptr> goal_handle) { + vortex_msgs::action::ReferenceFilterWaypoint>> goal_handle) { spdlog::info("Executing goal"); double convergence_threshold = @@ -172,7 +182,7 @@ void ReferenceFilterNode::execute( follower_->start(pose, twist, wp, convergence_threshold); auto result = std::make_shared< - vortex_msgs::action::ReferenceFilterQuatWaypoint::Result>(); + vortex_msgs::action::ReferenceFilterWaypoint::Result>(); rclcpp::Rate loop_rate(1000.0 / time_step_.count()); @@ -207,6 +217,10 @@ void ReferenceFilterNode::execute( fill_reference_msg(follower_->pose(), follower_->velocity()); reference_pub_->publish(final_reference_msg); + if (rpy_debug_pub_) { + rpy_debug_pub_->publish(fill_reference_rpy_msg( + follower_->pose(), follower_->velocity())); + } result->success = true; goal_handle->succeed(result); @@ -217,11 +231,15 @@ void ReferenceFilterNode::execute( auto reference_msg = fill_reference_msg(follower_->pose(), follower_->velocity()); reference_pub_->publish(reference_msg); + if (rpy_debug_pub_) { + rpy_debug_pub_->publish(fill_reference_rpy_msg( + follower_->pose(), follower_->velocity())); + } loop_rate.sleep(); } if (!rclcpp::ok() && goal_handle->is_active()) { auto result = std::make_shared< - vortex_msgs::action::ReferenceFilterQuatWaypoint::Result>(); + vortex_msgs::action::ReferenceFilterWaypoint::Result>(); result->success = false; try { diff --git a/mission/joystick_interface_auv/README.md b/mission/joystick_interface_auv/README.md index 36ae93c7b..eb7993f28 100644 --- a/mission/joystick_interface_auv/README.md +++ b/mission/joystick_interface_auv/README.md @@ -1,2 +1,44 @@ ## Joystick interface -A joystick interface for manual control of AUV. A ROS2 node that subscribes on inputs from the XBOX controller and publishes the according wrench message to be used in Thruster Allocation. + +A joystick interface for manual control and reference sending of the AUV. Subscribes to Xbox controller inputs and publishes wrench commands (manual mode) or pose references (reference/guidance mode) depending on the active operation mode. + +### Launching + +```bash +ros2 launch joystick_interface_auv joystick_interface_auv.launch.py +``` + +#### Launch arguments + +| Argument | Default | Description | +|---|---|---| +| `drone` | `nautilus` | Drone model — loads the matching config from `auv_setup/config/robots/.yaml` | +| `namespace` | `` | ROS namespace. Defaults to the drone name if left empty | +| `orientation_mode` | `euler` | Reference orientation representation: `euler` (publishes `ReferenceFilter` with RPY angles) or `quat` (publishes `ReferenceFilterQuat` with quaternion) | + +The `orientation_mode` must match the reference filter used by the active DP controller. Use `euler` with the adaptive backstepping controller (`reference_filter_dp`) and `quat` with the PID controller (`reference_filter_dp_quat`). + +Example — launch with quaternion mode for use with the PID controller: + +```bash +ros2 launch joystick_interface_auv joystick_interface_auv.launch.py orientation_mode:=quat +``` + +### Controller button mapping + +| Button | Action | +|---|---| +| **A** | Manual mode (direct wrench from joystick axes) | +| **B** | Toggle software killswitch | +| **X** | Autonomous mode | +| **Y** | Reference mode (joystick incrementally updates the pose reference sent to the DP controller) | + +In **reference mode**, the left stick controls surge/sway, triggers control heave, the right stick controls pitch/yaw, and shoulder buttons control roll. Movement is expressed in the body frame and rotated into the world frame before being added to the desired pose. + +### Config + +Gains for both manual wrench output and reference increments are set in `config/param_joystick_interface_auv.yaml`: + +- `joystick_*_gain` — scales raw axis/button input to force/torque in manual mode +- `guidance_*_gain` — scales input to position/orientation increments in reference mode +- `debounce_duration` — minimum seconds between button state changes (prevents double-triggers) diff --git a/mission/joystick_interface_auv/config/param_joystick_interface_auv.yaml b/mission/joystick_interface_auv/config/param_joystick_interface_auv.yaml index 5876ce830..e4720276e 100644 --- a/mission/joystick_interface_auv/config/param_joystick_interface_auv.yaml +++ b/mission/joystick_interface_auv/config/param_joystick_interface_auv.yaml @@ -2,7 +2,7 @@ ros__parameters: joystick_surge_gain: 60.0 joystick_sway_gain: 60.0 - joystick_heave_gain: 17.5 + joystick_heave_gain: 60.0 joystick_roll_gain: 30.0 joystick_pitch_gain: 20.0 joystick_yaw_gain: 25.0 diff --git a/mission/joystick_interface_auv/joystick_interface_auv/joystick_interface_auv_node.py b/mission/joystick_interface_auv/joystick_interface_auv/joystick_interface_auv_node.py index d50f83355..6031e3bab 100755 --- a/mission/joystick_interface_auv/joystick_interface_auv/joystick_interface_auv_node.py +++ b/mission/joystick_interface_auv/joystick_interface_auv/joystick_interface_auv_node.py @@ -1,18 +1,19 @@ #!/usr/bin/env python3 +import numpy as np import rclpy from geometry_msgs.msg import PoseWithCovarianceStamped, WrenchStamped from rclpy.node import Node, Parameter from sensor_msgs.msg import JointState, Joy from std_msgs.msg import Bool -from vortex_msgs.msg import OperationMode, ReferenceFilter +from vortex_msgs.msg import OperationMode, ReferenceFilter, ReferenceFilterQuat from vortex_msgs.srv import GetOperationMode, SetOperationMode, ToggleKillswitch -from vortex_utils.python_utils import PoseData +from vortex_utils.python_utils import PoseData, euler_to_quat +from vortex_utils_ros.ros_converter import pose_from_ros from vortex_utils_ros.qos_profiles import ( reliable_profile, sensor_data_profile, ) -from vortex_utils_ros.ros_converter import pose_from_ros from joystick_interface_auv.joystick_utils import ( Wired, @@ -116,8 +117,20 @@ def get_parameters(self): self.get_parameter(f'services.{param}').value, ) - self.declare_parameter('topics.guidance.dp', Parameter.Type.STRING) - self.guidance_topic = self.get_parameter('topics.guidance.dp').value + self.declare_parameter('orientation_mode', 'euler') + self._orientation_mode = self.get_parameter('orientation_mode').value + if self._orientation_mode not in ('euler', 'quat'): + self.get_logger().warn( + f"Unknown orientation_mode '{self._orientation_mode}', defaulting to 'euler'" + ) + self._orientation_mode = 'euler' + + self.declare_parameter('topics.guidance.dp_rpy', Parameter.Type.STRING) + self.declare_parameter('topics.guidance.dp_quat', Parameter.Type.STRING) + if self._orientation_mode == 'quat': + self.guidance_topic = self.get_parameter('topics.guidance.dp_quat').value + else: + self.guidance_topic = self.get_parameter('topics.guidance.dp_rpy').value def init_movement(self): self.surge = 0.0 @@ -155,9 +168,14 @@ def set_publishers_and_subscribers(self): self._wrench_publisher = self.create_publisher( WrenchStamped, self.wrench_input_topic, qos_profile=best_effort_qos ) - self._ref_publisher = self.create_publisher( - ReferenceFilter, self.guidance_topic, qos_profile=best_effort_qos - ) + if self._orientation_mode == 'quat': + self._ref_publisher = self.create_publisher( + ReferenceFilterQuat, self.guidance_topic, qos_profile=best_effort_qos + ) + else: + self._ref_publisher = self.create_publisher( + ReferenceFilter, self.guidance_topic, qos_profile=best_effort_qos + ) self._gripper_publisher = self.create_publisher( JointState, self.gripper_servos_topic, qos_profile=best_effort_qos @@ -219,7 +237,8 @@ def create_reference_message(self) -> ReferenceFilter: """Creates a reference message with the desired state values.""" reference_msg = ReferenceFilter() reference_msg.header.stamp = self.get_clock().now().to_msg() - reference_msg.header.frame_id = "odom" + # reference_msg.header.frame_id = "odom" + reference_msg.header.frame_id = "base_link" reference_msg.x = self._desired_state.x reference_msg.y = self._desired_state.y reference_msg.z = self._desired_state.z @@ -228,6 +247,25 @@ def create_reference_message(self) -> ReferenceFilter: reference_msg.yaw = self._desired_state.yaw return reference_msg + def create_reference_quat_message(self) -> ReferenceFilterQuat: + """Creates a reference message with quaternion orientation from the desired state.""" + q = euler_to_quat( + self._desired_state.roll, + self._desired_state.pitch, + self._desired_state.yaw, + ) + reference_msg = ReferenceFilterQuat() + reference_msg.header.stamp = self.get_clock().now().to_msg() + reference_msg.header.frame_id = "base_link" + reference_msg.x = self._desired_state.x + reference_msg.y = self._desired_state.y + reference_msg.z = self._desired_state.z + reference_msg.qx = float(q[0]) + reference_msg.qy = float(q[1]) + reference_msg.qz = float(q[2]) + reference_msg.qw = float(q[3]) + return reference_msg + def create_wrench_message(self) -> WrenchStamped: """Creates a 3D wrench message with the given x, y, heave, roll, pitch, and yaw values. @@ -254,6 +292,12 @@ def transition_to_xbox_mode(self): future.add_done_callback(self.operation_mode_response_callback) self.get_logger().info("XBOX mode") + def _create_reference_msg(self): + """Returns the appropriate reference message based on orientation_mode.""" + if self._orientation_mode == 'quat': + return self.create_reference_quat_message() + return self.create_reference_message() + def transition_to_reference_mode(self): """Publishes a pose message and signals that the operational mode has switched to Reference mode.""" self._desired_state = PoseData( @@ -264,7 +308,7 @@ def transition_to_reference_mode(self): pitch=self._current_state.pitch, yaw=self._current_state.yaw, ) - reference_msg = self.create_reference_message() + reference_msg = self._create_reference_msg() # Still autonomous mode, but now the reference is being controlled by the joystick request = SetOperationMode.Request() @@ -390,10 +434,22 @@ def update_reference(self): The position and orientation (roll, pitch, yaw) are updated using the current joystick inputs scaled by their respective parameters. + The linear velocities (surge, sway, heave) are transformed from the + body frame to the world frame using the current orientation. """ - self._desired_state.x += self.surge * self._guidance_surge_gain - self._desired_state.y += self.sway * self._guidance_sway_gain - self._desired_state.z -= self.heave * self._guidance_heave_gain + surge_vector = self.surge * self._guidance_surge_gain + sway_vector = self.sway * self._guidance_sway_gain + heave_vector = -self.heave * self._guidance_heave_gain + + body_frame_vector = np.array([surge_vector, sway_vector, heave_vector]) + + rotation_matrix = self._desired_state.as_rotation_matrix() + world_frame_vector = rotation_matrix @ body_frame_vector + + self._desired_state.x += world_frame_vector[0] + self._desired_state.y += world_frame_vector[1] + self._desired_state.z += world_frame_vector[2] + self._desired_state.roll += self.roll * self._guidance_roll_gain self._desired_state.pitch += self.pitch * self._guidance_pitch_gain self._desired_state.yaw += self.yaw * self._guidance_yaw_gain @@ -458,7 +514,7 @@ def joystick_cb(self, msg: Joy): self._wrench_publisher.publish(wrench_msg) else: self.update_reference() - ref_msg = self.create_reference_message() + ref_msg = self._create_reference_msg() self._ref_publisher.publish(ref_msg) close = float(buttons.get("stick_button_left", 0)) diff --git a/mission/joystick_interface_auv/launch/joystick_interface_auv.launch.py b/mission/joystick_interface_auv/launch/joystick_interface_auv.launch.py index 1359a43fd..e5acfdf9c 100755 --- a/mission/joystick_interface_auv/launch/joystick_interface_auv.launch.py +++ b/mission/joystick_interface_auv/launch/joystick_interface_auv.launch.py @@ -2,7 +2,8 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import OpaqueFunction +from launch.actions import DeclareLaunchArgument, OpaqueFunction +from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node from auv_setup.launch_arg_common import ( @@ -14,6 +15,7 @@ def launch_setup(context, *args, **kwargs): """Set up the joystick_interface_auv node with drone-specific config.""" drone, namespace = resolve_drone_and_namespace(context) + orientation_mode = LaunchConfiguration("orientation_mode").perform(context) joystick_params = os.path.join( get_package_share_directory("joystick_interface_auv"), @@ -35,7 +37,11 @@ def launch_setup(context, *args, **kwargs): name="joystick_interface_auv", namespace=namespace, output="screen", - parameters=[joystick_params, drone_params, {"drone": drone}], + parameters=[ + joystick_params, + drone_params, + {"drone": drone, "orientation_mode": orientation_mode}, + ], ) ] @@ -53,5 +59,13 @@ def generate_launch_description() -> LaunchDescription: """ return LaunchDescription( - declare_drone_and_namespace_args() + [OpaqueFunction(function=launch_setup)] + declare_drone_and_namespace_args() + + [ + DeclareLaunchArgument( + "orientation_mode", + default_value="euler", + description="Reference orientation representation: 'euler' (ReferenceFilter) or 'quat' (ReferenceFilterQuat)", + ), + OpaqueFunction(function=launch_setup), + ] ) diff --git a/motion/thruster_interface_auv/config/thruster_interface_auv_config.yaml b/motion/thruster_interface_auv/config/thruster_interface_auv_config.yaml index 295e1ef87..6ed8b5ea2 100644 --- a/motion/thruster_interface_auv/config/thruster_interface_auv_config.yaml +++ b/motion/thruster_interface_auv/config/thruster_interface_auv_config.yaml @@ -20,9 +20,10 @@ # LEFT: [2.55792, 27.51178, 147.67001, 1472.23069] # RIGHT: [1.33421, -18.75129, 121.29079, 1528.99886] - i2c: - bus: 7 - address: 0x21 + uart: + device: "/dev/serial0" + baud_rate: 115200 + packet_id: 1 debug: - flag: False + flag: True diff --git a/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp b/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp index 6e2f0ebd4..ad0ea2c45 100644 --- a/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp +++ b/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp @@ -1,17 +1,11 @@ #ifndef THRUSTER_INTERFACE_AUV__THRUSTER_INTERFACE_AUV_DRIVER_HPP_ #define THRUSTER_INTERFACE_AUV__THRUSTER_INTERFACE_AUV_DRIVER_HPP_ -#include -#include -#include -#include -#include -#include -#include +#include +#include + #include -#include -#include -#include +#include #include #include @@ -19,10 +13,10 @@ * @brief struct to hold the parameters for a single thruster */ struct ThrusterParameters { - uint8_t mapping; - int8_t direction; - uint16_t pwm_min; - uint16_t pwm_max; + std::uint8_t mapping; + std::int8_t direction; + std::uint16_t pwm_min; + std::uint16_t pwm_max; }; enum PolySide { @@ -33,8 +27,7 @@ enum PolySide { /** * @brief class instantiated by ThrusterInterfaceAUVNode to control the * thrusters, takes the thruster forces and converts them to PWM signals to be - * sent via I2C to the ESCs (PCA9685 Adafruit 16-Channel 12-bit PWM/Servo - * Driver) + * sent via UART to the ESC controller. * * @details Based on the datasheets found in /resources, approximate the map * with a piecewise (>0 and <0) third order polynomial. @@ -44,50 +37,55 @@ enum PolySide { * all the handling of the other voltages to save resources. Could be * re-implemented in the future for more flexibility if we ever need it to * operate at different voltages in different situations. + * + * @note Over UART, the PWM values are packed into a framed packet: + * [magic][id][length][payload][checksum], where the payload is 8 uint16_t. */ class ThrusterInterfaceAUVDriver { - public: +public: ~ThrusterInterfaceAUVDriver(); /** * @brief called from ThrusterInterfaceAUVNode .cpp when instantiating the * object, initializes all the params. * - * @param i2c_bus bus number used to communicate - * @param pico_i2c_address i2c address of the ESC that drive the + * @param serial_device serial device used to communicate + * (for example /dev/ttyUSB0) + * @param baud_rate UART baud rate + * @param packet_id packet ID sent in the UART frame * @param thruster_parameters describe mapping, direction, min and max pwm - * value for each thruster - * @param poly_coeffs LEFT(<0) and RIGHT(>0) third order - * polynomial coefficients + * value for each thruster + * @param right_coeffs RIGHT(>0) third order polynomial coefficients + * @param left_coeffs LEFT(<0) third order polynomial coefficients */ ThrusterInterfaceAUVDriver( - std::int16_t i2c_bus, - int pico_i2c_address, + const std::string& serial_device, + unsigned int baud_rate, + std::uint8_t packet_id, const std::vector& thruster_parameters, - const std::vector>& poly_coeffs); + const std::vector& right_coeffs, + const std::vector& left_coeffs); + + /** + * @brief initializes UART + * @return 0 on success, negative number on failure + */ + int init_uart(); + /** * @brief calls both 1) interpolate_forces_to_pwm() to * convert the thruster forces to PWM values and 2) send_data_to_escs() to - * send them to the ESCs via I2C + * send them over UART * * @param thruster_forces_array vector of forces for each thruster * - * @return std::vector vector of pwm values sent to each thruster + * @return std::optional> vector of pwm values sent to + * each thruster, or std::nullopt on failure */ - std::vector drive_thrusters( + std::optional> drive_thrusters( const std::vector& thruster_forces_array); - private: - int bus_fd_; ///< file descriptor for the I2C bus (integer >0 that uniquely - ///< identifies the device. -1 if it fails) - - int i2c_bus_; - int pico_i2c_address_; - std::vector thruster_parameters_; - std::vector> poly_coeffs_; - - uint16_t idle_pwm_value_; ///< pwm value when force = 0.00 - +private: /** * @brief only take the thruster forces and return PWM values * @@ -96,24 +94,22 @@ class ThrusterInterfaceAUVDriver { * @return std::vector vector of pwm values sent to each thruster * if we want to publish them for debug purposes */ - std::vector interpolate_forces_to_pwm( + std::vector interpolate_forces_to_pwm( const std::vector& thruster_forces_array); /** * @brief scalar map from force to pwm x->y. Choose coefficients [LEFT] or * [RIGHT] based on sign(force) * - * @param force scalar force value - * @param coeffs std::vector> coeffs contains the pair - * of coefficients + * @param force scalar force value * * @return std::uint16_t scalar pwm value */ - std::uint16_t force_to_pwm(double force, - const std::vector>& coeffs); + std::uint16_t force_to_pwm(double force); /** * @brief compute y = a*x^3 + b*x^2 + c*x + d + * * @param force x * @param coeffs a,b,c,d * @@ -123,33 +119,47 @@ class ThrusterInterfaceAUVDriver { /** * @brief only takes the pwm values computed and sends them - * to the ESCs via I2C + * over UART as a framed packet * * @param thruster_pwm_array vector of pwm values to send + * @return 0 on success, -1 on failure */ - void send_data_to_escs(const std::vector& thruster_pwm_array); + int send_data_to_escs(const std::vector& thruster_pwm_array); /** - * @brief convert Newtons to Kg + * @brief create UART packet with format: + * [magic][id][length][payload][checksum] * - * @param force Newtons + * @param id packet ID + * @param thruster_pwm_array vector of 8 pwm values to send as payload * - * @return double Kg + * @return std::vector serialized packet bytes */ - static constexpr double to_kg(double force) { return force / 9.80665; } + std::vector create_packet( + std::uint8_t id, + const std::vector& thruster_pwm_array) const; /** - * @brief convert pwm values to i2c bytes + * @brief convert Newtons to Kg * - * @param pwm pwm value + * @param force Newtons * - * @return std::array i2c data + * @return double Kg */ - static constexpr std::array pwm_to_i2c_data( - std::uint16_t pwm) { - return {static_cast((pwm >> 8) & 0xFF), - static_cast(pwm & 0xFF)}; - } + static constexpr double to_kg(double force) { return force / 9.80665; } + +private: + std::string serial_device_; + unsigned int baud_rate_; + std::uint8_t packet_id_; + + asio::io_context io_; + asio::serial_port serial_{io_}; + + std::vector thruster_parameters_; + std::vector right_coeffs_; + std::vector left_coeffs_; + std::uint16_t idle_pwm_value_{1500}; }; #endif // THRUSTER_INTERFACE_AUV__THRUSTER_INTERFACE_AUV_DRIVER_HPP_ diff --git a/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_ros.hpp b/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_ros.hpp index 58ca254fb..b849f9f71 100644 --- a/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_ros.hpp +++ b/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_ros.hpp @@ -2,21 +2,23 @@ #define THRUSTER_INTERFACE_AUV__THRUSTER_INTERFACE_AUV_ROS_HPP_ #include +#include +#include + #include #include #include #include -#include -#include #include + #include "thruster_interface_auv/thruster_interface_auv_driver.hpp" class ThrusterInterfaceAUVNode : public rclcpp::Node { - public: +public: ThrusterInterfaceAUVNode( const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); - private: +private: /** * @brief periodically receive thruster forces topic * @@ -26,7 +28,7 @@ class ThrusterInterfaceAUVNode : public rclcpp::Node { const vortex_msgs::msg::ThrusterForces::SharedPtr msg); /** - * @brief publish and send pwm commands to thrusters. Sinchronous with + * @brief publish and send PWM commands to thrusters. Synchronous with * thruster_forces_callback */ void pwm_callback(); @@ -43,7 +45,6 @@ class ThrusterInterfaceAUVNode : public rclcpp::Node { /** * @brief Initialize the parameter handler and a parameter event callback. - * */ void initialize_parameter_handler(); @@ -52,27 +53,38 @@ class ThrusterInterfaceAUVNode : public rclcpp::Node { */ void set_publisher(); - int i2c_bus_; - int i2c_address_; + /** + * @brief specific callback for updating debug_flag. + */ + void update_debug_flag(const rclcpp::Parameter& p); + +private: + std::string serial_device_; + unsigned int baud_rate_; + std::uint8_t packet_id_; + std::string subscriber_topic_name_; std::string publisher_topic_name_; + std::vector thruster_parameters_; - std::vector> poly_coeffs_; + std::vector left_coeffs_; + std::vector right_coeffs_; std::vector thruster_forces_array_; - bool debug_flag_; + bool debug_flag_{false}; std::unique_ptr - thruster_driver_; ///<-- pwm driver - rclcpp::Subscription:: - SharedPtr ///<-- thruster forces subscriber - thruster_forces_subscriber_; - rclcpp::Publisher< - std_msgs::msg::Int16MultiArray>::SharedPtr ///<-- pwm publisher - thruster_pwm_publisher_; + thruster_driver_; ///<-- UART/USART thruster driver + + rclcpp::Subscription::SharedPtr + thruster_forces_subscriber_; ///<-- thruster forces subscriber + + rclcpp::Publisher::SharedPtr + thruster_pwm_publisher_; ///<-- pwm publisher + rclcpp::TimerBase::SharedPtr watchdog_timer_; rclcpp::Time last_msg_time_; - rclcpp::Duration watchdog_timeout_ = std::chrono::seconds(1); + rclcpp::Duration watchdog_timeout_ = rclcpp::Duration::from_seconds(1.0); bool watchdog_triggered_ = false; /** @@ -90,11 +102,6 @@ class ThrusterInterfaceAUVNode : public rclcpp::Node { * made with the parameter event handler (`param_handler_`). */ rclcpp::ParameterCallbackHandle::SharedPtr debug_flag_parameter_cb; - - /** - * specific callback for updating debug_flag. - */ - void update_debug_flag(const rclcpp::Parameter& p); }; #endif // THRUSTER_INTERFACE_AUV__THRUSTER_INTERFACE_AUV_ROS_HPP_ diff --git a/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp b/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp index 86834a8e7..35305749a 100644 --- a/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp +++ b/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp @@ -1,127 +1,260 @@ #include "thruster_interface_auv/thruster_interface_auv_driver.hpp" -#include -#include -#include -#include + +#include +#include +#include ThrusterInterfaceAUVDriver::ThrusterInterfaceAUVDriver( - std::int16_t i2c_bus, - int pico_i2c_address, + const std::string& serial_device, + unsigned int baud_rate, + std::uint8_t packet_id, const std::vector& thruster_parameters, - const std::vector>& poly_coeffs) - : i2c_bus_(i2c_bus), - pico_i2c_address_(pico_i2c_address), + const std::vector& right_coeffs, + const std::vector& left_coeffs) + : serial_device_(serial_device), + baud_rate_(baud_rate), + packet_id_(packet_id), thruster_parameters_(thruster_parameters), - poly_coeffs_(poly_coeffs) { - std::string i2c_filename = std::format("/dev/i2c-{}", i2c_bus_); - bus_fd_ = - open(i2c_filename.c_str(), - O_RDWR); // Open the i2c bus for reading and writing (0_RDWR) - if (bus_fd_ < 0) { - std::runtime_error(std::format("ERROR: Failed to open I2C bus {} : {}", - i2c_bus_, strerror(errno))); + right_coeffs_(right_coeffs), + left_coeffs_(left_coeffs) { + idle_pwm_value_ = + static_cast( + (calc_poly(0.0, left_coeffs_) + calc_poly(0.0, right_coeffs_)) / 2); +} + +int ThrusterInterfaceAUVDriver::init_uart() { + std::error_code ec; + + serial_.open(serial_device_, ec); + if (ec) { + std::cerr << "Failed to open serial port " << serial_device_ + << ": " << ec.message() << '\n'; + return -1; } - idle_pwm_value_ = - (calc_poly(0, poly_coeffs_[LEFT]) + calc_poly(0, poly_coeffs_[RIGHT])) / - 2; + serial_.set_option(asio::serial_port_base::baud_rate(baud_rate_), ec); + if (ec) return -1; + + serial_.set_option(asio::serial_port_base::character_size(8), ec); + if (ec) return -1; + + serial_.set_option( + asio::serial_port_base::parity(asio::serial_port_base::parity::none), ec); + if (ec) return -1; + + serial_.set_option( + asio::serial_port_base::stop_bits(asio::serial_port_base::stop_bits::one), ec); + if (ec) return -1; + + serial_.set_option( + asio::serial_port_base::flow_control( + asio::serial_port_base::flow_control::none), + ec); + if (ec) return -1; + + return 0; } ThrusterInterfaceAUVDriver::~ThrusterInterfaceAUVDriver() { - if (bus_fd_ >= 0) { - send_data_to_escs(std::vector(thruster_parameters_.size(), - idle_pwm_value_)); - close(bus_fd_); + if (serial_.is_open()) { + send_data_to_escs( + std::vector(thruster_parameters_.size(), idle_pwm_value_)); + + std::error_code ec; + serial_.close(ec); } } std::vector ThrusterInterfaceAUVDriver::interpolate_forces_to_pwm( const std::vector& thruster_forces_array) { - // Convert Newtons to Kg (since the thruster datasheet is in Kg) - auto pwm_view = thruster_forces_array | std::views::transform(to_kg) | - std::views::transform([this](double force_in_kg) { - return force_to_pwm(force_in_kg, poly_coeffs_); - }); - return std::vector(pwm_view.begin(), pwm_view.end()); + std::vector pwm(thruster_forces_array.size()); + + for (std::size_t i = 0; i < thruster_forces_array.size(); ++i) { + const double force_in_kg = to_kg(thruster_forces_array[i]); + pwm[i] = force_to_pwm(force_in_kg); + } + + return pwm; } -std::uint16_t ThrusterInterfaceAUVDriver::force_to_pwm( - double force, - const std::vector>& coeffs) { - if (force < 0) { - return calc_poly(force, coeffs[LEFT]); - } else if (force > 0) { - return calc_poly(force, coeffs[RIGHT]); - } else { - return idle_pwm_value_; // 1500 +std::uint16_t ThrusterInterfaceAUVDriver::force_to_pwm(double force) { + // The LEFT/RIGHT polynomials have a gap at force=0 (~1468 vs ~1534) + // because they were fitted to data outside the ESC deadband. + // Forces below this threshold (in kg) return idle PWM to bridge the gap. + constexpr double deadband_kg = 0.03; + + if (std::abs(force) < deadband_kg) { + return idle_pwm_value_; + } + if (force < 0.0) { + return calc_poly(force, left_coeffs_); } + return calc_poly(force, right_coeffs_); } std::uint16_t ThrusterInterfaceAUVDriver::calc_poly( double force, const std::vector& coeffs) { - return static_cast(coeffs[0] * std::pow(force, 3) + - coeffs[1] * std::pow(force, 2) + - coeffs[2] * force + coeffs[3]); + return static_cast( + coeffs[0] * std::pow(force, 3) + + coeffs[1] * std::pow(force, 2) + + coeffs[2] * force + + coeffs[3]); } +// +// std::vector ThrusterInterfaceAUVDriver::create_packet( +// std::uint8_t id, +// const std::vector& thruster_pwm_array) const { +// constexpr std::uint8_t magic = 0xAA; +// constexpr std::size_t expected_thrusters = 8; +// +// if (thruster_pwm_array.size() != expected_thrusters) { +// return {}; +// } +// +// std::vector packet; +// packet.reserve(1 + 1 + 1 + expected_thrusters * 2 + 1); +// +// packet.push_back(magic); +// packet.push_back(id); +// +// const std::uint8_t payload_length = +// static_cast(thruster_pwm_array.size() * sizeof(std::uint16_t)); +// packet.push_back(payload_length); +// +// for (std::uint16_t value : thruster_pwm_array) { +// // little-endian +// packet.push_back(static_cast(value & 0xFF)); +// packet.push_back(static_cast((value >> 8) & 0xFF)); +// } +// +// std::uint8_t checksum = 0; +// for (std::uint8_t byte : packet) { +// checksum = static_cast(checksum + byte); +// } +// +// packet.push_back(checksum); +// return packet; +// } + +std::vector ThrusterInterfaceAUVDriver::create_packet( + std::uint8_t id, + const std::vector& thruster_pwm_array) const { + + std::vector packet; + + // 1. Magic byte + packet.push_back(0xAA); -void ThrusterInterfaceAUVDriver::send_data_to_escs( + // 2. ID + packet.push_back(id); + + // 3. Length (payload size in bytes) + uint8_t length = static_cast( + thruster_pwm_array.size() * sizeof(uint16_t)); + packet.push_back(length); + + // 4. Payload (little-endian) + for (uint16_t value : thruster_pwm_array) { + packet.push_back(static_cast(value & 0xFF)); + packet.push_back(static_cast((value >> 8) & 0xFF)); + } + + // 5. XOR checksum (matching your MCU) + uint8_t checksum = id ^ length; + for (uint16_t value : thruster_pwm_array) { + checksum ^= static_cast(value & 0xFF); + checksum ^= static_cast((value >> 8) & 0xFF); + } + + packet.push_back(checksum); + + return packet; +} + +// +// int ThrusterInterfaceAUVDriver::send_data_to_escs( +// const std::vector& thruster_pwm_array) { +// if (!serial_.is_open()) { +// return -1; +// } +// +// const auto packet = create_packet(0x04, thruster_pwm_array); +// if (packet.empty()) { +// return -1; +// } +// +// std::error_code ec; +// const auto bytes_written = asio::write(serial_, asio::buffer(packet), ec); +// +// if (ec || bytes_written != packet.size()) { +// std::cerr << "UART write failed: " +// << (ec ? ec.message() : "short write") << '\n'; +// return -1; +// } +// +// return 0; +// } +int ThrusterInterfaceAUVDriver::send_data_to_escs( const std::vector& thruster_pwm_array) { - constexpr std::size_t i2c_data_size = - 1 + 8 * 2; // 8 thrusters * (1xMSB + 1xLSB) - std::vector i2c_data_array; - i2c_data_array.reserve(i2c_data_size); - - i2c_data_array.push_back(0x00); // Start byte - std::ranges::for_each(thruster_pwm_array, [&](std::uint16_t pwm) { - std::array bytes = pwm_to_i2c_data(pwm); - std::ranges::copy(bytes, std::back_inserter(i2c_data_array)); - }); - - // Set the I2C slave address - if (ioctl(bus_fd_, I2C_SLAVE, pico_i2c_address_) < 0) { - throw std::runtime_error(std::format("Failed to open I2C bus {} : {}", - i2c_bus_, strerror(errno))); - return; + if (!serial_.is_open()) { + return -1; + } + + const auto packet = create_packet(0x04, thruster_pwm_array); + constexpr std::size_t header_size = 3; + + if (packet.size() < header_size) { + return -1; + } + + std::error_code ec; + + // Send header first: [magic][id][length] + const auto header_bytes_written = + asio::write(serial_, asio::buffer(packet.data(), header_size), ec); + + if (ec || header_bytes_written != header_size) { + std::cerr << "UART header write failed: " + << (ec ? ec.message() : "short write") << '\n'; + return -1; } - // Write data to the I2C device - if (write(bus_fd_, i2c_data_array.data(), i2c_data_size) != i2c_data_size) { - throw std::runtime_error(std::format( - "ERROR: Failed to write to I2C device : {}", strerror(errno))); + // Small delay so the receiver can switch state + std::this_thread::sleep_for(std::chrono::milliseconds(20)); + + // Send payload + checksum + const auto remaining_size = packet.size() - header_size; + const auto payload_bytes_written = + asio::write(serial_, + asio::buffer(packet.data() + header_size, remaining_size), + ec); + + if (ec || payload_bytes_written != remaining_size) { + std::cerr << "UART payload write failed: " + << (ec ? ec.message() : "short write") << '\n'; + return -1; } + + return 0; } -std::vector ThrusterInterfaceAUVDriver::drive_thrusters( +std::optional> ThrusterInterfaceAUVDriver::drive_thrusters( const std::vector& thruster_forces_array) { - // Apply thruster mapping and direction - std::vector mapped_forces(thruster_forces_array.size()); + std::vector mapped_forces(thruster_parameters_.size()); - std::ranges::transform(thruster_parameters_, mapped_forces.begin(), - [this, &thruster_forces_array](const auto& param) { - return thruster_forces_array[param.mapping] * - param.direction; - }); + for (std::size_t i = 0; i < thruster_parameters_.size(); ++i) { + const auto& param = thruster_parameters_[i]; + const std::size_t idx = param.mapping; + const double raw_force = thruster_forces_array[idx]; + mapped_forces[i] = raw_force * param.direction; + } - // Convert forces to PWM std::vector thruster_pwm_array = interpolate_forces_to_pwm(mapped_forces); - std::ranges::transform(thruster_pwm_array, thruster_pwm_array.begin(), - [this, i = 0](auto pwm) mutable { - auto result = std::clamp( - pwm, thruster_parameters_[i].pwm_min, - thruster_parameters_[i].pwm_max); - ++i; - return result; - }); - - try { - send_data_to_escs(thruster_pwm_array); - } catch (const std::exception& e) { - spdlog::error("ERROR: Failed to send PWM values - {}", e.what()); - } catch (...) { - spdlog::error("ERROR: Failed to send PWM values - Unknown exception"); + if (send_data_to_escs(thruster_pwm_array) != 0) { + return std::nullopt; } return thruster_pwm_array; diff --git a/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp b/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp index 603b2d46e..0bda24eb8 100644 --- a/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp +++ b/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp @@ -1,9 +1,15 @@ #include "thruster_interface_auv/thruster_interface_auv_ros.hpp" -#include + #include -#include +#include #include +#include +#include +#include +#include +#include + const auto start_message = R"( _____ _ _ ___ _ __ |_ _| |__ _ __ _ _ ___| |_ ___ _ __ |_ _|_ __ | |_ ___ _ __ / _| __ _ ___ ___ @@ -32,13 +38,26 @@ ThrusterInterfaceAUVNode::ThrusterInterfaceAUVNode( vortex::utils::qos_profiles::reliable_profile(1)); thruster_driver_ = std::make_unique( - i2c_bus_, i2c_address_, thruster_parameters_, poly_coeffs_); + serial_device_, + baud_rate_, + packet_id_, + thruster_parameters_, + right_coeffs_, + left_coeffs_); + + if (thruster_driver_->init_uart() != 0) { + spdlog::error("Failed to initialize UART thruster driver"); + } else { + spdlog::info("UART thruster driver initialized on {} @ {} baud, packet id 0x{:02X}", + serial_device_, baud_rate_, packet_id_); + } - thruster_forces_array_ = std::vector(8, 0.00); + thruster_forces_array_ = std::vector(8, 0.0); watchdog_timer_ = this->create_wall_timer( std::chrono::milliseconds(500), std::bind(&ThrusterInterfaceAUVNode::watchdog_callback, this)); + last_msg_time_ = this->now(); this->initialize_parameter_handler(); @@ -56,24 +75,38 @@ void ThrusterInterfaceAUVNode::thruster_forces_callback( } void ThrusterInterfaceAUVNode::pwm_callback() { - std::vector thruster_pwm_array = - thruster_driver_->drive_thrusters(this->thruster_forces_array_); + auto thruster_pwm_array_opt = + thruster_driver_->drive_thrusters(thruster_forces_array_); + + if (!thruster_pwm_array_opt.has_value()) { + spdlog::warn("Sending PWM values to thrusters failed"); + return; + } if (debug_flag_) { + const auto& thruster_pwm_array = thruster_pwm_array_opt.value(); + std_msgs::msg::Int16MultiArray pwm_message; - pwm_message.data = std::vector(thruster_pwm_array.begin(), - thruster_pwm_array.end()); + pwm_message.data = std::vector( + thruster_pwm_array.begin(), thruster_pwm_array.end()); + thruster_pwm_publisher_->publish(pwm_message); } } void ThrusterInterfaceAUVNode::watchdog_callback() { - auto now = this->now(); + const auto now = this->now(); + if ((now - last_msg_time_) >= watchdog_timeout_ && !watchdog_triggered_) { - thruster_forces_array_.assign(8, 0.00); - thruster_driver_->drive_thrusters(thruster_forces_array_); + thruster_forces_array_.assign(8, 0.0); + + if (!thruster_driver_->drive_thrusters(thruster_forces_array_).has_value()) { + spdlog::warn("Watchdog triggered, but failed to send zero command to thrusters"); + } else { + spdlog::warn("Watchdog triggered, all thrusters set to 0.0"); + } + watchdog_triggered_ = true; - spdlog::warn("Watchdog triggered, all thrusters set to 0.00"); } } @@ -81,8 +114,9 @@ void ThrusterInterfaceAUVNode::initialize_parameter_handler() { param_handler_ = std::make_shared(this); debug_flag_parameter_cb = param_handler_->add_parameter_callback( - "debug.flag", std::bind(&ThrusterInterfaceAUVNode::update_debug_flag, - this, std::placeholders::_1)); + "debug.flag", + std::bind(&ThrusterInterfaceAUVNode::update_debug_flag, this, + std::placeholders::_1)); } void ThrusterInterfaceAUVNode::update_debug_flag(const rclcpp::Parameter& p) { @@ -93,80 +127,90 @@ void ThrusterInterfaceAUVNode::update_debug_flag(const rclcpp::Parameter& p) { } void ThrusterInterfaceAUVNode::extract_all_parameters() { - this->declare_parameter>( + this->declare_parameter>( "propulsion.thrusters.thruster_to_pin_mapping"); - this->declare_parameter>( + this->declare_parameter>( "propulsion.thrusters.thruster_direction"); - this->declare_parameter>( + this->declare_parameter>( "propulsion.thrusters.thruster_PWM_min"); - this->declare_parameter>( + this->declare_parameter>( "propulsion.thrusters.thruster_PWM_max"); - // approx poly coeffs for 16V from thruster_interface_auv.yaml + // Approx poly coeffs for 16V from thruster_interface_auv.yaml this->declare_parameter>("coeffs.16V.LEFT"); this->declare_parameter>("coeffs.16V.RIGHT"); - this->declare_parameter("i2c.bus"); - this->declare_parameter("i2c.address"); + this->declare_parameter("uart.device"); + this->declare_parameter("uart.baud_rate"); + this->declare_parameter("uart.packet_id"); this->declare_parameter("topics.thruster_forces"); this->declare_parameter("topics.pwm_output"); this->declare_parameter("debug.flag"); - this->declare_parameter("propulsion.thrusters.watchdog_timeout"); - //----------------------------------------------------------------------- - - auto thruster_mapping = + const auto thruster_mapping = this->get_parameter("propulsion.thrusters.thruster_to_pin_mapping") .as_integer_array(); - auto thruster_direction = + const auto thruster_direction = this->get_parameter("propulsion.thrusters.thruster_direction") .as_integer_array(); - auto thruster_PWM_min = + const auto thruster_pwm_min = this->get_parameter("propulsion.thrusters.thruster_PWM_min") .as_integer_array(); - auto thruster_PWM_max = + const auto thruster_pwm_max = this->get_parameter("propulsion.thrusters.thruster_PWM_max") .as_integer_array(); - std::vector left_coeffs = - this->get_parameter("coeffs.16V.LEFT").as_double_array(); - std::vector right_coeffs = - this->get_parameter("coeffs.16V.RIGHT").as_double_array(); + left_coeffs_ = this->get_parameter("coeffs.16V.LEFT").as_double_array(); + right_coeffs_ = this->get_parameter("coeffs.16V.RIGHT").as_double_array(); - this->i2c_bus_ = this->get_parameter("i2c.bus").as_int(); - this->i2c_address_ = this->get_parameter("i2c.address").as_int(); + serial_device_ = this->get_parameter("uart.device").as_string(); + baud_rate_ = static_cast( + this->get_parameter("uart.baud_rate").as_int()); + packet_id_ = static_cast( + this->get_parameter("uart.packet_id").as_int()); - this->subscriber_topic_name_ = + subscriber_topic_name_ = this->get_parameter("topics.thruster_forces").as_string(); - this->publisher_topic_name_ = + publisher_topic_name_ = this->get_parameter("topics.pwm_output").as_string(); - this->debug_flag_ = this->get_parameter("debug.flag").as_bool(); + debug_flag_ = this->get_parameter("debug.flag").as_bool(); - auto create_thruster_parameters = [&](const int64_t& mapping, - const int64_t& direction) { - size_t index = &mapping - &thruster_mapping[0]; - return ThrusterParameters{ - static_cast(mapping), static_cast(direction), - static_cast(thruster_PWM_min[index]), - static_cast(thruster_PWM_max[index])}; - }; + const auto thruster_count = thruster_mapping.size(); - std::ranges::transform(thruster_mapping, thruster_direction, - std::back_inserter(this->thruster_parameters_), - create_thruster_parameters); + if (thruster_direction.size() != thruster_count || + thruster_pwm_min.size() != thruster_count || + thruster_pwm_max.size() != thruster_count) { + throw std::runtime_error( + "Thruster parameter arrays must all have the same length"); + } + + if (thruster_count != 8) { + spdlog::warn( + "UART packet format expects 8 thrusters, but config contains {} entries", + thruster_count); + } + + thruster_parameters_.clear(); + thruster_parameters_.reserve(thruster_count); + + for (std::size_t i = 0; i < thruster_count; ++i) { + thruster_parameters_.push_back(ThrusterParameters{ + static_cast(thruster_mapping[i]), + static_cast(thruster_direction[i]), + static_cast(thruster_pwm_min[i]), + static_cast(thruster_pwm_max[i]), + }); + } - this->poly_coeffs_.push_back(left_coeffs); - this->poly_coeffs_.push_back(right_coeffs); + const double timeout_threshold_param = + this->get_parameter("propulsion.thrusters.watchdog_timeout").as_double(); - double timout_treshold_param = - this->get_parameter("propulsion.thrusters.watchdog_timeout") - .as_double(); watchdog_timeout_ = std::chrono::duration_cast( - std::chrono::duration(timout_treshold_param)); + std::chrono::duration(timeout_threshold_param)); } RCLCPP_COMPONENTS_REGISTER_NODE(ThrusterInterfaceAUVNode) diff --git a/navigation/eskf/CMakeLists.txt b/navigation/eskf/CMakeLists.txt new file mode 100644 index 000000000..b292b256d --- /dev/null +++ b/navigation/eskf/CMakeLists.txt @@ -0,0 +1,88 @@ +cmake_minimum_required(VERSION 3.8) +project(eskf) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 20) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +if(NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE Release) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(tf2 REQUIRED) +find_package(vortex_msgs REQUIRED) +find_package(vortex_utils REQUIRED) +find_package(vortex_utils_ros REQUIRED) +find_package(spdlog REQUIRED) +find_package(fmt REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(tf2_eigen REQUIRED) + +if(NOT DEFINED EIGEN3_INCLUDE_DIR) + set(EIGEN3_INCLUDE_DIR ${EIGEN3_INCLUDE_DIRS}) +endif() + +include_directories(${EIGEN3_INCLUDE_DIR}) + +include_directories(include) + +set(LIB_NAME "${PROJECT_NAME}_component") + +add_library(${LIB_NAME} SHARED + src/eskf.cpp + src/eskf_ros.cpp +) + +ament_target_dependencies(${LIB_NAME} PUBLIC + rclcpp + rclcpp_components + geometry_msgs + nav_msgs + Eigen3 + tf2 + vortex_msgs + vortex_utils + vortex_utils_ros + spdlog + fmt + tf2_ros + tf2_eigen +) + +rclcpp_components_register_node( + ${LIB_NAME} + PLUGIN "ESKFNode" + EXECUTABLE ${PROJECT_NAME}_node +) + +ament_export_targets(export_${LIB_NAME}) + +install(TARGETS ${LIB_NAME} + EXPORT export_${LIB_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install( + DIRECTORY include/ + DESTINATION include +) + +install(DIRECTORY + launch + config + DESTINATION share/${PROJECT_NAME}/ +) + +ament_package() diff --git a/navigation/eskf/config/eskf_params.yaml b/navigation/eskf/config/eskf_params.yaml new file mode 100644 index 000000000..001ede019 --- /dev/null +++ b/navigation/eskf/config/eskf_params.yaml @@ -0,0 +1,32 @@ +/**: + eskf_node: + ros__parameters: + diag_Q_std: [0.05, 0.05, 0.1, 0.01, 0.01, 0.02, 0.001, 0.001, 0.001, 0.0001, 0.0001, 0.0001] + diag_p_init: [1.0, 1.0, 1.0, 0.5, 0.5, 0.5, 0.1, 0.1, 0.1, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001] + transform: + imu_frame_r: [ -1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, -1.0 ] + imu_frame_t: [ 0.0, 0.0, 0.0 ] + + dvl_frame_r: [ 0.0, -1.0, 0.0, + 1.0, 0.0, 0.0, + 0.0, 0.0, 1.0 ] + + dvl_frame_t: [ 0.4, 0.0, 0.2 ] + + depth_frame_t: [ 0.0, 0.0, 0.0 ] + use_tf_transforms: false + publish_tf: true + publish_pose: true + publish_twist: true + publish_rate_ms: 5 + add_gravity_to_imu: false + # biases in the IMU + initial_gyro_bias: [0.0, 0.0, 0.0] + initial_accel_bias: [0.0, 0.0, 0.0] + publish_biases: false + # environmental parameters + atmospheric_pressure: 101000.0 # [Pa] + water_density: 1026.0 # [kg/m3] + gravity: 9.82841 diff --git a/navigation/eskf/config/eskf_params_real_world.yaml b/navigation/eskf/config/eskf_params_real_world.yaml new file mode 100644 index 000000000..0f82da7d7 --- /dev/null +++ b/navigation/eskf/config/eskf_params_real_world.yaml @@ -0,0 +1,36 @@ +/**: + eskf_node: + ros__parameters: + diag_Q_std: [0.05, 0.05, 0.1, 0.01, 0.01, 0.02, 0.001, 0.001, 0.001, 0.0001, 0.0001, 0.0001] + diag_p_init: [1.0, 1.0, 1.0, 0.5, 0.5, 0.5, 0.1, 0.1, 0.1, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001] + transform: + imu_frame_r: [ 1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0 ] + imu_frame_t: [ 0.0, 0.0, 0.0 ] + + dvl_frame_r: [ 0.0, -1.0, 0.0, + 1.0, 0.0, 0.0, + 0.0, 0.0, 1.0 ] + + dvl_frame_t: [ 0.4, 0.0, 0.2 ] + + depth_frame_t: [ 0.0, 0.0, 0.0 ] + use_tf_transforms: false + publish_tf: true + publish_pose: true + publish_twist: true + publish_rate_ms: 5 + # biases in the IMU + add_gravity_to_imu: true + initial_gyro_bias: [0.0, 0.0, 0.0] + initial_accel_bias: [3.53e-2, 0.0, -0.0533] + publish_biases: true + # environmental parameters + atmospheric_pressure: 101000.0 # [Pa] + water_density: 1026.0 # [kg/m3] + gravity: 9.82841 + +# USED SENSORS + # IMU: Kongsberg mini MRU + # DVL & Depth: Nucleus 1000 diff --git a/navigation/eskf/include/eskf/eskf.hpp b/navigation/eskf/include/eskf/eskf.hpp new file mode 100644 index 000000000..96d354bdc --- /dev/null +++ b/navigation/eskf/include/eskf/eskf.hpp @@ -0,0 +1,110 @@ +#ifndef ESKF__ESKF_HPP_ +#define ESKF__ESKF_HPP_ + +#include +#include +#include "eskf/typedefs.hpp" +#include "typedefs.hpp" + +class ESKF { + public: + explicit ESKF(const EskfParams& params); + + // @brief Update the nominal state and error state + // @param imu_meas: IMU measurement + // @param dt: Time step + void imu_update(const ImuMeasurement& imu_meas, const double dt); + + // @brief Update the nominal state and error state + // @param dvl_meas: DVL measurement + void dvl_update(const SensorDVL& dvl_meas); + + void depth_update(const SensorDepth& depth_meas); + + inline StateQuat get_nominal_state() const { return current_nom_state_; } + + inline StateEuler get_error_state() const { return current_error_state_; } + + inline double get_nis() const { return nis_; } + + inline Eigen::Vector3d get_gravity() const { return g_; } + + private: + // @brief Predict the nominal state + // @param imu_meas: IMU measurement + // @param dt: Time step + // @return Predicted nominal state + void nominal_state_discrete(const ImuMeasurement& imu_meas, + const double dt); + + // @brief Predict the error state + // @param imu_meas: IMU measurement + // @param dt: Time step + // @return Predicted error state + void error_state_prediction(const ImuMeasurement& imu_meas, + const double dt); + + // @brief Calculate the NIS + // @param innovation: Innovation vector + // @param S: Innovation covariance matrix + void NIS(const Eigen::Vector3d& innovation, const Eigen::Matrix3d& S); + + // @brief Update the error state using a generic sensor measurement model + // @tparam SensorT Type of the sensor model (must satisfy + // SensorModelConcept) + // @param meas Sensor measurement instance + template + void measurement_update(const SensorT& meas); + + // @brief Inject the error state into the nominal state and reset the error + void injection_and_reset(); + + // @brief Van Loan discretization + // @param A_c: Continuous state transition matrix + // @param G_c: Continuous input matrix + // @return Discrete state transition matrix and discrete input matrix + std::pair van_loan_discretization( + const Eigen::Matrix15d& A_c, + const Eigen::Matrix15x12d& G_c, + const double dt); + + // Process noise covariance matrix + Eigen::Matrix12d Q_{}; + + // Normalized Innovation Squared + double nis_{}; + + // Member variable for the current error state + StateEuler current_error_state_{}; + + // Member variable for the current nominal state + StateQuat current_nom_state_{}; + + // gravity + Eigen::Vector3d g_{0.0, 0.0, -9.82841}; + + // accelometer noise parameters + float accm_std_{0.0}; + float accm_bias_std_{0.0}; + float accm_bias_p_{1e-16}; + + // gyroscope noise parameters + float gyro_std_{0.0}; + float gyro_bias_std_{0.0}; + float gyro_bias_p_{1e-16}; +}; + +// Measurement in world frame --> h(x) +Eigen::Vector3d calculate_h(const StateQuat& current_nom_state_); + +// Jacobian of h(x) with respect to the error state --> H +Eigen::Matrix3x15d calculate_h_jacobian(const StateQuat& current_nom_state_); + +// Jacobian of h(x) with respect to the nominal state --> Hx +Eigen::Matrix3x16d calculate_hx(const StateQuat& current_nom_state_); + +double compute_nis(const Eigen::VectorXd& innovation, const Eigen::MatrixXd& S); + +#include "eskf.tpp" // including template implementation + +#endif // ESKF__ESKF_HPP_ diff --git a/navigation/eskf/include/eskf/eskf.tpp b/navigation/eskf/include/eskf/eskf.tpp new file mode 100644 index 000000000..6189a609a --- /dev/null +++ b/navigation/eskf/include/eskf/eskf.tpp @@ -0,0 +1,29 @@ + + +template +void ESKF::measurement_update(const SensorT& meas) { + Eigen::VectorXd innovation = meas.innovation(current_nom_state_); + Eigen::MatrixXd H = meas.jacobian(current_nom_state_); + Eigen::MatrixXd R = meas.noise_covariance(); + + Eigen::MatrixXd P = current_error_state_.covariance; + + Eigen::MatrixXd S = H * P * H.transpose() + R; + + Eigen::MatrixXd PHt = P * H.transpose(); + + // More stable and faster than P * H.transpose() * S.inverse() + Eigen::MatrixXd K = S.ldlt().solve(PHt.transpose()).transpose(); + +#ifndef NDEBUG + nis_ = compute_nis(innovation, S); +#endif + + current_error_state_.set_from_vector(K * innovation); + + Eigen::MatrixXd I_KH = + Eigen::MatrixXd::Identity(P.rows(), P.cols()) - K * H; + current_error_state_.covariance = + I_KH * P * I_KH.transpose() + + K * R * K.transpose(); // Used joseph form for more stable calculations +} diff --git a/navigation/eskf/include/eskf/eskf_ros.hpp b/navigation/eskf/include/eskf/eskf_ros.hpp new file mode 100644 index 000000000..03d4c7266 --- /dev/null +++ b/navigation/eskf/include/eskf/eskf_ros.hpp @@ -0,0 +1,145 @@ +#ifndef ESKF__ESKF_ROS_HPP_ +#define ESKF__ESKF_ROS_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "eskf/eskf.hpp" +#include "eskf/typedefs.hpp" +#include "spdlog/spdlog.h" + +class ESKFNode : public rclcpp::Node { + public: + explicit ESKFNode( + const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); + + private: + // @brief Callback function for the imu topic + // @param msg: Imu message containing the imu data + void imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg); + + // @brief Callback function for the dvl topic + // @param msg: TwistWithCovarianceStamped message containing the dvl data + void dvl_callback( + const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg); + + void depth_callback(const sensor_msgs::msg::FluidPressure::SharedPtr msg); + + // @brief Publish the odometry message + void publish_odom(); + + // @brief Set the subscriber and publisher for the node + void set_subscribers_and_publisher(); + + // @brief Set the parameters for the eskf + void set_parameters(); + + // @brief lookup transforms + void lookup_static_transforms(); + + // @brief Create subs/pubs and start the publish timer. Called once + // transforms are available (or immediately if use_tf_transforms_ is false). + void complete_initialization(); + + // @brief broadcast the State as a TF + void publish_tf(const StateQuat& nom_state, + const rclcpp::Time& current_time); + + // Subscribers and Publishers + + rclcpp::Subscription::SharedPtr imu_sub_; + + rclcpp::Subscription< + geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr dvl_sub_; + + rclcpp::Subscription::SharedPtr depth_sub_; + + rclcpp::Publisher::SharedPtr odom_pub_; + + rclcpp::Publisher::SharedPtr + pose_pub_; + + rclcpp::Publisher::SharedPtr + twist_pub_; + + rclcpp::Publisher::SharedPtr nis_dvl_pub_; + + rclcpp::Publisher::SharedPtr nis_depth_pub_; + + rclcpp::Publisher::SharedPtr + accel_bias_pub_; + + rclcpp::Publisher::SharedPtr + gyro_bias_pub_; + + // Member variable for the ESKF instance + + std::chrono::milliseconds time_step_{1}; + + rclcpp::TimerBase::SharedPtr odom_pub_timer_; + + std::unique_ptr eskf_; + + bool first_imu_msg_received_ = false; + + Eigen::Matrix3d R_imu_eskf_{}; + Eigen::Vector3d T_imu_eskf_{}; + + Eigen::Matrix3d R_dvl_eskf_{}; + Eigen::Vector3d T_dvl_eskf_{}; + + Eigen::Vector3d T_depth_eskf_{}; + + rclcpp::Time last_imu_time_{}; + + // Latest gyro measurement (used for publishing odom output of eskf) + Eigen::Vector3d latest_gyro_measurement_{}; + + // TF2 Handling + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + std::unique_ptr tf_broadcaster_; + rclcpp::TimerBase::SharedPtr tf_timer_; + + std::string frame(const std::string& name) const { + return frame_prefix_.empty() ? name : frame_prefix_ + "/" + name; + } + + // Flags and Storage + std::string frame_prefix_{""}; + bool use_tf_transforms_ = false; + bool tf_sensors_loaded_ = false; + bool publish_tf_{false}; + bool publish_pose_{false}; + bool publish_twist_{false}; + bool publish_biases_{false}; + bool add_gravity_to_imu_{false}; + + // hold the transfer from Sensor -> Base Link + Eigen::Isometry3d Tf_base_imu_ = Eigen::Isometry3d::Identity(); + Eigen::Isometry3d Tf_base_dvl_ = Eigen::Isometry3d::Identity(); + Eigen::Isometry3d Tf_base_depth_ = Eigen::Isometry3d::Identity(); + + // gravity, water density and atmospheric pressure parameters + double gravity; + double water_density; + double atmospheric_pressure; +}; + +#endif // ESKF__ESKF_ROS_HPP_ diff --git a/navigation/eskf/include/eskf/typedefs.hpp b/navigation/eskf/include/eskf/typedefs.hpp new file mode 100644 index 000000000..ee04e8911 --- /dev/null +++ b/navigation/eskf/include/eskf/typedefs.hpp @@ -0,0 +1,138 @@ +/** + * @file typedefs.hpp + * @brief Contains the typedef and structs for the eskf. + */ +#ifndef ESKF__TYPEDEFS_HPP_ +#define ESKF__TYPEDEFS_HPP_ + +#include +#include +#include +#include + +namespace Eigen { +typedef Eigen::Matrix Vector19d; +typedef Eigen::Matrix Vector18d; +typedef Eigen::Matrix Matrix18d; +typedef Eigen::Matrix Matrix19d; +typedef Eigen::Matrix Matrix18x12d; +typedef Eigen::Matrix Matrix4x3d; +typedef Eigen::Matrix Matrix3x19d; +typedef Eigen::Matrix Matrix3x18d; +typedef Eigen::Matrix Matrix12d; +typedef Eigen::Matrix Matrix18d; +typedef Eigen::Matrix Matrix3x1d; +typedef Eigen::Matrix Matrix19x18d; +typedef Eigen::Matrix Matrix18x3d; +typedef Eigen::Matrix Matrix36d; +typedef Eigen::Matrix Matrix6d; +typedef Eigen::Matrix Matrix9d; +typedef Eigen::Matrix Matrix15d; +typedef Eigen::Matrix Vector12d; +typedef Eigen::Matrix Vector15d; +typedef Eigen::Matrix Vector16d; +typedef Eigen::Matrix Matrix15x12d; +typedef Eigen::Matrix Matrix16x15d; +typedef Eigen::Matrix Matrix3x15d; +typedef Eigen::Matrix Matrix3x16d; +typedef Eigen::Matrix Matrix30d; +} // namespace Eigen + +template +Eigen::Matrix createDiagonalMatrix( + const std::vector& diag) { + return Eigen::Map>(diag.data()) + .asDiagonal(); +} +struct StateQuat { + Eigen::Vector3d pos = Eigen::Vector3d::Zero(); + Eigen::Vector3d vel = Eigen::Vector3d::Zero(); + Eigen::Quaterniond quat = Eigen::Quaterniond::Identity(); + Eigen::Vector3d gyro_bias = Eigen::Vector3d::Zero(); + Eigen::Vector3d accel_bias = Eigen::Vector3d::Zero(); + + StateQuat() = default; + + Eigen::Vector16d as_vector() const { + Eigen::Vector16d vec{}; + vec << pos, vel, quat.w(), quat.x(), quat.y(), quat.z(), gyro_bias, + accel_bias; + return vec; + } + + StateQuat operator-(const StateQuat& other) const { + StateQuat diff{}; + diff.pos = pos - other.pos; + diff.vel = vel - other.vel; + diff.quat = quat * other.quat.inverse(); + diff.gyro_bias = gyro_bias - other.gyro_bias; + diff.accel_bias = accel_bias - other.accel_bias; + return diff; + } +}; + +struct StateEuler { + Eigen::Vector3d pos = Eigen::Vector3d::Zero(); + Eigen::Vector3d vel = Eigen::Vector3d::Zero(); + Eigen::Vector3d euler = Eigen::Vector3d::Zero(); + Eigen::Vector3d gyro_bias = Eigen::Vector3d::Zero(); + Eigen::Vector3d accel_bias = Eigen::Vector3d::Zero(); + + Eigen::Matrix15d covariance = Eigen::Matrix15d::Zero(); + + Eigen::Vector15d as_vector() const { + Eigen::Vector15d vec{}; + vec << pos, vel, euler, gyro_bias, accel_bias; + return vec; + } + + void set_from_vector(const Eigen::Vector15d& vec) { + pos = vec.block<3, 1>(0, 0); + vel = vec.block<3, 1>(3, 0); + euler = vec.block<3, 1>(6, 0); + gyro_bias = vec.block<3, 1>(9, 0); + accel_bias = vec.block<3, 1>(12, 0); + } +}; + +struct EskfParams { + Eigen::Matrix12d Q = Eigen::Matrix12d::Zero(); + Eigen::Matrix15d P = Eigen::Matrix15d::Zero(); + Eigen::Vector3d g_{0.0, 0.0, -9.81}; + Eigen::Vector3d initial_gyro_bias = Eigen::Vector3d::Zero(); + Eigen::Vector3d initial_accel_bias = Eigen::Vector3d::Zero(); +}; +struct ImuMeasurement { + Eigen::Vector3d accel = Eigen::Vector3d::Zero(); + Eigen::Vector3d gyro = Eigen::Vector3d::Zero(); +}; + +struct DvlMeasurement { + Eigen::Vector3d vel = Eigen::Vector3d::Zero(); + Eigen::Matrix3d cov = Eigen::Matrix3d::Zero(); +}; + +template +concept SensorModelConcept = requires(const T& meas, const StateQuat& state) { + { meas.innovation(state) } -> std::convertible_to; + { meas.jacobian(state) } -> std::convertible_to; + { meas.noise_covariance() } -> std::convertible_to; +}; // NOLINT(readability/braces) + +struct SensorDVL { + Eigen::Vector3d measurement; + Eigen::Matrix3d measurement_noise; + Eigen::VectorXd innovation(const StateQuat& state) const; + Eigen::MatrixXd jacobian(const StateQuat& state) const; + Eigen::MatrixXd noise_covariance() const; +}; + +struct SensorDepth { + double measurement; + double measurement_noise; + Eigen::VectorXd innovation(const StateQuat& state) const; + Eigen::MatrixXd jacobian(const StateQuat& state) const; + Eigen::MatrixXd noise_covariance() const; +}; + +#endif // ESKF__TYPEDEFS_HPP_ diff --git a/navigation/eskf/launch/eskf.launch.py b/navigation/eskf/launch/eskf.launch.py new file mode 100644 index 000000000..0d3acd889 --- /dev/null +++ b/navigation/eskf/launch/eskf.launch.py @@ -0,0 +1,62 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, OpaqueFunction +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + +from auv_setup.launch_arg_common import ( + declare_drone_and_namespace_args, + resolve_drone_and_namespace, +) + + +def launch_setup(context, *args, **kwargs): + drone, namespace = resolve_drone_and_namespace(context) + + drone_params = os.path.join( + get_package_share_directory("auv_setup"), + "config", + "robots", + f"{drone}.yaml", + ) + + use_sim = LaunchConfiguration('use_sim').perform(context).lower() == 'true' + + if use_sim: + param_file_name = "eskf_params.yaml" + else: + param_file_name = "eskf_params_real_world.yaml" + + eskf_params = os.path.join( + get_package_share_directory("eskf"), "config", param_file_name + ) + + eskf_node = Node( + package="eskf", + executable="eskf_node", + name="eskf_node", + namespace=namespace, + parameters=[ + eskf_params, + drone_params, + {"frame_prefix": namespace}, + ], + output="screen", + ) + + return [eskf_node] + + +def generate_launch_description(): + sim_arg = DeclareLaunchArgument( + 'use_sim', + default_value='false', + description='Set to "false" to load real-world hardware parameters.', + ) + return LaunchDescription( + [sim_arg] + + declare_drone_and_namespace_args() + + [OpaqueFunction(function=launch_setup)] + ) diff --git a/navigation/eskf/package.xml b/navigation/eskf/package.xml new file mode 100644 index 000000000..5d3f9fb9d --- /dev/null +++ b/navigation/eskf/package.xml @@ -0,0 +1,26 @@ + + + + eskf + 2.0.0 + Error-state Kalman filter + talhanc + MIT + + ament_cmake + + rclcpp + geometry_msgs + nav_msgs + eigen + tf2 + vortex_msgs + vortex_utils + vortex_utils_ros + tf2_ros + tf2_eigen + + + ament_cmake + + diff --git a/navigation/eskf/src/eskf.cpp b/navigation/eskf/src/eskf.cpp new file mode 100644 index 000000000..3db940929 --- /dev/null +++ b/navigation/eskf/src/eskf.cpp @@ -0,0 +1,221 @@ +#include "eskf/eskf.hpp" +#include +#include +#include +#include +#include "eskf/typedefs.hpp" + +double compute_nis(const Eigen::VectorXd& innovation, + const Eigen::MatrixXd& S) { + Eigen::MatrixXd S_inv = S.inverse(); + return (innovation.transpose() * S_inv * innovation)(0); +} + +ESKF::ESKF(const EskfParams& params) : Q_(params.Q) { + // Initialize Covariance + current_error_state_.covariance = params.P; + + g_ = params.g_; + + // Initialize Nominal Quaternion to Identity + current_nom_state_.quat = Eigen::Quaterniond::Identity(); + // Initialize nominal bias values + current_nom_state_.gyro_bias = params.initial_gyro_bias; + current_nom_state_.accel_bias = params.initial_accel_bias; +} + +std::pair ESKF::van_loan_discretization( + const Eigen::Matrix15d& A_c, + const Eigen::Matrix15x12d& G_c, + const double dt) { + Eigen::Matrix15d GQG_T = G_c * Q_ * G_c.transpose(); + Eigen::Matrix30d vanLoanMat = Eigen::Matrix30d::Zero(); + + vanLoanMat.topLeftCorner<15, 15>() = -A_c; + vanLoanMat.topRightCorner<15, 15>() = GQG_T; + vanLoanMat.bottomRightCorner<15, 15>() = A_c.transpose(); + + Eigen::Matrix30d vanLoanExp = (vanLoanMat * dt).exp(); + + Eigen::Matrix15d V1 = vanLoanExp.bottomRightCorner<15, 15>().transpose(); + Eigen::Matrix15d V2 = vanLoanExp.topRightCorner<15, 15>(); + + Eigen::Matrix15d A_d = V1; + Eigen::Matrix15d GQG_d = A_d * V2; + + return {A_d, GQG_d}; +} + +Eigen::Matrix3x16d calculate_hx(const StateQuat& current_nom_state_) { + Eigen::Matrix3x16d Hx = Eigen::Matrix3x16d::Zero(); + + Eigen::Quaterniond q = current_nom_state_.quat.normalized(); + Eigen::Matrix3d R_bn = q.toRotationMatrix(); + + Eigen::Vector3d v_n = current_nom_state_.vel; + + // Correct derivative w.r.t velocity (nominal state: v_n) + Hx.block<3, 3>(0, 3) = R_bn.transpose(); + + // Derivative w.r.t quaternion (nominal state: q) + // Compute partial derivative w.r.t quaternion directly: + double qw = q.w(); + Eigen::Vector3d q_vec(q.x(), q.y(), q.z()); + Eigen::Matrix3d I3 = Eigen::Matrix3d::Identity(); + + Eigen::Matrix dhdq; + dhdq.col(0) = 2 * (qw * v_n + q_vec.cross(v_n)); + dhdq.block<3, 3>(0, 1) = + 2 * (q_vec.dot(v_n) * I3 + q_vec * v_n.transpose() - + v_n * q_vec.transpose() - + qw * vortex::utils::math::get_skew_symmetric_matrix(v_n)); + + // Assign quaternion derivative (3x4 block at columns 6:9) + Hx.block<3, 4>(0, 6) = dhdq; + + return Hx; +} + +Eigen::Matrix3x15d calculate_h_jacobian(const StateQuat& current_nom_state_) { + Eigen::Matrix16x15d x_delta = Eigen::Matrix16x15d::Zero(); + x_delta.block<6, 6>(0, 0) = Eigen::Matrix6d::Identity(); + x_delta.block<4, 3>(6, 6) = + vortex::utils::math::get_transformation_matrix_attitude_quat( + current_nom_state_.quat); + x_delta.block<6, 6>(10, 9) = Eigen::Matrix6d::Identity(); + + Eigen::Matrix3x15d H = calculate_hx(current_nom_state_) * x_delta; + return H; +} + +Eigen::Vector3d calculate_h(const StateQuat& current_nom_state_) { + Eigen::Vector3d h; + Eigen::Matrix3d R_bn = + current_nom_state_.quat.normalized().toRotationMatrix().transpose(); + + h = R_bn * current_nom_state_.vel; + return h; +} + +void ESKF::nominal_state_discrete(const ImuMeasurement& imu_meas, + const double dt) { + Eigen::Vector3d acc = + current_nom_state_.quat.normalized().toRotationMatrix() * + (imu_meas.accel - current_nom_state_.accel_bias) + + this->g_; + Eigen::Vector3d gyro = (imu_meas.gyro - current_nom_state_.gyro_bias) * dt; + + current_nom_state_.pos = current_nom_state_.pos + + current_nom_state_.vel * dt + 0.5 * dt * dt * acc; + current_nom_state_.vel = current_nom_state_.vel + dt * acc; + + current_nom_state_.quat = + (current_nom_state_.quat * + vortex::utils::math::eigen_vector3d_to_quaternion(gyro)); + current_nom_state_.quat.normalize(); + + current_nom_state_.accel_bias = + current_nom_state_.accel_bias * std::exp(accm_bias_p_ * dt); + current_nom_state_.gyro_bias = + current_nom_state_.gyro_bias * std::exp(gyro_bias_p_ * dt); +} + +void ESKF::error_state_prediction(const ImuMeasurement& imu_meas, + const double dt) { + Eigen::Matrix3d R = current_nom_state_.quat.normalized().toRotationMatrix(); + Eigen::Vector3d acc = (imu_meas.accel - current_nom_state_.accel_bias); + Eigen::Vector3d gyro = (imu_meas.gyro - current_nom_state_.gyro_bias); + + Eigen::Matrix15d A_c = Eigen::Matrix15d::Zero(); + A_c.block<3, 3>(0, 3) = Eigen::Matrix3d::Identity(); + A_c.block<3, 3>(3, 6) = + -R * vortex::utils::math::get_skew_symmetric_matrix(acc); + A_c.block<3, 3>(6, 6) = + -vortex::utils::math::get_skew_symmetric_matrix(gyro); + A_c.block<3, 3>(3, 9) = -R; + A_c.block<3, 3>(9, 9) = -Eigen::Matrix3d::Identity(); + A_c.block<3, 3>(12, 12) = -Eigen::Matrix3d::Identity(); + A_c.block<3, 3>(6, 12) = -Eigen::Matrix3d::Identity(); + + Eigen::Matrix15x12d G_c = Eigen::Matrix15x12d::Zero(); + G_c.block<3, 3>(3, 0) = -R; + G_c.block<3, 3>(6, 3) = -Eigen::Matrix3d::Identity(); + G_c.block<3, 3>(9, 6) = Eigen::Matrix3d::Identity(); + G_c.block<3, 3>(12, 9) = Eigen::Matrix3d::Identity(); + + Eigen::Matrix15d A_d, GQG_d; + std::tie(A_d, GQG_d) = van_loan_discretization(A_c, G_c, dt); + + StateEuler next_error_state; + current_error_state_.covariance = + A_d * current_error_state_.covariance * A_d.transpose() + GQG_d; +} + +void ESKF::injection_and_reset() { + // injection + current_nom_state_.pos = current_nom_state_.pos + current_error_state_.pos; + current_nom_state_.vel = current_nom_state_.vel + current_error_state_.vel; + current_nom_state_.quat = current_nom_state_.quat * + vortex::utils::math::eigen_vector3d_to_quaternion( + current_error_state_.euler); + current_nom_state_.quat.normalize(); + current_nom_state_.gyro_bias = + current_nom_state_.gyro_bias + current_error_state_.gyro_bias; + current_nom_state_.accel_bias = + current_nom_state_.accel_bias + current_error_state_.accel_bias; + + // reset + current_error_state_.set_from_vector(Eigen::Vector15d::Zero()); +} + +void ESKF::imu_update(const ImuMeasurement& imu_meas, const double dt) { + nominal_state_discrete(imu_meas, dt); + error_state_prediction(imu_meas, dt); +} + +void ESKF::dvl_update(const SensorDVL& dvl_meas) { + measurement_update(dvl_meas); + injection_and_reset(); +} + +void ESKF::depth_update(const SensorDepth& depth_meas) { + measurement_update(depth_meas); + injection_and_reset(); +} + +// DVL sensor model implementations + +Eigen::VectorXd SensorDVL::innovation(const StateQuat& state) const { + Eigen::Vector3d innovation = this->measurement - calculate_h(state); + return innovation; +} + +Eigen::MatrixXd SensorDVL::jacobian(const StateQuat& state) const { + Eigen::Matrix3x15d H = calculate_h_jacobian(state); + return H; +} + +Eigen::MatrixXd SensorDVL::noise_covariance() const { + return this->measurement_noise; +} + +// Depth sensor model implementations + +Eigen::VectorXd SensorDepth::innovation(const StateQuat& state) const { + double predicted_depth = state.pos[2]; + Eigen::VectorXd innovation(1); + innovation(0) = this->measurement - predicted_depth; + return innovation; +} + +Eigen::MatrixXd SensorDepth::jacobian(const StateQuat& /*state*/) const { + Eigen::MatrixXd H = Eigen::MatrixXd::Zero(1, 15); + H(0, 2) = 1.0; + return H; +} + +Eigen::MatrixXd SensorDepth::noise_covariance() const { + Eigen::MatrixXd R(1, 1); + R(0, 0) = this->measurement_noise; + return R; +} diff --git a/navigation/eskf/src/eskf_ros.cpp b/navigation/eskf/src/eskf_ros.cpp new file mode 100644 index 000000000..6c6efca99 --- /dev/null +++ b/navigation/eskf/src/eskf_ros.cpp @@ -0,0 +1,482 @@ +#include "eskf/eskf_ros.hpp" +#include +#include +#include +#include "eskf/typedefs.hpp" + +auto start_message{R"( + ________ ______ ___ ____ ________ + |_ __ |.' ____ \ |_ ||_ _| |_ __ | + | |_ \_|| (___ \_| | |_/ / | |_ \_| + | _| _ _.____`. | __'. | _| + _| |__/ || \____) | _| | \ \_ _| |_ + |________| \______.'|____||____||_____| +)"}; + +ESKFNode::ESKFNode(const rclcpp::NodeOptions& options) + : Node("eskf_node", options) { + use_tf_transforms_ = this->declare_parameter("use_tf_transforms"); + tf_sensors_loaded_ = !use_tf_transforms_; + + frame_prefix_ = this->declare_parameter("frame_prefix", ""); + if (!frame_prefix_.empty() && frame_prefix_.back() == '/') { + frame_prefix_.pop_back(); + } + spdlog::info("frame_prefix set to '{}'", frame_prefix_); + + publish_tf_ = this->declare_parameter("publish_tf"); + if (publish_tf_) { + tf_broadcaster_ = + std::make_unique(*this); + } + + publish_pose_ = this->declare_parameter("publish_pose"); + publish_twist_ = this->declare_parameter("publish_twist"); + + publish_biases_ = this->declare_parameter("publish_biases"); + + // Declare these here so they appear in `ros2 param list` from startup, + // even though they are read in complete_initialization(). + this->declare_parameter("publish_rate_ms"); + this->declare_parameter("topics.imu"); + this->declare_parameter("topics.dvl_twist"); + this->declare_parameter("topics.pressure_sensor"); + this->declare_parameter("topics.odom"); + this->declare_parameter("topics.pose"); + this->declare_parameter("topics.twist"); + + if (use_tf_transforms_) { + tf_buffer_ = std::make_shared(this->get_clock()); + tf_listener_ = + std::make_shared(*tf_buffer_); + tf_timer_ = this->create_wall_timer( + std::chrono::milliseconds(500), + std::bind(&ESKFNode::lookup_static_transforms, this)); + } else { + spdlog::info( + "Using parameter-based sensor transforms. TF lookup disabled."); + complete_initialization(); + } +} + +void ESKFNode::set_subscribers_and_publisher() { + auto qos_sensor_data = vortex::utils::qos_profiles::sensor_data_profile(1); + + std::string imu_topic = this->get_parameter("topics.imu").as_string(); + imu_sub_ = this->create_subscription( + imu_topic, qos_sensor_data, + std::bind(&ESKFNode::imu_callback, this, std::placeholders::_1)); + + std::string dvl_topic = this->get_parameter("topics.dvl_twist").as_string(); + dvl_sub_ = this->create_subscription< + geometry_msgs::msg::TwistWithCovarianceStamped>( + dvl_topic, qos_sensor_data, + std::bind(&ESKFNode::dvl_callback, this, std::placeholders::_1)); + + std::string pressure_topic = + this->get_parameter("topics.pressure_sensor").as_string(); + depth_sub_ = this->create_subscription( + pressure_topic, qos_sensor_data, + std::bind(&ESKFNode::depth_callback, this, std::placeholders::_1)); + + // std::string odom_topic = this->get_parameter("topics.odom").as_string(); + odom_pub_ = this->create_publisher( + "/nautilus/odom_eskf", qos_sensor_data); + + if (publish_pose_) { + std::string pose_topic = this->get_parameter("topics.pose").as_string(); + pose_pub_ = this->create_publisher< + geometry_msgs::msg::PoseWithCovarianceStamped>(pose_topic, + qos_sensor_data); + } + + if (publish_twist_) { + std::string twist_topic = + this->get_parameter("topics.twist").as_string(); + twist_pub_ = this->create_publisher< + geometry_msgs::msg::TwistWithCovarianceStamped>(twist_topic, + qos_sensor_data); + } + + if (publish_biases_) { + accel_bias_pub_ = + this->create_publisher( + "eskf/accel_bias", qos_sensor_data); + gyro_bias_pub_ = + this->create_publisher( + "eskf/gyro_bias", qos_sensor_data); + } + +#ifndef NDEBUG + nis_dvl_pub_ = create_publisher( + "eskf/nis_dvl", vortex::utils::qos_profiles::reliable_profile()); + nis_depth_pub_ = create_publisher( + "eskf/nis_depth", vortex::utils::qos_profiles::reliable_profile()); +#endif +} + +void ESKFNode::set_parameters() { + if (!use_tf_transforms_) { + std::vector R_imu_correction = + this->declare_parameter>( + "transform.imu_frame_r"); + R_imu_eskf_ = Eigen::Map>( + R_imu_correction.data()); + + std::vector T_imu_correction = + this->declare_parameter>( + "transform.imu_frame_t"); + T_imu_eskf_ = Eigen::Map(T_imu_correction.data()); + + std::vector R_dvl_correction = + this->declare_parameter>( + "transform.dvl_frame_r"); + R_dvl_eskf_ = Eigen::Map>( + R_dvl_correction.data()); + + std::vector T_dvl_correction = + this->declare_parameter>( + "transform.dvl_frame_t"); + T_dvl_eskf_ = Eigen::Map(T_dvl_correction.data()); + + std::vector T_depth_correction = + this->declare_parameter>( + "transform.depth_frame_t"); + T_depth_eskf_ = Eigen::Map(T_depth_correction.data()); + } + + std::vector diag_Q_std; + this->declare_parameter>("diag_Q_std"); + + diag_Q_std = this->get_parameter("diag_Q_std").as_double_array(); + + if (diag_Q_std.size() != 12) { + throw std::runtime_error("diag_Q_std must have length 12"); + } + + Eigen::Matrix12d Q = Eigen::Map(diag_Q_std.data()) + .array() + .square() + .matrix() + .asDiagonal(); + + std::vector diag_p_init = + this->declare_parameter>("diag_p_init"); + if (diag_p_init.size() != 15) { + throw std::runtime_error("diag_p_init must have length 15"); + } + Eigen::Matrix15d P = createDiagonalMatrix<15>(diag_p_init); + + Eigen::Vector3d g_vec(0.0, 0.0, this->gravity); + + std::vector initial_gyro_bias = + this->declare_parameter>( + "initial_gyro_bias", std::vector{0.0, 0.0, 0.0}); + spdlog::info("initial_gyro_bias: [{}, {}, {}]", initial_gyro_bias[0], + initial_gyro_bias[1], initial_gyro_bias[2]); + if (initial_gyro_bias.size() != 3) { + throw std::runtime_error("initial_gyro_bias must have length 3"); + } + + std::vector initial_accel_bias = + this->declare_parameter>( + "initial_accel_bias", std::vector{0.0, 0.0, 0.0}); + spdlog::info("initial_accel_bias: [{}, {}, {}]", initial_accel_bias[0], + initial_accel_bias[1], initial_accel_bias[2]); + if (initial_accel_bias.size() != 3) { + throw std::runtime_error("initial_accel_bias must have length 3"); + } + + EskfParams eskf_params{ + .Q = Q, + .P = P, + .g_ = g_vec, + .initial_gyro_bias = + Eigen::Map(initial_gyro_bias.data()), + .initial_accel_bias = + Eigen::Map(initial_accel_bias.data())}; + + eskf_ = std::make_unique(eskf_params); + + add_gravity_to_imu_ = this->declare_parameter("add_gravity_to_imu"); + spdlog::info("add_gravity_to_imu: {}", add_gravity_to_imu_); +} + +void ESKFNode::imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg) { + rclcpp::Time current_time = msg->header.stamp; + + if (!first_imu_msg_received_) { + last_imu_time_ = current_time; + first_imu_msg_received_ = true; + return; + } + + double dt = (current_time - last_imu_time_).nanoseconds() * 1e-9; + last_imu_time_ = current_time; + + ImuMeasurement imu_measurement{}; + + Eigen::Vector3d raw_accel(msg->linear_acceleration.x, + msg->linear_acceleration.y, + msg->linear_acceleration.z); + + Eigen::Vector3d raw_gyro(msg->angular_velocity.x, msg->angular_velocity.y, + msg->angular_velocity.z); + + Eigen::Vector3d accel_aligned = R_imu_eskf_ * raw_accel; + + // currently the gyro and the accelorometer are rotated differently in sim. + // should be changed with the actual drone params. + // Eigen::Vector3d gyro_aligned = R_imu_eskf_ * raw_gyro; + Eigen::Vector3d gyro_aligned = raw_gyro; + imu_measurement.gyro = gyro_aligned; + + // lever arm correction for accelerometer + StateQuat nom_state = eskf_->get_nominal_state(); + Eigen::Vector3d omega = gyro_aligned - nom_state.gyro_bias; + + // a_corrected = a_meas - omega x (omega x T) + Eigen::Vector3d centripetal_accel = omega.cross(omega.cross(T_imu_eskf_)); + accel_aligned -= centripetal_accel; + + if (add_gravity_to_imu_) { + Eigen::Matrix3d R = nom_state.quat.normalized().toRotationMatrix(); + accel_aligned -= R.transpose() * eskf_->get_gravity(); + } + + imu_measurement.accel = accel_aligned; + + // save latest gyro readings (used for DVL correction and odom output) + latest_gyro_measurement_ = imu_measurement.gyro; + + eskf_->imu_update(imu_measurement, dt); +} + +void ESKFNode::dvl_callback( + const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) { + SensorDVL dvl_sensor; + + dvl_sensor.measurement << msg->twist.twist.linear.x, + msg->twist.twist.linear.y, msg->twist.twist.linear.z; + + dvl_sensor.measurement_noise << msg->twist.covariance[0], + msg->twist.covariance[1], msg->twist.covariance[2], + msg->twist.covariance[6], msg->twist.covariance[7], + msg->twist.covariance[8], msg->twist.covariance[12], + msg->twist.covariance[13], msg->twist.covariance[14]; + + // Apply the rotation and translation corrections to the DVL measurement + StateQuat nom_state = eskf_->get_nominal_state(); + // get the angular velocity + Eigen::Vector3d omega_corrected = + latest_gyro_measurement_ - nom_state.gyro_bias; + // correct rotation and translation: v_base = v_sensor - omega x T + dvl_sensor.measurement = R_dvl_eskf_ * dvl_sensor.measurement - + omega_corrected.cross(T_dvl_eskf_); + dvl_sensor.measurement_noise = + R_dvl_eskf_ * dvl_sensor.measurement_noise * R_dvl_eskf_.transpose(); + + eskf_->dvl_update(dvl_sensor); + +#ifndef NDEBUG + // Publish NIS in Debug mode + std_msgs::msg::Float64 nis_msg; + nis_msg.data = eskf_->get_nis(); + nis_dvl_pub_->publish(nis_msg); +#endif +} + +void ESKFNode::depth_callback( + const sensor_msgs::msg::FluidPressure::SharedPtr msg) { + SensorDepth depth_sensor; + // the simulation is a gauge sensor so we don't subtract atmospheric + // pressure. + depth_sensor.measurement = + -msg->fluid_pressure / (this->water_density * this->gravity); + depth_sensor.measurement_noise = msg->variance; + + // spdlog::info("depth meas is: {}",depth_sensor.measurement); + eskf_->depth_update(depth_sensor); + +#ifndef NDEBUG + // Publish NIS in Debug mode + std_msgs::msg::Float64 nis_msg; + nis_msg.data = eskf_->get_nis(); + nis_depth_pub_->publish(nis_msg); +#endif +} + +void ESKFNode::publish_odom() { + nav_msgs::msg::Odometry odom_msg; + StateQuat nom_state = eskf_->get_nominal_state(); + StateEuler error_state_ = eskf_->get_error_state(); + + odom_msg.pose.pose.position.x = nom_state.pos.x(); + odom_msg.pose.pose.position.y = nom_state.pos.y(); + odom_msg.pose.pose.position.z = nom_state.pos.z(); + + odom_msg.pose.pose.orientation.w = nom_state.quat.w(); + odom_msg.pose.pose.orientation.x = nom_state.quat.x(); + odom_msg.pose.pose.orientation.y = nom_state.quat.y(); + odom_msg.pose.pose.orientation.z = nom_state.quat.z(); + + // publishing the velocity in the body frame + Eigen::Matrix3d R_body_to_world = nom_state.quat.toRotationMatrix(); + + Eigen::Vector3d v_body = R_body_to_world.transpose() * nom_state.vel; + + odom_msg.twist.twist.linear.x = v_body.x(); + odom_msg.twist.twist.linear.y = v_body.y(); + odom_msg.twist.twist.linear.z = v_body.z(); + + // Add bias values to the angular velocity field of twist + Eigen::Vector3d body_angular_vel = + latest_gyro_measurement_ - nom_state.gyro_bias; + odom_msg.twist.twist.angular.x = body_angular_vel.x(); + odom_msg.twist.twist.angular.y = body_angular_vel.y(); + odom_msg.twist.twist.angular.z = body_angular_vel.z(); + + // If you also want to include gyro bias, you could add it to the covariance + // matrix or publish a separate topic for biases + rclcpp::Time current_time = this->now(); + odom_msg.header.stamp = current_time; + odom_msg.header.frame_id = frame("odom"); + + // Some cross terms of the covariance are ignored, and the acc/gyro biases + // cov are not published. Pos and orientation cov needs to be mapped from + // 6*6 matrix to an array (states 0-2) + + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + odom_msg.pose.covariance[i * 6 + j] = error_state_.covariance(i, j); + } + } + + // Orientation covariance (states 6–8) + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + odom_msg.pose.covariance[(i + 3) * 6 + (j + 3)] = + error_state_.covariance(i + 6, j + 6); + } + } + + // Linear velocity covariance + for (int i = 0; i < 3; ++i) { + for (int j = 0; j < 3; ++j) { + odom_msg.twist.covariance[i * 6 + j] = + error_state_.covariance(i + 3, j + 3); + } + } + odom_pub_->publish(odom_msg); + + if (publish_pose_) { + geometry_msgs::msg::PoseWithCovarianceStamped pose_msg; + pose_msg.header = odom_msg.header; + pose_msg.pose = odom_msg.pose; + pose_pub_->publish(pose_msg); + } + + if (publish_twist_) { + geometry_msgs::msg::TwistWithCovarianceStamped twist_msg; + twist_msg.header = odom_msg.header; + twist_msg.twist = odom_msg.twist; + twist_pub_->publish(twist_msg); + } + + if (publish_tf_) { + publish_tf(nom_state, current_time); + } + + if (publish_biases_) { + geometry_msgs::msg::Vector3Stamped accel_bias_msg; + accel_bias_msg.header.stamp = current_time; + accel_bias_msg.header.frame_id = + frame("base_link"); // Biases are in the body frame + + accel_bias_msg.vector.x = nom_state.accel_bias.x(); + accel_bias_msg.vector.y = nom_state.accel_bias.y(); + accel_bias_msg.vector.z = nom_state.accel_bias.z(); + + accel_bias_pub_->publish(accel_bias_msg); + + geometry_msgs::msg::Vector3Stamped gyro_bias_msg; + gyro_bias_msg.header = accel_bias_msg.header; + + gyro_bias_msg.vector.x = nom_state.gyro_bias.x(); + gyro_bias_msg.vector.y = nom_state.gyro_bias.y(); + gyro_bias_msg.vector.z = nom_state.gyro_bias.z(); + + gyro_bias_pub_->publish(gyro_bias_msg); + } +} + +void ESKFNode::lookup_static_transforms() { + try { + Tf_base_imu_ = tf2::transformToEigen(tf_buffer_->lookupTransform( + frame("base_link"), frame("imu_link"), tf2::TimePointZero)); + R_imu_eskf_ = Tf_base_imu_.rotation(); + T_imu_eskf_ = Tf_base_imu_.translation(); + + Tf_base_dvl_ = tf2::transformToEigen(tf_buffer_->lookupTransform( + frame("base_link"), frame("dvl_link"), tf2::TimePointZero)); + R_dvl_eskf_ = Tf_base_dvl_.rotation(); + T_dvl_eskf_ = Tf_base_dvl_.translation(); + + Tf_base_depth_ = tf2::transformToEigen(tf_buffer_->lookupTransform( + frame("base_link"), frame("pressure_sensor_link"), + tf2::TimePointZero)); + T_depth_eskf_ = Tf_base_depth_.translation(); + + tf_sensors_loaded_ = true; + tf_timer_->cancel(); + spdlog::info("All static transforms loaded successfully."); + complete_initialization(); + } catch (const tf2::TransformException& ex) { + spdlog::warn("TF Lookup failed (will retry): {}", ex.what()); + } +} + +void ESKFNode::complete_initialization() { + set_subscribers_and_publisher(); + // gravity, water density and atmospheric pressure. + this->gravity = -this->declare_parameter("gravity", 9.81); + this->water_density = + this->declare_parameter("water_density", 1000.0); + this->atmospheric_pressure = + this->declare_parameter("atmospheric_pressure", 100000.0); + set_parameters(); + + time_step_ = std::chrono::milliseconds( + this->get_parameter("publish_rate_ms").as_int()); + odom_pub_timer_ = this->create_wall_timer( + time_step_, std::bind(&ESKFNode::publish_odom, this)); + + spdlog::info(start_message); + +#ifndef NDEBUG + spdlog::info( + "______________________Debug mode is enabled______________________"); +#endif +} + +void ESKFNode::publish_tf(const StateQuat& nom_state, + const rclcpp::Time& time) { + geometry_msgs::msg::TransformStamped tf_msg; + + tf_msg.header.stamp = time; + tf_msg.header.frame_id = frame("odom"); + tf_msg.child_frame_id = frame("base_link"); + + tf_msg.transform.translation.x = nom_state.pos.x(); + tf_msg.transform.translation.y = nom_state.pos.y(); + tf_msg.transform.translation.z = nom_state.pos.z(); + + tf_msg.transform.rotation.w = nom_state.quat.w(); + tf_msg.transform.rotation.x = nom_state.quat.x(); + tf_msg.transform.rotation.y = nom_state.quat.y(); + tf_msg.transform.rotation.z = nom_state.quat.z(); + + tf_broadcaster_->sendTransform(tf_msg); +} + +RCLCPP_COMPONENTS_REGISTER_NODE(ESKFNode) diff --git a/navigation/eskf/test/eskf_consistency_test.py b/navigation/eskf/test/eskf_consistency_test.py new file mode 100644 index 000000000..662e06421 --- /dev/null +++ b/navigation/eskf/test/eskf_consistency_test.py @@ -0,0 +1,181 @@ +import rclpy +from rclpy.node import Node +from nav_msgs.msg import Odometry +from std_msgs.msg import Float64 +from std_msgs.msg import Float64MultiArray +import message_filters +import numpy as np +from scipy.spatial.transform import Rotation + +# --- IMPORT YOUR EXISTING QOS FUNCTION --- +try: + from vortex.utils.qos_profiles import sensor_data_profile +except ImportError: + # Fallback for testing without the vortex library + from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy + def sensor_data_profile(depth=5): + return QoSProfile(reliability=ReliabilityPolicy.BEST_EFFORT, history=HistoryPolicy.KEEP_LAST, depth=depth) + +class EskfValidator(Node): + def __init__(self): + super().__init__('eskf_validator') + + self.est_topic = '/nautilus/odom/eskf' + self.gt_topic = '/nautilus/odom' + + # --- Publishers (Foxglove Visualizers) --- + # Overall 3D RMSE + self.pub_rmse_pos_total = self.create_publisher(Float64, 'eskf_metrics/rmse/position/total', 10) + self.pub_rmse_vel_total = self.create_publisher(Float64, 'eskf_metrics/rmse/velocity/total', 10) + self.pub_rmse_ori = self.create_publisher(Float64, 'eskf_metrics/rmse/orientation_rad', 10) + + # Axis-Specific Position RMSE + self.pub_rmse_pos_x = self.create_publisher(Float64, 'eskf_metrics/rmse/position/x', 10) + self.pub_rmse_pos_y = self.create_publisher(Float64, 'eskf_metrics/rmse/position/y', 10) + self.pub_rmse_pos_z = self.create_publisher(Float64, 'eskf_metrics/rmse/position/z', 10) + + # Axis-Specific Velocity RMSE + self.pub_rmse_vel_x = self.create_publisher(Float64, 'eskf_metrics/rmse/velocity/x', 10) + self.pub_rmse_vel_y = self.create_publisher(Float64, 'eskf_metrics/rmse/velocity/y', 10) + self.pub_rmse_vel_z = self.create_publisher(Float64, 'eskf_metrics/rmse/velocity/z', 10) + + # Euler Angles & NEES + self.pub_euler_est = self.create_publisher(Float64MultiArray, 'debug/euler/est', 10) + self.pub_euler_gt = self.create_publisher(Float64MultiArray, 'debug/euler/gt', 10) + self.pub_nees_pos = self.create_publisher(Float64, 'eskf_metrics/nees/position', 10) + self.pub_nees_ori = self.create_publisher(Float64, 'eskf_metrics/nees/orientation', 10) + + # --- State for Running RMSE --- + self.n_samples = 0 + + # NumPy arrays make axis-by-axis tracking incredibly easy + self.sse_pos_xyz = np.array([0.0, 0.0, 0.0]) + self.sse_vel_xyz = np.array([0.0, 0.0, 0.0]) + + # Total Euclidean SSE + self.sse_pos_total = 0.0 + self.sse_vel_total = 0.0 + self.sse_ori = 0.0 + + # --- Subscribers --- + self.get_logger().info(f"Subscribing to {self.est_topic} and {self.gt_topic}...") + qos = sensor_data_profile(depth=5) + + self.sub_est = message_filters.Subscriber(self, Odometry, self.est_topic, qos_profile=qos) + self.sub_gt = message_filters.Subscriber(self, Odometry, self.gt_topic, qos_profile=qos) + + self.ts = message_filters.ApproximateTimeSynchronizer( + [self.sub_est, self.sub_gt], queue_size=10, slop=0.1 + ) + self.ts.registerCallback(self.callback) + + def callback(self, est_msg, gt_msg): + self.n_samples += 1 + + # =========================== + # 1. POSITION (Euclidean & Axis) + # =========================== + p_est = np.array([est_msg.pose.pose.position.x, est_msg.pose.pose.position.y, est_msg.pose.pose.position.z]) + p_gt = np.array([gt_msg.pose.pose.position.x, gt_msg.pose.pose.position.y, gt_msg.pose.pose.position.z]) + + err_pos_vec = p_est - p_gt + + # Update Axis-Specific RMSE (Squares elements individually) + self.sse_pos_xyz += err_pos_vec**2 + rmse_pos_xyz = np.sqrt(self.sse_pos_xyz / self.n_samples) + + self.publish_float(self.pub_rmse_pos_x, rmse_pos_xyz[0]) + self.publish_float(self.pub_rmse_pos_y, rmse_pos_xyz[1]) + self.publish_float(self.pub_rmse_pos_z, rmse_pos_xyz[2]) + + # Update Total 3D RMSE + self.sse_pos_total += np.linalg.norm(err_pos_vec)**2 + self.publish_float(self.pub_rmse_pos_total, np.sqrt(self.sse_pos_total / self.n_samples)) + + # =========================== + # 2. ORIENTATION (Quaternion) + # =========================== + q_est = [est_msg.pose.pose.orientation.x, est_msg.pose.pose.orientation.y, est_msg.pose.pose.orientation.z, est_msg.pose.pose.orientation.w] + q_gt = [gt_msg.pose.pose.orientation.x, gt_msg.pose.pose.orientation.y, gt_msg.pose.pose.orientation.z, gt_msg.pose.pose.orientation.w] + + r_est = Rotation.from_quat(q_est) + r_gt = Rotation.from_quat(q_gt) + + euler_est = r_est.as_euler('xyz', degrees=True) + euler_gt = r_gt.as_euler('xyz', degrees=True) + + msg_euler_est = Float64MultiArray() + msg_euler_est.data = euler_est.tolist() + self.pub_euler_est.publish(msg_euler_est) + + msg_euler_gt = Float64MultiArray() + msg_euler_gt.data = euler_gt.tolist() + self.pub_euler_gt.publish(msg_euler_gt) + + r_err = r_gt.inv() * r_est + err_ori_vec = r_err.as_rotvec() + + self.sse_ori += np.linalg.norm(err_ori_vec)**2 + self.publish_float(self.pub_rmse_ori, np.sqrt(self.sse_ori / self.n_samples)) + + # =========================== + # 3. VELOCITY (Euclidean & Axis) + # =========================== + v_est = np.array([est_msg.twist.twist.linear.x, est_msg.twist.twist.linear.y, est_msg.twist.twist.linear.z]) + v_gt = np.array([gt_msg.twist.twist.linear.x, gt_msg.twist.twist.linear.y, gt_msg.twist.twist.linear.z]) + + err_vel_vec = v_est - v_gt + + # Update Axis-Specific RMSE + self.sse_vel_xyz += err_vel_vec**2 + rmse_vel_xyz = np.sqrt(self.sse_vel_xyz / self.n_samples) + + self.publish_float(self.pub_rmse_vel_x, rmse_vel_xyz[0]) + self.publish_float(self.pub_rmse_vel_y, rmse_vel_xyz[1]) + self.publish_float(self.pub_rmse_vel_z, rmse_vel_xyz[2]) + + # Update Total 3D RMSE + self.sse_vel_total += np.linalg.norm(err_vel_vec)**2 + self.publish_float(self.pub_rmse_vel_total, np.sqrt(self.sse_vel_total / self.n_samples)) + + # =========================== + # 4. NEES CALCULATION + # =========================== + cov_pose = np.array(est_msg.pose.covariance).reshape(6, 6) + + # --- Position NEES --- + cov_pos = cov_pose[0:3, 0:3] + try: + cov_pos_inv = np.linalg.inv(cov_pos) + nees_pos = err_pos_vec.T @ cov_pos_inv @ err_pos_vec + self.publish_float(self.pub_nees_pos, nees_pos) + except np.linalg.LinAlgError: + pass + + # --- Orientation NEES --- + cov_ori = cov_pose[3:6, 3:6] + try: + cov_ori_inv = np.linalg.inv(cov_ori) + nees_ori = err_ori_vec.T @ cov_ori_inv @ err_ori_vec + self.publish_float(self.pub_nees_ori, nees_ori) + except np.linalg.LinAlgError: + pass + + def publish_float(self, publisher, value): + msg = Float64() + msg.data = float(value) + publisher.publish(msg) + +def main(args=None): + rclpy.init(args=args) + node = EskfValidator() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/navigation/odom_transformer/CMakeLists.txt b/navigation/odom_transformer/CMakeLists.txt new file mode 100644 index 000000000..6a27c4b62 --- /dev/null +++ b/navigation/odom_transformer/CMakeLists.txt @@ -0,0 +1,67 @@ +cmake_minimum_required(VERSION 3.8) +project(odom_transformer) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 20) +endif() + +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_components REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(tf2_eigen REQUIRED) + +include_directories(include ${EIGEN3_INCLUDE_DIRS}) + +set(LIB_NAME "${PROJECT_NAME}_component") + +add_library(${LIB_NAME} SHARED + src/odom_transformer.cpp +) + +ament_target_dependencies(${LIB_NAME} PUBLIC + rclcpp + rclcpp_components + nav_msgs + geometry_msgs + Eigen3 + tf2 + tf2_ros + tf2_eigen +) + +rclcpp_components_register_node( + ${LIB_NAME} + PLUGIN "OdomTransformer" + EXECUTABLE ${PROJECT_NAME}_node +) + +ament_export_targets(export_${LIB_NAME}) + +install(TARGETS ${LIB_NAME} + EXPORT export_${LIB_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install( + DIRECTORY include/ + DESTINATION include +) + +install(DIRECTORY + launch + config + DESTINATION share/${PROJECT_NAME}/ +) + +ament_package() diff --git a/navigation/odom_transformer/config/odom_transformer_params.yaml b/navigation/odom_transformer/config/odom_transformer_params.yaml new file mode 100644 index 000000000..8927450fe --- /dev/null +++ b/navigation/odom_transformer/config/odom_transformer_params.yaml @@ -0,0 +1,13 @@ +/**: + odom_transformer_node: + ros__parameters: + sensor_frame: "dvl_link" + publish_tf: true + publish_pose: true + publish_twist: true + rotate_yaw_180: false + topics: + input: "nucleus/odom" + output: "odom" + pose: "pose" + twist: "twist" diff --git a/navigation/odom_transformer/include/odom_transformer/odom_transformer.hpp b/navigation/odom_transformer/include/odom_transformer/odom_transformer.hpp new file mode 100644 index 000000000..b58f029dd --- /dev/null +++ b/navigation/odom_transformer/include/odom_transformer/odom_transformer.hpp @@ -0,0 +1,57 @@ +#ifndef ODOM_TRANSFORMER__ODOM_TRANSFORMER_HPP_ +#define ODOM_TRANSFORMER__ODOM_TRANSFORMER_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +class OdomTransformer : public rclcpp::Node { + public: + explicit OdomTransformer( + const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); + + private: + void odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg); + void lookup_static_transforms(); + void complete_initialization(); + + std::string frame(const std::string& name) const { + return frame_prefix_.empty() ? name : frame_prefix_ + "/" + name; + } + + // TF2 + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + std::unique_ptr tf_broadcaster_; + rclcpp::TimerBase::SharedPtr tf_timer_; + + // Pub / Sub + rclcpp::Subscription::SharedPtr odom_sub_; + rclcpp::Publisher::SharedPtr odom_pub_; + rclcpp::Publisher::SharedPtr + pose_pub_; + rclcpp::Publisher::SharedPtr + twist_pub_; + + // Transform from base_link to sensor_link + Eigen::Matrix3d R_base_sensor_ = Eigen::Matrix3d::Identity(); + Eigen::Vector3d t_base_sensor_ = Eigen::Vector3d::Zero(); + + std::string frame_prefix_; + std::string sensor_frame_; + bool tf_loaded_{false}; + bool publish_tf_{false}; + bool publish_pose_{false}; + bool publish_twist_{false}; + bool rotate_yaw_180_{false}; +}; + +#endif // ODOM_TRANSFORMER__ODOM_TRANSFORMER_HPP_ diff --git a/navigation/odom_transformer/launch/odom_transformer.launch.py b/navigation/odom_transformer/launch/odom_transformer.launch.py new file mode 100644 index 000000000..7e5bd759f --- /dev/null +++ b/navigation/odom_transformer/launch/odom_transformer.launch.py @@ -0,0 +1,50 @@ +import os +from os import path + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import OpaqueFunction +from launch_ros.actions import Node + +from auv_setup.launch_arg_common import ( + declare_drone_and_namespace_args, + resolve_drone_and_namespace, +) + +odom_transformer_params = path.join( + get_package_share_directory("odom_transformer"), + "config", + "odom_transformer_params.yaml", +) + + +def launch_setup(context, *args, **kwargs): + drone, namespace = resolve_drone_and_namespace(context) + + drone_params = os.path.join( + get_package_share_directory("auv_setup"), + "config", + "robots", + f"{drone}.yaml", + ) + + node = Node( + package="odom_transformer", + executable="odom_transformer_node", + name="odom_transformer_node", + namespace=namespace, + parameters=[ + odom_transformer_params, + drone_params, + {"frame_prefix": namespace}, + ], + output="screen", + ) + + return [node] + + +def generate_launch_description(): + return LaunchDescription( + declare_drone_and_namespace_args() + [OpaqueFunction(function=launch_setup)] + ) diff --git a/navigation/odom_transformer/package.xml b/navigation/odom_transformer/package.xml new file mode 100644 index 000000000..9658764c8 --- /dev/null +++ b/navigation/odom_transformer/package.xml @@ -0,0 +1,26 @@ + + + + odom_transformer + 0.1.0 + Transforms odometry from a sensor frame to base_link using tf2 static transforms + jorgen fjermedal + MIT + + ament_cmake + + rclcpp + rclcpp_components + nav_msgs + geometry_msgs + eigen + tf2 + tf2_ros + tf2_eigen + + auv_setup + + + ament_cmake + + diff --git a/navigation/odom_transformer/src/odom_transformer.cpp b/navigation/odom_transformer/src/odom_transformer.cpp new file mode 100644 index 000000000..a210c916c --- /dev/null +++ b/navigation/odom_transformer/src/odom_transformer.cpp @@ -0,0 +1,191 @@ +#include "odom_transformer/odom_transformer.hpp" +#include + +OdomTransformer::OdomTransformer(const rclcpp::NodeOptions& options) + : Node("odom_transformer_node", options) { + frame_prefix_ = this->declare_parameter("frame_prefix"); + if (!frame_prefix_.empty() && frame_prefix_.back() == '/') { + frame_prefix_.pop_back(); + } + + sensor_frame_ = this->declare_parameter("sensor_frame"); + publish_tf_ = this->declare_parameter("publish_tf"); + publish_pose_ = this->declare_parameter("publish_pose"); + publish_twist_ = this->declare_parameter("publish_twist"); + + if (publish_tf_) { + tf_broadcaster_ = + std::make_unique(*this); + } + + this->declare_parameter("topics.input"); + this->declare_parameter("topics.output"); + this->declare_parameter("topics.pose"); + this->declare_parameter("topics.twist"); + rotate_yaw_180_ = this->declare_parameter("rotate_yaw_180"); + + + tf_buffer_ = std::make_shared(this->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + tf_timer_ = this->create_wall_timer( + std::chrono::milliseconds(500), + std::bind(&OdomTransformer::lookup_static_transforms, this)); +} + +void OdomTransformer::lookup_static_transforms() { + try { + auto tf = tf2::transformToEigen(tf_buffer_->lookupTransform( + frame("base_link"), frame(sensor_frame_), tf2::TimePointZero)); + R_base_sensor_ = tf.rotation(); + t_base_sensor_ = tf.translation(); + + tf_loaded_ = true; + tf_timer_->cancel(); + RCLCPP_INFO(get_logger(), + "Loaded static transform: %s -> %s t=(%.3f, %.3f, %.3f)", + frame("base_link").c_str(), frame(sensor_frame_).c_str(), + t_base_sensor_.x(), t_base_sensor_.y(), t_base_sensor_.z()); + complete_initialization(); + } catch (const tf2::TransformException& ex) { + RCLCPP_WARN(get_logger(), "TF lookup failed (will retry): %s", + ex.what()); + } +} + +void OdomTransformer::complete_initialization() { + auto qos = rclcpp::QoS(1).best_effort(); + + auto input_topic = this->get_parameter("topics.input").as_string(); + auto output_topic = this->get_parameter("topics.output").as_string(); + + odom_sub_ = this->create_subscription( + input_topic, qos, + std::bind(&OdomTransformer::odom_callback, this, + std::placeholders::_1)); + + odom_pub_ = + this->create_publisher(output_topic, qos); + + if (publish_pose_) { + pose_pub_ = this->create_publisher< + geometry_msgs::msg::PoseWithCovarianceStamped>( + this->get_parameter("topics.pose").as_string(), qos); + } + + if (publish_twist_) { + twist_pub_ = this->create_publisher< + geometry_msgs::msg::TwistWithCovarianceStamped>( + this->get_parameter("topics.twist").as_string(), qos); + } + + RCLCPP_INFO(get_logger(), "Odom transformer: %s -> %s", input_topic.c_str(), + output_topic.c_str()); +} + +void OdomTransformer::odom_callback( + const nav_msgs::msg::Odometry::SharedPtr msg) { + // Current orientation of the sensor in odom frame + Eigen::Quaterniond q_odom_sensor( + msg->pose.pose.orientation.w, msg->pose.pose.orientation.x, + msg->pose.pose.orientation.y, msg->pose.pose.orientation.z); + + + // Velocities in sensor frame + Eigen::Vector3d v_sensor(msg->twist.twist.linear.x, + msg->twist.twist.linear.y, + msg->twist.twist.linear.z); + Eigen::Vector3d omega_sensor(msg->twist.twist.angular.x, + msg->twist.twist.angular.y, + msg->twist.twist.angular.z); + + if (rotate_yaw_180_) { + // 180 deg yaw flips X and Y, leaves Z unchanged + q_odom_sensor = + Eigen::Quaterniond(Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitZ())) + * q_odom_sensor; + v_sensor.x() = -v_sensor.x(); + v_sensor.y() = -v_sensor.y(); + omega_sensor.x() = -omega_sensor.x(); + omega_sensor.y() = -omega_sensor.y(); + msg->pose.pose.position.x = -msg->pose.pose.position.x; + msg->pose.pose.position.y = -msg->pose.pose.position.y; + } + + Eigen::Matrix3d R_odom_sensor = q_odom_sensor.toRotationMatrix(); + + // Orientation: R_odom_base = R_odom_sensor * R_base_sensor^-1 + Eigen::Matrix3d R_odom_base = R_odom_sensor * R_base_sensor_.transpose(); + Eigen::Quaterniond q_odom_base(R_odom_base); + q_odom_base.normalize(); + + // Position: p_base = p_sensor - R_odom_base * t_base_sensor + Eigen::Vector3d p_sensor(msg->pose.pose.position.x, + msg->pose.pose.position.y, + msg->pose.pose.position.z); + Eigen::Vector3d p_base = p_sensor - R_odom_base * t_base_sensor_; + + // Angular velocity: rotate from sensor frame to base_link frame + Eigen::Vector3d omega_base = R_base_sensor_ * omega_sensor; + + // Linear velocity: lever arm correction + // v_base = R_base_sensor * v_sensor - omega_base x t_base_sensor + Eigen::Vector3d v_base = + R_base_sensor_ * v_sensor - omega_base.cross(t_base_sensor_); + + // Build output odometry + auto out = std::make_unique(); + out->header.stamp = msg->header.stamp; + out->header.frame_id = frame("odom"); + out->child_frame_id = frame("base_link"); + + out->pose.pose.position.x = p_base.x(); + out->pose.pose.position.y = p_base.y(); + out->pose.pose.position.z = p_base.z(); + out->pose.pose.orientation.w = q_odom_base.w(); + out->pose.pose.orientation.x = q_odom_base.x(); + out->pose.pose.orientation.y = q_odom_base.y(); + out->pose.pose.orientation.z = q_odom_base.z(); + + out->twist.twist.linear.x = v_base.x(); + out->twist.twist.linear.y = v_base.y(); + out->twist.twist.linear.z = v_base.z(); + out->twist.twist.angular.x = omega_base.x(); + out->twist.twist.angular.y = omega_base.y(); + out->twist.twist.angular.z = omega_base.z(); + + out->pose.covariance = msg->pose.covariance; + out->twist.covariance = msg->twist.covariance; + + if (pose_pub_) { + auto pose_msg = + std::make_unique(); + pose_msg->header = out->header; + pose_msg->pose = out->pose; + pose_pub_->publish(std::move(pose_msg)); + } + + if (twist_pub_) { + auto twist_msg = + std::make_unique(); + twist_msg->header = out->header; + twist_msg->header.frame_id = out->child_frame_id; + twist_msg->twist = out->twist; + twist_pub_->publish(std::move(twist_msg)); + } + + if (tf_broadcaster_) { + geometry_msgs::msg::TransformStamped tf_msg; + tf_msg.header.stamp = msg->header.stamp; + tf_msg.header.frame_id = frame("odom"); + tf_msg.child_frame_id = frame("base_link"); + tf_msg.transform.translation.x = p_base.x(); + tf_msg.transform.translation.y = p_base.y(); + tf_msg.transform.translation.z = p_base.z(); + tf_msg.transform.rotation = out->pose.pose.orientation; + tf_broadcaster_->sendTransform(tf_msg); + } + + odom_pub_->publish(std::move(out)); +} + +RCLCPP_COMPONENTS_REGISTER_NODE(OdomTransformer) diff --git a/scripts/ci_install_dependencies.sh b/scripts/ci_install_dependencies.sh index 90b877d6c..cbca12a89 100755 --- a/scripts/ci_install_dependencies.sh +++ b/scripts/ci_install_dependencies.sh @@ -27,7 +27,18 @@ sudo update-alternatives --install /usr/bin/gcc gcc /usr/bin/gcc-13 100 sudo update-alternatives --install /usr/bin/g++ g++ /usr/bin/g++-13 100 sudo update-alternatives --install /usr/bin/gcov gcov /usr/bin/gcov-13 100 +# Install asio +sudo apt-get update -qq +sudo apt-get install -y \ + build-essential \ + cmake \ + git \ + libasio-dev + # Install casadi using CasADi install script "$SCRIPT_DIR/install_casadi.sh" +# Install Control Toolbox using the provided script +"$SCRIPT_DIR/install_ct.sh" + echo "Done installing additional dependencies." diff --git a/scripts/install_ct.sh b/scripts/install_ct.sh new file mode 100755 index 000000000..6758039b5 --- /dev/null +++ b/scripts/install_ct.sh @@ -0,0 +1,73 @@ +#!/usr/bin/env bash +set -euo pipefail + +echo "Installing Control Toolbox..." + +# ---- Config ---- +WORKSPACE_DIR="${HOME}/third_party/ct_ws" +SRC_DIR="${WORKSPACE_DIR}/src" +INSTALL_PREFIX="${HOME}/.local" +BUILD_TYPE="Release" + +KINDR_REPO="https://github.com/ANYbotics/kindr.git" +CT_REPO="https://github.com/ethz-adrl/control-toolbox.git" + +KINDR_DIR="${SRC_DIR}/kindr" +CT_DIR="${SRC_DIR}/control-toolbox" + +mkdir -p "${SRC_DIR}" +mkdir -p "${INSTALL_PREFIX}" + +echo "Workspace: ${WORKSPACE_DIR}" +echo "Install prefix: ${INSTALL_PREFIX}" + +# ---- System deps ---- +sudo apt-get update +sudo apt-get install -y \ + git \ + build-essential \ + cmake \ + libeigen3-dev \ + libboost-all-dev + +# ---- Clone/update kindr ---- +if [ ! -d "${KINDR_DIR}/.git" ]; then + git clone "${KINDR_REPO}" "${KINDR_DIR}" +else + git -C "${KINDR_DIR}" pull --ff-only +fi + +# ---- Build/install kindr ---- +cmake -S "${KINDR_DIR}" -B "${KINDR_DIR}/build" \ + -DUSE_CMAKE=true \ + -DCMAKE_BUILD_TYPE="${BUILD_TYPE}" \ + -DCMAKE_INSTALL_PREFIX="${INSTALL_PREFIX}" + +cmake --build "${KINDR_DIR}/build" -j"$(nproc)" +cmake --install "${KINDR_DIR}/build" + +# ---- Clone/update control-toolbox ---- +if [ ! -d "${CT_DIR}/.git" ]; then + git clone "${CT_REPO}" "${CT_DIR}" +else + git -C "${CT_DIR}" pull --ff-only +fi + +# ---- Build CT using the provided script ---- +chmod +x "${CT_DIR}/ct/build_ct.sh" + +export CMAKE_PREFIX_PATH="${INSTALL_PREFIX}:${CMAKE_PREFIX_PATH:-}" + +pushd "${CT_DIR}/ct" >/dev/null +./build_ct.sh \ + -DCMAKE_BUILD_TYPE="${BUILD_TYPE}" \ + -DCMAKE_PREFIX_PATH="${INSTALL_PREFIX}" +popd >/dev/null + +echo +echo "Control Toolbox build complete." +echo "You may want this in your shell before building your ROS 2 package:" +echo " export CMAKE_PREFIX_PATH=\"${INSTALL_PREFIX}:\$CMAKE_PREFIX_PATH\"" +echo +echo "CT source: ${CT_DIR}" +echo "Kindr install: ${INSTALL_PREFIX}" \ No newline at end of file diff --git a/scripts/nautilus.sh b/scripts/nautilus.sh new file mode 100755 index 000000000..4583d4bd1 --- /dev/null +++ b/scripts/nautilus.sh @@ -0,0 +1,9 @@ +#!/bin/bash + +tmux new-session -d -s "nautilus" + +tmux split-window -v -t "nautilus:0" +tmux split-window -h -t "nautilus:0.0" +tmux split-window -h -t "nautilus:0.2" + +tmux attach-session -t "nautilus" diff --git a/scripts/nautilus_dp_launch.sh b/scripts/nautilus_dp_launch.sh new file mode 100755 index 000000000..e69de29bb diff --git a/tests/ros_node_tests/eskf_node_test.sh b/tests/ros_node_tests/eskf_node_test.sh new file mode 100644 index 000000000..35ba6e91e --- /dev/null +++ b/tests/ros_node_tests/eskf_node_test.sh @@ -0,0 +1,39 @@ +#!/bin/bash +set -e +set -o pipefail + +echo "Testing that the ESKF node is able to start up and publish odom" + +# Load ROS 2 environment +echo "Setting up ROS 2 environment..." +. /opt/ros/humble/setup.sh +. "${WORKSPACE:-$HOME/ros2_ws}/install/setup.bash" + +# Function to terminate processes safely on error +cleanup() { + echo "Error detected. Cleaning up..." + kill -TERM -"$ESKF_PID" || true + exit 1 +} +trap cleanup ERR + +# Launch eskf node +setsid ros2 launch eskf eskf.launch.py & +ESKF_PID=$! +echo "Launched eskf with PID: $ESKF_PID" + +# Check for ROS errors before continuing +if journalctl -u ros2 | grep -i "error"; then + echo "Error detected in ROS logs. Exiting..." + exit 1 +fi + +# Check if eskf correctly publishes odom +echo "Waiting for odom data..." +timeout 10s ros2 topic echo /orca/odom --once +echo "Got odom data" + +# Terminate processes +kill -TERM -"$ESKF_PID" + +echo "Test completed successfully." diff --git a/tests/simulator_tests/los_test/send_goal.py b/tests/simulator_tests/los_test/send_goal.py index 38e3f504b..2e6063925 100644 --- a/tests/simulator_tests/los_test/send_goal.py +++ b/tests/simulator_tests/los_test/send_goal.py @@ -1,6 +1,7 @@ import rclpy from rclpy.action import ActionClient from rclpy.node import Node +from std_msgs.msg import Header from vortex_msgs.action import LOSGuidance @@ -9,53 +10,115 @@ def __init__(self): super().__init__('los_guidance_client') # Create the action client self._action_client = ActionClient(self, LOSGuidance, '/nautilus/los_guidance') + super().__init__("los_guidance_client") + + self.declare_parameter("drone", "orca") + self.declare_parameter("x", 20.0) + self.declare_parameter("y", 20.0) + self.declare_parameter("z", 2.5) + + self.drone = self.get_parameter("drone").value + self.goal_x = float(self.get_parameter("x").value) + self.goal_y = float(self.get_parameter("y").value) + self.goal_z = float(self.get_parameter("z").value) + + self._action_client = ActionClient( + self, + LOSGuidance, + f"/{self.drone}/los_guidance", + ) + + self.get_logger().info(f"Using drone namespace: {self.drone}") self.send_goal() def send_goal(self): + self.get_logger().info("Waiting for action server...") + if not self._action_client.wait_for_server(timeout_sec=10.0): + self.get_logger().error("Action server not available") + self.shutdown_with_code(1) + return + goal_msg = LOSGuidance.Goal() - # Create a message with the goal - goal_msg.goal.point.x = 20.0 - goal_msg.goal.point.y = 20.0 - goal_msg.goal.point.z = 5.0 + header = Header() + header.frame_id = "world_ned" + header.stamp = self.get_clock().now().to_msg() + goal_msg.goal.header = header + + goal_msg.goal.point.x = self.goal_x + goal_msg.goal.point.y = self.goal_y + goal_msg.goal.point.z = self.goal_z + + self.get_logger().info( + f"Sending goal: x={self.goal_x:.2f}, y={self.goal_y:.2f}, z={self.goal_z:.2f}" + ) - # Send the goal asynchronously - self._action_client.wait_for_server(timeout_sec=10.0) - self.get_logger().info('Sending goal...') - self._send_goal_future = self._action_client.send_goal_async(goal_msg) + self._send_goal_future = self._action_client.send_goal_async( + goal_msg, + feedback_callback=self.feedback_callback, + ) self._send_goal_future.add_done_callback(self.goal_response_callback) def goal_response_callback(self, future): goal_handle = future.result() + + if goal_handle is None: + self.get_logger().error("Failed to send goal") + self.shutdown_with_code(1) + return + if not goal_handle.accepted: - self.get_logger().info('Goal rejected :(') + self.get_logger().error("Goal rejected") + self.shutdown_with_code(1) return - self.get_logger().info('Goal accepted :)') + self.get_logger().info("Goal accepted") self._get_result_future = goal_handle.get_result_async() self._get_result_future.add_done_callback(self.get_result_callback) def get_result_callback(self, future): - result = future.result().result.success - self.get_logger().info(f'Goal result: {result}') - if result: - self.destroy_node() - if rclpy.ok(): - rclpy.shutdown() - exit(0) + result_msg = future.result() + + if result_msg is None: + self.get_logger().error("Did not receive result") + self.shutdown_with_code(1) + return + + result = result_msg.result + status = result_msg.status + + self.get_logger().info(f"Result status: {status}") + self.get_logger().info(f"Goal success: {result.success}") + + if result.success: + self.get_logger().info("Goal reached successfully") + self.shutdown_with_code(0) else: - self.get_logger().info('Goal failed :(') - self.destroy_node() - if rclpy.ok(): - rclpy.shutdown() - exit(1) + self.get_logger().error("Goal failed") + self.shutdown_with_code(1) + + def feedback_callback(self, feedback_msg): + self.get_logger().debug("Received feedback") + + def shutdown_with_code(self, code): + self.destroy_node() + if rclpy.ok(): + rclpy.shutdown() def main(args=None): rclpy.init(args=args) - action_client = LOSGuidanceClient() - rclpy.spin(action_client) + node = LOSGuidanceClient() + + try: + rclpy.spin(node) + except KeyboardInterrupt: + node.get_logger().info("Interrupted by user") + finally: + if rclpy.ok(): + node.destroy_node() + rclpy.shutdown() -if __name__ == '__main__': +if __name__ == "__main__": main() diff --git a/tests/simulator_tests/waypoint_navigation_quat/simulator_test.sh b/tests/simulator_tests/waypoint_navigation_quat/simulator_test.sh index 36c0fafab..bbec942f9 100755 --- a/tests/simulator_tests/waypoint_navigation_quat/simulator_test.sh +++ b/tests/simulator_tests/waypoint_navigation_quat/simulator_test.sh @@ -8,7 +8,7 @@ echo "Setting up ROS 2 environment..." export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/local/lib # Get the directory of this script dynamically -SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" # Function to terminate processes safely on error cleanup() { diff --git a/utility_scripts/build_ws_drone.sh b/utility_scripts/build_ws_drone.sh new file mode 100755 index 000000000..ce6a1ea96 --- /dev/null +++ b/utility_scripts/build_ws_drone.sh @@ -0,0 +1,164 @@ +#!/bin/bash +set -e + +WS_DIR="$HOME/vscopium/ros2_ws" +SRC_DIR="$WS_DIR/src" +GITHUB_ORG="https://github.com/vortexntnu" + +# Repos to clone +REPOS=( + "vortex-auv" + "vortex-msgs" + "vortex-utils" +) + +# ---- Install dependencies script ---- + +# python3 -m pip install --upgrade pip +# install numpy, pynput, joy, wheel +# install ros-humble-xacro, ros-humble-joy +# run casadi install script + +# ------------ Clone missing repos ------------ +mkdir -p "$SRC_DIR" + +for repo in "${REPOS[@]}"; do + if [ -d "$SRC_DIR/$repo" ]; then + echo "[SKIP] $SRC_DIR/$repo already exists" + else + echo "[CLONE] $GITHUB_ORG/$repo.git -> $SRC_DIR/$repo" + git clone "$GITHUB_ORG/$repo.git" "$SRC_DIR/$repo" + fi +done + +# ---------- Build ---------- +cd "$WS_DIR" + +# Default packages to build +CONTROL_PKGS=( + vortex_msgs + vortex_utils + vortex_utils_ros + thrust_allocator_auv + thruster_interface_auv + dp_adapt_backs_controller + pid_controller_dp + reference_filter_dp + auv_setup + operation_mode_manager + los_guidance +) + +#TODO: add perception pkgs and a seperate launch arg. +PERCEPTION_PKGS=() + +# Use arguments if provided, otherwise use defaults +if [ $# -gt 0 ]; then + PKGS=("$@") +else + PKGS=("${CONTROL_PKGS[@]}") +fi + +BUILD_FAILED=0 +echo "[BUILD] Building packages: ${PKGS[*]}" +colcon build \ + --packages-up-to "${PKGS[@]}" \ + --symlink-install \ + --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=ON || + BUILD_FAILED=1 + +# ---------- Handle result ---------- +if [ $BUILD_FAILED -eq 1 ]; then + echo "" + echo " ╔══════════════════════════════════════════════════╗ " + echo " ║ BUILD FAILED ║ " + echo " ╚══════════════════════════════════════════════════╝ " + echo "" + echo "⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⡖⢀⠀⠒⠒⠒⠒⠒⠒⠒⠀⠀⠀⠀⠀⠀⠀⡀⠀⠀⠀⠀⠀⠀⠀" + echo "⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⠿⠛⠙⠉⠉⠉⠉⠉⠉⠉⠙⠃⠀⠀⠠⠄⠂⠐⠀⠀⠀⠀⠀⠀⠀⠉⠁⠀⠀⠀⢸⠀⠀⠀⠀⠀" + echo "⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⡿⠋⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢸⠀⠀⠀⠀⠀" + echo "⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⡟⠀⠀⠀⠀⠀⠀⠀⡀⣀⢀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠈⠀⠀⠀⠀⠀" + echo "⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⡟⠀⠀⠀⠀⠀⢀⣶⢿⣵⣮⣞⣶⣆⡶⣴⣀⠀⠀⠀⠀⠀⠀⠡⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀" + echo "⠉⠉⠁⠂⠐⠂⡐⠈⠻⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⠓⢀⡖⠀⢠⣴⣿⣿⣿⣿⣿⣿⣿⣿⣿⣶⣭⢚⠤⡀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠅" + echo "⠀⠀⠀⠀⠀⠀⠀⠀⠉⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⠀⣾⠃⣴⣿⣿⢻⣿⣿⣿⣿⣿⣿⣿⣿⣿⡎⣧⢣⡅⢣⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢠" + echo "⠀⠀⠀⠀⠀⠀⠀⠀⠐⠨⣿⣿⣿⣿⣿⣿⣿⠿⠋⠀⠉⢠⢿⣟⣯⣿⣾⣿⣿⣿⣿⣿⣿⣿⣿⡿⣜⠮⡜⣌⠂⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠠⠀⠈" + echo "⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠚⠿⠿⠿⠿⠿⠏⠀⠂⣴⣲⢠⣛⣾⣿⣿⣿⣿⣿⢿⣿⣿⣿⣿⣿⣿⣯⡟⡴⢂⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠠⠀⠔" + echo "⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡀⠀⠀⠀⠀⠀⠀⠈⠷⡙⢰⣉⡴⣀⡉⠉⠋⠙⢉⣿⡟⠛⠛⠋⠛⠙⡉⠘⠁⠀⢀⣞⠀⠀⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠡⠈" + echo "⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠐⡀⠀⠀⠀⠀⠀⠀⠀⠀⢠⠃⠀⠉⠘⠁⠀⢀⣠⣾⣧⡀⠀⠀⠐⠃⠂⠁⠀⠀⠠⠈⠄⠈⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠁⠂" + echo "⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠐⠠⠀⠀⠀⠀⠀⠀⠀⢀⣋⢶⣠⣤⣤⡶⢮⣿⣿⣿⣷⠄⢦⣄⣀⡀⢄⠂⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀" + echo "⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠁⠁⠒⣀⠀⠀⠀⠀⠀⠀⠜⣿⡿⣿⡟⣡⡿⢿⣿⣿⢿⡒⢌⠻⣯⠗⡎⠄⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀" + echo "⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠁⠀⠒⠤⣄⣀⢀⡀⠀⠀⢙⢣⡽⡅⠀⠀⠀⠀⠀⠀⢈⡖⠁⠉⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢰⡶⡲⡒⡒⣴⡶⣒⣖⢶⣶" + echo "⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⠀⠀⠈⠄⡁⠢⠐⠠⢌⠁⠁⠀⠱⣈⠎⢙⡳⣍⠾⠶⡲⠦⣐⠎⠲⠀⡄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠘⠓⠔⠥⠇⠛⠆⠋⠭⠚⠛" + echo "⠀⠀⠐⠀⠀⠀⠀⢀⠀⠀⠂⢌⡐⢠⠒⣤⣁⠎⠅⣈⡀⠀⠀⠀⠑⢪⠀⡈⣈⣁⡀⠁⣠⡁⠀⠀⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀" + echo "⢀⣄⣐⣠⡈⠤⡁⠆⡑⣀⠉⠆⠉⢂⡙⢀⣂⣤⠚⣵⢯⡄⠀⠀⠀⠀⠡⠀⠀⠉⠘⠋⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀" + echo "⠈⣨⣧⣶⣟⠷⢛⣡⢤⡲⡜⢮⠌⣍⢿⣦⠐⣈⢷⡌⠞⡜⢆⠀⠀⠀⠀⠡⣀⢄⣀⣀⡀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠠⣀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀" + echo "⣾⣿⠟⣴⣞⣯⠻⣜⣣⢟⣹⢂⢯⡘⢎⡞⣦⠀⢂⠙⢦⠉⢄⠊⢀⠀⠀⠀⠀⠊⠐⠁⠈⠀⠀⠀⠀⠀⠀⠀⠀⡀⢀⠠⠀⠀⡣⢤⡉⢄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀" + echo "⣿⡧⣏⢳⡬⢷⡙⢆⡳⢎⡳⢜⡢⢝⡲⢸⢥⠣⢌⠢⣤⣉⠢⠄⡄⡀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡀⢆⡱⠌⢀⠐⢢⢍⠢⢈⠄⡀⠀⠀⠀⠀⠀⠀⠀" + echo "⡟⣠⠻⣄⠻⣜⢻⡘⢣⡛⠼⣀⠻⡘⡤⢛⡄⢿⡀⢧⣛⢧⢇⡄⠀⠠⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠠⠀⠀⢠⡘⠄⡤⢀⠄⡀⠘⡄⠇⡘⠠⣀⠀⠀⠀⠀⠀⠀⠀" + echo "⠐⣡⠛⣌⠳⣌⠳⣌⠣⡜⡱⢌⢣⠱⢌⠣⡜⢢⡑⢎⡝⣊⠞⡼⣉⠖⡠⢐⠀⠀⠀⠀⠀⠀⠀⠀⠀⢐⠀⡘⠤⣌⠳⠀⡌⠎⡄⠀⠘⡀⣁⠒⢠⠂⠀⠀⠀⠀⠀⠀" + echo "⠠⢡⠉⡄⠣⢌⠳⡄⡓⢬⡐⠎⣄⠣⢊⠱⡘⡄⢙⢢⠹⣤⢋⡴⠡⢎⠱⡈⠆⠀⢀⠀⠀⠀⠀⢀⠀⡜⠠⣘⠲⢌⡱⠠⡑⢌⠰⡡⠀⠐⢠⠘⠠⢀⠃⠀⠀⠀⠀⠀" + echo "⢡⠂⠥⠐⡀⢈⠳⣐⡉⠆⡜⠒⡄⢣⠉⢆⠱⡐⠈⢂⠓⡌⢣⠘⡡⢊⠐⡘⠄⠀⠂⡀⠂⠌⣠⡔⢂⠩⠐⢤⢋⠆⡀⠂⠔⡈⠆⠥⠁⠀⠠⠈⡐⠨⢘⠀⠀⠀⠀⠀" + echo "⢀⠣⢈⠱⡐⠀⡇⠢⢅⡍⠰⡉⡔⢡⠊⡄⢣⠘⡀⠀⢂⠐⠠⢁⠐⠀⢂⡉⠆⠀⠡⢐⣹⣾⣿⣿⡆⡣⢘⢢⢉⠂⠠⢁⡘⠤⣉⠒⠀⢂⠀⠀⠤⠑⢂⠂⠀⠀⢀⠀" + echo "⡀⢃⠆⠐⡈⠡⢌⡑⠢⢌⠱⡐⢌⠢⡑⢌⠂⡅⢂⠁⡀⠂⢁⠠⠈⢀⠀⡜⠀⢈⠐⢿⣿⣿⣿⣿⡇⡅⡌⠆⠌⠀⠐⠠⡐⢢⠐⡀⠈⢀⠀⠀⠠⢁⠂⠌⠀⢀⢰⣤" + echo "⡘⠆⠌⠀⠠⢁⠢⠌⡁⠎⢠⠑⡈⠆⡑⢂⠥⢘⠠⢂⠀⠄⠂⠀⡀⠂⠠⢌⠀⣢⡍⢎⡻⣿⣿⣿⡇⣼⣿⣎⠀⠀⢂⠡⠐⢂⠡⠀⢀⠂⠌⠀⠀⠠⠀⠌⠀⠀⠂⠹" + echo "" + echo "" + exit 1 +fi + +# ---------- Merge compile_commands.json ---------- +# Each package gets its own compile_commands.json under build//. +# Merge them into a single file at the workspace root for clangd / IDEs. +echo "[MERGE] Combining compile_commands.json files..." +python3 -c " +import json, glob, pathlib + +ws = pathlib.Path('$WS_DIR') +combined = [] +for f in sorted(ws.glob('build/*/compile_commands.json')): + combined.extend(json.loads(f.read_text())) + +out = ws / 'compile_commands.json' +out.write_text(json.dumps(combined, indent=2)) +print(f' -> Wrote {len(combined)} entries to {out}') +" + +# ---------- Source ---------- +source "$WS_DIR/install/setup.bash" + +echo "" +echo " ╔══════════════════════════════════════════════════╗" +echo " ║ BUILD SUCCESSFUL ║" +echo " ╚══════════════════════════════════════════════════╝" +echo "" +echo "⠀⠀⢀⠀⣠⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀" +echo "⢀⠀⣿⡂⢹⡇⠀⠀⣰⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀" +echo "⢸⡇⢸⣇⢸⣇⠀⢀⣿⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢾⠀⠀⣯⡀⡆⠀⠀" +echo "⢸⣷⢸⣇⣸⣇⠀⣾⠏⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⣀⣀⣀⣠⣀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢳⣂⠀⣿⡄⢸⡀⣤" +echo "⢠⣿⣿⣿⣿⣿⣿⠇⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⣾⣿⣿⣊⡝⠛⠙⠂⠄⠠⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠈⢿⣦⣼⣷⣼⣁⠼" +echo "⢸⣿⣿⣿⣿⣿⣿⣀⢀⣀⣀⡀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣰⣿⣿⣿⣿⡻⣥⢋⡔⡀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠻⣿⣂⣜⣿⡟⢿⣿⣿⣄" +echo "⠈⣿⣿⣿⣿⣿⣿⣿⠿⠋⠉⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣿⣿⣿⣿⣿⣷⢯⣿⣾⡔⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠙⢪⣷⣿⢿⣿⣿" +echo "⠀⣿⣿⣟⢿⠿⠋⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢻⣿⡟⠛⠉⡉⢸⡉⠁⢀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢢⣽⣗⣿⠇" +echo "⠀⣿⣿⣿⡏⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠺⣿⡇⣤⡤⢔⡿⣇⠀⢦⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⣿⣿⣯⠀" +echo "⠘⡟⣛⠋⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠈⡇⣿⣿⠗⡲⠏⠟⠿⠀⠈⠓⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⣿⠍⠁⠁⠀" +echo "⠃⡜⡠⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠘⣼⣿⡟⢡⡿⠿⠷⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢸⣟⠒⠂⠂" +echo "⠐⢐⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢻⠸⣡⢶⣿⣟⡃⠀⠘⠀⠀⢀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣼⡇⠀⡀⠀" +echo "⢠⡏⠀⠃⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⡰⢨⠣⠉⠉⠋⠉⠀⠀⠀⠀⢈⠀⡂⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠠⡿⠀⠀⠀⠀" +echo "⢺⡇⢸⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⣽⡿⢛⢭⠏⣢⠍⠈⠖⠀⠀⠒⣶⢦⡁⠂⠀⠀⠀⠀⠀⠯⠤⣤⣴⢶⣍⠝⣯⣦⡀⠀⠀⠀⠀⠀⠀⠀⠀⢌⣿⠱⠀⠀⠀⠀⠀" +echo "⣯⣯⠸⣀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡠⠄⠀⠈⠀⠁⠀⠀⠀⠀⠀⠀⠀⠂⠀⠀⠏⠈⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠧⠍⠶⠤⠈⣆⠀⠀⠀⠀⠀⠀⠀⣷⡻⠀⣼⠀⠀⠀" +echo "⣯⣨⡀⢀⡠⠤⣐⠤⣀⣰⠔⠊⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠁⠑⠐⠐⠢⠺⠥⡾⠉⡠⠀⠀⠀" +echo "⠋⠙⠈⠉⠉⠁⠈⠈⠀⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀" +echo "⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀" +echo "⠓⠂⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀" +echo "⠀⠀⠇⣣⡁⢶⣠⢀⣀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⢶⠀⡶⣲⠀⣆⡒⣰⠒⢦⢰⠀⢰⡆⣴⠐⣶⠒⣐⣒⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⣠⣴⣺⣿⣿⣿⠛" +echo "⠀⠀⠑⢌⠻⣗⣔⠉⡅⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠞⠚⠃⠻⠴⠃⠦⠝⠘⠤⠎⠸⠤⠘⠧⠞⠀⠛⠀⠰⠤⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣼⡟⣾⣿⣿⣿⠃⠀" +echo "⠀⠀⠀⠀⠉⠢⠁⠀⠀⠀⠀⢀⣤⣤⣤⣄⠀⠀⢠⣤⠀⠀⣤⣄⠀⠀⠀⣤⣤⠀⢠⣤⣤⣤⣤⣤⡄⢠⣤⣄⠀⠀⠀⠀⣤⣤⡄⠀⠀⠀⢠⣤⡄⠀⠀⠀⢘⡮⡝⣿⣿⡿⢆⠁⠀" +echo "⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣰⣿⠏⠉⠉⢿⣷⠀⢸⣿⠀⠠⣿⣿⣧⡀⠀⣿⣿⠀⢸⣿⡏⠉⠉⠉⠁⢼⣿⣿⡄⠀⠀⢸⡿⣿⡇⠀⠀⢀⣿⢻⣷⠀⠀⠀⠞⡜⣹⣿⣿⡙⢆⠀⠀" +echo "⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣿⣿⠀⠀⠀⠀⠀⠀⢸⣿⠀⠐⣿⡯⢻⣷⡀⣿⣿⠀⢸⣿⣷⣶⣶⡆⠀⢺⣿⠹⣿⡀⢠⣿⠃⣿⡇⠀⠀⣾⡟⠀⢿⣧⠀⠀⠀⠠⢽⣿⣯⡙⠀⠀⠀" +echo "⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢿⣿⡀⠀⠀⣠⣤⠀⢸⣿⠀⢈⣿⡧⠀⠹⣿⣿⣿⠀⢸⣿⡇⠀⠀⠀⠀⢸⣿⡄⢻⣧⣾⡏⢠⣿⡇⠀⣼⣿⣷⣶⣾⣿⣇⠀⠀⠀⠘⣿⢣⠜⠁⠀⠀" +echo "⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠈⢿⣿⣶⣾⣿⠏⠀⢸⣿⠀⠀⣿⡷⠀⠀⠹⣿⣿⠀⢸⣿⣿⣿⣿⣿⡆⢸⣿⡆⠀⢿⡿⠀⢰⣿⡇⢀⣿⡏⠀⠀⠀⢹⣿⡀⠀⠀⠀⠀⠈⡆⠀⠀⠀" +echo "⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠈⠉⠉⠀⠀⠀⠈⠉⠀⠀⠉⠁⠀⠀⠀⠉⠉⠀⠈⠉⠉⠈⠉⠉⠁⠈⠉⠀⠀⠈⠁⠀⠀⠉⠁⠈⠉⠀⠀⠀⠀⠈⠉⠁⠐⡀⠀⠀⠀⠀⠀⠀⠀" +echo "" +echo "" +echo " Remember to source your new changes bossman! " +echo "" diff --git a/utility_scripts/build_ws_topside.sh b/utility_scripts/build_ws_topside.sh new file mode 100755 index 000000000..a4b8fe0c1 --- /dev/null +++ b/utility_scripts/build_ws_topside.sh @@ -0,0 +1,179 @@ +#!/bin/bash +set -e + +WS_DIR="$HOME/vscopium/ros2_ws" +SRC_DIR="$WS_DIR/src" +GITHUB_ORG="https://github.com/vortexntnu" + +# Repos to clone +REPOS=( + "stonefish_ros2" + "vortex-auv" + "vortex-msgs" + "vortex-stonefish-interface" + "vortex-stonefish-sim" + "vortex-utils" + "vortex-ci" +) + +# ---- Install dependencies ---- + +echo "[DEPS] Upgrading pip..." +python3 -m pip install --upgrade pip + +echo "[DEPS] Installing Python packages..." +python3 -m pip install numpy pynput wheel + +echo "[DEPS] Installing ROS apt packages..." +ROS_DISTRO="${ROS_DISTRO:-humble}" +sudo apt-get install -y --no-install-recommends \ + ros-${ROS_DISTRO}-xacro \ + ros-${ROS_DISTRO}-joy + +# ------------ Clone missing repos ------------ +mkdir -p "$SRC_DIR" + +for repo in "${REPOS[@]}"; do + if [ -d "$SRC_DIR/$repo" ]; then + echo "[SKIP] $SRC_DIR/$repo already exists" + else + echo "[CLONE] $GITHUB_ORG/$repo.git -> $SRC_DIR/$repo" + git clone "$GITHUB_ORG/$repo.git" "$SRC_DIR/$repo" + fi +done + +# ---------- Build ---------- +cd "$WS_DIR" + +# Default packages to build +DEFAULT_PKGS=( + vortex_msgs + vortex_utility_nodes + vortex_utils + vortex_utils_ros + stonefish_ros2 + vortex_sim_interface + stonefish_sim + stonefish_sim_interface + thrust_allocator_auv + thruster_interface_auv + dp_adapt_backs_controller + pid_controller_dp + reference_filter_dp + keyboard_joy + joystick_interface_auv + auv_setup + operation_mode_manager + los_guidance +) + +# Use arguments if provided, otherwise use defaults +if [ $# -gt 0 ]; then + PKGS=("$@") +else + PKGS=("${DEFAULT_PKGS[@]}") +fi + +BUILD_FAILED=0 +echo "[BUILD] Building packages: ${PKGS[*]}" +colcon build \ + --packages-up-to "${PKGS[@]}" \ + --symlink-install \ + --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=ON || + BUILD_FAILED=1 + +# ---------- Handle result ---------- +if [ $BUILD_FAILED -eq 1 ]; then + echo "" + echo " ╔══════════════════════════════════════════════════╗ " + echo " ║ BUILD FAILED ║ " + echo " ╚══════════════════════════════════════════════════╝ " + echo "" + echo "⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⣶⡖⢀⠀⠒⠒⠒⠒⠒⠒⠒⠀⠀⠀⠀⠀⠀⠀⡀⠀⠀⠀⠀⠀⠀⠀" + echo "⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⠿⠛⠙⠉⠉⠉⠉⠉⠉⠉⠙⠃⠀⠀⠠⠄⠂⠐⠀⠀⠀⠀⠀⠀⠀⠉⠁⠀⠀⠀⢸⠀⠀⠀⠀⠀" + echo "⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⡿⠋⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢸⠀⠀⠀⠀⠀" + echo "⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⡟⠀⠀⠀⠀⠀⠀⠀⡀⣀⢀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠈⠀⠀⠀⠀⠀" + echo "⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⡟⠀⠀⠀⠀⠀⢀⣶⢿⣵⣮⣞⣶⣆⡶⣴⣀⠀⠀⠀⠀⠀⠀⠡⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀" + echo "⠉⠉⠁⠂⠐⠂⡐⠈⠻⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⠓⢀⡖⠀⢠⣴⣿⣿⣿⣿⣿⣿⣿⣿⣿⣶⣭⢚⠤⡀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠅" + echo "⠀⠀⠀⠀⠀⠀⠀⠀⠉⣿⣿⣿⣿⣿⣿⣿⣿⣿⣿⠀⣾⠃⣴⣿⣿⢻⣿⣿⣿⣿⣿⣿⣿⣿⣿⡎⣧⢣⡅⢣⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢠" + echo "⠀⠀⠀⠀⠀⠀⠀⠀⠐⠨⣿⣿⣿⣿⣿⣿⣿⠿⠋⠀⠉⢠⢿⣟⣯⣿⣾⣿⣿⣿⣿⣿⣿⣿⣿⡿⣜⠮⡜⣌⠂⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠠⠀⠈" + echo "⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠚⠿⠿⠿⠿⠿⠏⠀⠂⣴⣲⢠⣛⣾⣿⣿⣿⣿⣿⢿⣿⣿⣿⣿⣿⣿⣯⡟⡴⢂⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠠⠀⠔" + echo "⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡀⠀⠀⠀⠀⠀⠀⠈⠷⡙⢰⣉⡴⣀⡉⠉⠋⠙⢉⣿⡟⠛⠛⠋⠛⠙⡉⠘⠁⠀⢀⣞⠀⠀⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠡⠈" + echo "⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠐⡀⠀⠀⠀⠀⠀⠀⠀⠀⢠⠃⠀⠉⠘⠁⠀⢀⣠⣾⣧⡀⠀⠀⠐⠃⠂⠁⠀⠀⠠⠈⠄⠈⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠁⠂" + echo "⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠐⠠⠀⠀⠀⠀⠀⠀⠀⢀⣋⢶⣠⣤⣤⡶⢮⣿⣿⣿⣷⠄⢦⣄⣀⡀⢄⠂⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀" + echo "⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠁⠁⠒⣀⠀⠀⠀⠀⠀⠀⠜⣿⡿⣿⡟⣡⡿⢿⣿⣿⢿⡒⢌⠻⣯⠗⡎⠄⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀" + echo "⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠁⠀⠒⠤⣄⣀⢀⡀⠀⠀⢙⢣⡽⡅⠀⠀⠀⠀⠀⠀⢈⡖⠁⠉⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢰⡶⡲⡒⡒⣴⡶⣒⣖⢶⣶" + echo "⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⠀⠀⠈⠄⡁⠢⠐⠠⢌⠁⠁⠀⠱⣈⠎⢙⡳⣍⠾⠶⡲⠦⣐⠎⠲⠀⡄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠘⠓⠔⠥⠇⠛⠆⠋⠭⠚⠛" + echo "⠀⠀⠐⠀⠀⠀⠀⢀⠀⠀⠂⢌⡐⢠⠒⣤⣁⠎⠅⣈⡀⠀⠀⠀⠑⢪⠀⡈⣈⣁⡀⠁⣠⡁⠀⠀⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀" + echo "⢀⣄⣐⣠⡈⠤⡁⠆⡑⣀⠉⠆⠉⢂⡙⢀⣂⣤⠚⣵⢯⡄⠀⠀⠀⠀⠡⠀⠀⠉⠘⠋⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀" + echo "⠈⣨⣧⣶⣟⠷⢛⣡⢤⡲⡜⢮⠌⣍⢿⣦⠐⣈⢷⡌⠞⡜⢆⠀⠀⠀⠀⠡⣀⢄⣀⣀⡀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠠⣀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀" + echo "⣾⣿⠟⣴⣞⣯⠻⣜⣣⢟⣹⢂⢯⡘⢎⡞⣦⠀⢂⠙⢦⠉⢄⠊⢀⠀⠀⠀⠀⠊⠐⠁⠈⠀⠀⠀⠀⠀⠀⠀⠀⡀⢀⠠⠀⠀⡣⢤⡉⢄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀" + echo "⣿⡧⣏⢳⡬⢷⡙⢆⡳⢎⡳⢜⡢⢝⡲⢸⢥⠣⢌⠢⣤⣉⠢⠄⡄⡀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡀⢆⡱⠌⢀⠐⢢⢍⠢⢈⠄⡀⠀⠀⠀⠀⠀⠀⠀" + echo "⡟⣠⠻⣄⠻⣜⢻⡘⢣⡛⠼⣀⠻⡘⡤⢛⡄⢿⡀⢧⣛⢧⢇⡄⠀⠠⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠠⠀⠀⢠⡘⠄⡤⢀⠄⡀⠘⡄⠇⡘⠠⣀⠀⠀⠀⠀⠀⠀⠀" + echo "⠐⣡⠛⣌⠳⣌⠳⣌⠣⡜⡱⢌⢣⠱⢌⠣⡜⢢⡑⢎⡝⣊⠞⡼⣉⠖⡠⢐⠀⠀⠀⠀⠀⠀⠀⠀⠀⢐⠀⡘⠤⣌⠳⠀⡌⠎⡄⠀⠘⡀⣁⠒⢠⠂⠀⠀⠀⠀⠀⠀" + echo "⠠⢡⠉⡄⠣⢌⠳⡄⡓⢬⡐⠎⣄⠣⢊⠱⡘⡄⢙⢢⠹⣤⢋⡴⠡⢎⠱⡈⠆⠀⢀⠀⠀⠀⠀⢀⠀⡜⠠⣘⠲⢌⡱⠠⡑⢌⠰⡡⠀⠐⢠⠘⠠⢀⠃⠀⠀⠀⠀⠀" + echo "⢡⠂⠥⠐⡀⢈⠳⣐⡉⠆⡜⠒⡄⢣⠉⢆⠱⡐⠈⢂⠓⡌⢣⠘⡡⢊⠐⡘⠄⠀⠂⡀⠂⠌⣠⡔⢂⠩⠐⢤⢋⠆⡀⠂⠔⡈⠆⠥⠁⠀⠠⠈⡐⠨⢘⠀⠀⠀⠀⠀" + echo "⢀⠣⢈⠱⡐⠀⡇⠢⢅⡍⠰⡉⡔⢡⠊⡄⢣⠘⡀⠀⢂⠐⠠⢁⠐⠀⢂⡉⠆⠀⠡⢐⣹⣾⣿⣿⡆⡣⢘⢢⢉⠂⠠⢁⡘⠤⣉⠒⠀⢂⠀⠀⠤⠑⢂⠂⠀⠀⢀⠀" + echo "⡀⢃⠆⠐⡈⠡⢌⡑⠢⢌⠱⡐⢌⠢⡑⢌⠂⡅⢂⠁⡀⠂⢁⠠⠈⢀⠀⡜⠀⢈⠐⢿⣿⣿⣿⣿⡇⡅⡌⠆⠌⠀⠐⠠⡐⢢⠐⡀⠈⢀⠀⠀⠠⢁⠂⠌⠀⢀⢰⣤" + echo "⡘⠆⠌⠀⠠⢁⠢⠌⡁⠎⢠⠑⡈⠆⡑⢂⠥⢘⠠⢂⠀⠄⠂⠀⡀⠂⠠⢌⠀⣢⡍⢎⡻⣿⣿⣿⡇⣼⣿⣎⠀⠀⢂⠡⠐⢂⠡⠀⢀⠂⠌⠀⠀⠠⠀⠌⠀⠀⠂⠹" + echo "" + echo "" + exit 1 +fi + +# ---------- Merge compile_commands.json ---------- +# Each package gets its own compile_commands.json under build//. +# Merge them into a single file at the workspace root for clangd / IDEs. +echo "[MERGE] Combining compile_commands.json files..." +python3 -c " +import json, glob, pathlib + +ws = pathlib.Path('$WS_DIR') +combined = [] +for f in sorted(ws.glob('build/*/compile_commands.json')): + combined.extend(json.loads(f.read_text())) + +out = ws / 'compile_commands.json' +out.write_text(json.dumps(combined, indent=2)) +print(f' -> Wrote {len(combined)} entries to {out}') +" + +# ---------- Source ---------- +source "$WS_DIR/install/setup.bash" + +echo "" +echo " ╔══════════════════════════════════════════════════╗" +echo " ║ BUILD SUCCESSFUL ║" +echo " ╚══════════════════════════════════════════════════╝" +echo "" +echo "⠀⠀⢀⠀⣠⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀" +echo "⢀⠀⣿⡂⢹⡇⠀⠀⣰⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀" +echo "⢸⡇⢸⣇⢸⣇⠀⢀⣿⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢾⠀⠀⣯⡀⡆⠀⠀" +echo "⢸⣷⢸⣇⣸⣇⠀⣾⠏⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⣀⣀⣀⣠⣀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢳⣂⠀⣿⡄⢸⡀⣤" +echo "⢠⣿⣿⣿⣿⣿⣿⠇⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⣾⣿⣿⣊⡝⠛⠙⠂⠄⠠⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠈⢿⣦⣼⣷⣼⣁⠼" +echo "⢸⣿⣿⣿⣿⣿⣿⣀⢀⣀⣀⡀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣰⣿⣿⣿⣿⡻⣥⢋⡔⡀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠻⣿⣂⣜⣿⡟⢿⣿⣿⣄" +echo "⠈⣿⣿⣿⣿⣿⣿⣿⠿⠋⠉⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣿⣿⣿⣿⣿⣷⢯⣿⣾⡔⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠙⢪⣷⣿⢿⣿⣿" +echo "⠀⣿⣿⣟⢿⠿⠋⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢻⣿⡟⠛⠉⡉⢸⡉⠁⢀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢢⣽⣗⣿⠇" +echo "⠀⣿⣿⣿⡏⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠺⣿⡇⣤⡤⢔⡿⣇⠀⢦⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⣿⣿⣯⠀" +echo "⠘⡟⣛⠋⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠈⡇⣿⣿⠗⡲⠏⠟⠿⠀⠈⠓⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⣿⠍⠁⠁⠀" +echo "⠃⡜⡠⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠘⣼⣿⡟⢡⡿⠿⠷⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢸⣟⠒⠂⠂" +echo "⠐⢐⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢻⠸⣡⢶⣿⣟⡃⠀⠘⠀⠀⢀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣼⡇⠀⡀⠀" +echo "⢠⡏⠀⠃⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⡰⢨⠣⠉⠉⠋⠉⠀⠀⠀⠀⢈⠀⡂⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠠⡿⠀⠀⠀⠀" +echo "⢺⡇⢸⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⣽⡿⢛⢭⠏⣢⠍⠈⠖⠀⠀⠒⣶⢦⡁⠂⠀⠀⠀⠀⠀⠯⠤⣤⣴⢶⣍⠝⣯⣦⡀⠀⠀⠀⠀⠀⠀⠀⠀⢌⣿⠱⠀⠀⠀⠀⠀" +echo "⣯⣯⠸⣀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡠⠄⠀⠈⠀⠁⠀⠀⠀⠀⠀⠀⠀⠂⠀⠀⠏⠈⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠧⠍⠶⠤⠈⣆⠀⠀⠀⠀⠀⠀⠀⣷⡻⠀⣼⠀⠀⠀" +echo "⣯⣨⡀⢀⡠⠤⣐⠤⣀⣰⠔⠊⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠁⠑⠐⠐⠢⠺⠥⡾⠉⡠⠀⠀⠀" +echo "⠋⠙⠈⠉⠉⠁⠈⠈⠀⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀" +echo "⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀" +echo "⠓⠂⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀" +echo "⠀⠀⠇⣣⡁⢶⣠⢀⣀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⢶⠀⡶⣲⠀⣆⡒⣰⠒⢦⢰⠀⢰⡆⣴⠐⣶⠒⣐⣒⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⣠⣴⣺⣿⣿⣿⠛" +echo "⠀⠀⠑⢌⠻⣗⣔⠉⡅⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠞⠚⠃⠻⠴⠃⠦⠝⠘⠤⠎⠸⠤⠘⠧⠞⠀⠛⠀⠰⠤⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣼⡟⣾⣿⣿⣿⠃⠀" +echo "⠀⠀⠀⠀⠉⠢⠁⠀⠀⠀⠀⢀⣤⣤⣤⣄⠀⠀⢠⣤⠀⠀⣤⣄⠀⠀⠀⣤⣤⠀⢠⣤⣤⣤⣤⣤⡄⢠⣤⣄⠀⠀⠀⠀⣤⣤⡄⠀⠀⠀⢠⣤⡄⠀⠀⠀⢘⡮⡝⣿⣿⡿⢆⠁⠀" +echo "⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣰⣿⠏⠉⠉⢿⣷⠀⢸⣿⠀⠠⣿⣿⣧⡀⠀⣿⣿⠀⢸⣿⡏⠉⠉⠉⠁⢼⣿⣿⡄⠀⠀⢸⡿⣿⡇⠀⠀⢀⣿⢻⣷⠀⠀⠀⠞⡜⣹⣿⣿⡙⢆⠀⠀" +echo "⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣿⣿⠀⠀⠀⠀⠀⠀⢸⣿⠀⠐⣿⡯⢻⣷⡀⣿⣿⠀⢸⣿⣷⣶⣶⡆⠀⢺⣿⠹⣿⡀⢠⣿⠃⣿⡇⠀⠀⣾⡟⠀⢿⣧⠀⠀⠀⠠⢽⣿⣯⡙⠀⠀⠀" +echo "⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢿⣿⡀⠀⠀⣠⣤⠀⢸⣿⠀⢈⣿⡧⠀⠹⣿⣿⣿⠀⢸⣿⡇⠀⠀⠀⠀⢸⣿⡄⢻⣧⣾⡏⢠⣿⡇⠀⣼⣿⣷⣶⣾⣿⣇⠀⠀⠀⠘⣿⢣⠜⠁⠀⠀" +echo "⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠈⢿⣿⣶⣾⣿⠏⠀⢸⣿⠀⠀⣿⡷⠀⠀⠹⣿⣿⠀⢸⣿⣿⣿⣿⣿⡆⢸⣿⡆⠀⢿⡿⠀⢰⣿⡇⢀⣿⡏⠀⠀⠀⢹⣿⡀⠀⠀⠀⠀⠈⡆⠀⠀⠀" +echo "⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠈⠉⠉⠀⠀⠀⠈⠉⠀⠀⠉⠁⠀⠀⠀⠉⠉⠀⠈⠉⠉⠈⠉⠉⠁⠈⠉⠀⠀⠈⠁⠀⠀⠉⠁⠈⠉⠀⠀⠀⠀⠈⠉⠁⠐⡀⠀⠀⠀⠀⠀⠀⠀" +echo "" +echo "" +echo " Remember to source your new changes bossman! " +echo "" diff --git a/utility_scripts/launch_drone_topside.sh b/utility_scripts/launch_drone_topside.sh new file mode 100755 index 000000000..80681499d --- /dev/null +++ b/utility_scripts/launch_drone_topside.sh @@ -0,0 +1,57 @@ +#!/bin/bash +# Launch drone simulation stack in a tmux session + +SESSION="drone_launch" +S="source ~/vscopium/ros2_ws/install/setup.bash" + +# Kill existing session if it exists +tmux kill-session -t "$SESSION" 2>/dev/null + +# Launch Foxglove Studio only if not already running +if ! pgrep -f foxglove-studio &>/dev/null; then + foxglove-studio &>/dev/null & +fi + +# ============================================= +# Window 1: sim (4 equal panes) +# ============================================= +tmux new-session -d -s "$SESSION" -n "sim" + +# Grab the initial pane ID +PANE_SIM=$(tmux list-panes -t "$SESSION:sim" -F '#{pane_id}') + +# Stonefish simulation (top-left) +tmux send-keys -t "$PANE_SIM" "clear && $S && ros2 launch stonefish_sim simulation.launch.py drone:=nautilus" Enter + +# Split right -> Keyboard joy (top-right) +PANE_JOY=$(tmux split-window -h -t "$PANE_SIM" -P -F '#{pane_id}') +tmux send-keys -t "$PANE_JOY" "clear && $S && ros2 launch keyboard_joy keyboard_joy_node.launch.py" Enter + +# Split sim pane down -> AUV setup (bottom-left) +PANE_DP=$(tmux split-window -v -t "$PANE_SIM" -P -F '#{pane_id}') +tmux send-keys -t "$PANE_DP" "clear && $S && ros2 launch auv_setup dp.launch.py" Enter + +# Split joy pane down -> Drone sim (bottom-right) +PANE_DRONE=$(tmux split-window -v -t "$PANE_JOY" -P -F '#{pane_id}') +tmux send-keys -t "$PANE_DRONE" "clear && $S && ros2 launch stonefish_sim drone_sim.launch.py" Enter + +# Force equal pane sizes +tmux select-layout -t "$SESSION:sim" tiled + +# ============================================= +# Window 2: tools (2 panes) +# ============================================= +tmux new-window -t "$SESSION" -n "tools" + +PANE_FOX=$(tmux list-panes -t "$SESSION:tools" -F '#{pane_id}') +tmux send-keys -t "$PANE_FOX" "clear && $S && ros2 launch foxglove_bridge foxglove_bridge_launch.xml" Enter + +# Split down -> Message publisher +PANE_MSG=$(tmux split-window -v -t "$PANE_FOX" -P -F '#{pane_id}') +tmux send-keys -t "$PANE_MSG" "clear && $S && ros2 launch vortex_utility_nodes message_publisher.launch.py" Enter + +# ============================================= +# Focus first window and attach +# ============================================= +tmux select-window -t "$SESSION:sim" +tmux attach-session -t "$SESSION" diff --git a/utility_scripts/launch_drone_vehicle.sh b/utility_scripts/launch_drone_vehicle.sh new file mode 100755 index 000000000..1ddf250e4 --- /dev/null +++ b/utility_scripts/launch_drone_vehicle.sh @@ -0,0 +1,128 @@ +#!/bin/bash +# Launch drone vehicle stack in a tmux session +# Usage: ./launch_drone_vehicle.sh [--ori_type quat|euler] [--controller_type pid|adapt|adapt_quat] + +SESSION="drone_vehicle" + +# ============================================= +# Parse arguments +# ============================================= +ORI_TYPE="quat" +CONTROLLER_TYPE="adapt_quat" + +while [[ $# -gt 0 ]]; do + case "$1" in + --ori_type | -o) + ORI_TYPE="$2" + shift 2 + ;; + --controller_type | -c) + CONTROLLER_TYPE="$2" + shift 2 + ;; + *) + echo "Unknown argument: $1" + echo "Usage: $0 [--ori_type quat|euler] [--controller_type pid|adapt|adapt_quat]" + exit 1 + ;; + esac +done + +# Validate +if [[ "$ORI_TYPE" != "quat" && "$ORI_TYPE" != "euler" ]]; then + echo "Error: ori_type must be 'quat' or 'euler' (got: $ORI_TYPE)" + exit 1 +fi + +if [[ "$CONTROLLER_TYPE" != "pid" && "$CONTROLLER_TYPE" != "adapt" && "$CONTROLLER_TYPE" != "adapt_quat" ]]; then + echo "Error: controller_type must be 'pid', 'adapt' or 'adapt_quat' (got: $CONTROLLER_TYPE)" + exit 1 +fi + +# Cross-validate: pid uses quat reference filter, adaptive uses euler +if [[ "$CONTROLLER_TYPE" == "pid" && "$ORI_TYPE" != "quat" ]]; then + echo "Warning: pid controller uses quaternion representation — consider --ori_type quat" +fi +if [[ "$CONTROLLER_TYPE" == "adapt" && "$ORI_TYPE" != "euler" ]]; then + echo "Warning: adaptive controller uses euler representation — consider --ori_type euler" +fi +if [[ "$CONTROLLER_TYPE" == "adapt_quat" && "$ORI_TYPE" != "quat" ]]; then + echo "Warning: adapt_quat controller uses quaternion representation — consider --ori_type quat" +fi + +# Select the DP launch file +if [[ "$CONTROLLER_TYPE" == "adapt_quat" ]]; then + DP_LAUNCH="auv_setp dp_quat.launch.py" +elif [[ "$CONTROLLER_TYPE" == "pid" ]]; then + DP_LAUNCH="auv_setp dp.launch.py controller_type:=pid orientation_mode:=$ORI_TYPE" +else + DP_LAUNCH="auv_setp dp.launch.py controller_type:=adaptive orientation_mode:=$ORI_TYPE" +fi + +echo "[LAUNCH] ori_type=$ORI_TYPE controller_type=$CONTROLLER_TYPE dp_launch=$DP_LAUNCH" + +# Kill existing session if it exists +tmux kill-session -t "$SESSION" 2>/dev/null + +# ============================================= +# Window 1: control (4 equal panes) +# ============================================= +tmux new-session -d -s "$SESSION" -n "control" + +PANE_C1=$(tmux list-panes -t "$SESSION:control" -F '#{pane_id}') +tmux send-keys -t "$PANE_C1" "s && ros2 launch auv_setup thruster.launch.py" Enter + +PANE_C2=$(tmux split-window -h -t "$PANE_C1" -P -F '#{pane_id}') +tmux send-keys -t "$PANE_C2" "s && ros2 launch operation_mode_manager operation_mode_manager.launch.py" Enter + +PANE_C3=$(tmux split-window -v -t "$PANE_C1" -P -F '#{pane_id}') +tmux send-keys -t "$PANE_C3" "s && ros2 launch $DP_LAUNCH" Enter + +PANE_C4=$(tmux split-window -v -t "$PANE_C2" -P -F '#{pane_id}') +tmux send-keys -t "$PANE_C4" "s" Enter + +tmux select-layout -t "$SESSION:control" tiled + +# ============================================= +# Window 2: perception (4 equal panes) +# ============================================= +tmux new-window -t "$SESSION" -n "perception" + +PANE_P1=$(tmux list-panes -t "$SESSION:perception" -F '#{pane_id}') +tmux send-keys -t "$PANE_P1" "s && ros2 launch auv_setup nucleus_odom_transformer.launch.py" Enter + +PANE_P2=$(tmux split-window -h -t "$PANE_P1" -P -F '#{pane_id}') +tmux send-keys -t "$PANE_P2" "s" Enter + +PANE_P3=$(tmux split-window -v -t "$PANE_P1" -P -F '#{pane_id}') +tmux send-keys -t "$PANE_P3" "s" Enter + +PANE_P4=$(tmux split-window -v -t "$PANE_P2" -P -F '#{pane_id}') +tmux send-keys -t "$PANE_P4" "s" Enter + +tmux select-layout -t "$SESSION:perception" tiled + +# ============================================= +# Window 3: misc (4 equal panes) +# ============================================= +tmux new-window -t "$SESSION" -n "misc" + +PANE_M1=$(tmux list-panes -t "$SESSION:misc" -F '#{pane_id}') +tmux send-keys -t "$PANE_M1" "s" Enter + +PANE_M2=$(tmux split-window -h -t "$PANE_M1" -P -F '#{pane_id}') +tmux send-keys -t "$PANE_M2" "s" Enter + +PANE_M3=$(tmux split-window -v -t "$PANE_M1" -P -F '#{pane_id}') +tmux send-keys -t "$PANE_M3" "s" Enter + +PANE_M4=$(tmux split-window -v -t "$PANE_M2" -P -F '#{pane_id}') +tmux send-keys -t "$PANE_M4" "s" Enter + +tmux select-layout -t "$SESSION:misc" tiled + +# ============================================= +# Focus control window and attach +# ============================================= +tmux select-window -t "$SESSION:control" +tmux attach-session -t "$SESSION"