Development

.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

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

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

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

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

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,18 +111,21 @@
       odom: "odom"
       operation_mode: "operation_mode"
       killswitch: "killswitch"
+      mission_wipe: "mission/wipe"
       reference_pose: "reference_pose"
       landmarks: "landmarks"
       waypoint: "waypoint"
       guidance:
         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,10 +136,12 @@
       landmark_convergence: "landmark_convergence"
       waypoint_manager: "waypoint_manager"
 
+
     services:
       set_operation_mode: "set_operation_mode"
       set_killswitch: "set_killswitch"
       toggle_killswitch: "toggle_killswitch"
       get_operation_mode: "get_operation_mode"
       waypoint_addition: "waypoint_addition"
       start_mission: "start_mission"
+      reset_odom_origin: "reset_odom_origin"

auv_setup/description/nautilus.urdf.xacro

@@ -32,7 +32,7 @@
   <joint name="base_to_pressure_sensor" type="fixed">
     <parent link="base_link"/>
     <child link="pressure_sensor_link"/>
-    <origin xyz="-0.130 0.003 0.2414" rpy="0.0 0.0 0.0"/>
+    <origin xyz="-0.035 -0.115 -0.095" rpy="0.0 0.0 0.0"/>
   </joint>
 
   <!-- base to fls -->
@@ -55,7 +55,7 @@
   <joint name="base_to_front_camera" type="fixed">
     <parent link="base_link"/>
     <child link="front_camera_link"/>
-    <origin xyz="0.36362 0.0 -0.023395" rpy="3.14159 0 0"/>
+    <origin xyz="0.36362 0.0 -0.023395" rpy="3.14159 -0.261666667 0"/>
   </joint>
 
   <xacro:realsense_d555 frame="front_camera_link" base="front_camera" name="realsense"/>
@@ -100,4 +100,13 @@
     <limit lower="-0.033" upper="0" effort="0" velocity="0"/>
   </joint>
 
+  <!-- Virtual reference frame at the centre of the gripper jaw opening -->
+  <link name="grip_point"></link>
+
+  <joint name="grip_point_joint" type="fixed">
+    <parent link="shoulder"/>
+    <child link="grip_point"/>
+    <origin xyz="0.2505 0.0 0.0" rpy="0.0 0.0 0.0"/>
+  </joint>
+
 </robot>

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)]
+    )

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)]
+    )