From 9b03548533426f607403aed96a09dcd746692c9f Mon Sep 17 00:00:00 2001 From: Grant Date: Sun, 3 May 2026 09:26:04 +0000 Subject: [PATCH] =?UTF-8?q?Initial=20workspace=20=E2=80=94=20six-package?= =?UTF-8?q?=20autonomy=20stack?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .gitignore | 23 +++ Dockerfile | 45 +++++ README.md | 46 +++++ docker-entrypoint.sh | 13 ++ src/rov_bringup/CMakeLists.txt | 5 + src/rov_bringup/launch/rov_dev.launch.py | 37 ++++ src/rov_bringup/launch/rov_full.launch.py | 82 ++++++++ src/rov_bringup/package.xml | 17 ++ src/rov_control/config/failsafe.yaml | 8 + src/rov_control/launch/control.launch.py | 29 +++ src/rov_control/package.xml | 17 ++ src/rov_control/resource/rov_control | 0 src/rov_control/rov_control/__init__.py | 0 .../rov_control/failsafe_monitor.py | 135 +++++++++++++ .../rov_control/motion_controller.py | 91 +++++++++ src/rov_control/setup.py | 25 +++ src/rov_interfaces/CMakeLists.txt | 16 ++ src/rov_interfaces/msg/FailsafeStatus.msg | 12 ++ src/rov_interfaces/msg/InspectionWaypoint.msg | 8 + src/rov_interfaces/msg/MissionStatus.msg | 11 ++ src/rov_interfaces/package.xml | 17 ++ src/rov_interfaces/srv/MissionCommand.srv | 6 + src/rov_mission/config/dev.yaml | 8 + src/rov_mission/config/field.yaml | 8 + src/rov_mission/launch/mission.launch.py | 30 +++ src/rov_mission/package.xml | 16 ++ src/rov_mission/resource/rov_mission | 0 src/rov_mission/rov_mission/__init__.py | 0 .../rov_mission/mission_executor.py | 183 ++++++++++++++++++ src/rov_mission/setup.py | 24 +++ src/rov_navigation/config/ekf.yaml | 42 ++++ .../launch/navigation.launch.py | 43 ++++ src/rov_navigation/package.xml | 19 ++ src/rov_navigation/resource/rov_navigation | 0 src/rov_navigation/rov_navigation/__init__.py | 0 .../rov_navigation/depth_node.py | 48 +++++ .../rov_navigation/state_estimator.py | 66 +++++++ src/rov_navigation/setup.py | 25 +++ src/rov_perception/config/cameras.yaml | 8 + .../launch/perception.launch.py | 27 +++ src/rov_perception/package.xml | 18 ++ src/rov_perception/resource/rov_perception | 0 src/rov_perception/rov_perception/__init__.py | 0 .../rov_perception/camera_node.py | 63 ++++++ .../rov_perception/feature_detector.py | 48 +++++ src/rov_perception/setup.py | 25 +++ 46 files changed, 1344 insertions(+) create mode 100644 .gitignore create mode 100644 Dockerfile create mode 100644 README.md create mode 100644 docker-entrypoint.sh create mode 100644 src/rov_bringup/CMakeLists.txt create mode 100644 src/rov_bringup/launch/rov_dev.launch.py create mode 100644 src/rov_bringup/launch/rov_full.launch.py create mode 100644 src/rov_bringup/package.xml create mode 100644 src/rov_control/config/failsafe.yaml create mode 100644 src/rov_control/launch/control.launch.py create mode 100644 src/rov_control/package.xml create mode 100644 src/rov_control/resource/rov_control create mode 100644 src/rov_control/rov_control/__init__.py create mode 100644 src/rov_control/rov_control/failsafe_monitor.py create mode 100644 src/rov_control/rov_control/motion_controller.py create mode 100644 src/rov_control/setup.py create mode 100644 src/rov_interfaces/CMakeLists.txt create mode 100644 src/rov_interfaces/msg/FailsafeStatus.msg create mode 100644 src/rov_interfaces/msg/InspectionWaypoint.msg create mode 100644 src/rov_interfaces/msg/MissionStatus.msg create mode 100644 src/rov_interfaces/package.xml create mode 100644 src/rov_interfaces/srv/MissionCommand.srv create mode 100644 src/rov_mission/config/dev.yaml create mode 100644 src/rov_mission/config/field.yaml create mode 100644 src/rov_mission/launch/mission.launch.py create mode 100644 src/rov_mission/package.xml create mode 100644 src/rov_mission/resource/rov_mission create mode 100644 src/rov_mission/rov_mission/__init__.py create mode 100644 src/rov_mission/rov_mission/mission_executor.py create mode 100644 src/rov_mission/setup.py create mode 100644 src/rov_navigation/config/ekf.yaml create mode 100644 src/rov_navigation/launch/navigation.launch.py create mode 100644 src/rov_navigation/package.xml create mode 100644 src/rov_navigation/resource/rov_navigation create mode 100644 src/rov_navigation/rov_navigation/__init__.py create mode 100644 src/rov_navigation/rov_navigation/depth_node.py create mode 100644 src/rov_navigation/rov_navigation/state_estimator.py create mode 100644 src/rov_navigation/setup.py create mode 100644 src/rov_perception/config/cameras.yaml create mode 100644 src/rov_perception/launch/perception.launch.py create mode 100644 src/rov_perception/package.xml create mode 100644 src/rov_perception/resource/rov_perception create mode 100644 src/rov_perception/rov_perception/__init__.py create mode 100644 src/rov_perception/rov_perception/camera_node.py create mode 100644 src/rov_perception/rov_perception/feature_detector.py create mode 100644 src/rov_perception/setup.py diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..c627481 --- /dev/null +++ b/.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 diff --git a/Dockerfile b/Dockerfile new file mode 100644 index 0000000..2d93c75 --- /dev/null +++ b/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"] diff --git a/README.md b/README.md new file mode 100644 index 0000000..cc38c38 --- /dev/null +++ b/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:= +``` + +## 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 | diff --git a/docker-entrypoint.sh b/docker-entrypoint.sh new file mode 100644 index 0000000..d14e202 --- /dev/null +++ b/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 "$@" diff --git a/src/rov_bringup/CMakeLists.txt b/src/rov_bringup/CMakeLists.txt new file mode 100644 index 0000000..b96265f --- /dev/null +++ b/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() diff --git a/src/rov_bringup/launch/rov_dev.launch.py b/src/rov_bringup/launch/rov_dev.launch.py new file mode 100644 index 0000000..c820d90 --- /dev/null +++ b/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', + ), + ]) diff --git a/src/rov_bringup/launch/rov_full.launch.py b/src/rov_bringup/launch/rov_full.launch.py new file mode 100644 index 0000000..4ce5d8a --- /dev/null +++ b/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') + ), + ), + ]) diff --git a/src/rov_bringup/package.xml b/src/rov_bringup/package.xml new file mode 100644 index 0000000..d983ca1 --- /dev/null +++ b/src/rov_bringup/package.xml @@ -0,0 +1,17 @@ + + + rov_bringup + 0.1.0 + Top-level bringup launch files for full ROV autonomy stack + Grant + MIT + ament_cmake + rov_navigation + rov_perception + rov_control + rov_mission + mavros + + ament_cmake + + diff --git a/src/rov_control/config/failsafe.yaml b/src/rov_control/config/failsafe.yaml new file mode 100644 index 0000000..de5a1f6 --- /dev/null +++ b/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 diff --git a/src/rov_control/launch/control.launch.py b/src/rov_control/launch/control.launch.py new file mode 100644 index 0000000..84a7ca0 --- /dev/null +++ b/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', + ), + ]) diff --git a/src/rov_control/package.xml b/src/rov_control/package.xml new file mode 100644 index 0000000..1a028fd --- /dev/null +++ b/src/rov_control/package.xml @@ -0,0 +1,17 @@ + + + rov_control + 0.1.0 + Motion control, depth hold, and thruster management + Grant + MIT + ament_python + rclpy + std_msgs + geometry_msgs + mavros_msgs + rov_interfaces + + ament_python + + diff --git a/src/rov_control/resource/rov_control b/src/rov_control/resource/rov_control new file mode 100644 index 0000000..e69de29 diff --git a/src/rov_control/rov_control/__init__.py b/src/rov_control/rov_control/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/rov_control/rov_control/failsafe_monitor.py b/src/rov_control/rov_control/failsafe_monitor.py new file mode 100644 index 0000000..ca4a7c5 --- /dev/null +++ b/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() diff --git a/src/rov_control/rov_control/motion_controller.py b/src/rov_control/rov_control/motion_controller.py new file mode 100644 index 0000000..4c29303 --- /dev/null +++ b/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() diff --git a/src/rov_control/setup.py b/src/rov_control/setup.py new file mode 100644 index 0000000..8bc23fa --- /dev/null +++ b/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', + ], + }, +) diff --git a/src/rov_interfaces/CMakeLists.txt b/src/rov_interfaces/CMakeLists.txt new file mode 100644 index 0000000..5f73af1 --- /dev/null +++ b/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() diff --git a/src/rov_interfaces/msg/FailsafeStatus.msg b/src/rov_interfaces/msg/FailsafeStatus.msg new file mode 100644 index 0000000..03e41ee --- /dev/null +++ b/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 diff --git a/src/rov_interfaces/msg/InspectionWaypoint.msg b/src/rov_interfaces/msg/InspectionWaypoint.msg new file mode 100644 index 0000000..692b3aa --- /dev/null +++ b/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 diff --git a/src/rov_interfaces/msg/MissionStatus.msg b/src/rov_interfaces/msg/MissionStatus.msg new file mode 100644 index 0000000..bcc47ef --- /dev/null +++ b/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 diff --git a/src/rov_interfaces/package.xml b/src/rov_interfaces/package.xml new file mode 100644 index 0000000..b215a38 --- /dev/null +++ b/src/rov_interfaces/package.xml @@ -0,0 +1,17 @@ + + + rov_interfaces + 0.1.0 + Custom ROS2 messages and services for ROV autonomy stack + Grant + MIT + ament_cmake + std_msgs + geometry_msgs + rosidl_default_generators + rosidl_interface_packages + rosidl_default_runtime + + ament_cmake + + diff --git a/src/rov_interfaces/srv/MissionCommand.srv b/src/rov_interfaces/srv/MissionCommand.srv new file mode 100644 index 0000000..aeca77d --- /dev/null +++ b/src/rov_interfaces/srv/MissionCommand.srv @@ -0,0 +1,6 @@ +string command +string mission_id +string[] parameters +--- +bool success +string message diff --git a/src/rov_mission/config/dev.yaml b/src/rov_mission/config/dev.yaml new file mode 100644 index 0000000..0e24c67 --- /dev/null +++ b/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 diff --git a/src/rov_mission/config/field.yaml b/src/rov_mission/config/field.yaml new file mode 100644 index 0000000..349ffac --- /dev/null +++ b/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 diff --git a/src/rov_mission/launch/mission.launch.py b/src/rov_mission/launch/mission.launch.py new file mode 100644 index 0000000..8cf33aa --- /dev/null +++ b/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 + ), + ]) diff --git a/src/rov_mission/package.xml b/src/rov_mission/package.xml new file mode 100644 index 0000000..d10c22f --- /dev/null +++ b/src/rov_mission/package.xml @@ -0,0 +1,16 @@ + + + rov_mission + 0.1.0 + Mission planning, waypoint execution, and inspection sequencing + Grant + MIT + ament_python + rclpy + std_msgs + geometry_msgs + rov_interfaces + + ament_python + + diff --git a/src/rov_mission/resource/rov_mission b/src/rov_mission/resource/rov_mission new file mode 100644 index 0000000..e69de29 diff --git a/src/rov_mission/rov_mission/__init__.py b/src/rov_mission/rov_mission/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/rov_mission/rov_mission/mission_executor.py b/src/rov_mission/rov_mission/mission_executor.py new file mode 100644 index 0000000..cdd4f24 --- /dev/null +++ b/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() diff --git a/src/rov_mission/setup.py b/src/rov_mission/setup.py new file mode 100644 index 0000000..fa09a91 --- /dev/null +++ b/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', + ], + }, +) diff --git a/src/rov_navigation/config/ekf.yaml b/src/rov_navigation/config/ekf.yaml new file mode 100644 index 0000000..8fc9a06 --- /dev/null +++ b/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] diff --git a/src/rov_navigation/launch/navigation.launch.py b/src/rov_navigation/launch/navigation.launch.py new file mode 100644 index 0000000..1cd9ed9 --- /dev/null +++ b/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', + ), + ]) diff --git a/src/rov_navigation/package.xml b/src/rov_navigation/package.xml new file mode 100644 index 0000000..2160bf3 --- /dev/null +++ b/src/rov_navigation/package.xml @@ -0,0 +1,19 @@ + + + rov_navigation + 0.1.0 + State estimation, EKF, and localisation for ROV autonomy + Grant + MIT + ament_python + rclpy + std_msgs + geometry_msgs + sensor_msgs + nav_msgs + robot_localization + rov_interfaces + + ament_python + + diff --git a/src/rov_navigation/resource/rov_navigation b/src/rov_navigation/resource/rov_navigation new file mode 100644 index 0000000..e69de29 diff --git a/src/rov_navigation/rov_navigation/__init__.py b/src/rov_navigation/rov_navigation/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/rov_navigation/rov_navigation/depth_node.py b/src/rov_navigation/rov_navigation/depth_node.py new file mode 100644 index 0000000..cf47411 --- /dev/null +++ b/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() diff --git a/src/rov_navigation/rov_navigation/state_estimator.py b/src/rov_navigation/rov_navigation/state_estimator.py new file mode 100644 index 0000000..96722f4 --- /dev/null +++ b/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() diff --git a/src/rov_navigation/setup.py b/src/rov_navigation/setup.py new file mode 100644 index 0000000..d95fa7a --- /dev/null +++ b/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', + ], + }, +) diff --git a/src/rov_perception/config/cameras.yaml b/src/rov_perception/config/cameras.yaml new file mode 100644 index 0000000..02a811a --- /dev/null +++ b/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 diff --git a/src/rov_perception/launch/perception.launch.py b/src/rov_perception/launch/perception.launch.py new file mode 100644 index 0000000..68f76e3 --- /dev/null +++ b/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', + ), + ]) diff --git a/src/rov_perception/package.xml b/src/rov_perception/package.xml new file mode 100644 index 0000000..bad483e --- /dev/null +++ b/src/rov_perception/package.xml @@ -0,0 +1,18 @@ + + + rov_perception + 0.1.0 + Vision pipeline and feature detection for ROV inspection + Grant + MIT + ament_python + rclpy + sensor_msgs + std_msgs + cv_bridge + image_transport + rov_interfaces + + ament_python + + diff --git a/src/rov_perception/resource/rov_perception b/src/rov_perception/resource/rov_perception new file mode 100644 index 0000000..e69de29 diff --git a/src/rov_perception/rov_perception/__init__.py b/src/rov_perception/rov_perception/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/rov_perception/rov_perception/camera_node.py b/src/rov_perception/rov_perception/camera_node.py new file mode 100644 index 0000000..9e12bde --- /dev/null +++ b/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() diff --git a/src/rov_perception/rov_perception/feature_detector.py b/src/rov_perception/rov_perception/feature_detector.py new file mode 100644 index 0000000..b26778b --- /dev/null +++ b/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() diff --git a/src/rov_perception/setup.py b/src/rov_perception/setup.py new file mode 100644 index 0000000..7542592 --- /dev/null +++ b/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', + ], + }, +)