Code Examples

Connect to the vehicle, arm, and execute basic motion commands.

#!/usr/bin/env python3
"""Basic vehicle control example for Mini-UUV"""

from mini_uuv import UUVehicle
import time

# Initialize connection
vehicle = UUVehicle(port="/dev/ttyUSB0")

try:
    # Arm the vehicle
    if vehicle.arm():
        print("Vehicle armed successfully")

        # Set depth hold mode
        vehicle.set_mode(UUVehicle.MODE_DEPTH_HOLD)

        # Descend to 5 meters
        vehicle.set_depth_target(5.0)
        time.sleep(10)

        # Move forward at 0.5 m/s
        vehicle.set_velocity(forward=0.5, lateral=0.0, vertical=0.0)
        time.sleep(5)

        # Stop and surface
        vehicle.set_velocity(0, 0, 0)
        vehicle.set_depth_target(0)

finally:
    vehicle.disarm()
    vehicle.close()

Read and log sensor data to CSV file for later analysis.

#!/usr/bin/env python3
"""Sensor data logging example"""

from mini_uuv.sensors import IMU, DVL, DepthSensor
import csv
import time

# Initialize sensors
imu = IMU(port="/dev/ttyIMU")
dvl = DVL(port="/dev/ttyDVL")
depth = DepthSensor(i2c_addr=0x76)

# Open CSV file for logging
with open('sensor_log.csv', 'w', newline='') as f:
    writer = csv.writer(f)
    writer.writerow(['timestamp', 'ax', 'ay', 'az', 'gx', 'gy', 'gz',
                     'vx', 'vy', 'vz', 'depth', 'altitude'])

    start_time = time.time()
    while time.time() - start_time < 60:  # Log for 60 seconds
        imu_data = imu.read()
        dvl_data = dvl.read()
        depth_val = depth.read_depth()

        writer.writerow([
            time.time() - start_time,
            imu_data.accel.x, imu_data.accel.y, imu_data.accel.z,
            imu_data.gyro.x, imu_data.gyro.y, imu_data.gyro.z,
            dvl_data.velocity.x, dvl_data.velocity.y, dvl_data.velocity.z,
            depth_val, dvl_data.altitude
        ])

        time.sleep(0.02)  # 50 Hz

print("Logging complete!")

Execute an autonomous waypoint mission with position feedback.

#!/usr/bin/env python3
"""Waypoint mission execution example"""

from mini_uuv import UUVehicle
from mini_uuv.mission import WaypointMission, Waypoint
import numpy as np

# Define waypoints (x, y, depth in meters)
waypoints = [
    Waypoint(10, 0, 5, speed=1.0),
    Waypoint(10, 10, 5, speed=0.8),
    Waypoint(0, 10, 3, speed=1.0),
    Waypoint(0, 0, 0, speed=0.5)  # Return to start
]

# Create mission
mission = WaypointMission(waypoints, acceptance_radius=1.0)

# Initialize vehicle
vehicle = UUVehicle(port="/dev/ttyUSB0")
vehicle.arm()
vehicle.set_mode(UUVehicle.MODE_WAYPOINT)

# Execute mission
for wp in mission:
    print(f"Navigating to waypoint: {wp}")
    vehicle.goto(wp.x, wp.y, wp.depth, speed=wp.speed)

    # Wait until waypoint reached
    while not vehicle.waypoint_reached(acceptance_radius=1.0):
        state = vehicle.get_state()
        dist = np.sqrt((state.x - wp.x)**2 + (state.y - wp.y)**2)
        print(f"  Distance to waypoint: {dist:.2f}m")
        time.sleep(1)

    print(f"Waypoint reached!")

vehicle.disarm()
print("Mission complete!")

Real-time sensor fusion using Extended Kalman Filter.

#!/usr/bin/env python3
"""EKF navigation with sensor fusion"""

