Initial workspace — six-package autonomy stack

.gitignore

@@ -0,0 +1,23 @@
+# ROS2 build artifacts
+build/
+install/
+log/
+
+# Python
+__pycache__/
+*.py[cod]
+*.egg-info/
+.eggs/
+
+# Docker
+*.tar
+
+# OS
+.DS_Store
+Thumbs.db
+
+# Editor
+.vscode/
+.idea/
+*.swp
+*.swo

Dockerfile

@@ -0,0 +1,45 @@
+# ============================================================
+# ROV Autonomy Stack — ARM64 Container Image
+# Target: Raspberry Pi 5 running Ubuntu 24.04 LTS
+# Base: ROS2 Jazzy on Ubuntu 24.04
+# ============================================================
+
+FROM ros:jazzy-ros-base-noble
+
+# Set non-interactive apt installs
+ENV DEBIAN_FRONTEND=noninteractive
+
+# Install ROS2 dependencies and tools
+RUN apt-get update && apt-get install -y \
+    # ROS2 packages required by the stack
+    ros-jazzy-mavros \
+    ros-jazzy-mavros-extras \
+    ros-jazzy-robot-localization \
+    ros-jazzy-cv-bridge \
+    ros-jazzy-image-transport \
+    # Python dependencies
+    python3-pip \
+    python3-opencv \
+    # Build tools
+    python3-colcon-common-extensions \
+    && rm -rf /var/lib/apt/lists/*
+
+# Install geographic datasets required by MAVROS
+RUN /opt/ros/jazzy/lib/mavros/install_geographiclib_datasets.sh
+
+# Create workspace directory
+WORKDIR /ros2_ws
+
+# Copy workspace source into image
+COPY src/ src/
+
+# Build the workspace
+RUN /bin/bash -c "source /opt/ros/jazzy/setup.bash && \
+    colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release"
+
+# Copy and configure entrypoint
+COPY docker-entrypoint.sh /docker-entrypoint.sh
+RUN chmod +x /docker-entrypoint.sh
+
+ENTRYPOINT ["/docker-entrypoint.sh"]
+CMD ["ros2", "launch", "rov_bringup", "rov_full.launch.py"]

README.md

@@ -0,0 +1,46 @@
+# ROV Autonomy Stack
+
+ROS2 Jazzy workspace for the BlueROV2 Heavy autonomous inspection system.
+
+## Package Structure
+
+| Package | Purpose |
+|---|---|
+| `rov_interfaces` | Custom messages and services |
+| `rov_navigation` | State estimation, EKF, depth |
+| `rov_perception` | Camera nodes, feature detection |
+| `rov_control` | Motion controller, failsafe monitor |
+| `rov_mission` | Mission executor, waypoint sequencing |
+| `rov_bringup` | Top-level launch files |
+
+## Quick Start (on RPi5)
+
+```bash
+# Pull image from Harbor
+docker pull registry.symbytech.com/rov-autonomy/rov-stack:latest
+
+# Run full stack (dev environment)
+docker run --rm --network host \
+  registry.symbytech.com/rov-autonomy/rov-stack:latest \
+  ros2 launch rov_bringup rov_full.launch.py env:=dev blueos_ip:=<RPi4_IP>
+```
+
+## Building Locally (on server with buildx)
+
+```bash
+docker buildx build --platform linux/arm64 \
+  -t registry.symbytech.com/rov-autonomy/rov-stack:dev \
+  --push .
+```
+
+## Topic Reference
+
+| Topic | Type | Publisher | Subscribers |
+|---|---|---|---|
+| `/imu/data` | sensor_msgs/Imu | Xsens driver | state_estimator |
+| `/mavros/imu/data` | sensor_msgs/Imu | MAVROS | state_estimator |
+| `/rov/state` | nav_msgs/Odometry | state_estimator | motion_controller, mission_executor |
+| `/rov/cmd_vel` | geometry_msgs/Twist | mission_executor | motion_controller |
+| `/rov/failsafe` | rov_interfaces/FailsafeStatus | failsafe_monitor | motion_controller, mission_executor |
+| `/rov/mission/status` | rov_interfaces/MissionStatus | mission_executor | web UI |
+| `/camera/front/image_raw` | sensor_msgs/Image | camera_node | feature_detector |

docker-entrypoint.sh

@@ -0,0 +1,13 @@
+#!/bin/bash
+# Source ROS2 and workspace, then execute the provided command
+
+set -e
+
+# Source ROS2 base
+source /opt/ros/jazzy/setup.bash
+
+# Source the built workspace
+source /ros2_ws/install/setup.bash
+
+# Execute the command passed to the container
+exec "$@"

src/rov_bringup/CMakeLists.txt

@@ -0,0 +1,5 @@
+cmake_minimum_required(VERSION 3.8)
+project(rov_bringup)
+find_package(ament_cmake REQUIRED)
+install(DIRECTORY launch config DESTINATION share/${PROJECT_NAME})
+ament_package()

src/rov_bringup/launch/rov_dev.launch.py

@@ -0,0 +1,37 @@
+"""
+Development bringup — minimal stack for desk testing.
+No cameras. MAVROS pointed at simulator or bench RPi4.
+"""
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            'blueos_ip', default_value='192.168.1.x',
+            description='IP address of BlueOS'
+        ),
+
+        # MAVROS only
+        Node(
+            package='mavros',
+            executable='mavros_node',
+            name='mavros',
+            output='screen',
+            parameters=[{
+                'fcu_url': ['udp://', LaunchConfiguration('blueos_ip'), ':14550@14555'],
+            }],
+        ),
+
+        # Failsafe monitor — always required
+        Node(
+            package='rov_control',
+            executable='failsafe_monitor',
+            name='failsafe_monitor',
+            output='screen',
+        ),
+    ])

src/rov_bringup/launch/rov_full.launch.py

@@ -0,0 +1,82 @@
+"""
+Full ROV autonomy stack bringup.
+Launches all subsystems: MAVROS, navigation, control, perception, mission.
+
+Usage:
+  ros2 launch rov_bringup rov_full.launch.py env:=dev
+  ros2 launch rov_bringup rov_full.launch.py env:=field
+"""
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory
+import os
+
+
+def generate_launch_description():
+    # Package directories
+    nav_pkg = get_package_share_directory('rov_navigation')
+    ctrl_pkg = get_package_share_directory('rov_control')
+    perc_pkg = get_package_share_directory('rov_perception')
+    mission_pkg = get_package_share_directory('rov_mission')
+    bringup_pkg = get_package_share_directory('rov_bringup')
+
+    env_arg = DeclareLaunchArgument(
+        'env', default_value='dev',
+        description='Deployment environment: dev, pool, field'
+    )
+
+    blueos_ip_arg = DeclareLaunchArgument(
+        'blueos_ip', default_value='192.168.1.x',
+        description='IP address of BlueOS (RPi4)'
+    )
+
+    return LaunchDescription([
+        env_arg,
+        blueos_ip_arg,
+
+        # MAVROS — bridge to ArduSub on RPi4
+        Node(
+            package='mavros',
+            executable='mavros_node',
+            name='mavros',
+            output='screen',
+            parameters=[{
+                'fcu_url': ['udp://', LaunchConfiguration('blueos_ip'), ':14550@14555'],
+                'gcs_url': '',
+                'target_system_id': 1,
+                'target_component_id': 1,
+            }],
+        ),
+
+        # Navigation stack
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                os.path.join(nav_pkg, 'launch', 'navigation.launch.py')
+            ),
+        ),
+
+        # Control stack (failsafe first, then motion)
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                os.path.join(ctrl_pkg, 'launch', 'control.launch.py')
+            ),
+        ),
+
+        # Perception stack
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                os.path.join(perc_pkg, 'launch', 'perception.launch.py')
+            ),
+        ),
+
+        # Mission executor
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                os.path.join(mission_pkg, 'launch', 'mission.launch.py')
+            ),
+        ),
+    ])

src/rov_bringup/package.xml

@@ -0,0 +1,17 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>rov_bringup</name>
+  <version>0.1.0</version>
+  <description>Top-level bringup launch files for full ROV autonomy stack</description>
+  <maintainer email="grant@symbytech.com">Grant</maintainer>
+  <license>MIT</license>
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <depend>rov_navigation</depend>
+  <depend>rov_perception</depend>
+  <depend>rov_control</depend>
+  <depend>rov_mission</depend>
+  <depend>mavros</depend>
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/rov_control/config/failsafe.yaml

@@ -0,0 +1,8 @@
+# Failsafe configuration
+# Tune thresholds before any pool or field testing
+
+failsafe_monitor:
+  ros__parameters:
+    low_battery_threshold: 0.20   # Surface when battery below 20%
+    max_depth_m: 50.0             # Emergency surface if depth exceeded
+    comms_timeout_s: 5.0          # Surface after 5s comms loss

src/rov_control/launch/control.launch.py

@@ -0,0 +1,29 @@
+"""Control stack launch file."""
+
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory
+import os
+
+
+def generate_launch_description():
+    pkg_dir = get_package_share_directory('rov_control')
+    failsafe_config = os.path.join(pkg_dir, 'config', 'failsafe.yaml')
+
+    return LaunchDescription([
+        # Failsafe monitor — always start first
+        Node(
+            package='rov_control',
+            executable='failsafe_monitor',
+            name='failsafe_monitor',
+            output='screen',
+            parameters=[failsafe_config],
+        ),
+        # Motion controller
+        Node(
+            package='rov_control',
+            executable='motion_controller',
+            name='motion_controller',
+            output='screen',
+        ),
+    ])

src/rov_control/package.xml

@@ -0,0 +1,17 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>rov_control</name>
+  <version>0.1.0</version>
+  <description>Motion control, depth hold, and thruster management</description>
+  <maintainer email="grant@symbytech.com">Grant</maintainer>
+  <license>MIT</license>
+  <buildtool_depend>ament_python</buildtool_depend>
+  <depend>rclpy</depend>
+  <depend>std_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>mavros_msgs</depend>
+  <depend>rov_interfaces</depend>
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

src/rov_control/rov_control/failsafe_monitor.py

@@ -0,0 +1,135 @@
+#!/usr/bin/env python3
+"""
+Failsafe monitor node.
+Monitors all safety-critical conditions and publishes failsafe state.
+This node must be running at all times during autonomous operation.
+
+Conditions monitored:
+  - Comms loss (heartbeat timeout)
+  - Low battery (from MAVROS battery state)
+  - Maximum depth exceeded
+  - Obstacle detection (future — proximity sensors)
+  - Manual abort command
+
+Topics subscribed:
+  /mavros/battery        (sensor_msgs/BatteryState)  — battery level
+  /mavros/state          (mavros_msgs/State)          — FCU connection state
+  /rov/mission/abort     (std_msgs/Bool)              — manual abort trigger
+
+Topics published:
+  /rov/failsafe          (rov_interfaces/FailsafeStatus) — current failsafe state
+"""
+
+import rclpy
+from rclpy.node import Node
+from rclpy.duration import Duration
+from std_msgs.msg import Bool
+from sensor_msgs.msg import BatteryState
+from mavros_msgs.msg import State
+from rov_interfaces.msg import FailsafeStatus
+
+
+class FailsafeMonitor(Node):
+    """Monitors safety conditions and publishes failsafe status."""
+
+    def __init__(self):
+        super().__init__('failsafe_monitor')
+
+        # --- Parameters ---
+        self.declare_parameter('low_battery_threshold', 0.20)  # 20% remaining
+        self.declare_parameter('max_depth_m', 50.0)            # metres
+        self.declare_parameter('comms_timeout_s', 5.0)         # seconds
+
+        self.low_batt_thresh = self.get_parameter('low_battery_threshold').value
+        self.max_depth = self.get_parameter('max_depth_m').value
+        self.comms_timeout = self.get_parameter('comms_timeout_s').value
+
+        # Internal state
+        self.last_heartbeat = self.get_clock().now()
+        self.battery_percent = 1.0
+        self.fcu_connected = False
+
+        # --- Subscribers ---
+        self.battery_sub = self.create_subscription(
+            BatteryState, '/mavros/battery', self.battery_callback, 10)
+        self.state_sub = self.create_subscription(
+            State, '/mavros/state', self.state_callback, 10)
+        self.abort_sub = self.create_subscription(
+            Bool, '/rov/mission/abort', self.abort_callback, 10)
+
+        # --- Publishers ---
+        self.failsafe_pub = self.create_publisher(FailsafeStatus, '/rov/failsafe', 10)
+
+        # --- Timer: evaluate failsafe conditions at 10Hz ---
+        self.timer = self.create_timer(0.1, self.evaluate_failsafe)
+
+        self.get_logger().info('FailsafeMonitor node started')
+
+    def battery_callback(self, msg: BatteryState):
+        """Update battery state."""
+        self.battery_percent = msg.percentage
+
+    def state_callback(self, msg: State):
+        """Update FCU connection state and heartbeat time."""
+        self.fcu_connected = msg.connected
+        if msg.connected:
+            self.last_heartbeat = self.get_clock().now()
+
+    def abort_callback(self, msg: Bool):
+        """Handle manual abort command."""
+        if msg.data:
+            self.get_logger().warn('Manual abort received')
+            self.publish_failsafe(
+                manual_abort=True,
+                action=FailsafeStatus.ACTION_SURFACE,
+                message='Manual abort commanded'
+            )
+
+    def evaluate_failsafe(self):
+        """Evaluate all failsafe conditions and publish status."""
+        now = self.get_clock().now()
+        elapsed = (now - self.last_heartbeat).nanoseconds / 1e9
+
+        comms_loss = elapsed > self.comms_timeout
+        low_battery = self.battery_percent < self.low_batt_thresh
+
+        if comms_loss:
+            self.publish_failsafe(
+                comms_loss=True,
+                action=FailsafeStatus.ACTION_SURFACE,
+                message=f'Comms lost for {elapsed:.1f}s'
+            )
+        elif low_battery:
+            self.publish_failsafe(
+                low_battery=True,
+                action=FailsafeStatus.ACTION_SURFACE,
+                message=f'Low battery: {self.battery_percent*100:.0f}%'
+            )
+
+    def publish_failsafe(self, comms_loss=False, low_battery=False,
+                          depth_exceeded=False, obstacle_detected=False,
+                          manual_abort=False,
+                          action=FailsafeStatus.ACTION_NONE, message=''):
+        """Publish a failsafe status message."""
+        msg = FailsafeStatus()
+        msg.header.stamp = self.get_clock().now().to_msg()
+        msg.comms_loss = comms_loss
+        msg.low_battery = low_battery
+        msg.depth_exceeded = depth_exceeded
+        msg.obstacle_detected = obstacle_detected
+        msg.manual_abort = manual_abort
+        msg.action = action
+        msg.message = message
+        self.failsafe_pub.publish(msg)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = FailsafeMonitor()
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

src/rov_control/rov_control/motion_controller.py

@@ -0,0 +1,91 @@
+#!/usr/bin/env python3
+"""
+Motion controller node.
+Translates high-level velocity setpoints into MAVROS commands.
+Implements depth hold and heading hold modes.
+
+Topics subscribed:
+  /rov/cmd_vel       (geometry_msgs/Twist)   — velocity setpoint from mission planner
+  /rov/state         (nav_msgs/Odometry)     — current state from navigation stack
+  /rov/failsafe      (rov_interfaces/FailsafeStatus) — emergency override
+
+Topics published:
+  /mavros/setpoint_velocity/cmd_vel  (geometry_msgs/TwistStamped) — to ArduSub
+"""
+
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import Twist, TwistStamped
+from nav_msgs.msg import Odometry
+from rov_interfaces.msg import FailsafeStatus
+
+
+class MotionController(Node):
+    """Translates velocity setpoints to MAVROS commands."""
+
+    def __init__(self):
+        super().__init__('motion_controller')
+
+        # --- Parameters ---
+        self.declare_parameter('max_surge_ms', 0.5)   # m/s forward
+        self.declare_parameter('max_heave_ms', 0.3)   # m/s vertical
+        self.declare_parameter('max_yaw_rads', 0.3)   # rad/s yaw rate
+
+        self.max_surge = self.get_parameter('max_surge_ms').value
+        self.max_heave = self.get_parameter('max_heave_ms').value
+        self.max_yaw = self.get_parameter('max_yaw_rads').value
+
+        self.failsafe_active = False
+
+        # --- Subscribers ---
+        self.cmd_sub = self.create_subscription(
+            Twist, '/rov/cmd_vel', self.cmd_callback, 10)
+        self.state_sub = self.create_subscription(
+            Odometry, '/rov/state', self.state_callback, 10)
+        self.failsafe_sub = self.create_subscription(
+            FailsafeStatus, '/rov/failsafe', self.failsafe_callback, 10)
+
+        # --- Publishers ---
+        self.mavros_pub = self.create_publisher(
+            TwistStamped, '/mavros/setpoint_velocity/cmd_vel', 10)
+
+        self.get_logger().info('MotionController node started')
+
+    def cmd_callback(self, msg: Twist):
+        """Translate velocity setpoint to MAVROS command."""
+        if self.failsafe_active:
+            return  # Failsafe overrides all motion commands
+
+        out = TwistStamped()
+        out.header.stamp = self.get_clock().now().to_msg()
+        out.header.frame_id = 'base_link'
+
+        # Clamp velocities to safe limits
+        out.twist.linear.x = max(-self.max_surge, min(self.max_surge, msg.linear.x))
+        out.twist.linear.z = max(-self.max_heave, min(self.max_heave, msg.linear.z))
+        out.twist.angular.z = max(-self.max_yaw, min(self.max_yaw, msg.angular.z))
+
+        self.mavros_pub.publish(out)
+
+    def state_callback(self, msg: Odometry):
+        """Update internal state estimate."""
+        # TODO: use for depth hold and heading hold PID
+        pass
+
+    def failsafe_callback(self, msg: FailsafeStatus):
+        """Handle failsafe state changes."""
+        self.failsafe_active = (msg.action != FailsafeStatus.ACTION_NONE)
+        if self.failsafe_active:
+            self.get_logger().warn(f'Failsafe active: {msg.message}')
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = MotionController()
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

src/rov_control/setup.py

@@ -0,0 +1,25 @@
+from setuptools import setup
+import os
+from glob import glob
+
+package_name = 'rov_control'
+
+setup(
+    name=package_name,
+    version='0.1.0',
+    packages=[package_name],
+    data_files=[
+        ('share/ament_index/resource_index/packages', ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+        (os.path.join('share', package_name, 'launch'), glob('launch/*.py')),
+        (os.path.join('share', package_name, 'config'), glob('config/*.yaml')),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    entry_points={
+        'console_scripts': [
+            'motion_controller = rov_control.motion_controller:main',
+            'failsafe_monitor = rov_control.failsafe_monitor:main',
+        ],
+    },
+)

src/rov_interfaces/CMakeLists.txt

@@ -0,0 +1,16 @@
+cmake_minimum_required(VERSION 3.8)
+project(rov_interfaces)
+find_package(ament_cmake REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "msg/MissionStatus.msg"
+  "msg/FailsafeStatus.msg"
+  "msg/InspectionWaypoint.msg"
+  "srv/MissionCommand.srv"
+  DEPENDENCIES std_msgs geometry_msgs
+)
+ament_export_dependencies(rosidl_default_runtime)
+ament_package()

src/rov_interfaces/msg/FailsafeStatus.msg

@@ -0,0 +1,12 @@
+std_msgs/Header header
+bool comms_loss
+bool low_battery
+bool depth_exceeded
+bool obstacle_detected
+bool manual_abort
+uint8 action
+uint8 ACTION_NONE=0
+uint8 ACTION_HOVER=1
+uint8 ACTION_SURFACE=2
+uint8 ACTION_EMERGENCY_SURFACE=3
+string message

src/rov_interfaces/msg/InspectionWaypoint.msg

@@ -0,0 +1,8 @@
+std_msgs/Header header
+string waypoint_id
+geometry_msgs/Point position
+float64 heading_deg
+float64 standoff_m
+float64 loiter_time_s
+bool capture_image
+string panel_id

src/rov_interfaces/msg/MissionStatus.msg

@@ -0,0 +1,11 @@
+std_msgs/Header header
+string mission_id
+uint8 state
+uint8 STATE_IDLE=0
+uint8 STATE_RUNNING=1
+uint8 STATE_PAUSED=2
+uint8 STATE_COMPLETE=3
+uint8 STATE_ABORTED=4
+float32 progress_percent
+string current_task
+string message

src/rov_interfaces/package.xml

@@ -0,0 +1,17 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>rov_interfaces</name>
+  <version>0.1.0</version>
+  <description>Custom ROS2 messages and services for ROV autonomy stack</description>
+  <maintainer email="grant@symbytech.com">Grant</maintainer>
+  <license>MIT</license>
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <depend>std_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>rosidl_default_generators</depend>
+  <member_of_group>rosidl_interface_packages</member_of_group>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/rov_interfaces/srv/MissionCommand.srv

@@ -0,0 +1,6 @@
+string command
+string mission_id
+string[] parameters
+---
+bool success
+string message

src/rov_mission/config/dev.yaml

@@ -0,0 +1,8 @@
+# Development configuration
+# Looser tolerances for desk/pool testing
+
+mission_executor:
+  ros__parameters:
+    waypoint_arrival_radius_m: 0.5   # metres — looser for dev
+    waypoint_timeout_s: 60.0
+    image_capture_delay_s: 1.0

src/rov_mission/config/field.yaml

@@ -0,0 +1,8 @@
+# Field deployment configuration
+# Used when deploying to actual inspection site
+
+mission_executor:
+  ros__parameters:
+    waypoint_arrival_radius_m: 0.3   # metres — tighter in field
+    waypoint_timeout_s: 120.0         # seconds per waypoint before abort
+    image_capture_delay_s: 2.0        # settle time before capture

src/rov_mission/launch/mission.launch.py

@@ -0,0 +1,30 @@
+"""Mission executor launch file."""
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory
+import os
+
+
+def generate_launch_description():
+    pkg_dir = get_package_share_directory('rov_mission')
+
+    env_arg = DeclareLaunchArgument(
+        'env', default_value='dev',
+        description='Deployment environment: dev, field'
+    )
+
+    env = LaunchConfiguration('env')
+
+    return LaunchDescription([
+        env_arg,
+        Node(
+            package='rov_mission',
+            executable='mission_executor',
+            name='mission_executor',
+            output='screen',
+            # Config selected by env argument at launch time
+        ),
+    ])

src/rov_mission/package.xml

@@ -0,0 +1,16 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>rov_mission</name>
+  <version>0.1.0</version>
+  <description>Mission planning, waypoint execution, and inspection sequencing</description>
+  <maintainer email="grant@symbytech.com">Grant</maintainer>
+  <license>MIT</license>
+  <buildtool_depend>ament_python</buildtool_depend>
+  <depend>rclpy</depend>
+  <depend>std_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>rov_interfaces</depend>
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

src/rov_mission/rov_mission/mission_executor.py

@@ -0,0 +1,183 @@
+#!/usr/bin/env python3
+"""
+Mission executor node.
+Manages inspection mission state machine.
+Sequences waypoints, triggers image capture, handles replanning.
+
+States: IDLE → RUNNING → (PAUSED | COMPLETE | ABORTED)
+
+Topics subscribed:
+  /rov/state         (nav_msgs/Odometry)              — current position
+  /rov/failsafe      (rov_interfaces/FailsafeStatus)  — safety override
+
+Topics published:
+  /rov/cmd_vel       (geometry_msgs/Twist)            — velocity setpoints to controller
+  /rov/mission/status (rov_interfaces/MissionStatus)  — current mission state
+
+Services provided:
+  /rov/mission/command  (rov_interfaces/MissionCommand) — start/pause/abort/resume
+"""
+
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import Twist
+from nav_msgs.msg import Odometry
+from rov_interfaces.msg import MissionStatus, FailsafeStatus, InspectionWaypoint
+from rov_interfaces.srv import MissionCommand
+
+
+class MissionExecutor(Node):
+    """Executes inspection missions from a waypoint list."""
+
+    # Mission states
+    STATE_IDLE = 'IDLE'
+    STATE_RUNNING = 'RUNNING'
+    STATE_PAUSED = 'PAUSED'
+    STATE_COMPLETE = 'COMPLETE'
+    STATE_ABORTED = 'ABORTED'
+
+    def __init__(self):
+        super().__init__('mission_executor')
+
+        self.state = self.STATE_IDLE
+        self.waypoints = []          # List of InspectionWaypoint
+        self.current_wp_index = 0
+        self.mission_id = ''
+        self.current_position = None
+        self.failsafe_active = False
+
+        # --- Subscribers ---
+        self.state_sub = self.create_subscription(
+            Odometry, '/rov/state', self.state_callback, 10)
+        self.failsafe_sub = self.create_subscription(
+            FailsafeStatus, '/rov/failsafe', self.failsafe_callback, 10)
+
+        # --- Publishers ---
+        self.cmd_pub = self.create_publisher(Twist, '/rov/cmd_vel', 10)
+        self.status_pub = self.create_publisher(MissionStatus, '/rov/mission/status', 10)
+
+        # --- Services ---
+        self.cmd_srv = self.create_service(
+            MissionCommand, '/rov/mission/command', self.command_callback)
+
+        # --- Timer: mission loop at 10Hz ---
+        self.timer = self.create_timer(0.1, self.mission_loop)
+
+        self.get_logger().info('MissionExecutor node started')
+
+    def state_callback(self, msg: Odometry):
+        """Update current position."""
+        self.current_position = msg.pose.pose.position
+
+    def failsafe_callback(self, msg: FailsafeStatus):
+        """Handle failsafe — abort mission if active."""
+        was_active = self.failsafe_active
+        self.failsafe_active = (msg.action != FailsafeStatus.ACTION_NONE)
+        if self.failsafe_active and not was_active:
+            self.get_logger().warn('Failsafe triggered — aborting mission')
+            self.state = self.STATE_ABORTED
+
+    def command_callback(self, request, response):
+        """Handle mission command service calls."""
+        cmd = request.command.upper()
+
+        if cmd == 'START':
+            if self.state == self.STATE_IDLE:
+                self.mission_id = request.mission_id
+                self.current_wp_index = 0
+                self.state = self.STATE_RUNNING
+                response.success = True
+                response.message = f'Mission {self.mission_id} started'
+            else:
+                response.success = False
+                response.message = f'Cannot start — current state: {self.state}'
+
+        elif cmd == 'PAUSE':
+            if self.state == self.STATE_RUNNING:
+                self.state = self.STATE_PAUSED
+                response.success = True
+                response.message = 'Mission paused'
+            else:
+                response.success = False
+                response.message = f'Cannot pause — current state: {self.state}'
+
+        elif cmd == 'RESUME':
+            if self.state == self.STATE_PAUSED:
+                self.state = self.STATE_RUNNING
+                response.success = True
+                response.message = 'Mission resumed'
+            else:
+                response.success = False
+                response.message = f'Cannot resume — current state: {self.state}'
+
+        elif cmd == 'ABORT':
+            self.state = self.STATE_ABORTED
+            self.stop_motion()
+            response.success = True
+            response.message = 'Mission aborted'
+
+        else:
+            response.success = False
+            response.message = f'Unknown command: {cmd}'
+
+        return response
+
+    def mission_loop(self):
+        """Main mission execution loop — called at 10Hz."""
+        self.publish_status()
+
+        if self.state != self.STATE_RUNNING:
+            return
+
+        if not self.waypoints:
+            self.get_logger().info('No waypoints — mission complete')
+            self.state = self.STATE_COMPLETE
+            return
+
+        if self.current_wp_index >= len(self.waypoints):
+            self.get_logger().info('All waypoints reached — mission complete')
+            self.state = self.STATE_COMPLETE
+            return
+
+        # TODO: navigate to current waypoint
+        # TODO: check arrival condition
+        # TODO: trigger image capture if required
+        # TODO: advance to next waypoint on arrival
+
+    def stop_motion(self):
+        """Publish zero velocity to stop the ROV."""
+        self.cmd_pub.publish(Twist())  # All zeros = stop
+
+    def publish_status(self):
+        """Publish current mission status."""
+        msg = MissionStatus()
+        msg.header.stamp = self.get_clock().now().to_msg()
+        msg.mission_id = self.mission_id
+        msg.current_task = f'Waypoint {self.current_wp_index}/{len(self.waypoints)}'
+        msg.message = self.state
+
+        if len(self.waypoints) > 0:
+            msg.progress_percent = (self.current_wp_index / len(self.waypoints)) * 100.0
+
+        # Map state string to uint8
+        state_map = {
+            self.STATE_IDLE: MissionStatus.STATE_IDLE,
+            self.STATE_RUNNING: MissionStatus.STATE_RUNNING,
+            self.STATE_PAUSED: MissionStatus.STATE_PAUSED,
+            self.STATE_COMPLETE: MissionStatus.STATE_COMPLETE,
+            self.STATE_ABORTED: MissionStatus.STATE_ABORTED,
+        }
+        msg.state = state_map.get(self.state, MissionStatus.STATE_IDLE)
+        self.status_pub.publish(msg)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = MissionExecutor()
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

src/rov_mission/setup.py

@@ -0,0 +1,24 @@
+from setuptools import setup
+import os
+from glob import glob
+
+package_name = 'rov_mission'
+
+setup(
+    name=package_name,
+    version='0.1.0',
+    packages=[package_name],
+    data_files=[
+        ('share/ament_index/resource_index/packages', ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+        (os.path.join('share', package_name, 'launch'), glob('launch/*.py')),
+        (os.path.join('share', package_name, 'config'), glob('config/*.yaml')),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    entry_points={
+        'console_scripts': [
+            'mission_executor = rov_mission.mission_executor:main',
+        ],
+    },
+)

src/rov_navigation/config/ekf.yaml

@@ -0,0 +1,42 @@
+# robot_localization EKF configuration for ROV
+# Fuses: Xsens IMU (primary), depth sensor
+# Future: DVL when available
+
+ekf_filter_node:
+  ros__parameters:
+    frequency: 30.0
+    sensor_timeout: 0.1
+    two_d_mode: false
+    publish_tf: true
+    map_frame: map
+    odom_frame: odom
+    base_link_frame: base_link
+    world_frame: odom
+
+    # Xsens IMU — primary attitude source
+    imu0: /imu/data
+    imu0_config: [false, false, false,   # x, y, z position
+                  true,  true,  true,    # roll, pitch, yaw
+                  false, false, false,   # vx, vy, vz
+                  true,  true,  true,    # vroll, vpitch, vyaw
+                  true,  true,  true]    # ax, ay, az
+    imu0_differential: false
+    imu0_relative: false
+    imu0_remove_gravitational_acceleration: true
+
+    # Process noise — tune after pool testing
+    process_noise_covariance: [0.05, 0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,
+                               0,    0.05, 0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,
+                               0,    0,    0.06, 0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,
+                               0,    0,    0,    0.03, 0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,
+                               0,    0,    0,    0,    0.03, 0,    0,    0,    0,    0,    0,    0,    0,    0,    0,
+                               0,    0,    0,    0,    0,    0.06, 0,    0,    0,    0,    0,    0,    0,    0,    0,
+                               0,    0,    0,    0,    0,    0,    0.025,0,    0,    0,    0,    0,    0,    0,    0,
+                               0,    0,    0,    0,    0,    0,    0,    0.025,0,    0,    0,    0,    0,    0,    0,
+                               0,    0,    0,    0,    0,    0,    0,    0,    0.04, 0,    0,    0,    0,    0,    0,
+                               0,    0,    0,    0,    0,    0,    0,    0,    0,    0.01, 0,    0,    0,    0,    0,
+                               0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0.01, 0,    0,    0,    0,
+                               0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0.02, 0,    0,    0,
+                               0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0.01, 0,    0,
+                               0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0.01, 0,
+                               0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0.015]

src/rov_navigation/launch/navigation.launch.py

@@ -0,0 +1,43 @@
+"""
+Navigation stack launch file.
+Launches: state_estimator, depth_node, robot_localization EKF
+"""
+
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from ament_index_python.packages import get_package_share_directory
+import os
+
+
+def generate_launch_description():
+    pkg_dir = get_package_share_directory('rov_navigation')
+    ekf_config = os.path.join(pkg_dir, 'config', 'ekf.yaml')
+
+    return LaunchDescription([
+        # EKF from robot_localization
+        Node(
+            package='robot_localization',
+            executable='ekf_node',
+            name='ekf_filter_node',
+            output='screen',
+            parameters=[ekf_config],
+        ),
+
+        # State estimator
+        Node(
+            package='rov_navigation',
+            executable='state_estimator',
+            name='state_estimator',
+            output='screen',
+        ),
+
+        # Depth node
+        Node(
+            package='rov_navigation',
+            executable='depth_node',
+            name='depth_node',
+            output='screen',
+        ),
+    ])

src/rov_navigation/package.xml

@@ -0,0 +1,19 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>rov_navigation</name>
+  <version>0.1.0</version>
+  <description>State estimation, EKF, and localisation for ROV autonomy</description>
+  <maintainer email="grant@symbytech.com">Grant</maintainer>
+  <license>MIT</license>
+  <buildtool_depend>ament_python</buildtool_depend>
+  <depend>rclpy</depend>
+  <depend>std_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>robot_localization</depend>
+  <depend>rov_interfaces</depend>
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

src/rov_navigation/rov_navigation/depth_node.py

@@ -0,0 +1,48 @@
+#!/usr/bin/env python3
+"""
+Depth node.
+Reads depth/pressure from ArduSub via MAVROS and republishes as Float64.
+
+Topics subscribed:
+  /mavros/global_position/rel_alt  (std_msgs/Float64) — altitude from MAVROS
+
+Topics published:
+  /rov/depth  (std_msgs/Float64) — depth in metres (positive down)
+"""
+
+import rclpy
+from rclpy.node import Node
+from std_msgs.msg import Float64
+
+
+class DepthNode(Node):
+    """Republishes depth from MAVROS in a clean topic."""
+
+    def __init__(self):
+        super().__init__('depth_node')
+
+        self.sub = self.create_subscription(
+            Float64, '/mavros/global_position/rel_alt',
+            self.alt_callback, 10)
+
+        self.pub = self.create_publisher(Float64, '/rov/depth', 10)
+
+        self.get_logger().info('DepthNode started')
+
+    def alt_callback(self, msg: Float64):
+        """Convert altitude to depth (negate) and republish."""
+        depth_msg = Float64()
+        depth_msg.data = -msg.data  # ArduSub reports negative depth as altitude
+        self.pub.publish(depth_msg)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = DepthNode()
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

src/rov_navigation/rov_navigation/state_estimator.py

@@ -0,0 +1,66 @@
+#!/usr/bin/env python3
+"""
+State estimator node.
+Fuses IMU, depth, and (future) DVL data via robot_localization EKF.
+Publishes /rov/state as nav_msgs/Odometry.
+
+Topics subscribed:
+  /imu/data          (sensor_msgs/Imu)       — Xsens IMU
+  /mavros/imu/data   (sensor_msgs/Imu)       — ArduSub IMU (backup)
+  /rov/depth         (std_msgs/Float64)       — depth from pressure sensor
+
+Topics published:
+  /rov/state         (nav_msgs/Odometry)      — fused state estimate
+"""
+
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import Imu
+from nav_msgs.msg import Odometry
+from std_msgs.msg import Float64
+
+
+class StateEstimator(Node):
+    """Fuses sensor inputs into a single state estimate."""
+
+    def __init__(self):
+        super().__init__('state_estimator')
+
+        # --- Parameters ---
+        self.declare_parameter('use_xsens', True)
+        self.declare_parameter('use_mavros_imu', False)
+        self.use_xsens = self.get_parameter('use_xsens').value
+        self.use_mavros_imu = self.get_parameter('use_mavros_imu').value
+
+        # --- Subscribers ---
+        self.imu_sub = self.create_subscription(
+            Imu, '/imu/data', self.imu_callback, 10)
+        self.depth_sub = self.create_subscription(
+            Float64, '/rov/depth', self.depth_callback, 10)
+
+        # --- Publishers ---
+        self.state_pub = self.create_publisher(Odometry, '/rov/state', 10)
+
+        self.get_logger().info('StateEstimator node started')
+
+    def imu_callback(self, msg: Imu):
+        """Handle incoming IMU data."""
+        # TODO: forward to EKF or process directly
+        pass
+
+    def depth_callback(self, msg: Float64):
+        """Handle incoming depth measurement."""
+        # TODO: incorporate depth into state estimate
+        pass
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = StateEstimator()
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

src/rov_navigation/setup.py

@@ -0,0 +1,25 @@
+from setuptools import setup
+import os
+from glob import glob
+
+package_name = 'rov_navigation'
+
+setup(
+    name=package_name,
+    version='0.1.0',
+    packages=[package_name],
+    data_files=[
+        ('share/ament_index/resource_index/packages', ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+        (os.path.join('share', package_name, 'launch'), glob('launch/*.py')),
+        (os.path.join('share', package_name, 'config'), glob('config/*.yaml')),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    entry_points={
+        'console_scripts': [
+            'state_estimator = rov_navigation.state_estimator:main',
+            'depth_node = rov_navigation.depth_node:main',
+        ],
+    },
+)

src/rov_perception/config/cameras.yaml

@@ -0,0 +1,8 @@
+# Camera configuration
+# Update rtsp_url values to match your IP cameras
+
+camera_node:
+  ros__parameters:
+    rtsp_url: "rtsp://192.168.2.100:554/stream1"
+    frame_id: "camera_front"
+    fps: 15

src/rov_perception/launch/perception.launch.py

@@ -0,0 +1,27 @@
+"""Perception stack launch file."""
+
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory
+import os
+
+
+def generate_launch_description():
+    pkg_dir = get_package_share_directory('rov_perception')
+    cam_config = os.path.join(pkg_dir, 'config', 'cameras.yaml')
+
+    return LaunchDescription([
+        Node(
+            package='rov_perception',
+            executable='camera_node',
+            name='camera_node',
+            output='screen',
+            parameters=[cam_config],
+        ),
+        Node(
+            package='rov_perception',
+            executable='feature_detector',
+            name='feature_detector',
+            output='screen',
+        ),
+    ])

src/rov_perception/package.xml

@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>rov_perception</name>
+  <version>0.1.0</version>
+  <description>Vision pipeline and feature detection for ROV inspection</description>
+  <maintainer email="grant@symbytech.com">Grant</maintainer>
+  <license>MIT</license>
+  <buildtool_depend>ament_python</buildtool_depend>
+  <depend>rclpy</depend>
+  <depend>sensor_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>cv_bridge</depend>
+  <depend>image_transport</depend>
+  <depend>rov_interfaces</depend>
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

src/rov_perception/rov_perception/camera_node.py

@@ -0,0 +1,63 @@
+#!/usr/bin/env python3
+"""
+Camera node.
+Pulls RTSP streams from IP cameras and publishes as ROS2 image topics.
+
+Topics published:
+  /camera/front/image_raw    (sensor_msgs/Image)
+  /camera/front/camera_info  (sensor_msgs/CameraInfo)
+
+Parameters:
+  rtsp_url    — RTSP stream URL
+  frame_id    — camera frame name for TF
+  fps         — target capture framerate
+"""
+
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import Image, CameraInfo
+from cv_bridge import CvBridge
+
+
+class CameraNode(Node):
+    """Captures RTSP stream and publishes as ROS2 image topic."""
+
+    def __init__(self):
+        super().__init__('camera_node')
+
+        # --- Parameters ---
+        self.declare_parameter('rtsp_url', 'rtsp://192.168.1.100:554/stream')
+        self.declare_parameter('frame_id', 'camera_front')
+        self.declare_parameter('fps', 15)
+
+        self.rtsp_url = self.get_parameter('rtsp_url').value
+        self.frame_id = self.get_parameter('frame_id').value
+        self.fps = self.get_parameter('fps').value
+
+        # --- Publishers ---
+        self.image_pub = self.create_publisher(Image, '/camera/front/image_raw', 10)
+        self.info_pub = self.create_publisher(CameraInfo, '/camera/front/camera_info', 10)
+
+        self.bridge = CvBridge()
+
+        # TODO: initialise OpenCV VideoCapture with rtsp_url
+        # TODO: create timer at self.fps rate to capture and publish frames
+
+        self.get_logger().info(f'CameraNode started — stream: {self.rtsp_url}')
+
+    def capture_and_publish(self):
+        """Capture one frame and publish."""
+        # TODO: implement frame capture and publish
+        pass
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = CameraNode()
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

src/rov_perception/rov_perception/feature_detector.py

@@ -0,0 +1,48 @@
+#!/usr/bin/env python3
+"""
+Feature detector node.
+Detects inspection-relevant features (corrosion, marine growth, damage) in camera frames.
+Phase 5 implementation — stub only for Phase 1.
+
+Topics subscribed:
+  /camera/front/image_raw  (sensor_msgs/Image)
+
+Topics published:
+  /perception/detections   (std_msgs/String)  — JSON detection results (placeholder)
+"""
+
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import Image
+from std_msgs.msg import String
+
+
+class FeatureDetector(Node):
+    """Detects features in camera images for inspection reporting."""
+
+    def __init__(self):
+        super().__init__('feature_detector')
+
+        self.sub = self.create_subscription(
+            Image, '/camera/front/image_raw', self.image_callback, 10)
+
+        self.pub = self.create_publisher(String, '/perception/detections', 10)
+
+        self.get_logger().info('FeatureDetector node started (stub)')
+
+    def image_callback(self, msg: Image):
+        """Process incoming image frame."""
+        # TODO Phase 5: run inference, publish detections
+        pass
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = FeatureDetector()
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

src/rov_perception/setup.py

@@ -0,0 +1,25 @@
+from setuptools import setup
+import os
+from glob import glob
+
+package_name = 'rov_perception'
+
+setup(
+    name=package_name,
+    version='0.1.0',
+    packages=[package_name],
+    data_files=[
+        ('share/ament_index/resource_index/packages', ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+        (os.path.join('share', package_name, 'launch'), glob('launch/*.py')),
+        (os.path.join('share', package_name, 'config'), glob('config/*.yaml')),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    entry_points={
+        'console_scripts': [
+            'camera_node = rov_perception.camera_node:main',
+            'feature_detector = rov_perception.feature_detector:main',
+        ],
+    },
+)