diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 9f98afb0b..c1c6b271e 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/config/environments/longbeach.yaml b/auv_setup/config/environments/longbeach.yaml index 9d516aba5..8b4a140c7 100644 --- a/auv_setup/config/environments/longbeach.yaml +++ b/auv_setup/config/environments/longbeach.yaml @@ -2,13 +2,14 @@ # # Gravity value from EGM2008 12th order via wolframalpha.com # Pressure value from 10 year average via wolframalpha.com +/**: + ros__parameters: + atmosphere: + pressure: 101500.0 -atmosphere: - pressure: 101500 + water: + density: 1000.0 # Freshwater + # density: 1025.0 # Saltwater -water: - density: 1000 # Freshwater - # density: 1025 # Saltwater - -gravity: - acceleration: 9.79609 + gravity: + acceleration: 9.79609 diff --git a/auv_setup/config/environments/stonefish_sim.yaml b/auv_setup/config/environments/stonefish_sim.yaml new file mode 100644 index 000000000..eb18df92f --- /dev/null +++ b/auv_setup/config/environments/stonefish_sim.yaml @@ -0,0 +1,12 @@ +# Environmental parameters from the stonefish simulator +# Verify these match you scenario file +/**: + ros__parameters: + atmosphere: + pressure: 101300.0 + + water: + density: 1031.0 # Verify this matches the scenario config + + gravity: + acceleration: 9.81 diff --git a/auv_setup/config/environments/trondheim_freshwater.yaml b/auv_setup/config/environments/trondheim_freshwater.yaml index 59c915c90..187856459 100644 --- a/auv_setup/config/environments/trondheim_freshwater.yaml +++ b/auv_setup/config/environments/trondheim_freshwater.yaml @@ -2,12 +2,13 @@ # # Pressure value from 10 year average via wolframalpha.com # Gravity value from EGM2008 12th order via wolframalpha.com +/**: + ros__parameters: + atmosphere: + pressure: 100000.0 # phone measurement from mclab -atmosphere: - pressure: 100000 # phone measurement from mclab + water: + density: 1000.0 -water: - density: 1000 - -gravity: - acceleration: 9.82841 + gravity: + acceleration: 9.82841 diff --git a/auv_setup/config/environments/trondheim_saltwater.yaml b/auv_setup/config/environments/trondheim_saltwater.yaml index 7b85ec50c..9788777ce 100644 --- a/auv_setup/config/environments/trondheim_saltwater.yaml +++ b/auv_setup/config/environments/trondheim_saltwater.yaml @@ -3,12 +3,13 @@ # Pressure value from 10 year average via wolframalpha.com # Density from wolgframalpha.com query "ocean density" (default parameters) # Gravity value from EGM2008 12th order via wolframalpha.com +/**: + ros__parameters: + atmosphere: + pressure: 101000.0 # [Pa] -atmosphere: - pressure: 101000 # [Pa] + water: + density: 1026.0 # [kg/m3] -water: - density: 1026 # [kg/m3] - -gravity: - acceleration: 9.82841 + gravity: + acceleration: 9.82841 diff --git a/auv_setup/config/robots/nautilus.yaml b/auv_setup/config/robots/nautilus.yaml index 2e81fdeb8..936415809 100644 --- a/auv_setup/config/robots/nautilus.yaml +++ b/auv_setup/config/robots/nautilus.yaml @@ -98,7 +98,9 @@ topics: wrench_input: "wrench_input" thruster_forces: "thruster_forces" + camera_light: "camera_light" pwm_output: "pwm" + camera_light: "camera_light" current: "power_sense_module/current" voltage: "power_sense_module/voltage" pressure: "pressure" @@ -109,6 +111,7 @@ odom: "odom" operation_mode: "operation_mode" killswitch: "killswitch" + mission_wipe: "mission/wipe" reference_pose: "reference_pose" landmarks: "landmarks" waypoint: "waypoint" @@ -116,11 +119,13 @@ los: "guidance/los" dp_rpy: "guidance/dp_rpy" dp_quat: "guidance/dp_quat" + dp_quat_target_pose: "guidance/dp_quat_target_pose" dvl_twist: "dvl/twist" dvl_altitude: "dvl/altitude" imu: "imu/data_raw" sonar_info: "fls/sonar_info" - pressure_sensor: "pressure_sensor" + pressure_sensor: "pressure" + magnetometer: "magnetometer" gripper_servos: "gripper_servos" @@ -131,6 +136,7 @@ landmark_convergence: "landmark_convergence" waypoint_manager: "waypoint_manager" + services: set_operation_mode: "set_operation_mode" set_killswitch: "set_killswitch" @@ -138,3 +144,4 @@ get_operation_mode: "get_operation_mode" waypoint_addition: "waypoint_addition" start_mission: "start_mission" + reset_odom_origin: "reset_odom_origin" diff --git a/auv_setup/description/nautilus.urdf.xacro b/auv_setup/description/nautilus.urdf.xacro index 03e5af5ef..edd969238 100644 --- a/auv_setup/description/nautilus.urdf.xacro +++ b/auv_setup/description/nautilus.urdf.xacro @@ -32,7 +32,7 @@ - + @@ -55,7 +55,7 @@ - + @@ -100,4 +100,13 @@ + + + + + + + + + diff --git a/auv_setup/launch/mru.launch.py b/auv_setup/launch/mru.launch.py new file mode 100644 index 000000000..803ff7cb8 --- /dev/null +++ b/auv_setup/launch/mru.launch.py @@ -0,0 +1,115 @@ +"""Kongsberg MRU — IMU driver. + +Two modes: + + standalone=true (default) + Launches a regular Node. + + standalone=false + Uses LoadComposableNodes to attach to an already-running container + identified by `container_name`. +""" + +import os + +import yaml +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 LoadComposableNodes, Node +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) + standalone = LaunchConfiguration('standalone').perform(context).lower() == 'true' + container_name = LaunchConfiguration('container_name').perform(context) + host_ip = LaunchConfiguration('host_ip').perform(context) + + drone_params = os.path.join( + get_package_share_directory('auv_setup'), + 'config', + 'robots', + f'{drone}.yaml', + ) + + with open(drone_params) as f: + robot_topics = yaml.safe_load(f)['/**']['ros__parameters']['topics'] + + parameters = [ + { + "imu_pub_topic": robot_topics['imu'], + 'frame_id': f'/{namespace}/imu_link', + 'connection_params.remote_ip': '10.0.0.20', + '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': host_ip, + 'mru_settings.format': 'MRUBIN', + 'mru_settings.interval': 5, + 'mru_settings.token': 21, + } + ] + + if standalone: + return [ + Node( + package='mru_ros_interface', + executable='mru_ros_interface_node', + name='mru_ros_interface_node', + namespace=namespace, + parameters=parameters, + output='screen', + ) + ] + else: + return [ + LoadComposableNodes( + target_container=container_name, + composable_node_descriptions=[ + ComposableNode( + package='mru_ros_interface', + plugin='MruRosInterface', + name='mru_ros_interface_node', + namespace=namespace, + parameters=parameters, + extra_arguments=[{'use_intra_process_comms': True}], + ) + ], + ) + ] + + +def generate_launch_description(): + return LaunchDescription( + [ + DeclareLaunchArgument( + 'standalone', + default_value='true', + description=( + 'true = launch as a regular node; ' + 'false = attach to an existing container named by container_name' + ), + ), + DeclareLaunchArgument( + 'container_name', + default_value='', + description='Container to attach to when standalone=false', + ), + DeclareLaunchArgument( + 'host_ip', + default_value='10.0.0.67', + description='IP address of this host machine, sent to the MRU so it knows where to stream data.', + ), + ] + + declare_drone_and_namespace_args() + + [OpaqueFunction(function=_launch_setup)] + ) diff --git a/auv_setup/launch/ms5837.launch.py b/auv_setup/launch/ms5837.launch.py new file mode 100644 index 000000000..dee88c448 --- /dev/null +++ b/auv_setup/launch/ms5837.launch.py @@ -0,0 +1,74 @@ +"""MS5837 pressure/depth sensor driver.""" + +import os + +import yaml +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) + environment = LaunchConfiguration('environment').perform(context) + + env_params_path = os.path.join( + get_package_share_directory('auv_setup'), + 'config', + 'environments', + f'{environment}.yaml', + ) + + with open(env_params_path) as f: + env = yaml.safe_load(f)['/**']['ros__parameters'] + + fluid_density = env['water']['density'] + atmospheric_pressure = env['atmosphere']['pressure'] + gravity = env['gravity']['acceleration'] + + return [ + Node( + package='ms5837_driver', + executable='ms5837_node', + name='ms5837_driver_node', + namespace=namespace, + parameters=[ + { + 'frame_id': f'{namespace}/pressure_sensor_link', + 'fluid_density': fluid_density, + 'atmospheric_pressure': atmospheric_pressure, + 'gravity': gravity, + 'publish_depth': True, + 'publish_altitude': False, + } + ], + output='screen', + ) + ] + + +def generate_launch_description(): + return LaunchDescription( + [ + DeclareLaunchArgument( + 'environment', + default_value='trondheim_freshwater', + description='Environment config to load from auv_setup/config/environments/.', + choices=[ + 'longbeach', + 'stonefish_sim', + 'trondheim_freshwater', + 'trondheim_saltwater', + ], + ), + ] + + declare_drone_and_namespace_args() + + [OpaqueFunction(function=_launch_setup)] + ) diff --git a/auv_setup/launch/nucleus.launch.py b/auv_setup/launch/nucleus.launch.py new file mode 100644 index 000000000..a2db9ef52 --- /dev/null +++ b/auv_setup/launch/nucleus.launch.py @@ -0,0 +1,134 @@ +"""Nortek Nucleus 1000 — DVL/INS/pressure driver. + +Two modes: + + standalone=true (default) + Launches a regular Node. + + standalone=false + Uses LoadComposableNodes to attach to an already-running container + identified by `container_name`. +""" + +import os + +import yaml +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 LoadComposableNodes, Node +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) + standalone = LaunchConfiguration('standalone').perform(context).lower() == 'true' + container_name = LaunchConfiguration('container_name').perform(context) + + drone_params = os.path.join( + get_package_share_directory('auv_setup'), + 'config', + 'robots', + f'{drone}.yaml', + ) + + with open(drone_params) as f: + robot_topics = yaml.safe_load(f)['/**']['ros__parameters']['topics'] + + parameters = [ + { + 'frame_id': f'/{namespace}/dvl_link', + 'qos': 'best_effort', + 'connection_params.remote_ip': '10.0.0.42', + 'connection_params.data_remote_port': 9000, + 'connection_params.password': '', + 'enable_imu': False, + 'enable_ins_odom': True, + 'enable_dvl': True, + 'enable_pressure': True, + 'enable_altimeter': True, + 'enable_magnetometer': True, + '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}/nucleus/imu/data", + "ins_pub_topic": f"/{namespace}/nucleus/odom", + "dvl_pub_topic": robot_topics['dvl_twist'], + "altimeter_pub_topic": robot_topics['dvl_altitude'], + "pressure_pub_topic": f"/{namespace}/nucleus/pressure", # Not used atm + "magnetometer_pub_topic": robot_topics['magnetometer'], + "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': 0.0, + 'instrument_settings.rotyz': 0.0, + 'instrument_settings.rotxz': 0.0, + } + ] + + if standalone: + return [ + Node( + package='nortek_nucleus_ros_interface', + executable='nortek_nucleus_ros_interface_node', + name='nortek_nucleus_ros_interface_node', + namespace=namespace, + parameters=parameters, + output='screen', + ) + ] + else: + return [ + LoadComposableNodes( + target_container=container_name, + composable_node_descriptions=[ + ComposableNode( + package='nortek_nucleus_ros_interface', + plugin='NortekNucleusRosInterface', + name='nortek_nucleus_ros_interface_node', + namespace=namespace, + parameters=parameters, + extra_arguments=[{'use_intra_process_comms': True}], + ) + ], + ) + ] + + +def generate_launch_description(): + return LaunchDescription( + [ + DeclareLaunchArgument( + 'standalone', + default_value='true', + description=( + 'true = launch as a regular node; ' + 'false = attach to an existing container named by container_name' + ), + ), + DeclareLaunchArgument( + 'container_name', + default_value='', + description='Container to attach to when standalone=false', + ), + ] + + 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 index 5f07b3cf1..07165ed41 100644 --- a/auv_setup/launch/nucleus_odom_transformer.launch.py +++ b/auv_setup/launch/nucleus_odom_transformer.launch.py @@ -1,10 +1,24 @@ +"""Launch the Nucleus driver and odom_transformer together. + +Forwards Nucleus INS odometry through odom_transformer to produce the AUV's +odometry and sets up the transform tree. + +When launch_nucleus=true (default), both the Nucleus driver and odom_transformer +run inside a shared ComposableNodeContainer for intra-process comms. + +When launch_nucleus=false, only the odom_transformer is launched as a plain node, +assuming the Nucleus driver is already running elsewhere. +""" + import os from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription, OpaqueFunction +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch_ros.actions import Node +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, @@ -12,111 +26,97 @@ ) -# Simple launch file to forward INS data from the nucleus and use as odometry source. -# Launches the nucleus, transforms INS to base_link and sets up the AUV transform tree. def launch_setup(context, *args, **kwargs): drone, namespace = resolve_drone_and_namespace(context) + launch_nucleus = LaunchConfiguration('launch_nucleus').perform(context).lower() == 'true' + container_name = LaunchConfiguration('container_name').perform(context) - drone_params = os.path.join( - get_package_share_directory("auv_setup"), - "config", - "robots", - f"{drone}.yaml", - ) + auv_setup_dir = get_package_share_directory('auv_setup') + drone_params = os.path.join(auv_setup_dir, 'config', 'robots', f'{drone}.yaml') drone_description_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join( - get_package_share_directory("auv_setup"), - "launch", - "drone_description.launch.py", - ) + os.path.join(auv_setup_dir, 'launch', 'drone_description.launch.py') ), - launch_arguments={ - "drone": drone, - "namespace": namespace, - }.items(), + 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", - "qos": "best_effort", - "connection_params.remote_ip": "10.0.0.42", - "connection_params.data_remote_port": 9000, - "connection_params.password": "", - "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": 0.0, # Transform currently not working. Maybe fix later - "instrument_settings.rotyz": 0.0, - "instrument_settings.rotxz": 0.0, - }, - drone_params, - ], - output="screen", - ) + odom_transformer_params = [ + { + 'sensor_frame': 'dvl_link', + 'publish_tf': True, + 'publish_pose': True, + 'publish_twist': 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}, + ] - 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, - "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", - ) + if launch_nucleus: + nucleus_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(auv_setup_dir, 'launch', 'nucleus.launch.py') + ), + launch_arguments={ + 'drone': drone, + 'namespace': namespace, + 'standalone': 'false', + 'container_name': f'/{namespace}/{container_name}', + }.items(), + ) - return [ - drone_description_launch, - nortek_nucleus_ros_interface_node, - odom_transformer_node, - ] + container = ComposableNodeContainer( + name=container_name, + namespace=namespace, + package='rclcpp_components', + executable='component_container_mt', + composable_node_descriptions=[ + ComposableNode( + package='odom_transformer', + plugin='OdomTransformer', + name='odom_transformer_node', + namespace=namespace, + parameters=odom_transformer_params, + extra_arguments=[{'use_intra_process_comms': True}], + ) + ], + output='screen', + ) + + return [drone_description_launch, container, nucleus_launch] + + else: + return [ + drone_description_launch, + Node( + package='odom_transformer', + executable='odom_transformer_node', + name='odom_transformer_node', + namespace=namespace, + parameters=odom_transformer_params, + output='screen', + ), + ] def generate_launch_description(): return LaunchDescription( - declare_drone_and_namespace_args() + [OpaqueFunction(function=launch_setup)] + [ + DeclareLaunchArgument( + 'launch_nucleus', + default_value='true', + description='Set to false if the Nucleus driver is already running.', + ), + DeclareLaunchArgument( + 'container_name', + default_value='nucleus_odom_container', + description='Name of the shared component container (used when launch_nucleus=true).', + ), + ] + + declare_drone_and_namespace_args() + + [OpaqueFunction(function=launch_setup)] ) diff --git a/auv_setup/launch/state_estimation.launch.py b/auv_setup/launch/state_estimation.launch.py index 0abc27650..87ce496fd 100644 --- a/auv_setup/launch/state_estimation.launch.py +++ b/auv_setup/launch/state_estimation.launch.py @@ -1,207 +1,110 @@ +"""Full state estimation bringup. + +Runs the ESKF, Nortek Nucleus, and MRU all inside a single shared +ComposableNodeContainer for intra-process comms. +""" + import os from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription, OpaqueFunction +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PythonExpression 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, -) +from auv_setup.launch_arg_common import declare_drone_and_namespace_args -def launch_setup(context, *args, **kwargs): - drone, namespace = resolve_drone_and_namespace(context) +def generate_launch_description(): + auv_setup_dir = get_package_share_directory('auv_setup') + eskf_dir = get_package_share_directory('eskf') - drone_params = os.path.join( - get_package_share_directory("auv_setup"), - "config", - "robots", - f"{drone}.yaml", - ) + # Mirrors resolve_drone_and_namespace: falls back to drone name if namespace is empty. + namespace = PythonExpression([ + '"', LaunchConfiguration('namespace'), '" or "', LaunchConfiguration('drone'), '"' + ]) - 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(), - ) + # /{namespace}/{container_name} + fqn_container = PythonExpression([ + '"/" + ("', LaunchConfiguration('namespace'), '" or "', LaunchConfiguration('drone'), + '") + "/" + "', LaunchConfiguration('container_name'), '"' + ]) - 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}], + return LaunchDescription( + [ + DeclareLaunchArgument( + 'environment', + default_value='trondheim_freshwater', + description='Environment config to load from auv_setup/config/environments/.', + choices=['longbeach', 'stonefish_sim', 'trondheim_freshwater', 'trondheim_saltwater'], ), - 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}], + DeclareLaunchArgument( + 'container_name', + default_value='eskf_container', + description='Name of the shared component container.', ), - ComposableNode( - package="nortek_nucleus_ros_interface", - plugin="NortekNucleusRosInterface", - name="nortek_nucleus_ros_interface_node", - namespace=namespace, - parameters=[ - { - "frame_id": f"/{namespace}/nucleus_frame", - "qos": "best_effort", - "connection_params.remote_ip": "10.0.0.42", - "connection_params.data_remote_port": 9000, - "connection_params.password": "", - "enable_imu": False, - "enable_ins_odom": False, - "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}/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": 0.0, # Transform currently not working. Maybe fix later - "instrument_settings.rotyz": 0.0, - "instrument_settings.rotxz": 0.0, - } - ], - extra_arguments=[{"use_intra_process_comms": True}], + DeclareLaunchArgument( + 'host_ip', + default_value='10.0.0.67', + description='IP address of this host machine, sent to the MRU so it knows where to stream data.', ), - ], - output="screen", - arguments=["--ros-args", "--log-level", "error"], - ) - - return [drone_description_launch, container] - - -def generate_launch_description(): - return LaunchDescription( - declare_drone_and_namespace_args() + ] + + declare_drone_and_namespace_args() + [ - OpaqueFunction(function=launch_setup), + ComposableNodeContainer( + name=LaunchConfiguration('container_name'), + namespace=namespace, + package='rclcpp_components', + executable='component_container_mt', + composable_node_descriptions=[], + output='screen', + arguments=['--ros-args', '--log-level', 'error'], + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(auv_setup_dir, 'launch', 'drone_description.launch.py') + ), + launch_arguments={ + 'drone': LaunchConfiguration('drone'), + 'namespace': namespace, + }.items(), + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(eskf_dir, 'launch', 'eskf.launch.py') + ), + launch_arguments={ + 'drone': LaunchConfiguration('drone'), + 'namespace': namespace, + 'standalone': 'false', + 'container_name': fqn_container, + 'environment': LaunchConfiguration('environment'), + 'act_as_odom_source': 'true', + }.items(), + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(auv_setup_dir, 'launch', 'nucleus.launch.py') + ), + launch_arguments={ + 'drone': LaunchConfiguration('drone'), + 'namespace': namespace, + 'standalone': 'false', + 'container_name': fqn_container, + }.items(), + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(auv_setup_dir, 'launch', 'mru.launch.py') + ), + launch_arguments={ + 'drone': LaunchConfiguration('drone'), + 'namespace': namespace, + 'standalone': 'false', + 'container_name': fqn_container, + 'host_ip': LaunchConfiguration('host_ip'), + }.items(), + ), ] ) diff --git a/guidance/reference_filter_dp/config/reference_filter_params.yaml b/guidance/reference_filter_dp/config/reference_filter_params.yaml index 1aef0ec13..d0b332f54 100644 --- a/guidance/reference_filter_dp/config/reference_filter_params.yaml +++ b/guidance/reference_filter_dp/config/reference_filter_params.yaml @@ -1,5 +1,7 @@ /**: ros__parameters: zeta: [1., 1., 1., 1., 1., 1.] - omega: [0.2, 0.2, 0.2, 0.2, 0.2, 0.2] + omega: [0.125, 0.125, 0.125, 0.125, 0.125, 0.125] time_step_ms: 10 + altitude_control_enabled: false # Set true to subscribe to DVL altitude and enable keep_altitude waypoints + altitude_lp_alpha: 0.2 # IIR coefficient for DVL altitude low-pass filter [0,1) 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..fba30a417 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 @@ -39,6 +39,18 @@ class WaypointFollower { */ Eigen::Vector18d step(); + /** + * @brief Update the waypoint target without resetting filter state. + * + * Preserves all filter dynamical state (state_.segment<6>(6) for velocity + * and state_.segment<6>(12) for acceleration) so the third-order filter + * continues evolving from its current state. Use this on preemption; use + * start() only for cold-start (first goal after node init). + * + * Thread-safe. + */ + void retarget(const Waypoint& waypoint, double convergence_threshold); + /** * @brief Check if the measured pose has converged to the reference goal. * @param measured_pose Current measured pose. @@ -46,6 +58,25 @@ class WaypointFollower { */ bool within_convergance(const Eigen::Vector6d& measured_pose) const; + /** + * @brief Convergence check that excludes z from the position error. + * + * Use during altitude-hold mode: the z goal tracks a noisy altitude + * measurement, so including it in the convergence criterion would prevent + * the action from ever succeeding. + */ + bool within_convergance_ignore_z( + const Eigen::Vector6d& measured_pose) const; + + /** + * @brief Update only the z component of the reference goal. + * + * Used during altitude-hold mode to continuously track seafloor distance + * without disturbing x/y/orientation targets. + * @param target_ned_z The desired NED z coordinate for the AUV. + */ + void update_z_goal(double target_ned_z); + /** * @brief Update the reference goal pose mid-sequence. * @param reference_goal_pose The new reference pose. 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 f29b5fb8c..997ae72c2 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 @@ -23,6 +23,9 @@ enum class WaypointMode : uint8_t { XY_AND_YAW = 5, ///< Control x, y and yaw; hold z, force roll=pitch=0. XY_FORWARD_DIR = 6, ///< Control x, y; hold z; yaw auto-computed toward target. + LEVEL_ORIENTATION = + 7, ///< Hold current position and yaw; drive roll=pitch=0. + ONLY_Z = 8, ///< Hold current x, y, orientation; control z only. }; /** @@ -31,6 +34,8 @@ enum class WaypointMode : uint8_t { struct Waypoint { PoseEuler pose{}; WaypointMode mode = WaypointMode::FULL_POSE; + bool keep_altitude{false}; + double desired_altitude{0.0}; }; } // namespace vortex::guidance diff --git a/guidance/reference_filter_dp/include/reference_filter_dp/ros/reference_filter_ros.hpp b/guidance/reference_filter_dp/include/reference_filter_dp/ros/reference_filter_ros.hpp index 12b6d60aa..b075290f4 100644 --- a/guidance/reference_filter_dp/include/reference_filter_dp/ros/reference_filter_ros.hpp +++ b/guidance/reference_filter_dp/include/reference_filter_dp/ros/reference_filter_ros.hpp @@ -8,8 +8,10 @@ #include #include #include +#include #include #include +#include #include #include #include "reference_filter_dp/lib/waypoint_follower.hpp" @@ -24,42 +26,34 @@ class ReferenceFilterNode : public rclcpp::Node { ~ReferenceFilterNode(); private: - // @brief Set the subscribers and publishers void set_subscribers_and_publisher(); - - // @brief Set the action server void set_action_server(); - - // @brief Initializes the reference filter with ROS parameters. void set_refererence_filter(); + void setup_reset_subscription(); + void on_system_reset(std_msgs::msg::Empty::ConstSharedPtr msg); - /// @brief Accept all incoming goals unconditionally. rclcpp_action::GoalResponse handle_goal( const rclcpp_action::GoalUUID& uuid, std::shared_ptr goal); - /// @brief Accept all cancel requests. rclcpp_action::CancelResponse handle_cancel( const std::shared_ptr> 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); - /** - * @brief Execute the action goal in a loop until convergence or - * preemption. - * @param goal_handle The goal handle. - */ void execute(const std::shared_ptr> goal_handle); + vortex_msgs::action::GuidanceWaypoint>> goal_handle, + bool retarget); rclcpp_action::Server::SharedPtr action_server_; + rclcpp::Subscription::SharedPtr reset_sub_; + ReferenceFilterParams filter_params_; std::unique_ptr follower_; @@ -76,7 +70,8 @@ class ReferenceFilterNode : public rclcpp::Node { rclcpp::Subscription< geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr twist_sub_; - rclcpp::TimerBase::SharedPtr reference_pub_timer_; + rclcpp::Subscription::SharedPtr + altitude_sub_; std::chrono::milliseconds time_step_{}; @@ -84,9 +79,15 @@ class ReferenceFilterNode : public rclcpp::Node { vortex::utils::types::Twist current_twist_; + bool altitude_control_enabled_{false}; + double current_altitude_{0.0}; + bool altitude_valid_{false}; + double altitude_lp_alpha_{0.9}; + std::mutex sensor_mutex_; std::atomic preempted_{false}; + std::atomic executing_{false}; std::mutex execute_mutex_; std::thread execute_thread_; }; 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 af96b5f95..bc0de698c 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 @@ -34,6 +34,10 @@ inline WaypointMode waypoint_mode_from_ros( return WaypointMode::XY_AND_YAW; case vortex_msgs::msg::WaypointMode::XY_FORWARD_DIR: return WaypointMode::XY_FORWARD_DIR; + case vortex_msgs::msg::WaypointMode::LEVEL_ORIENTATION: + return WaypointMode::LEVEL_ORIENTATION; + case vortex_msgs::msg::WaypointMode::ONLY_Z: + return WaypointMode::ONLY_Z; default: throw std::invalid_argument("Invalid ROS waypoint mode: " + std::to_string(mode_msg.mode)); @@ -47,6 +51,8 @@ inline vortex::guidance::Waypoint waypoint_from_ros( wp.pose = vortex::utils::ros_conversions::ros_pose_to_pose_euler(ros_wp.pose); wp.mode = waypoint_mode_from_ros(ros_wp.waypoint_mode); + wp.keep_altitude = ros_wp.keep_altitude; + wp.desired_altitude = ros_wp.desired_altitude; return wp; } diff --git a/guidance/reference_filter_dp/src/lib/waypoint_follower.cpp b/guidance/reference_filter_dp/src/lib/waypoint_follower.cpp index 4da1d7c31..aeb278fce 100644 --- a/guidance/reference_filter_dp/src/lib/waypoint_follower.cpp +++ b/guidance/reference_filter_dp/src/lib/waypoint_follower.cpp @@ -42,6 +42,15 @@ Eigen::Vector18d WaypointFollower::step() { return state_; } +void WaypointFollower::retarget(const Waypoint& waypoint, + double convergence_threshold) { + std::lock_guard lock(mutex_); + waypoint_mode_ = waypoint.mode; + convergence_threshold_ = convergence_threshold; + reference_goal_ = apply_mode_logic(waypoint.pose.to_vector(), + waypoint_mode_, state_.head<6>()); +} + bool WaypointFollower::within_convergance( const Eigen::Vector6d& measured_pose) const { std::lock_guard lock(mutex_); @@ -49,6 +58,20 @@ bool WaypointFollower::within_convergance( convergence_threshold_); } +bool WaypointFollower::within_convergance_ignore_z( + const Eigen::Vector6d& measured_pose) const { + std::lock_guard lock(mutex_); + Eigen::Vector6d adjusted = measured_pose; + adjusted(2) = reference_goal_(2); + return has_converged(adjusted, reference_goal_, waypoint_mode_, + convergence_threshold_); +} + +void WaypointFollower::update_z_goal(double target_ned_z) { + std::lock_guard lock(mutex_); + reference_goal_(2) = target_ned_z; +} + void WaypointFollower::set_reference(const PoseEuler& reference_goal_pose) { std::lock_guard lock(mutex_); reference_goal_ = apply_mode_logic(reference_goal_pose.to_vector(), diff --git a/guidance/reference_filter_dp/src/lib/waypoint_utils.cpp b/guidance/reference_filter_dp/src/lib/waypoint_utils.cpp index 0ee7a2523..8465444d9 100644 --- a/guidance/reference_filter_dp/src/lib/waypoint_utils.cpp +++ b/guidance/reference_filter_dp/src/lib/waypoint_utils.cpp @@ -74,6 +74,23 @@ Eigen::Vector6d apply_mode_logic(const Eigen::Vector6d& reference_in, reference_out(5) = ssa(std::atan2(dy, dx)); break; } + + case WaypointMode::LEVEL_ORIENTATION: + reference_out(0) = current_state(0); + reference_out(1) = current_state(1); + reference_out(2) = current_state(2); + reference_out(3) = 0.0; + reference_out(4) = 0.0; + reference_out(5) = current_state(5); + break; + + case WaypointMode::ONLY_Z: + reference_out(0) = current_state(0); + reference_out(1) = current_state(1); + reference_out(3) = current_state(3); + reference_out(4) = current_state(4); + reference_out(5) = current_state(5); + break; } return reference_out; @@ -105,6 +122,10 @@ bool has_converged(const Eigen::Vector6d& measured_pose, return std::sqrt(ep.head<2>().squaredNorm() + ea(2) * ea(2)); case WaypointMode::XY_FORWARD_DIR: return ep.head<2>().norm(); + case WaypointMode::LEVEL_ORIENTATION: + return ea.head<2>().norm(); + case WaypointMode::ONLY_Z: + return std::abs(ep(2)); case WaypointMode::FULL_POSE: default: return std::sqrt(ep.squaredNorm() + ea.squaredNorm()); 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 ba984821f..1bd643091 100644 --- a/guidance/reference_filter_dp/src/ros/reference_filter_ros.cpp +++ b/guidance/reference_filter_dp/src/ros/reference_filter_ros.cpp @@ -29,6 +29,8 @@ ReferenceFilterNode::ReferenceFilterNode(const rclcpp::NodeOptions& options) set_refererence_filter(); + setup_reset_subscription(); + spdlog::info(start_message); } @@ -44,6 +46,8 @@ void ReferenceFilterNode::set_subscribers_and_publisher() { this->declare_parameter("topics.twist"); this->declare_parameter("topics.guidance.dp_rpy"); this->declare_parameter("topics.reference_pose"); + altitude_control_enabled_ = + this->declare_parameter("altitude_control_enabled", false); std::string pose_topic = this->get_parameter("topics.pose").as_string(); std::string twist_topic = this->get_parameter("topics.twist").as_string(); @@ -84,6 +88,53 @@ void ReferenceFilterNode::set_subscribers_and_publisher() { current_twist_ = vortex::utils::ros_conversions::ros_twist_to_twist( msg->twist.twist); }); + + if (altitude_control_enabled_) { + this->declare_parameter("topics.dvl_altitude"); + this->declare_parameter("altitude_lp_alpha", 0.9); + + std::string dvl_altitude_topic = + this->get_parameter("topics.dvl_altitude").as_string(); + altitude_lp_alpha_ = + this->get_parameter("altitude_lp_alpha").as_double(); + + altitude_sub_ = + this->create_subscription( + dvl_altitude_topic, qos_sensor_data, + [this](const vortex_msgs::msg::DVLAltitude::SharedPtr msg) { + std::lock_guard lock(sensor_mutex_); + if (!altitude_valid_) { + current_altitude_ = msg->altitude; + altitude_valid_ = true; + } else { + current_altitude_ = + altitude_lp_alpha_ * current_altitude_ + + (1.0 - altitude_lp_alpha_) * msg->altitude; + } + }); + + spdlog::info("Altitude control enabled, subscribing to '{}'", + dvl_altitude_topic); + } +} + +void ReferenceFilterNode::setup_reset_subscription() { + reset_sub_ = this->create_subscription( + "mission/wipe", vortex::utils::qos_profiles::reliable_profile(1), + [this](std_msgs::msg::Empty::ConstSharedPtr msg) { + on_system_reset(msg); + }); +} + +void ReferenceFilterNode::on_system_reset( + std_msgs::msg::Empty::ConstSharedPtr) { + std::lock_guard lock(execute_mutex_); + preempted_ = true; + if (execute_thread_.joinable()) { + execute_thread_.join(); + } + preempted_ = false; + spdlog::info("ReferenceFilter: reset complete"); } void ReferenceFilterNode::set_action_server() { @@ -137,40 +188,78 @@ void ReferenceFilterNode::handle_accepted( rclcpp_action::ServerGoalHandle> goal_handle) { std::lock_guard lock(execute_mutex_); + const bool retarget = executing_.load(); preempted_ = true; if (execute_thread_.joinable()) { execute_thread_.join(); } preempted_ = false; - execute_thread_ = - std::thread([this, goal_handle]() { execute(goal_handle); }); + execute_thread_ = std::thread( + [this, goal_handle, retarget]() { execute(goal_handle, retarget); }); } void ReferenceFilterNode::execute( - const std::shared_ptr< - rclcpp_action::ServerGoalHandle> - goal_handle) { - spdlog::info("Executing goal"); - - double convergence_threshold = - goal_handle->get_goal()->convergence_threshold; + const std::shared_ptr> goal_handle, + bool retarget) { + executing_ = true; - if (convergence_threshold <= 0.0) { - convergence_threshold = 0.1; + double threshold = goal_handle->get_goal()->convergence_threshold; + if (threshold <= 0.0) { + threshold = 0.1; spdlog::warn( - "ReferenceFilter: Invalid convergence_threshold received (<= 0). " - "Using default 0.1"); + "ReferenceFilter: invalid convergence_threshold (<= 0), using 0.1"); + } + + auto wp = waypoint_from_ros(goal_handle->get_goal()->waypoint); + + if (wp.keep_altitude && altitude_control_enabled_ && + wp.desired_altitude <= 0.0) { + executing_ = false; + auto result = + std::make_shared(); + result->success = false; + goal_handle->abort(result); + spdlog::error( + "ReferenceFilter: desired_altitude must be > 0, got {:.3f}", + wp.desired_altitude); + return; } - const auto wp = waypoint_from_ros(goal_handle->get_goal()->waypoint); + if (wp.keep_altitude && altitude_control_enabled_) { + const auto [pose, current_alt, alt_valid] = [this] { + std::lock_guard lock(sensor_mutex_); + return std::tuple{current_pose_, current_altitude_, + altitude_valid_}; + }(); + if (!alt_valid) { + spdlog::warn( + "ReferenceFilter: keep_altitude requested but no DVL altitude " + "received yet; proceeding with waypoint z as-is"); + } else { + wp.pose.z = pose.z + current_alt - wp.desired_altitude; + } + spdlog::info( + "Altitude-hold mode: desired_altitude={:.2f} m, initial " + "z_goal={:.3f}", + wp.desired_altitude, wp.pose.z); + } - const auto [pose, twist] = [this] { - std::lock_guard lock(sensor_mutex_); - return std::pair{current_pose_, current_twist_}; - }(); + if (retarget) { + follower_->retarget(wp, threshold); + spdlog::info("Executing goal (filter state preserved)"); + } else { + const auto [pose, twist] = [this] { + std::lock_guard lock(sensor_mutex_); + return std::pair{current_pose_, current_twist_}; + }(); + follower_->start(pose, twist, wp, threshold); + spdlog::info("Executing goal (cold start)"); + } - follower_->start(pose, twist, wp, convergence_threshold); + const bool keep_altitude = wp.keep_altitude && altitude_control_enabled_; + const double desired_altitude = wp.desired_altitude; auto result = std::make_shared(); @@ -179,6 +268,7 @@ void ReferenceFilterNode::execute( while (rclcpp::ok()) { if (preempted_.load()) { + executing_ = false; result->success = false; goal_handle->abort(result); spdlog::info("Goal preempted by newer goal"); @@ -186,6 +276,7 @@ void ReferenceFilterNode::execute( } if (goal_handle->is_canceling()) { + executing_ = false; result->success = false; goal_handle->canceled(result); spdlog::info("Goal canceled"); @@ -194,44 +285,48 @@ void ReferenceFilterNode::execute( Eigen::Vector18d filter_state = follower_->step(); + if (keep_altitude) { + const auto [current_z, current_alt] = [this] { + std::lock_guard lock(sensor_mutex_); + return std::pair{current_pose_.z, current_altitude_}; + }(); + follower_->update_z_goal(current_z + current_alt - + desired_altitude); + } + + reference_pub_->publish(fill_reference_msg(filter_state)); + const auto current_pose_vector = [this] { std::lock_guard lock(sensor_mutex_); return current_pose_.to_vector(); }(); - bool target_reached = - follower_->within_convergance(current_pose_vector); + const bool converged = + keep_altitude + ? follower_->within_convergance_ignore_z(current_pose_vector) + : follower_->within_convergance(current_pose_vector); - if (target_reached) { + if (converged) { follower_->snap_state_to_reference(); - auto final_reference_msg = - std::make_unique( - fill_reference_msg(follower_->state())); - - reference_pub_->publish(std::move(final_reference_msg)); + reference_pub_->publish(fill_reference_msg(follower_->state())); + executing_ = false; result->success = true; goal_handle->succeed(result); spdlog::info("Goal reached"); return; } - auto reference_msg = - std::make_unique( - fill_reference_msg(filter_state)); - reference_pub_->publish(std::move(reference_msg)); loop_rate.sleep(); } + if (!rclcpp::ok() && goal_handle->is_active()) { - auto result = - std::make_shared(); + executing_ = false; result->success = false; - try { goal_handle->abort(result); } catch (...) { - // Ignore exceptions during shutdown } } } diff --git a/guidance/reference_filter_dp_quat/CMakeLists.txt b/guidance/reference_filter_dp_quat/CMakeLists.txt index cfe51ca33..e69ae2a97 100644 --- a/guidance/reference_filter_dp_quat/CMakeLists.txt +++ b/guidance/reference_filter_dp_quat/CMakeLists.txt @@ -11,6 +11,7 @@ endif() find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) find_package(rclcpp_action REQUIRED) find_package(rclcpp_components REQUIRED) find_package(vortex_msgs REQUIRED) @@ -52,6 +53,7 @@ ament_target_dependencies(${COMPONENT_LIB_NAME} rclcpp rclcpp_components rclcpp_action + std_msgs geometry_msgs nav_msgs vortex_msgs 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 71aad134a..89403d362 100644 --- a/guidance/reference_filter_dp_quat/config/reference_filter_params.yaml +++ b/guidance/reference_filter_dp_quat/config/reference_filter_params.yaml @@ -1,6 +1,10 @@ /**: ros__parameters: zeta: [1., 1., 1., 1., 1., 1.] - omega: [0.2, 0.2, 0.2, 0.2, 0.2, 0.2] + omega: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1] time_step_ms: 10 publish_rpy_debug: false + altitude_control_enabled: true # Set true to subscribe to DVL altitude and enable keep_altitude waypoints + altitude_lp_alpha: 0.2 # IIR coefficient for DVL altitude low-pass filter [0,1) + debug: + goal_publish_mode: "timer" # "none" | "timer" (every execute loop step) | "on_new_goal" diff --git a/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/waypoint_follower.hpp b/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/waypoint_follower.hpp index 6505036cd..454c2e8f2 100644 --- a/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/waypoint_follower.hpp +++ b/guidance/reference_filter_dp_quat/include/reference_filter_dp_quat/lib/waypoint_follower.hpp @@ -48,6 +48,17 @@ class WaypointFollower { */ void step(); + /** + * @brief Update the waypoint target without resetting filter state. + * + * Preserves all filter dynamical state (the equivalent of nominal_pose_ + * plus the velocity and acceleration slots of the state vector) so the + * third-order filter continues evolving from its current state. Use + * this on preemption; use start() only for cold-start (first goal after + * node init). + */ + void retarget(const Waypoint& waypoint, double convergence_threshold); + /** * @brief Check if the measured pose has converged to the waypoint goal. * @param measured_pose Current measured pose. @@ -55,12 +66,33 @@ class WaypointFollower { */ bool within_convergance(const Pose& measured_pose) const; + /** + * @brief Convergence check that excludes z from the position error. + * + * Use this during altitude-hold mode: the z goal tracks a noisy altitude + * measurement, so including it in the convergence criterion would prevent + * the action from ever succeeding. + * @param measured_pose Current measured pose. + * @return True if x/y/orientation error is within the convergence + * threshold. + */ + bool within_convergance_ignore_z(const Pose& measured_pose) const; + /** * @brief Update the reference goal pose mid-sequence. * @param reference_goal_pose The new reference pose. */ void set_reference(const Pose& reference_goal_pose); + /** + * @brief Update only the z component of the waypoint goal. + * + * Used during altitude-hold mode to continuously track seafloor distance + * without disturbing the x/y/orientation targets. + * @param target_ned_z The desired NED z coordinate for the AUV. + */ + void update_z_goal(double target_ned_z); + /** * @brief Snap the nominal pose to the waypoint goal and zero * errors/velocity. 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 4daf2871f..4e1658c71 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 @@ -8,8 +8,11 @@ #include #include #include +#include +#include #include #include +#include #include #include #include @@ -17,6 +20,8 @@ namespace vortex::guidance { +enum class DebugPublishMode { none, timer, on_new_goal }; + class ReferenceFilterNode : public rclcpp::Node { public: explicit ReferenceFilterNode( @@ -25,42 +30,40 @@ class ReferenceFilterNode : public rclcpp::Node { ~ReferenceFilterNode(); private: - // @brief Set the subscribers and publishers void set_subscribers_and_publisher(); - - // @brief Set the action server void set_action_server(); - - // @brief Initializes the reference filter with ROS parameters. void set_refererence_filter(); - /// @brief Accept all incoming goals unconditionally. + void setup_reset_subscription(); + void setup_debug_publisher(); + void publish_debug_goal(); + void publish_waypoint_goal(const vortex::utils::types::Waypoint& wp, + double convergence_threshold); + + void on_system_reset(std_msgs::msg::Empty::ConstSharedPtr msg); + rclcpp_action::GoalResponse handle_goal( const rclcpp_action::GoalUUID& uuid, std::shared_ptr goal); - /// @brief Accept all cancel requests. rclcpp_action::CancelResponse handle_cancel( const std::shared_ptr> 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); - /** - * @brief Execute the action goal in a loop until convergence or - * preemption. - * @param goal_handle The goal handle. - */ void execute(const std::shared_ptr> goal_handle); + vortex_msgs::action::GuidanceWaypoint>> goal_handle, + bool retarget); rclcpp_action::Server::SharedPtr action_server_; + rclcpp::Subscription::SharedPtr reset_sub_; + ReferenceFilterParams filter_params_; std::unique_ptr follower_; @@ -71,7 +74,14 @@ class ReferenceFilterNode : public rclcpp::Node { rclcpp::Publisher::SharedPtr rpy_debug_pub_; + rclcpp::Publisher::SharedPtr + debug_goal_pub_; + + rclcpp::Publisher::SharedPtr + waypoint_goal_pub_; + bool publish_rpy_debug_{false}; + DebugPublishMode debug_mode_{DebugPublishMode::none}; rclcpp::Subscription::SharedPtr reference_sub_; @@ -82,15 +92,24 @@ class ReferenceFilterNode : public rclcpp::Node { rclcpp::Subscription< geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr twist_sub_; + rclcpp::Subscription::SharedPtr + altitude_sub_; + std::chrono::milliseconds time_step_{}; vortex::utils::types::Pose current_pose_; vortex::utils::types::Twist current_twist_; + bool altitude_control_enabled_{false}; + double current_altitude_{0.0}; + bool altitude_valid_{false}; + double altitude_lp_alpha_{0.9}; + std::mutex sensor_mutex_; std::atomic preempted_{false}; + std::atomic executing_{false}; std::mutex execute_mutex_; std::thread execute_thread_; }; diff --git a/guidance/reference_filter_dp_quat/package.xml b/guidance/reference_filter_dp_quat/package.xml index d7aa188a2..2b8fa54bc 100644 --- a/guidance/reference_filter_dp_quat/package.xml +++ b/guidance/reference_filter_dp_quat/package.xml @@ -10,6 +10,7 @@ ament_cmake rclcpp + std_msgs rclcpp_action geometry_msgs vortex_msgs 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 a03164f94..c38b96a3e 100644 --- a/guidance/reference_filter_dp_quat/src/lib/waypoint_follower.cpp +++ b/guidance/reference_filter_dp_quat/src/lib/waypoint_follower.cpp @@ -39,6 +39,15 @@ void WaypointFollower::step() { inject_and_reset(); } +void WaypointFollower::retarget(const Waypoint& waypoint, + double convergence_threshold) { + std::lock_guard lock(mutex_); + waypoint_mode_ = waypoint.mode; + convergence_threshold_ = convergence_threshold; + waypoint_goal_ = vortex::utils::waypoints::compute_waypoint_goal( + waypoint.pose, waypoint_mode_, nominal_pose_); +} + Eigen::Vector6d WaypointFollower::update_reference() const { Eigen::Vector6d filter_reference; filter_reference.head<3>() = @@ -58,9 +67,9 @@ void WaypointFollower::inject_and_reset() { Eigen::Quaterniond delta_quat( Eigen::AngleAxisd(angle, delta_orientation.normalized())); 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. + /** 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(); @@ -76,12 +85,26 @@ bool WaypointFollower::within_convergance(const Pose& measured_pose) const { measured_pose, waypoint_goal_, waypoint_mode_, convergence_threshold_); } +bool WaypointFollower::within_convergance_ignore_z( + const Pose& measured_pose) const { + std::lock_guard lock(mutex_); + Pose adjusted = measured_pose; + adjusted.z = waypoint_goal_.z; + return vortex::utils::waypoints::has_converged( + adjusted, waypoint_goal_, waypoint_mode_, convergence_threshold_); +} + void WaypointFollower::set_reference(const Pose& reference_goal_pose) { std::lock_guard lock(mutex_); waypoint_goal_ = vortex::utils::waypoints::compute_waypoint_goal( reference_goal_pose, waypoint_mode_, nominal_pose_); } +void WaypointFollower::update_z_goal(double target_ned_z) { + std::lock_guard lock(mutex_); + waypoint_goal_.z = target_ned_z; +} + void WaypointFollower::snap_state_to_reference() { std::lock_guard lock(mutex_); nominal_pose_ = waypoint_goal_; 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 f1da967d6..b415b4155 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 @@ -29,6 +29,10 @@ ReferenceFilterNode::ReferenceFilterNode(const rclcpp::NodeOptions& options) set_refererence_filter(); + setup_reset_subscription(); + + setup_debug_publisher(); + spdlog::info(start_message); } @@ -44,6 +48,8 @@ void ReferenceFilterNode::set_subscribers_and_publisher() { this->declare_parameter("topics.twist"); this->declare_parameter("topics.guidance.dp_quat"); this->declare_parameter("topics.reference_pose"); + altitude_control_enabled_ = + this->declare_parameter("altitude_control_enabled", false); std::string pose_topic = this->get_parameter("topics.pose").as_string(); std::string twist_topic = this->get_parameter("topics.twist").as_string(); @@ -70,6 +76,7 @@ void ReferenceFilterNode::set_subscribers_and_publisher() { reference_sub_ = this->create_subscription( reference_pose_topic, qos_sensor_data, [this](const geometry_msgs::msg::PoseStamped::SharedPtr msg) { + // Introduce altitude handling here?? follower_->set_reference( vortex::utils::ros_conversions::ros_pose_to_pose(msg->pose)); }); @@ -93,6 +100,125 @@ void ReferenceFilterNode::set_subscribers_and_publisher() { current_twist_ = vortex::utils::ros_conversions::ros_twist_to_twist( msg->twist.twist); }); + + const std::string waypoint_goal_topic = this->declare_parameter( + "topics.guidance.waypoint_mode", "guidance/waypoint_mode"); + waypoint_goal_pub_ = this->create_publisher( + waypoint_goal_topic, + vortex::utils::qos_profiles::reliable_profile(1)); + + if (altitude_control_enabled_) { + this->declare_parameter("topics.dvl_altitude"); + this->declare_parameter("altitude_lp_alpha", 0.9); + + std::string dvl_altitude_topic = + this->get_parameter("topics.dvl_altitude").as_string(); + altitude_lp_alpha_ = + this->get_parameter("altitude_lp_alpha").as_double(); + + altitude_sub_ = + this->create_subscription( + dvl_altitude_topic, qos_sensor_data, + [this](const vortex_msgs::msg::DVLAltitude::SharedPtr msg) { + std::lock_guard lock(sensor_mutex_); + if (msg->altitude <= 0.0) { + return; // Ignore invalid altitude readings + } + if (!altitude_valid_) { + current_altitude_ = msg->altitude; + altitude_valid_ = true; + } else { + current_altitude_ = + altitude_lp_alpha_ * current_altitude_ + + (1.0 - altitude_lp_alpha_) * msg->altitude; + } + }); + + spdlog::info("Altitude control enabled, subscribing to '{}'", + dvl_altitude_topic); + } +} + +void ReferenceFilterNode::setup_reset_subscription() { + reset_sub_ = this->create_subscription( + "mission/wipe", vortex::utils::qos_profiles::reliable_profile(1), + [this](std_msgs::msg::Empty::ConstSharedPtr msg) { + on_system_reset(msg); + }); +} + +void ReferenceFilterNode::setup_debug_publisher() { + const std::string mode_str = + this->declare_parameter("debug.goal_publish_mode", "none"); + + if (mode_str == "timer") { + debug_mode_ = DebugPublishMode::timer; + } else if (mode_str == "on_new_goal") { + debug_mode_ = DebugPublishMode::on_new_goal; + } else { + debug_mode_ = DebugPublishMode::none; + return; + } + + const std::string topic = this->declare_parameter( + "topics.guidance.dp_quat_target_pose", "guidance/dp_quat_target_pose"); + + debug_goal_pub_ = this->create_publisher( + topic, rclcpp::QoS(rclcpp::KeepLast(10)).best_effort()); + + spdlog::info("Reference goal debug publisher active (mode: {}, topic: {})", + mode_str, topic); +} + +void ReferenceFilterNode::publish_debug_goal() { + if (!debug_goal_pub_ || !executing_.load()) { + return; + } + const Pose goal = follower_->waypoint_goal(); + geometry_msgs::msg::PoseStamped msg; + msg.header.stamp = this->get_clock()->now(); + msg.header.frame_id = "odom"; + msg.pose.position.x = goal.x; + msg.pose.position.y = goal.y; + msg.pose.position.z = goal.z; + msg.pose.orientation.w = goal.qw; + msg.pose.orientation.x = goal.qx; + msg.pose.orientation.y = goal.qy; + msg.pose.orientation.z = goal.qz; + debug_goal_pub_->publish(msg); +} + +void ReferenceFilterNode::publish_waypoint_goal( + const vortex::utils::types::Waypoint& wp, double convergence_threshold) { + const Eigen::Vector3d euler = vortex::utils::math::quat_to_euler( + wp.pose.ori_quaternion()); + + vortex_msgs::msg::WaypointDebug msg; + msg.pose.header.stamp = this->get_clock()->now(); + msg.pose.header.frame_id = "odom"; + msg.pose.x = wp.pose.x; + msg.pose.y = wp.pose.y; + msg.pose.z = wp.pose.z; + msg.pose.roll = euler(0); + msg.pose.pitch = euler(1); + msg.pose.yaw = euler(2); + msg.mode = vortex::utils::waypoints::waypoint_mode_to_string(wp.mode); + msg.convergence_threshold = convergence_threshold; + msg.keep_altitude = wp.keep_altitude; + msg.desired_altitude = wp.desired_altitude; + msg.require_altitude_convergence = wp.require_altitude_convergence; + waypoint_goal_pub_->publish(msg); +} + +void ReferenceFilterNode::on_system_reset( + std_msgs::msg::Empty::ConstSharedPtr) { + std::lock_guard lock(execute_mutex_); + preempted_ = true; + if (execute_thread_.joinable()) { + execute_thread_.join(); + } + preempted_ = false; + spdlog::info("ReferenceFilter: reset complete"); } void ReferenceFilterNode::set_action_server() { @@ -146,41 +272,89 @@ void ReferenceFilterNode::handle_accepted( rclcpp_action::ServerGoalHandle> goal_handle) { std::lock_guard lock(execute_mutex_); + const bool retarget = executing_.load(); preempted_ = true; if (execute_thread_.joinable()) { execute_thread_.join(); } preempted_ = false; - execute_thread_ = - std::thread([this, goal_handle]() { execute(goal_handle); }); + execute_thread_ = std::thread( + [this, goal_handle, retarget]() { execute(goal_handle, retarget); }); } void ReferenceFilterNode::execute( - const std::shared_ptr< - rclcpp_action::ServerGoalHandle> - goal_handle) { - spdlog::info("Executing goal"); - - double convergence_threshold = - goal_handle->get_goal()->convergence_threshold; + const std::shared_ptr> goal_handle, + bool retarget) { + executing_ = true; - if (convergence_threshold <= 0.0) { - convergence_threshold = 0.1; + double threshold = goal_handle->get_goal()->convergence_threshold; + if (threshold <= 0.0) { + threshold = 0.1; spdlog::warn( - "ReferenceFilter: Invalid convergence_threshold received (<= 0). " - "Using default 0.1"); + "ReferenceFilter: invalid convergence_threshold (<= 0), using 0.1"); } - const auto wp = vortex::utils::waypoints::waypoint_from_ros( + auto wp = vortex::utils::waypoints::waypoint_from_ros( goal_handle->get_goal()->waypoint); - const auto [pose, twist] = [this] { - std::lock_guard lock(sensor_mutex_); - return std::pair{current_pose_, current_twist_}; - }(); + publish_waypoint_goal(wp, threshold); - follower_->start(pose, twist, wp, convergence_threshold); + if (wp.keep_altitude && altitude_control_enabled_ && + wp.desired_altitude <= 0.0) { + executing_ = false; + auto result = + std::make_shared(); + result->success = false; + goal_handle->abort(result); + spdlog::error( + "ReferenceFilter: desired_altitude must be > 0, got {:.3f}", + wp.desired_altitude); + return; + } + + if (wp.keep_altitude && altitude_control_enabled_) { + const auto [pose, current_alt, alt_valid] = [this] { + std::lock_guard lock(sensor_mutex_); + return std::tuple{current_pose_, current_altitude_, + altitude_valid_}; + }(); + if (!alt_valid) { + spdlog::warn( + "ReferenceFilter: keep_altitude requested but no DVL altitude " + "received yet; proceeding with waypoint z as-is"); + } else { + wp.pose.z = pose.z + current_alt - wp.desired_altitude; + } + spdlog::info( + "Altitude-hold mode: desired_altitude={:.2f} m, " + "initial_altitude={:.2f}, initial z_goal={:.3f}", + wp.desired_altitude, current_altitude_, wp.pose.z); + } else if (wp.keep_altitude && !altitude_control_enabled_) { + spdlog::warn( + "ReferenceFilter: keep_altitude requested but altitude control is " + "not enabled; proceeding with waypoint z as-is"); + } + + if (retarget) { + follower_->retarget(wp, threshold); + spdlog::info("Executing goal (filter state preserved)"); + } else { + const auto [pose, twist] = [this] { + std::lock_guard lock(sensor_mutex_); + return std::pair{current_pose_, current_twist_}; + }(); + follower_->start(pose, twist, wp, threshold); + spdlog::info("Executing goal (cold start)"); + } + + if (debug_mode_ == DebugPublishMode::on_new_goal) { + publish_debug_goal(); + } + + const bool keep_altitude = wp.keep_altitude && altitude_control_enabled_; + const double desired_altitude = wp.desired_altitude; auto result = std::make_shared(); @@ -189,6 +363,8 @@ void ReferenceFilterNode::execute( while (rclcpp::ok()) { if (preempted_.load()) { + executing_ = false; + waypoint_goal_pub_->publish(vortex_msgs::msg::WaypointDebug{}); result->success = false; goal_handle->abort(result); spdlog::info("Goal preempted by newer goal"); @@ -196,6 +372,8 @@ void ReferenceFilterNode::execute( } if (goal_handle->is_canceling()) { + executing_ = false; + waypoint_goal_pub_->publish(vortex_msgs::msg::WaypointDebug{}); result->success = false; goal_handle->canceled(result); spdlog::info("Goal canceled"); @@ -204,49 +382,62 @@ void ReferenceFilterNode::execute( follower_->step(); + if (keep_altitude) { + const auto [current_z, current_alt] = [this] { + std::lock_guard lock(sensor_mutex_); + return std::pair{current_pose_.z, current_altitude_}; + }(); + follower_->update_z_goal(current_z + current_alt - + desired_altitude); + } + + reference_pub_->publish( + fill_reference_msg(follower_->pose(), follower_->velocity())); + if (publish_rpy_debug_) { + rpy_debug_pub_->publish(fill_reference_rpy_msg( + follower_->pose(), follower_->velocity())); + } + if (debug_mode_ == DebugPublishMode::timer) { + publish_debug_goal(); + } + const auto current_pose = [this] { std::lock_guard lock(sensor_mutex_); return current_pose_; }(); - bool target_reached = follower_->within_convergance(current_pose); + const bool converged = + (keep_altitude && !wp.require_altitude_convergence) + ? follower_->within_convergance_ignore_z(current_pose) + : follower_->within_convergance(current_pose); - if (target_reached) { + if (converged) { follower_->snap_state_to_reference(); - auto final_reference_msg = - fill_reference_msg(follower_->pose(), follower_->velocity()); - - reference_pub_->publish(final_reference_msg); - if (rpy_debug_pub_) { + reference_pub_->publish( + fill_reference_msg(follower_->pose(), follower_->velocity())); + if (publish_rpy_debug_) { rpy_debug_pub_->publish(fill_reference_rpy_msg( follower_->pose(), follower_->velocity())); } + executing_ = false; + waypoint_goal_pub_->publish(vortex_msgs::msg::WaypointDebug{}); result->success = true; goal_handle->succeed(result); spdlog::info("Goal reached"); return; } - 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(); + executing_ = false; result->success = false; - try { goal_handle->abort(result); } catch (...) { - // Ignore exceptions during shutdown } } } 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 e4515343d..d9704888e 100755 --- a/mission/joystick_interface_auv/launch/joystick_interface_auv.launch.py +++ b/mission/joystick_interface_auv/launch/joystick_interface_auv.launch.py @@ -63,7 +63,7 @@ def generate_launch_description() -> LaunchDescription: + [ DeclareLaunchArgument( "orientation_mode", - default_value="euler", + default_value="quat", description="Reference orientation representation: 'euler' (ReferenceFilter) or 'quat' (ReferenceFilterQuat)", choices=["euler", "quat"], ), diff --git a/mission/landmark_server/CMakeLists.txt b/mission/landmark_server/CMakeLists.txt index b880fac0c..b18a31ee0 100644 --- a/mission/landmark_server/CMakeLists.txt +++ b/mission/landmark_server/CMakeLists.txt @@ -11,6 +11,7 @@ endif() find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) find_package(rclcpp_components REQUIRED) find_package(spdlog REQUIRED) find_package(fmt REQUIRED) @@ -36,6 +37,7 @@ add_library(${LIB_NAME} SHARED ament_target_dependencies(${LIB_NAME} PUBLIC rclcpp rclcpp_components + std_msgs vortex_msgs vortex_utils vortex_utils_ros diff --git a/mission/landmark_server/include/landmark_server/landmark_server_ros.hpp b/mission/landmark_server/include/landmark_server/landmark_server_ros.hpp index bfb4efd85..810a2fd85 100644 --- a/mission/landmark_server/include/landmark_server/landmark_server_ros.hpp +++ b/mission/landmark_server/include/landmark_server/landmark_server_ros.hpp @@ -16,6 +16,7 @@ #include #include #include +#include #include #include #include @@ -134,6 +135,10 @@ class LandmarkServerNode : public rclcpp::Node { void create_track_manager(); + void setup_reset_subscription(); + + void on_system_reset(std_msgs::msg::Empty::ConstSharedPtr msg); + void setup_debug_publishers(); void publish_debug_tracks(); @@ -160,6 +165,9 @@ class LandmarkServerNode : public rclcpp::Node { reference_filter_client_; std::unique_ptr track_manager_; + vortex::filtering::TrackManagerConfig track_manager_config_; + + rclcpp::Subscription::SharedPtr reset_sub_; std::vector measurements_; diff --git a/mission/landmark_server/package.xml b/mission/landmark_server/package.xml index 33f25b857..0eacc2fff 100644 --- a/mission/landmark_server/package.xml +++ b/mission/landmark_server/package.xml @@ -19,6 +19,7 @@ vortex_utils_ros auv_setup tf2_geometry_msgs + std_msgs launch_testing launch_testing_ros diff --git a/mission/landmark_server/src/landmark_server_ros.cpp b/mission/landmark_server/src/landmark_server_ros.cpp index d36c102ab..2d21b1e3f 100644 --- a/mission/landmark_server/src/landmark_server_ros.cpp +++ b/mission/landmark_server/src/landmark_server_ros.cpp @@ -37,6 +37,7 @@ void LandmarkServerNode::setup_ros_communicators() { create_convergence_action_server(); create_reference_action_client(); create_timer(); + setup_reset_subscription(); debug_ = this->declare_parameter("debug.enable"); if (debug_) { @@ -245,10 +246,54 @@ void LandmarkServerNode::create_track_manager() { this->declare_parameter( "track_config.default.clutter_intensity"); + track_manager_config_ = config; track_manager_ = std::make_unique(config); } +void LandmarkServerNode::setup_reset_subscription() { + rclcpp::SubscriptionOptions sub_opts; + sub_opts.callback_group = timer_cb_group_; + reset_sub_ = this->create_subscription( + "mission/wipe", vortex::utils::qos_profiles::reliable_profile(1), + [this](std_msgs::msg::Empty::ConstSharedPtr msg) { + on_system_reset(msg); + }, + sub_opts); +} + +void LandmarkServerNode::on_system_reset(std_msgs::msg::Empty::ConstSharedPtr) { + if (active_landmark_polling_goal_ && + active_landmark_polling_goal_->is_active()) { + auto result = + std::make_shared(); + active_landmark_polling_goal_->abort(result); + } + active_landmark_polling_goal_ = nullptr; + + if (active_landmark_convergence_goal_ && + active_landmark_convergence_goal_->is_active()) { + auto result = + std::make_shared(); + active_landmark_convergence_goal_->abort(result); + } + active_landmark_convergence_goal_ = nullptr; + + cancel_reference_filter_goal(); + + convergence_active_ = false; + convergence_session_id_++; + convergence_track_lost_ = false; + convergence_dead_reckoning_handoff_ = false; + convergence_last_known_track_.reset(); + rf_state_ = RFState::IDLE; + + track_manager_ = std::make_unique( + track_manager_config_); + + spdlog::info("LandmarkServer: reset complete"); +} + void LandmarkServerNode::timer_callback() { std::vector measurements_snapshot; { diff --git a/mission/operation_mode_manager/include/operation_mode_manager.hpp b/mission/operation_mode_manager/include/operation_mode_manager.hpp index 224f565f6..23f8bfb31 100644 --- a/mission/operation_mode_manager/include/operation_mode_manager.hpp +++ b/mission/operation_mode_manager/include/operation_mode_manager.hpp @@ -6,6 +6,7 @@ #include #include #include +#include #include #include @@ -47,6 +48,8 @@ class OperationModeManager : public rclcpp::Node { rclcpp::Publisher::SharedPtr wrench_pub_; rclcpp::Publisher::SharedPtr killswitch_pub_; rclcpp::Publisher::SharedPtr mode_pub_; + rclcpp::Publisher::SharedPtr killswitch_string_pub_; + rclcpp::Publisher::SharedPtr mode_string_pub_; rclcpp::Service::SharedPtr set_operation_mode_service_; diff --git a/mission/operation_mode_manager/src/operation_mode_manager.cpp b/mission/operation_mode_manager/src/operation_mode_manager.cpp index 9d3a97d98..5913e7e64 100644 --- a/mission/operation_mode_manager/src/operation_mode_manager.cpp +++ b/mission/operation_mode_manager/src/operation_mode_manager.cpp @@ -41,6 +41,8 @@ void OperationModeManager::setup_publishers() { this->get_parameter("topics.killswitch").as_string(); const auto operation_mode_topic = this->get_parameter("topics.operation_mode").as_string(); + const auto killswitch_string_topic = killswitch_topic + "_string"; + const auto operation_mode_string_topic = operation_mode_topic + "_string"; wrench_pub_ = this->create_publisher( wrench_input_topic, @@ -51,6 +53,14 @@ void OperationModeManager::setup_publishers() { mode_pub_ = this->create_publisher( operation_mode_topic, vortex::utils::qos_profiles::reliable_profile(1)); + + killswitch_string_pub_ = this->create_publisher( + killswitch_string_topic, + vortex::utils::qos_profiles::reliable_profile(1)); + + mode_string_pub_ = this->create_publisher( + operation_mode_string_topic, + vortex::utils::qos_profiles::reliable_profile(1)); } void OperationModeManager::setup_services() { @@ -152,6 +162,14 @@ void OperationModeManager::publish_mode() { std_msgs::msg::Bool killswitch_msg; killswitch_msg.data = killswitch_; killswitch_pub_->publish(killswitch_msg); + + std_msgs::msg::String killswitch_string_msg; + killswitch_string_msg.data = "killswitch: " + std::string(killswitch_ ? "true" : "false"); + killswitch_string_pub_->publish(killswitch_string_msg); + + std_msgs::msg::String mode_string_msg; + mode_string_msg.data = "mode: " + vortex::utils::types::mode_to_string(mode_); + mode_string_pub_->publish(mode_string_msg); } void OperationModeManager::publish_empty_wrench() { diff --git a/mission/waypoint_manager/CMakeLists.txt b/mission/waypoint_manager/CMakeLists.txt index 451de3132..939b70721 100644 --- a/mission/waypoint_manager/CMakeLists.txt +++ b/mission/waypoint_manager/CMakeLists.txt @@ -11,6 +11,7 @@ endif() find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) find_package(rclcpp_action REQUIRED) find_package(rclcpp_components REQUIRED) find_package(vortex_msgs REQUIRED) @@ -30,6 +31,7 @@ ament_target_dependencies(${LIB_NAME} PUBLIC rclcpp rclcpp_components rclcpp_action + std_msgs vortex_msgs vortex_utils_ros tf2_geometry_msgs diff --git a/mission/waypoint_manager/include/waypoint_manager/waypoint_manager_ros.hpp b/mission/waypoint_manager/include/waypoint_manager/waypoint_manager_ros.hpp index 2f0085c05..268fe0a1c 100644 --- a/mission/waypoint_manager/include/waypoint_manager/waypoint_manager_ros.hpp +++ b/mission/waypoint_manager/include/waypoint_manager/waypoint_manager_ros.hpp @@ -5,6 +5,7 @@ #include #include +#include #include #include #include @@ -40,6 +41,12 @@ class WaypointManagerNode : public rclcpp::Node { // @brief Create the service servers for SendWaypoints. void set_waypoint_service_server(); + // @brief Subscribe to the system-wide reset topic. + void setup_reset_subscription(); + + // @brief Abort any active goal and clear state on reset. + void on_system_reset(std_msgs::msg::Empty::ConstSharedPtr msg); + // @brief Create the debug waypoint publisher and optional timer. void setup_debug_publisher(); @@ -106,6 +113,8 @@ class WaypointManagerNode : public rclcpp::Node { rclcpp::Service::SharedPtr waypoint_service_server_; + rclcpp::Subscription::SharedPtr reset_sub_; + rclcpp::Publisher::SharedPtr debug_waypoint_pub_; rclcpp::TimerBase::SharedPtr debug_timer_; diff --git a/mission/waypoint_manager/package.xml b/mission/waypoint_manager/package.xml index be6d9148b..361169e74 100644 --- a/mission/waypoint_manager/package.xml +++ b/mission/waypoint_manager/package.xml @@ -10,6 +10,7 @@ ament_cmake rclcpp + std_msgs rclcpp_action geometry_msgs vortex_msgs diff --git a/mission/waypoint_manager/src/waypoint_manager_ros.cpp b/mission/waypoint_manager/src/waypoint_manager_ros.cpp index a6faf50d9..c85029290 100644 --- a/mission/waypoint_manager/src/waypoint_manager_ros.cpp +++ b/mission/waypoint_manager/src/waypoint_manager_ros.cpp @@ -2,6 +2,7 @@ #include #include #include +#include namespace vortex::mission { @@ -10,6 +11,7 @@ WaypointManagerNode::WaypointManagerNode(const rclcpp::NodeOptions& options) set_reference_action_client(); set_waypoint_action_server(); set_waypoint_service_server(); + setup_reset_subscription(); setup_debug_publisher(); spdlog::info("WaypointManagerNode started"); @@ -104,6 +106,24 @@ void WaypointManagerNode::publish_current_waypoint() { debug_waypoint_pub_->publish(waypoints_[current_index_]); } +void WaypointManagerNode::setup_reset_subscription() { + reset_sub_ = this->create_subscription( + "mission/wipe", vortex::utils::qos_profiles::reliable_profile(1), + [this](std_msgs::msg::Empty::ConstSharedPtr msg) { + on_system_reset(msg); + }); +} + +void WaypointManagerNode::on_system_reset( + std_msgs::msg::Empty::ConstSharedPtr) { + if (active_action_goal_ && active_action_goal_->is_active()) { + auto res = construct_result(false); + active_action_goal_->abort(res); + } + cleanup_mission_state(); + spdlog::info("WaypointManager: reset complete"); +} + void WaypointManagerNode::set_waypoint_service_server() { std::string service_name = this->declare_parameter("services.waypoint_addition"); diff --git a/motion/thruster_interface_auv/CMakeLists.txt b/motion/thruster_interface_auv/CMakeLists.txt index ef8cad9dc..390d40cb7 100644 --- a/motion/thruster_interface_auv/CMakeLists.txt +++ b/motion/thruster_interface_auv/CMakeLists.txt @@ -20,7 +20,8 @@ set(LIB_NAME "${PROJECT_NAME}_component") add_library(${LIB_NAME} SHARED src/thruster_interface_auv_ros.cpp - src/thruster_interface_auv_driver.cpp) + src/thruster_interface_auv_driver.cpp + src/can_interface.cpp) ament_target_dependencies(${LIB_NAME} PUBLIC rclcpp 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 099bddda3..860ec0722 100644 --- a/motion/thruster_interface_auv/config/thruster_interface_auv_config.yaml +++ b/motion/thruster_interface_auv/config/thruster_interface_auv_config.yaml @@ -19,10 +19,9 @@ # 20V: # LEFT: [2.55792, 27.51178, 147.67001, 1472.23069] # RIGHT: [1.33421, -18.75129, 121.29079, 1528.99886] - - uart: - device: "/dev/serial0" - baud_rate: 115200 + + can: + interface: "can0" debug: flag: True diff --git a/motion/thruster_interface_auv/include/can_interface.hpp b/motion/thruster_interface_auv/include/can_interface.hpp new file mode 100644 index 000000000..03e71ef03 --- /dev/null +++ b/motion/thruster_interface_auv/include/can_interface.hpp @@ -0,0 +1,59 @@ +#ifndef CAN_INTERFACE_H +#define CAN_INTERFACE_H + +#include +#include +#include +#include +#include +#include + +enum class can_status { + OK = 0, + ERR_ALREADY_INITIALIZED = -1, + ERR_SOCKET = -2, + ERR_CANFD_SUPPORT = -3, + ERR_LOOPBACK = -4, + ERR_INTERFACE_INDEX = -5, + ERR_BIND = -6, + ERR_NOT_INITIALIZED = -7, + ERR_DATA_LENGTH = -8, + ERR_SEND = -9, + ERR_RECEIVE = -10, + ERR_SETTING_TIMEOUT = -11, + ERR_SETTING_FILTER = -12, + ERR_CLEARING_FILTERS = -13 +}; + +class can_interface { + private: + int socket_fd_; + bool is_initialized_; + std::string interface_name_; + + std::thread receive_thread_; + std::atomic receiving_ = false; + + public: + can_interface(); + ~can_interface(); + + can_status init(const std::string& ifname = "can0"); + can_status send(uint32_t can_id, + const uint8_t* data, + uint8_t len, + bool use_brs = true); + can_status receive(struct canfd_frame& frame); + can_status receive(struct canfd_frame& frame, int timeout_ms); + can_status start_async_receive( + std::function callback); + void stop_async_receive(); + can_status set_filter(uint32_t can_id, uint32_t can_mask = 0x7FF); + can_status set_filters(const struct can_filter* filters, + size_t num_filters); + can_status clear_filters(); + bool is_initialized() const; + std::string get_interface_name() const; +}; + +#endif // CAN_INTERFACE_H 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 3c9d1347c..9c770cdf2 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,18 +1,17 @@ #ifndef THRUSTER_INTERFACE_AUV__THRUSTER_INTERFACE_AUV_DRIVER_HPP_ #define THRUSTER_INTERFACE_AUV__THRUSTER_INTERFACE_AUV_DRIVER_HPP_ +#include "can_interface.hpp" + #include -// Need to include utility before asio -// clang-format off -#include -#include -// clang-format on #include +#include #include #include -#include #include +#include + /** * @brief struct to hold the parameters for a single thruster */ @@ -42,7 +41,7 @@ using CurrentMeasurementsCallback = /** * @brief class instantiated by ThrusterInterfaceAUVNode to control the * thrusters, takes the thruster forces and converts them to PWM signals to be - * sent via UART to the ESC controller. + * sent via CAN FD to the ESC controller. * * @details Based on the datasheets found in /resources, approximate the map * with a piecewise (>0 and <0) third order polynomial. @@ -53,8 +52,8 @@ using CurrentMeasurementsCallback = * 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. + * @note Over CAN FD, the PWM values are packed into one CAN FD frame payload, + * where the payload is 8 uint16_t. */ class ThrusterInterfaceAUVDriver { public: @@ -64,10 +63,8 @@ class ThrusterInterfaceAUVDriver { * @brief called from ThrusterInterfaceAUVNode .cpp when instantiating the * object, initializes all the params. * - * @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 can_interface_name CAN interface used to communicate + * (for example can0 or vcan0) * @param thruster_parameters describe mapping, direction, min and max pwm * value for each thruster * @param right_coeffs RIGHT(>0) third order polynomial @@ -75,22 +72,21 @@ class ThrusterInterfaceAUVDriver { * @param left_coeffs LEFT(<0) third order polynomial coefficients */ ThrusterInterfaceAUVDriver( - const std::string& serial_device, - unsigned int baud_rate, + const std::string& can_interface_name, const std::vector& thruster_parameters, const std::vector& right_coeffs, const std::vector& left_coeffs); /** - * @brief initializes UART + * @brief initializes CAN FD * @return 0 on success, negative number on failure */ - int init_uart(); + int init_can(); /** * @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 over UART + * send them over CAN FD * * @param thruster_forces_array vector of forces for each thruster * @@ -113,6 +109,8 @@ class ThrusterInterfaceAUVDriver { void set_killswitch_event_callback(KillswitchEventCallback callback); void set_current_measurements_callback( CurrentMeasurementsCallback callback); + int disable_thrusters(); + int enable_thrusters(); private: /** @@ -148,102 +146,25 @@ class ThrusterInterfaceAUVDriver { /** * @brief only takes the pwm values computed and sends them - * over UART as a framed packet + * over CAN FD * * @param thruster_pwm_array vector of pwm values to send * @return 0 on success, -1 on failure */ int send_data_to_escs(const std::vector& thruster_pwm_array); + void handle_can_frame(const struct canfd_frame& frame, can_status status); - /** - * @brief create UART packet with format: - * [magic][id][length][payload][checksum] - * - * @param id packet ID - * @param thruster_pwm_array vector of 8 pwm values to send as payload - * - * @return std::vector serialized packet bytes - */ - std::vector create_packet( - std::uint8_t id, - const std::vector& thruster_pwm_array) const; - - /** - * @brief convert Newtons to Kg - * - * @param force Newtons - * - * @return double Kg - */ static constexpr double to_kg(double force) { return force / 9.80665; } - /** - * @brief start the asynchronous UART receive loop - */ - void start_receive(); - - /** - * @brief issue one asynchronous read on the UART port - */ - void do_receive(); - - /** - * @brief process the accumulated receive buffer and extract valid frames - */ - void process_receive_buffer(); - - /** - * @brief handle one decoded UART frame - * - * @param frame_bytes complete frame bytes including header and checksum - */ - void handle_received_frame(const std::vector& frame_bytes); - - /** - * @brief compute checksum for framed UART packets - * - * @param msg_id message id - * @param length payload length - * @param payload pointer to payload bytes - * - * @return checksum byte - */ - static std::uint8_t compute_checksum(std::uint8_t msg_id, - std::uint8_t length, - const std::uint8_t* payload); - private: - static constexpr std::uint8_t UART_START_BYTE = 0xAA; - static constexpr std::size_t MAX_PAYLOAD_SIZE = 64; - static constexpr std::size_t READ_CHUNK_SIZE = 256; - - // Outgoing - static constexpr std::uint8_t MSG_TURN_THRUSTERS_OFF = 0x01U; - static constexpr std::uint8_t MSG_TURN_LIGHTS_OFF = 0x02U; - static constexpr std::uint8_t MSG_RESET = 0x03; - static constexpr std::uint8_t MSG_SET_THRUSTER_PWM = 0x04; - static constexpr std::uint8_t MSG_SET_LIGHT_PWM = 0x05; - // Incoming - static constexpr std::uint8_t MSG_FLT_EVENT = 0x10; - static constexpr std::uint8_t MSG_PGOOD_EVENT = 0x11; - static constexpr std::uint8_t MSG_KILLSWITCH_EVENT = 0x12; - static constexpr std::uint8_t MSG_CURRENT_MEASUREMENTS = 0x13; - - std::string serial_device_; - unsigned int baud_rate_; - - asio::io_context io_; - asio::serial_port serial_{io_}; - std::thread io_thread_; + std::string can_interface_name_; + can_interface can_; std::vector thruster_parameters_; std::vector right_coeffs_; std::vector left_coeffs_; std::uint16_t idle_pwm_value_{1500}; - std::array read_buf_{}; - std::vector receive_buffer_; - FaultEventCallback fault_event_callback_; PGoodEventCallback pgood_event_callback_; KillswitchEventCallback killswitch_event_callback_; 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 b35098722..5a96d6e76 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 @@ -73,8 +73,7 @@ class ThrusterInterfaceAUVNode : public rclcpp::Node { void pwm_callback(); private: - std::string serial_device_; - unsigned int baud_rate_; + std::string can_interface_; std::string subscriber_topic_name_; std::string publisher_topic_name_; @@ -88,7 +87,7 @@ class ThrusterInterfaceAUVNode : public rclcpp::Node { bool debug_flag_{false}; std::unique_ptr - thruster_driver_; ///<-- UART/USART thruster driver + thruster_driver_; ///<-- CAN FD thruster driver rclcpp::Subscription::SharedPtr thruster_forces_subscriber_; ///<-- thruster forces subscriber @@ -112,6 +111,7 @@ class ThrusterInterfaceAUVNode : public rclcpp::Node { rclcpp::Time last_msg_time_; rclcpp::Duration watchdog_timeout_ = rclcpp::Duration::from_seconds(1.0); bool watchdog_triggered_ = false; + /** * @brief Manages parameter events for the node. * diff --git a/motion/thruster_interface_auv/src/can_interface.cpp b/motion/thruster_interface_auv/src/can_interface.cpp new file mode 100644 index 000000000..5c8ad8a03 --- /dev/null +++ b/motion/thruster_interface_auv/src/can_interface.cpp @@ -0,0 +1,199 @@ +#include "can_interface.hpp" +#include +#include +#include +#include +#include +#include +#include + +can_interface::can_interface() : socket_fd_(-1), is_initialized_(false) {} + +can_interface::~can_interface() { + if (is_initialized_) { + close(socket_fd_); + } +} + +can_status can_interface::init(const std::string& ifname) { + interface_name_ = ifname; + + socket_fd_ = socket(PF_CAN, SOCK_RAW, CAN_RAW); + if (socket_fd_ < 0) { + return can_status::ERR_SOCKET; + } + + int enable_canfd = 1; + if (setsockopt(socket_fd_, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &enable_canfd, + sizeof(enable_canfd)) < 0) { + close(socket_fd_); + return can_status::ERR_CANFD_SUPPORT; + } + + struct ifreq ifr; + std::strcpy(ifr.ifr_name, interface_name_.c_str()); + if (ioctl(socket_fd_, SIOCGIFINDEX, &ifr) < 0) { + close(socket_fd_); + return can_status::ERR_INTERFACE_INDEX; + } + + struct sockaddr_can addr; + addr.can_family = AF_CAN; + addr.can_ifindex = ifr.ifr_ifindex; + + if (bind(socket_fd_, (struct sockaddr*)&addr, sizeof(addr)) < 0) { + close(socket_fd_); + return can_status::ERR_BIND; + } + + is_initialized_ = true; + + return can_status::OK; +} + +can_status can_interface::send(uint32_t can_id, + const uint8_t* data, + uint8_t len, + bool use_brs) { + if (!is_initialized_) { + return can_status::ERR_NOT_INITIALIZED; + } + + if (len > 64) { + return can_status::ERR_DATA_LENGTH; + } + + struct canfd_frame frame; + memset(&frame, 0, sizeof(frame)); + + frame.can_id = can_id; + frame.len = len; + frame.flags = use_brs ? CANFD_BRS : 0; + + if (data != nullptr && len > 0) { + memcpy(frame.data, data, len); + } + + if (write(socket_fd_, &frame, sizeof(frame)) != sizeof(frame)) { + return can_status::ERR_SEND; + } + + return can_status::OK; +} + +can_status can_interface::receive(struct canfd_frame& frame) { + if (!is_initialized_) { + return can_status::ERR_NOT_INITIALIZED; + } + + ssize_t nbytes = read(socket_fd_, &frame, sizeof(frame)); + + if (nbytes != CANFD_MTU) { + return can_status::ERR_RECEIVE; + } + + return can_status::OK; +} + +can_status can_interface::receive(struct canfd_frame& frame, int timeout_ms) { + if (!is_initialized_) { + return can_status::ERR_NOT_INITIALIZED; + } + + struct timeval tv; + tv.tv_sec = timeout_ms / 1000; + tv.tv_usec = (timeout_ms % 1000) * 1000; + + if (setsockopt(socket_fd_, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv)) < 0) { + return can_status::ERR_SETTING_TIMEOUT; + } + + ssize_t nbytes = read(socket_fd_, &frame, sizeof(frame)); + + if (nbytes != CANFD_MTU) { + return can_status::ERR_RECEIVE; + } + + return can_status::OK; +} + +can_status can_interface::start_async_receive( + std::function callback) { + if (!is_initialized_) { + return can_status::ERR_NOT_INITIALIZED; + } + + receiving_ = true; + + receive_thread_ = std::thread([this, callback]() { + while (receiving_) { + struct canfd_frame frame; + can_status status = receive(frame, 1000); + callback(frame, status); + } + }); + return can_status::OK; +} + +void can_interface::stop_async_receive() { + receiving_ = false; + if (receive_thread_.joinable()) { + receive_thread_.join(); + } +} + +can_status can_interface::set_filter(uint32_t can_id, uint32_t can_mask) { + if (!is_initialized_) { + return can_status::ERR_NOT_INITIALIZED; + } + + struct can_filter filter; + filter.can_id = can_id; + filter.can_mask = can_mask; + + if (setsockopt(socket_fd_, SOL_CAN_RAW, CAN_RAW_FILTER, &filter, + sizeof(filter)) < 0) { + return can_status::ERR_SETTING_FILTER; + } + + return can_status::OK; +} + +can_status can_interface::set_filters(const struct can_filter* filters, + size_t num_filters) { + if (!is_initialized_) { + return can_status::ERR_NOT_INITIALIZED; + } + + if (setsockopt(socket_fd_, SOL_CAN_RAW, CAN_RAW_FILTER, filters, + num_filters * sizeof(struct can_filter)) < 0) { + return can_status::ERR_SETTING_FILTER; + } + + return can_status::OK; +} + +can_status can_interface::clear_filters() { + if (!is_initialized_) { + return can_status::ERR_NOT_INITIALIZED; + } + + struct can_filter filter; + filter.can_id = 0x0; + filter.can_mask = 0x0; + + if (setsockopt(socket_fd_, SOL_CAN_RAW, CAN_RAW_FILTER, &filter, + sizeof(filter)) < 0) { + return can_status::ERR_CLEARING_FILTERS; + } + + return can_status::OK; +} + +bool can_interface::is_initialized() const { + return is_initialized_; +} + +std::string can_interface::get_interface_name() const { + return interface_name_; +} 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 1ccd73041..6f087f61e 100644 --- a/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp +++ b/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp @@ -1,23 +1,36 @@ #include "thruster_interface_auv/thruster_interface_auv_driver.hpp" #include -#include +#include +#include #include #include #include -#include -#include -#include +#include #include +#include + + + +static constexpr std::uint32_t CAN_ID_DISABLE_THRUSTERS = 0x369U; +static constexpr std::uint32_t CAN_ID_ENABLE_THRUSTERS = 0x36AU; +static constexpr std::uint32_t CAN_ID_RESET = 0x3BAU; +static constexpr std::uint32_t CAN_ID_SET_THRUSTERS_PWM = 0x36CU; +static constexpr std::uint32_t CAN_ID_SET_LIGHT_PWM = 0x36DU; + +static constexpr std::uint32_t CAN_ID_FLT_EVENT = 0x36EU; +static constexpr std::uint32_t CAN_ID_PGOOD_EVENT = 0x36FU; +static constexpr std::uint32_t CAN_ID_KILLSWITCH_EVENT = 0x370U; +static constexpr std::uint32_t CAN_ID_CURRENT_MEASUREMENTS = 0x371U; + + ThrusterInterfaceAUVDriver::ThrusterInterfaceAUVDriver( - const std::string& serial_device, - unsigned int baud_rate, + const std::string& can_interface_name, const std::vector& thruster_parameters, const std::vector& right_coeffs, const std::vector& left_coeffs) - : serial_device_(serial_device), - baud_rate_(baud_rate), + : can_interface_name_(can_interface_name), thruster_parameters_(thruster_parameters), right_coeffs_(right_coeffs), left_coeffs_(left_coeffs) { @@ -26,63 +39,53 @@ ThrusterInterfaceAUVDriver::ThrusterInterfaceAUVDriver( } ThrusterInterfaceAUVDriver::~ThrusterInterfaceAUVDriver() { - std::error_code ec; - - if (serial_.is_open()) { + if (can_.is_initialized()) { send_data_to_escs(std::vector( thruster_parameters_.size(), idle_pwm_value_)); - - serial_.cancel(ec); - serial_.close(ec); - } - - io_.stop(); - - if (io_thread_.joinable()) { - io_thread_.join(); + can_.stop_async_receive(); } } -int ThrusterInterfaceAUVDriver::init_uart() { - std::error_code ec; - - serial_.open(serial_device_, ec); - if (ec) { - return -1; - } - serial_.set_option(asio::serial_port::baud_rate(baud_rate_), ec); - if (ec) { - return -1; - } - - serial_.set_option(asio::serial_port::character_size(8), ec); - if (ec) { - return -1; +int ThrusterInterfaceAUVDriver::init_can() { + can_status status = can_.init(can_interface_name_); + if (status != can_status::OK) { + return static_cast(status); } - serial_.set_option( - asio::serial_port::parity(asio::serial_port::parity::none), ec); - if (ec) { - return -1; + struct can_filter filters[] = { + { + .can_id = CAN_ID_FLT_EVENT, + .can_mask = CAN_SFF_MASK, + }, + { + .can_id = CAN_ID_PGOOD_EVENT, + .can_mask = CAN_SFF_MASK, + }, + { + .can_id = CAN_ID_KILLSWITCH_EVENT, + .can_mask = CAN_SFF_MASK, + }, + { + .can_id = CAN_ID_CURRENT_MEASUREMENTS, + .can_mask = CAN_SFF_MASK, + }, + }; + + status = can_.set_filters(filters, sizeof(filters) / sizeof(filters[0])); + if (status != can_status::OK) { + return static_cast(status); } - serial_.set_option( - asio::serial_port::stop_bits(asio::serial_port::stop_bits::one), ec); - if (ec) { - return -1; - } + status = can_.start_async_receive( + [this](const struct canfd_frame& frame, can_status rx_status) { + handle_can_frame(frame, rx_status); + }); - serial_.set_option( - asio::serial_port::flow_control(asio::serial_port::flow_control::none), - ec); - if (ec) { - return -1; + if (status != can_status::OK) { + return static_cast(status); } - start_receive(); - io_thread_ = std::thread([this]() { io_.run(); }); - return 0; } @@ -116,124 +119,34 @@ std::uint16_t ThrusterInterfaceAUVDriver::force_to_pwm(double force) { 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]); -} - -std::vector ThrusterInterfaceAUVDriver::create_packet( - std::uint8_t id, - const std::vector& thruster_pwm_array) const { - std::vector packet; - - packet.reserve(1 + 1 + 1 + - thruster_pwm_array.size() * sizeof(std::uint16_t) + 1); - - packet.push_back(UART_START_BYTE); - packet.push_back(id); - - const std::uint8_t length = static_cast( - thruster_pwm_array.size() * sizeof(std::uint16_t)); - packet.push_back(length); - - for (std::uint16_t value : thruster_pwm_array) { - packet.push_back(static_cast(value & 0xFF)); - packet.push_back(static_cast((value >> 8) & 0xFF)); - } - - std::uint8_t checksum = id ^ length; - for (std::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(MSG_SET_THRUSTER_PWM, thruster_pwm_array); - constexpr std::size_t header_size = 3; - - if (packet.size() < header_size) { - return -1; - } - - std::error_code ec; - - const auto header_bytes_written = - asio::write(serial_, asio::buffer(packet.data(), header_size), ec); - - if (ec || header_bytes_written != header_size) { - return -1; - } - - std::this_thread::sleep_for(std::chrono::milliseconds(5)); - - 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) { - return -1; - } - - return 0; -} - -int ThrusterInterfaceAUVDriver::set_camera_light(float percentage) { - if (!serial_.is_open()) { - return -1; - } - - std::vector camera_light_pwm_array(1); - camera_light_pwm_array[0] = - static_cast(1100 + 800 * percentage); - - const auto packet = - create_packet(MSG_SET_LIGHT_PWM, camera_light_pwm_array); - constexpr std::size_t header_size = 3; - - if (packet.size() < header_size) { - return -1; - } - - std::error_code ec; - - const auto header_bytes_written = - asio::write(serial_, asio::buffer(packet.data(), header_size), ec); - - if (ec || header_bytes_written != header_size) { - return -1; - } - - std::this_thread::sleep_for(std::chrono::milliseconds(5)); - - 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) { - return -1; + if (coeffs.size() < 4) { + return idle_pwm_value_; } - return 0; + return static_cast( + coeffs[0] * std::pow(force, 3) + + coeffs[1] * std::pow(force, 2) + + coeffs[2] * force + + coeffs[3]); } std::optional> ThrusterInterfaceAUVDriver::drive_thrusters( const std::vector& thruster_forces_array) { + if (thruster_forces_array.size() < thruster_parameters_.size()) { + return std::nullopt; + } + std::vector mapped_forces(thruster_parameters_.size()); for (std::size_t i = 0; i < thruster_parameters_.size(); ++i) { const auto& param = thruster_parameters_[i]; const std::size_t idx = param.mapping; + + if (idx >= thruster_forces_array.size()) { + return std::nullopt; + } + const double raw_force = thruster_forces_array[idx]; mapped_forces[i] = raw_force * param.direction; } @@ -248,185 +161,185 @@ ThrusterInterfaceAUVDriver::drive_thrusters( return thruster_pwm_array; } -void ThrusterInterfaceAUVDriver::start_receive() { - do_receive(); -} - -void ThrusterInterfaceAUVDriver::do_receive() { - serial_.async_read_some( - asio::buffer(read_buf_), - [this](const std::error_code& ec, std::size_t bytes_transferred) { - if (ec) { - return; - } - - receive_buffer_.insert(receive_buffer_.end(), read_buf_.begin(), - read_buf_.begin() + bytes_transferred); - - process_receive_buffer(); - do_receive(); - }); -} - -std::uint8_t ThrusterInterfaceAUVDriver::compute_checksum( - std::uint8_t msg_id, - std::uint8_t length, - const std::uint8_t* payload) { - std::uint8_t checksum = msg_id ^ length; - for (std::size_t i = 0; i < length; ++i) { - checksum ^= payload[i]; +int ThrusterInterfaceAUVDriver::send_data_to_escs( + const std::vector& thruster_pwm_array) { + if (!can_.is_initialized()) { + return static_cast(can_status::ERR_NOT_INITIALIZED); } - return checksum; -} - -void ThrusterInterfaceAUVDriver::process_receive_buffer() { - while (true) { - if (receive_buffer_.size() < 4) { - return; - } - - auto start_it = std::find(receive_buffer_.begin(), - receive_buffer_.end(), UART_START_BYTE); - - if (start_it == receive_buffer_.end()) { - receive_buffer_.clear(); - return; - } + if (thruster_pwm_array.size() != 8) { + return -1; + } - if (start_it != receive_buffer_.begin()) { - receive_buffer_.erase(receive_buffer_.begin(), start_it); - } + std::array payload{}; - if (receive_buffer_.size() < 4) { - return; - } + for (std::size_t i = 0; i < thruster_pwm_array.size(); ++i) { + const std::uint16_t value = thruster_pwm_array[i]; - const std::uint8_t start = receive_buffer_[0]; - const std::uint8_t msg_id = receive_buffer_[1]; - const std::uint8_t length = receive_buffer_[2]; + payload[2 * i] = + static_cast(value & 0xFF); - if (start != UART_START_BYTE) { - receive_buffer_.erase(receive_buffer_.begin()); - continue; - } + payload[2 * i + 1] = + static_cast((value >> 8) & 0xFF); + } - if (length > MAX_PAYLOAD_SIZE) { - receive_buffer_.erase(receive_buffer_.begin()); - continue; - } + const can_status status = can_.send( + CAN_ID_SET_THRUSTERS_PWM, + payload.data(), + static_cast(payload.size()), + true // use BRS + ); - const std::size_t full_frame_size = - 4u + static_cast(length); + if (status != can_status::OK) { + return static_cast(status); + } - if (receive_buffer_.size() < full_frame_size) { - return; - } + return 0; +} - const std::uint8_t* payload_ptr = receive_buffer_.data() + 3; - const std::uint8_t received_checksum = receive_buffer_[3 + length]; - const std::uint8_t expected_checksum = - compute_checksum(msg_id, length, payload_ptr); +int ThrusterInterfaceAUVDriver::set_camera_light(float percentage) { + if (!can_.is_initialized()) { + return static_cast(can_status::ERR_NOT_INITIALIZED); + } - if (received_checksum != expected_checksum) { - receive_buffer_.erase(receive_buffer_.begin()); - continue; - } + percentage = std::clamp(percentage, 0.0f, 1.0f); - std::vector frame_bytes( - receive_buffer_.begin(), - receive_buffer_.begin() + - static_cast(full_frame_size)); + const std::uint16_t pwm = + static_cast(1100.0f + 800.0f * percentage); - handle_received_frame(frame_bytes); + std::array payload{}; + payload[0] = static_cast(pwm & 0xFF); + payload[1] = static_cast((pwm >> 8) & 0xFF); - receive_buffer_.erase(receive_buffer_.begin(), - receive_buffer_.begin() + - static_cast(full_frame_size)); - } -} + const can_status status = can_.send( + CAN_ID_SET_LIGHT_PWM, + payload.data(), + static_cast(payload.size()), + true + ); -void ThrusterInterfaceAUVDriver::handle_received_frame( - const std::vector& frame_bytes) { - if (frame_bytes.size() < 4) { - return; + if (status != can_status::OK) { + return static_cast(status); } - const std::uint8_t msg_id = frame_bytes[1]; - const std::uint8_t length = frame_bytes[2]; + return 0; +} - if (frame_bytes.size() != static_cast(length) + 4u) { +void ThrusterInterfaceAUVDriver::handle_can_frame( + const struct canfd_frame& frame, + can_status status) { + if (status != can_status::OK) { return; } - const std::uint8_t* payload = frame_bytes.data() + 3; - - switch (msg_id) { - case MSG_FLT_EVENT: { - if (length != 2) { + switch (frame.can_id) { + case CAN_ID_FLT_EVENT: { + if (frame.len != 2) { break; } - const std::uint8_t channel = payload[0]; - const std::uint8_t code = payload[1]; + const std::uint8_t channel = frame.data[0]; + const std::uint8_t code = frame.data[1]; if (fault_event_callback_) { fault_event_callback_(channel, code); } + break; } - case MSG_PGOOD_EVENT: { - if (length != 2) { + case CAN_ID_PGOOD_EVENT: { + if (frame.len != 2) { break; } - const std::uint8_t channel = payload[0]; - const std::uint8_t code = payload[1]; + const std::uint8_t channel = frame.data[0]; + const std::uint8_t code = frame.data[1]; if (pgood_event_callback_) { pgood_event_callback_(channel, code); } + break; } - case MSG_KILLSWITCH_EVENT: { - if (length != 0) { + case CAN_ID_KILLSWITCH_EVENT: { + if (frame.len != 0) { break; } if (killswitch_event_callback_) { killswitch_event_callback_(); } + break; } - case MSG_CURRENT_MEASUREMENTS: { + case CAN_ID_CURRENT_MEASUREMENTS: { constexpr std::size_t num_currents = 8; constexpr std::size_t expected_length = num_currents * sizeof(float); - if (length != expected_length) { + if (frame.len != expected_length) { break; } std::array currents{}; - - std::memcpy(currents.data(), payload, expected_length); + std::memcpy(currents.data(), frame.data, expected_length); if (current_measurements_callback_) { current_measurements_callback_(currents); } + break; } - default: { + default: break; - } } } +int ThrusterInterfaceAUVDriver::disable_thrusters() { + if (!can_.is_initialized()) { + return static_cast(can_status::ERR_NOT_INITIALIZED); + } + + const std::uint8_t dummy = 0; + + const can_status status = can_.send( + CAN_ID_DISABLE_THRUSTERS, + &dummy, + 0, + true); + + if (status != can_status::OK) { + return static_cast(status); + } + + return 0; +} + +int ThrusterInterfaceAUVDriver::enable_thrusters() { + if (!can_.is_initialized()) { + return static_cast(can_status::ERR_NOT_INITIALIZED); + } + + const std::uint8_t dummy = 0; + + const can_status status = can_.send( + CAN_ID_ENABLE_THRUSTERS, + &dummy, + 0, + true); + + if (status != can_status::OK) { + return static_cast(status); + } + + return 0; +} + void ThrusterInterfaceAUVDriver::set_fault_event_callback( FaultEventCallback callback) { fault_event_callback_ = std::move(callback); 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 8a95a503c..9b0e8cf82 100644 --- a/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp +++ b/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp @@ -7,6 +7,7 @@ #include #include #include +#include #include #include @@ -63,8 +64,7 @@ ThrusterInterfaceAUVNode::ThrusterInterfaceAUVNode( vortex::utils::qos_profiles::sensor_data_profile(10)); thruster_driver_ = std::make_unique( - serial_device_, baud_rate_, thruster_parameters_, right_coeffs_, - left_coeffs_); + can_interface_, thruster_parameters_, right_coeffs_, left_coeffs_); thruster_driver_->set_fault_event_callback( [this](std::uint8_t channel, std::uint8_t code) { @@ -93,11 +93,11 @@ ThrusterInterfaceAUVNode::ThrusterInterfaceAUVNode( current_measurements_publisher_->publish(msg); }); - if (thruster_driver_->init_uart() != 0) { - spdlog::error("Failed to initialize UART thruster driver"); + if (thruster_driver_->init_can() != 0) { + spdlog::error("Failed to initialize CAN FD thruster driver"); } else { - spdlog::info("UART thruster driver initialized on {} @ {} baud", - serial_device_, baud_rate_); + spdlog::info("CAN FD thruster driver initialized on {}", + can_interface_); } thruster_forces_array_ = std::vector(8, 0.0); @@ -202,8 +202,7 @@ void ThrusterInterfaceAUVNode::extract_all_parameters() { this->declare_parameter>("coeffs.16V.LEFT"); this->declare_parameter>("coeffs.16V.RIGHT"); - this->declare_parameter("uart.device"); - this->declare_parameter("uart.baud_rate"); + this->declare_parameter("can.interface"); this->declare_parameter("topics.thruster_forces"); this->declare_parameter("topics.pwm_output"); @@ -228,9 +227,7 @@ void ThrusterInterfaceAUVNode::extract_all_parameters() { left_coeffs_ = this->get_parameter("coeffs.16V.LEFT").as_double_array(); right_coeffs_ = this->get_parameter("coeffs.16V.RIGHT").as_double_array(); - serial_device_ = this->get_parameter("uart.device").as_string(); - baud_rate_ = static_cast( - this->get_parameter("uart.baud_rate").as_int()); + can_interface_ = this->get_parameter("can.interface").as_string(); subscriber_topic_name_ = this->get_parameter("topics.thruster_forces").as_string(); @@ -252,8 +249,8 @@ void ThrusterInterfaceAUVNode::extract_all_parameters() { if (thruster_count != 8) { spdlog::warn( - "UART packet format expects 8 thrusters, but config contains {} " - "entries", + "CAN FD thruster payload expects 8 thrusters, but config contains " + "{} entries", thruster_count); } 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..c72f13c0e --- /dev/null +++ b/navigation/eskf/config/eskf_params.yaml @@ -0,0 +1,38 @@ +/**: + 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] + sensors: + imu: + use_tf_transform: true + transform: + r: [ -1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, -1.0 ] + t: [ 0.0, 0.0, 0.0 ] + + dvl: + use_tf_transform: true + use_msg_noise: true + measurement_noise_std_diag: [0.1, 0.1, 0.1] # [m/s] + transform: + r: [ 0.0, -1.0, 0.0, + 1.0, 0.0, 0.0, + 0.0, 0.0, 1.0 ] + t: [ 0.4, 0.0, 0.2 ] + + pressure: + use_tf_transform: true + use_msg_noise: true # Stonefish provides variance from its noise model + measurement_noise: 40000.0 # [Pa^2] pressure variance + transform: + t: [ 0.0, 0.0, 0.0 ] + + publish_tf: true + publish_pose: true + publish_twist: true + publish_rate_ms: 5 + publish_biases: false + publish_nis: true + pressure_is_gauge: true # Stonefish publishes gauge pressure 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..515ca3a0f --- /dev/null +++ b/navigation/eskf/config/eskf_params_real_world.yaml @@ -0,0 +1,40 @@ +/**: + 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] + sensors: + imu: # Kongsberg Mini MRU + use_tf_transform: true + transform: + r: [ 1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0 ] + t: [ 0.0, 0.0, 0.0 ] + + dvl: # Nortek Nucleus 1000 + use_tf_transform: true + use_msg_noise: true + measurement_noise_std_diag: [0.1, 0.1, 0.1] # [m/s] + transform: + r: [ 0.0, -1.0, 0.0, + 1.0, 0.0, 0.0, + 0.0, 0.0, 1.0 ] + t: [ 0.4, 0.0, 0.2 ] + + pressure: # MS5837 blue robotics pressure sensor + use_tf_transform: true + use_msg_noise: false # MS5837 driver publishes variance=0, use config noise + measurement_noise: 40000.0 # [Pa^2] pressure variance + transform: + t: [ 0.0, 0.0, 0.0 ] + + publish_tf: true + publish_pose: true + publish_twist: true + publish_rate_ms: 5 + publish_biases: true + publish_nis: true + publish_dvl_body: false + publish_depth: true + pressure_is_gauge: false # MS5837 publishes absolute pressure per ROS convention diff --git a/navigation/eskf/include/eskf/eskf.hpp b/navigation/eskf/include/eskf/eskf.hpp new file mode 100644 index 000000000..ff8fc323d --- /dev/null +++ b/navigation/eskf/include/eskf/eskf.hpp @@ -0,0 +1,107 @@ +#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 NominalState get_nominal_state() const { return current_nom_state_; } + + inline ErrorState get_error_state() const { return current_error_state_; } + + inline double get_nis_dvl() const { return nis_dvl_; } + inline double get_nis_depth() const { return nis_depth_; } + + 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 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 + // @return NIS for this measurement update + template + double 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_{}; + + double nis_dvl_{}; + double nis_depth_{}; + + // Member variable for the current error state + ErrorState current_error_state_{}; + + // Member variable for the current nominal state + NominalState 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 NominalState& current_nom_state_); + +// Jacobian of h(x) with respect to the error state --> H +Eigen::Matrix3x15d calculate_h_jacobian(const NominalState& current_nom_state_); + +// Jacobian of h(x) with respect to the nominal state --> Hx +Eigen::Matrix3x16d calculate_hx(const NominalState& 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..0ac7f3bf7 --- /dev/null +++ b/navigation/eskf/include/eskf/eskf.tpp @@ -0,0 +1,27 @@ + + +template +double 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(); + + 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 + + return compute_nis(innovation, S); +} diff --git a/navigation/eskf/include/eskf/eskf_ros.hpp b/navigation/eskf/include/eskf/eskf_ros.hpp new file mode 100644 index 000000000..5374aa624 --- /dev/null +++ b/navigation/eskf/include/eskf/eskf_ros.hpp @@ -0,0 +1,160 @@ +#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" + +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::ConstSharedPtr msg); + + // @brief Callback function for the dvl topic + // @param msg: TwistWithCovarianceStamped message containing the dvl data + void dvl_callback( + const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr + msg); + + void pressure_callback( + const sensor_msgs::msg::FluidPressure::ConstSharedPtr 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 all + // required TF transforms are available (or immediately if none use TF). + void complete_initialization(); + + // @brief broadcast the State as a TF + void publish_tf(const NominalState& 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 + dvl_body_pub_; + + rclcpp::Publisher::SharedPtr 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; + } + + bool publish_nis_{false}; + bool publish_dvl_body_{false}; + bool publish_depth_{false}; + + // Flags and Storage + std::string frame_prefix_{""}; + bool publish_tf_{false}; + bool publish_pose_{false}; + bool publish_twist_{false}; + bool publish_biases_{false}; + + bool imu_use_tf_transform_{true}; + Eigen::Isometry3d Tf_base_imu_ = Eigen::Isometry3d::Identity(); + + bool dvl_use_tf_transform_{true}; + bool dvl_use_msg_noise_{true}; + Eigen::Vector3d dvl_measurement_noise_std_{0.1, 0.1, 0.1}; + Eigen::Isometry3d Tf_base_dvl_ = Eigen::Isometry3d::Identity(); + + bool pressure_use_tf_transform_{true}; + bool pressure_use_msg_noise_{false}; + double pressure_measurement_noise_{40000.0}; + Eigen::Isometry3d Tf_base_depth_ = Eigen::Isometry3d::Identity(); + + double gravity_; + double water_density_; + double atmospheric_pressure_; + bool pressure_is_gauge_{false}; +}; + +#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..fcc3ecf6f --- /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 NominalState { + 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(); + + NominalState() = 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; + } + + NominalState operator-(const NominalState& other) const { + NominalState 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 ErrorState { + 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.82841}; +}; + +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 NominalState& 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 NominalState& state) const; + Eigen::MatrixXd jacobian(const NominalState& state) const; + Eigen::MatrixXd noise_covariance() const; +}; + +struct SensorDepth { + double measurement; + double measurement_noise; + Eigen::VectorXd innovation(const NominalState& state) const; + Eigen::MatrixXd jacobian(const NominalState& 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..46e1651aa --- /dev/null +++ b/navigation/eskf/launch/eskf.launch.py @@ -0,0 +1,126 @@ +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 LoadComposableNodes, Node +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) + act_as_odom_source = ( + LaunchConfiguration('act_as_odom_source').perform(context).lower() == 'true' + ) + standalone = LaunchConfiguration('standalone').perform(context).lower() == 'true' + container_name = LaunchConfiguration('container_name').perform(context) + use_sim = LaunchConfiguration('use_sim').perform(context).lower() == 'true' + environment = ( + 'stonefish_sim' + if use_sim + else LaunchConfiguration('environment').perform(context) + ) + + drone_params = os.path.join( + get_package_share_directory('auv_setup'), 'config', 'robots', f'{drone}.yaml' + ) + eskf_params = os.path.join( + get_package_share_directory('eskf'), + 'config', + 'eskf_params.yaml' if use_sim else 'eskf_params_real_world.yaml', + ) + env_params = os.path.join( + get_package_share_directory('auv_setup'), + 'config', + 'environments', + f'{environment}.yaml', + ) + + params = [eskf_params, env_params, drone_params, {'frame_prefix': namespace}] + remappings = [] + if not act_as_odom_source: + params.append({'publish_tf': False}) + remappings = [ + ('odom', 'eskf/odom'), + ('pose', 'eskf/pose'), + ('twist', 'eskf/twist'), + ] + + if standalone: + return [ + Node( + package='eskf', + executable='eskf_node', + name='eskf_node', + namespace=namespace, + parameters=params, + remappings=remappings, + output='screen', + ) + ] + else: + return [ + LoadComposableNodes( + target_container=container_name, + composable_node_descriptions=[ + ComposableNode( + package='eskf', + plugin='ESKFNode', + name='eskf_node', + namespace=namespace, + parameters=params, + remappings=remappings, + extra_arguments=[{'use_intra_process_comms': True}], + ) + ], + ) + ] + + +def generate_launch_description(): + return LaunchDescription( + [ + DeclareLaunchArgument( + 'act_as_odom_source', + default_value='true', + description='If true, publish on standard topics and enable TF. If false, remap to eskf/* topics and disable TF.', + ), + DeclareLaunchArgument( + 'standalone', + default_value='true', + description=( + 'true = launch as a regular node; ' + 'false = attach to an existing container named by container_name' + ), + ), + DeclareLaunchArgument( + 'container_name', + default_value='', + description='Container to attach to when standalone=false', + ), + DeclareLaunchArgument( + 'use_sim', + default_value='false', + description='Load simulation parameters instead of real-world ones.', + ), + DeclareLaunchArgument( + 'environment', + default_value='trondheim_freshwater', + description='Environment config to load from auv_setup/config/environments/.', + choices=[ + 'longbeach', + 'stonefish_sim', + 'trondheim_freshwater', + 'trondheim_saltwater', + ], + ), + ] + + declare_drone_and_namespace_args() + + [OpaqueFunction(function=launch_setup)] + ) diff --git a/navigation/eskf/launch/eskf_debug.launch.py b/navigation/eskf/launch/eskf_debug.launch.py new file mode 100644 index 000000000..f0d7ea42c --- /dev/null +++ b/navigation/eskf/launch/eskf_debug.launch.py @@ -0,0 +1,180 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, OpaqueFunction, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import Command, LaunchConfiguration +from launch_ros.actions import Node +from launch_ros.parameter_descriptions import ParameterValue + + +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) + act_as_odom_source = ( + LaunchConfiguration('act_as_odom_source').perform(context).lower() == 'true' + ) + + 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' + + param_file_name = "eskf_params.yaml" if use_sim else "eskf_params_real_world.yaml" + eskf_params = os.path.join( + get_package_share_directory("eskf"), "config", param_file_name + ) + + environment = ( + 'stonefish_sim' + if use_sim + else LaunchConfiguration('environment').perform(context) + ) + env_params = os.path.join( + get_package_share_directory("auv_setup"), + "config", + "environments", + f"{environment}.yaml", + ) + + params = [eskf_params, env_params, drone_params, {"frame_prefix": namespace}] + remappings = [] + if not act_as_odom_source: + params.append({"publish_tf": False}) + remappings = [ + ("odom", "eskf/odom"), + ("pose", "eskf/pose"), + ("twist", "eskf/twist"), + ] + + eskf_odom_topic = "odom" if act_as_odom_source else "eskf/odom" + + eskf_node = Node( + package="eskf", + executable="eskf_node", + name="eskf_node", + namespace=namespace, + parameters=params, + remappings=remappings, + output="screen", + ) + + xacro_path = os.path.join( + get_package_share_directory("auv_setup"), + "description", + f"{drone}.urdf.xacro", + ) + + robot_state_publisher_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + namespace=namespace, + output="screen", + remappings=[("joint_states", "servo_state")], + parameters=[ + { + "robot_description": ParameterValue( + Command(["xacro", " ", xacro_path]), + value_type=str, + ), + "frame_prefix": f"{namespace}/", + } + ], + ) + + odom_transformer_node = Node( + package="odom_transformer", + executable="odom_transformer_node", + name="odom_transformer_node", + namespace=namespace, + parameters=[ + { + "sensor_frame": "dvl_link", + "publish_tf": False, + "publish_pose": False, + "publish_twist": False, + "topics.input": "nucleus/odom", + "topics.output": "nucleus/odom_relative", + "topics.pose": "pose", + "topics.twist": "twist", + }, + drone_params, + {"frame_prefix": namespace}, + ], + output="screen", + ) + + rpy_publisher_node = Node( + package="vortex_utility_nodes", + executable="rpy_publisher_node", + name="rpy_publisher_node", + namespace=namespace, + parameters=[ + { + "input_topics": ["nucleus/odom_relative", eskf_odom_topic], + "output_topics": ["nucleus/odom_relative/rpy", eskf_odom_topic + "/rpy"], + "input_types": ["odometry", "odometry"], + } + ], + output="screen", + ) + + 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(), + ) + + + return [drone_description_launch, eskf_node, robot_state_publisher_node, rpy_publisher_node, odom_transformer_node] + + +def generate_launch_description(): + sim_arg = DeclareLaunchArgument( + 'use_sim', + default_value='false', + description='Set to "false" to load real-world hardware parameters.', + ) + environment_arg = DeclareLaunchArgument( + 'environment', + default_value='trondheim_freshwater', + description=( + 'Environment config to load from auv_setup/config/environments/. ' + 'If use_sim is true env config is set to stonefish_sim' + ), + choices=[ + 'longbeach', + 'stonefish_sim', + 'trondheim_freshwater', + 'trondheim_saltwater', + ], + ) + act_as_odom_source_arg = DeclareLaunchArgument( + 'act_as_odom_source', + default_value='true', + description='If true, publish on standard topics and enable TF. If false, remap to eskf/* topics and disable TF.', + ) + return LaunchDescription( + [sim_arg, environment_arg, act_as_odom_source_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..6f474e052 --- /dev/null +++ b/navigation/eskf/src/eskf.cpp @@ -0,0 +1,219 @@ +#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(); +} + +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 NominalState& 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 NominalState& 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 NominalState& 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); + + ErrorState 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) { + nis_dvl_ = measurement_update(dvl_meas); + injection_and_reset(); +} + +void ESKF::depth_update(const SensorDepth& depth_meas) { + nis_depth_ = measurement_update(depth_meas); + injection_and_reset(); +} + +// DVL sensor model implementations + +Eigen::VectorXd SensorDVL::innovation(const NominalState& state) const { + Eigen::Vector3d innovation = this->measurement - calculate_h(state); + return innovation; +} + +Eigen::MatrixXd SensorDVL::jacobian(const NominalState& 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 NominalState& 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 NominalState& /*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..0a960cd29 --- /dev/null +++ b/navigation/eskf/src/eskf_ros.cpp @@ -0,0 +1,529 @@ +#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) { + 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"); + + 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"); + + imu_use_tf_transform_ = + this->declare_parameter("sensors.imu.use_tf_transform"); + dvl_use_tf_transform_ = + this->declare_parameter("sensors.dvl.use_tf_transform"); + dvl_use_msg_noise_ = + this->declare_parameter("sensors.dvl.use_msg_noise"); + pressure_use_tf_transform_ = + this->declare_parameter("sensors.pressure.use_tf_transform"); + pressure_use_msg_noise_ = + this->declare_parameter("sensors.pressure.use_msg_noise"); + + const bool any_use_tf = imu_use_tf_transform_ || dvl_use_tf_transform_ || + pressure_use_tf_transform_; + + if (any_use_tf) { + 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, + [this](const sensor_msgs::msg::Imu::ConstSharedPtr msg) { + imu_callback(msg); + }); + + 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, + [this]( + const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr + msg) { dvl_callback(msg); }); + + std::string pressure_topic = + this->get_parameter("topics.pressure_sensor").as_string(); + depth_sub_ = this->create_subscription( + pressure_topic, qos_sensor_data, + [this](const sensor_msgs::msg::FluidPressure::ConstSharedPtr msg) { + pressure_callback(msg); + }); + + odom_pub_ = this->create_publisher( + this->get_parameter("topics.odom").as_string(), qos_sensor_data); + + if (publish_pose_) { + pose_pub_ = this->create_publisher< + geometry_msgs::msg::PoseWithCovarianceStamped>( + this->get_parameter("topics.pose").as_string(), qos_sensor_data); + } + + if (publish_twist_) { + twist_pub_ = this->create_publisher< + geometry_msgs::msg::TwistWithCovarianceStamped>( + this->get_parameter("topics.twist").as_string(), 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); + } + + if (publish_nis_) { + nis_dvl_pub_ = create_publisher( + "eskf/nis_dvl", qos_sensor_data); + nis_depth_pub_ = create_publisher( + "eskf/nis_depth", qos_sensor_data); + } + + if (publish_dvl_body_) { + dvl_body_pub_ = + create_publisher( + "eskf/dvl_body", qos_sensor_data); + } + + if (publish_depth_) { + depth_pub_ = create_publisher("eskf/depth", + qos_sensor_data); + } +} + +void ESKFNode::set_parameters() { + if (!imu_use_tf_transform_) { + std::vector R_imu = + this->declare_parameter>( + "sensors.imu.transform.r"); + R_imu_eskf_ = Eigen::Map>( + R_imu.data()); + + std::vector T_imu = + this->declare_parameter>( + "sensors.imu.transform.t"); + T_imu_eskf_ = Eigen::Map(T_imu.data()); + } + + if (!dvl_use_tf_transform_) { + std::vector R_dvl = + this->declare_parameter>( + "sensors.dvl.transform.r"); + R_dvl_eskf_ = Eigen::Map>( + R_dvl.data()); + + std::vector T_dvl = + this->declare_parameter>( + "sensors.dvl.transform.t"); + T_dvl_eskf_ = Eigen::Map(T_dvl.data()); + } + + if (!pressure_use_tf_transform_) { + std::vector T_depth = + this->declare_parameter>( + "sensors.pressure.transform.t"); + T_depth_eskf_ = Eigen::Map(T_depth.data()); + } + + if (!dvl_use_msg_noise_) { + auto diag = this->declare_parameter>( + "sensors.dvl.measurement_noise_std_diag"); + if (diag.size() != 3) { + throw std::runtime_error( + "sensors.dvl.measurement_noise_std_diag must have length 3"); + } + dvl_measurement_noise_std_ = Eigen::Vector3d(diag[0], diag[1], diag[2]); + } + + if (!pressure_use_msg_noise_) { + pressure_measurement_noise_ = this->declare_parameter( + "sensors.pressure.measurement_noise"); + } + + 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, gravity_); + + EskfParams eskf_params{ + .Q = Q, + .P = P, + .g_ = g_vec, + }; + + eskf_ = std::make_unique(eskf_params); +} + +void ESKFNode::imu_callback(const sensor_msgs::msg::Imu::ConstSharedPtr 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; + + Eigen::Vector3d gyro_aligned = R_imu_eskf_ * raw_gyro; + imu_measurement.gyro = gyro_aligned; + + // lever arm correction for accelerometer + NominalState 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; + + 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::ConstSharedPtr msg) { + SensorDVL dvl_sensor; + + dvl_sensor.measurement << msg->twist.twist.linear.x, + msg->twist.twist.linear.y, msg->twist.twist.linear.z; + + if (dvl_use_msg_noise_) { + 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]; + } else { + dvl_sensor.measurement_noise = + dvl_measurement_noise_std_.array().square().matrix().asDiagonal(); + } + + // Apply the rotation and translation corrections to the DVL measurement + NominalState 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(); + + if (publish_dvl_body_) { + geometry_msgs::msg::TwistWithCovarianceStamped dvl_body_msg; + dvl_body_msg.header.stamp = msg->header.stamp; + dvl_body_msg.header.frame_id = frame("base_link"); + dvl_body_msg.twist.twist.linear.x = dvl_sensor.measurement.x(); + dvl_body_msg.twist.twist.linear.y = dvl_sensor.measurement.y(); + dvl_body_msg.twist.twist.linear.z = dvl_sensor.measurement.z(); + Eigen::Map>( + dvl_body_msg.twist.covariance.data()) + .topLeftCorner<3, 3>() = dvl_sensor.measurement_noise; + dvl_body_pub_->publish(dvl_body_msg); + } + + eskf_->dvl_update(dvl_sensor); + + if (publish_nis_) { + std_msgs::msg::Float64 nis_msg; + nis_msg.data = eskf_->get_nis_dvl(); + nis_dvl_pub_->publish(nis_msg); + } +} + +void ESKFNode::pressure_callback( + const sensor_msgs::msg::FluidPressure::ConstSharedPtr msg) { + SensorDepth depth_sensor; + const double p_gauge = pressure_is_gauge_ + ? msg->fluid_pressure + : msg->fluid_pressure - atmospheric_pressure_; + depth_sensor.measurement = + p_gauge / (water_density_ * gravity_) - T_depth_eskf_.z(); + + const double pressure_variance = + pressure_use_msg_noise_ && msg->variance > 0.0 + ? msg->variance + : pressure_measurement_noise_; + + depth_sensor.measurement_noise = + pressure_variance / std::pow(water_density_ * gravity_, 2); + + if (publish_depth_) { + std_msgs::msg::Float64 depth_msg; + depth_msg.data = depth_sensor.measurement; + depth_pub_->publish(depth_msg); + } + + eskf_->depth_update(depth_sensor); + + if (publish_nis_) { + std_msgs::msg::Float64 nis_msg; + nis_msg.data = eskf_->get_nis_depth(); + nis_depth_pub_->publish(nis_msg); + } +} + +void ESKFNode::publish_odom() { + nav_msgs::msg::Odometry odom_msg; + NominalState nom_state = eskf_->get_nominal_state(); + ErrorState 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 { + if (imu_use_tf_transform_) { + 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(); + } + + if (dvl_use_tf_transform_) { + 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(); + } + + if (pressure_use_tf_transform_) { + 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_timer_->cancel(); + spdlog::info("All required static transforms loaded successfully."); + complete_initialization(); + } catch (const tf2::TransformException& ex) { + spdlog::warn("TF lookup failed (will retry): {}", ex.what()); + } +} + +void ESKFNode::complete_initialization() { + publish_nis_ = this->declare_parameter("publish_nis", false); + publish_dvl_body_ = + this->declare_parameter("publish_dvl_body", false); + publish_depth_ = this->declare_parameter("publish_depth", false); + set_subscribers_and_publisher(); + this->gravity_ = this->declare_parameter("gravity.acceleration"); + this->water_density_ = this->declare_parameter("water.density"); + this->atmospheric_pressure_ = + this->declare_parameter("atmosphere.pressure"); + this->pressure_is_gauge_ = + this->declare_parameter("pressure_is_gauge"); + set_parameters(); + + time_step_ = std::chrono::milliseconds( + this->get_parameter("publish_rate_ms").as_int()); + odom_pub_timer_ = + this->create_wall_timer(time_step_, [this]() { publish_odom(); }); + + spdlog::info(start_message); + + if (publish_nis_) { + spdlog::info( + "NIS publishing enabled on eskf/nis_dvl and eskf/nis_depth"); + } +} + +void ESKFNode::publish_tf(const NominalState& 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 index 6a27c4b62..bec84d6db 100644 --- a/navigation/odom_transformer/CMakeLists.txt +++ b/navigation/odom_transformer/CMakeLists.txt @@ -18,6 +18,7 @@ find_package(Eigen3 REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_ros REQUIRED) find_package(tf2_eigen REQUIRED) +find_package(std_srvs REQUIRED) include_directories(include ${EIGEN3_INCLUDE_DIRS}) @@ -36,6 +37,7 @@ ament_target_dependencies(${LIB_NAME} PUBLIC tf2 tf2_ros tf2_eigen + std_srvs ) rclcpp_components_register_node( diff --git a/navigation/odom_transformer/config/odom_transformer_params.yaml b/navigation/odom_transformer/config/odom_transformer_params.yaml index d99f65ab1..4ca476d8e 100644 --- a/navigation/odom_transformer/config/odom_transformer_params.yaml +++ b/navigation/odom_transformer/config/odom_transformer_params.yaml @@ -2,11 +2,11 @@ odom_transformer_node: ros__parameters: sensor_frame: "dvl_link" - publish_tf: true - publish_pose: true - publish_twist: true + publish_tf: false + publish_pose: false + publish_twist: false topics: input: "nucleus/odom" - output: "odom" + output: "nucleus/odom_relative" 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 index 0a845e57b..ebcd34a1a 100644 --- a/navigation/odom_transformer/include/odom_transformer/odom_transformer.hpp +++ b/navigation/odom_transformer/include/odom_transformer/odom_transformer.hpp @@ -10,6 +10,7 @@ #include #include #include +#include #include #include @@ -33,13 +34,14 @@ class OdomTransformer : public rclcpp::Node { std::unique_ptr tf_broadcaster_; rclcpp::TimerBase::SharedPtr tf_timer_; - // Pub / Sub + // Pub / Sub / Services rclcpp::Subscription::SharedPtr odom_sub_; rclcpp::Publisher::SharedPtr odom_pub_; rclcpp::Publisher::SharedPtr pose_pub_; rclcpp::Publisher::SharedPtr twist_pub_; + rclcpp::Service::SharedPtr reset_origin_srv_; // Transform from base_link to sensor_link Eigen::Matrix3d R_base_sensor_ = Eigen::Matrix3d::Identity(); diff --git a/navigation/odom_transformer/package.xml b/navigation/odom_transformer/package.xml index 9658764c8..8c5aa334a 100644 --- a/navigation/odom_transformer/package.xml +++ b/navigation/odom_transformer/package.xml @@ -17,6 +17,7 @@ tf2 tf2_ros tf2_eigen + std_srvs auv_setup diff --git a/navigation/odom_transformer/src/odom_transformer.cpp b/navigation/odom_transformer/src/odom_transformer.cpp index cdf51c2cc..d73f30930 100644 --- a/navigation/odom_transformer/src/odom_transformer.cpp +++ b/navigation/odom_transformer/src/odom_transformer.cpp @@ -80,6 +80,17 @@ void OdomTransformer::complete_initialization() { this->get_parameter("topics.twist").as_string(), qos); } + reset_origin_srv_ = this->create_service( + "reset_odom_origin", + [this](const std_srvs::srv::Trigger::Request::SharedPtr, + std_srvs::srv::Trigger::Response::SharedPtr response) { + origin_set_ = false; + response->success = true; + response->message = + "Odom origin reset; next message sets new origin."; + RCLCPP_INFO(get_logger(), "Odom origin reset requested."); + }); + RCLCPP_INFO(get_logger(), "Odom transformer: %s -> %s", input_topic.c_str(), output_topic.c_str()); } @@ -112,12 +123,17 @@ void OdomTransformer::odom_callback( // Capture the first base_link pose as the odom frame origin if (!origin_set_) { - R_origin_ = R_odom_base; + double yaw0 = std::atan2(R_odom_base(1, 0), R_odom_base(0, 0)); + + R_origin_ = Eigen::AngleAxisd(yaw0, Eigen::Vector3d::UnitZ()) + .toRotationMatrix(); + p_origin_ = p_base; origin_set_ = true; } - // Express pose relative to origin so t=0 is identity + // Express pose relative to initial position and yaw, preserving odometry + // roll/pitch Eigen::Matrix3d R_out = R_origin_.transpose() * R_odom_base; Eigen::Vector3d p_out = R_origin_.transpose() * (p_base - p_origin_); Eigen::Quaterniond q_out(R_out); 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/utility_scripts/launch_drone_sim.sh b/utility_scripts/launch_drone_sim.sh index 662a77e08..f29d0e6c3 100755 --- a/utility_scripts/launch_drone_sim.sh +++ b/utility_scripts/launch_drone_sim.sh @@ -1,35 +1,42 @@ #!/bin/bash # Launch drone simulation stack in a tmux session -# Usage: ./launch_drone_sim.sh [--scenario ] +# Usage: ./launch_drone_sim.sh [--scenario ] [--domain-id ] # --scenario Stonefish scenario to load (default: default) # GPU scenarios: default, docking, pipeline, structure, # orca_demo, freya_demo, orca_freya_demo, tacc # No-GPU scenarios: nautilus_no_gpu, orca_no_gpu, freya_no_gpu +# --domain-id ROS_DOMAIN_ID to use (default: 0) usage() { cat < Stonefish scenario to load (default: default) - GPU: default, docking, pipeline, structure, - orca_demo, freya_demo, orca_freya_demo, tacc - No-GPU: nautilus_no_gpu, orca_no_gpu, freya_no_gpu - -h, --help Show this help message + --scenario Stonefish scenario to load (default: default) + GPU: default, docking, pipeline, structure, + orca_demo, freya_demo, orca_freya_demo, tacc + No-GPU: nautilus_no_gpu, orca_no_gpu, freya_no_gpu + --domain-id ROS_DOMAIN_ID to use (default: 0) + --keyboard-joy Enable keyboard joystick control (default: true) + -h, --help Show this help message EOF } SCENARIO="default" +DOMAIN_ID="0" +KEYBOARD_JOY="true" while [[ $# -gt 0 ]]; do case "$1" in - --scenario) SCENARIO="$2"; shift 2 ;; - -h|--help) usage; exit 0 ;; + --scenario) SCENARIO="$2"; shift 2 ;; + --domain-id) DOMAIN_ID="$2"; shift 2 ;; + --keyboard-joy) KEYBOARD_JOY="$2"; shift 2 ;; + -h|--help) usage; exit 0 ;; *) echo "Unknown argument: $1"; usage; exit 1 ;; esac done SESSION="drone_launch" -S="source install/setup.bash" +S="source install/setup.bash && export ROS_DOMAIN_ID=$DOMAIN_ID" # Kill existing session if it exists tmux kill-session -t "$SESSION" 2>/dev/null @@ -45,7 +52,7 @@ fi tmux new-session -d -s "$SESSION" -n "sim" PANE_SIM=$(tmux list-panes -t "$SESSION:sim" -F '#{pane_id}') -tmux send-keys -t "$PANE_SIM" "clear && $S && ros2 launch stonefish_sim vortex_sim_launch.py keyboard_joy:=true scenario:=$SCENARIO" Enter +tmux send-keys -t "$PANE_SIM" "clear && $S && ros2 launch stonefish_sim vortex_sim_launch.py keyboard_joy:=$KEYBOARD_JOY scenario:=$SCENARIO" Enter PANE_P2=$(tmux split-window -h -t "$PANE_SIM" -P -F '#{pane_id}') tmux send-keys -t "$PANE_P2" "clear && $S && ros2 launch auv_setup dp_quat.launch.py" Enter @@ -67,7 +74,7 @@ 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 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 +tmux send-keys -t "$PANE_MSG" "clear && $S && ros2 launch vortex_utility_nodes rpy_publisher.launch.py " Enter # ============================================= # Focus first window and attach