from mini_uuv.navigation import NavigationEKF
from mini_uuv.sensors import IMU, DVL, DepthSensor
import numpy as np
import time

# Initialize sensors
imu = IMU(port="/dev/ttyIMU")
dvl = DVL(port="/dev/ttyDVL")
depth = DepthSensor()

# Initialize EKF
ekf = NavigationEKF(
    initial_position=[0, 0, 0],
    process_noise=np.diag([0.01, 0.01, 0.01, 0.001, 0.001, 0.001]),
    measurement_noise={
        'dvl': np.diag([0.01, 0.01, 0.01]),
        'depth': 0.02,
        'heading': 0.05
    }
)

dt = 0.02  # 50 Hz
last_time = time.time()

while True:
    current_time = time.time()
    dt = current_time - last_time
    last_time = current_time

    # Read sensors
    imu_data = imu.read()

    # Prediction step with IMU
    ekf.predict(dt, imu_data.accel, imu_data.gyro)

    # Update with DVL (when valid)
    if dvl.is_valid():
        dvl_data = dvl.read()
        ekf.update_dvl(dvl_data.velocity)

    # Update with depth
    depth_val = depth.read_depth()
    ekf.update_depth(depth_val)

    # Get estimated state
    pos, vel, att = ekf.get_state()
    print(f"Position: ({pos[0]:.2f}, {pos[1]:.2f}, {pos[2]:.2f})")

    time.sleep(dt)

Basic ROS2 node for UUV control with publishers and subscribers.

#!/usr/bin/env python3
"""ROS2 node for UUV control"""

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist, PoseStamped
from std_msgs.msg import Float64
from sensor_msgs.msg import Imu

class UUVControlNode(Node):
    def __init__(self):
        super().__init__('uuv_control_node')

        # Publishers
        self.cmd_pub = self.create_publisher(Twist, '/uuv/cmd_vel', 10)
        self.depth_pub = self.create_publisher(Float64, '/uuv/depth_setpoint', 10)

        # Subscribers
        self.pose_sub = self.create_subscription(
            PoseStamped, '/uuv/pose', self.pose_callback, 10)
        self.imu_sub = self.create_subscription(
            Imu, '/uuv/imu', self.imu_callback, 10)

        # Timer for control loop
        self.timer = self.create_timer(0.02, self.control_loop)  # 50 Hz

        self.current_depth = 0.0
        self.target_depth = 5.0

    def pose_callback(self, msg):
        self.current_depth = -msg.pose.position.z  # NED frame

    def imu_callback(self, msg):
        # Process IMU data
        pass

    def control_loop(self):
        # Simple depth control
        depth_error = self.target_depth - self.current_depth

        cmd = Twist()
        cmd.linear.z = max(-1.0, min(1.0, depth_error * 0.5))
        self.cmd_pub.publish(cmd)

def main(args=None):
    rclpy.init(args=args)
    node = UUVControlNode()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

"""ROS2 launch file for Mini-UUV system"""

from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    return LaunchDescription([
        # Navigation node
        Node(
            package='mini_uuv_navigation',
            executable='navigation_node',
            name='navigation',
            parameters=[{'use_sim_time': False}]
        ),
        # Controller node
        Node(
            package='mini_uuv_control',
            executable='controller_node',
            name='controller',
            parameters=[{
                'depth_kp': 2.0,
                'depth_ki': 0.5,
                'depth_kd': 1.0,
                'heading_kp': 1.5,
                'heading_ki': 0.2,
                'heading_kd': 0.8
            }]
        ),
        # Thruster driver
        Node(
            package='mini_uuv_drivers',
            executable='thruster_driver',
            name='thrusters',
            parameters=[{'port': '/dev/ttyThruster'}]
        ),
        # Sensor nodes
        Node(
            package='mini_uuv_drivers',
            executable='imu_driver',
            name='imu'
        ),
        Node(
            package='mini_uuv_drivers',
            executable='dvl_driver',
            name='dvl'
        ),
    ])