SirBlob/simulation
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Camera Sim Update

Browse Source
This commit is contained in:
SirBlobby
2026-02-12 14:29:32 -05:00
parent 0e427f3597
commit 92da41138b
27 changed files with 1353 additions and 1317 deletions
Download Patch File Download Diff File Expand all files Collapse all files

56
activate_venv.sh
View File

@@ -1,56 +0,0 @@
#!/bin/bash
# UAV-UGV Simulation - Environment Activation
# Usage: source activate_venv.sh
SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
# ROS 2 Setup
if [ -f "/opt/ros/jazzy/setup.bash" ]; then
source /opt/ros/jazzy/setup.bash
ROS_VER="jazzy"
elif [ -f "/opt/ros/humble/setup.bash" ]; then
source /opt/ros/humble/setup.bash
ROS_VER="humble"
else
echo "[WARN] ROS 2 not found in /opt/ros/"
ROS_VER="none"
fi
# ArduPilot Environment
export ARDUPILOT_HOME="$HOME/ardupilot"
export PATH="$ARDUPILOT_HOME/Tools/autotest:$PATH"
export PATH="$HOME/.local/bin:$PATH"
# Deactivate existing venv
if [ -n "$VIRTUAL_ENV" ]; then
deactivate 2>/dev/null || true
fi
# Activate project venv
if [ -f "$SCRIPT_DIR/venv/bin/activate" ]; then
source "$SCRIPT_DIR/venv/bin/activate"
fi
# Gazebo paths (new Gazebo - Ignition/Harmonic)
export GZ_SIM_RESOURCE_PATH="$SCRIPT_DIR/models:$SCRIPT_DIR/worlds:${GZ_SIM_RESOURCE_PATH:-}"
# ArduPilot Gazebo plugin
if [ -d "$HOME/ardupilot_gazebo/build" ]; then
export GZ_SIM_SYSTEM_PLUGIN_PATH="$HOME/ardupilot_gazebo/build:${GZ_SIM_SYSTEM_PLUGIN_PATH:-}"
export GZ_SIM_RESOURCE_PATH="$HOME/ardupilot_gazebo/models:$HOME/ardupilot_gazebo/worlds:$GZ_SIM_RESOURCE_PATH"
fi
# WSL environment
if grep -qEi "(microsoft|wsl)" /proc/version 2>/dev/null; then
if [ -d "/mnt/wslg" ]; then
export DISPLAY=:0
else
export DISPLAY=$(cat /etc/resolv.conf 2>/dev/null | grep nameserver | awk '{print $2}'):0
fi
export LIBGL_ALWAYS_INDIRECT=0
export MESA_GL_VERSION_OVERRIDE=3.3
fi
echo -e "\033[0;32mEnvironment activated (ROS 2 $ROS_VER)\033[0m"
echo ""
echo "Run simulation: bash scripts/run_simulation.sh"

6
config/ardupilot_gps_denied.parm
View File

@@ -70,3 +70,9 @@ FS_GCS_ENABLE 0
# ====================
LOG_BITMASK 176126
LOG_DISARMED 0
# ====================
# Landing Speed
# ====================
LAND_SPEED 150 # Final descent cm/s (default 50)
LAND_SPEED_HIGH 250 # Initial descent cm/s (default 0=WPNAV_SPEED_DN)

76
config/geofence_params.yaml
View File

@@ -1,76 +0,0 @@
# Geofence Configuration
# GPS is ONLY used for safety boundaries, NOT navigation
/**:
ros__parameters:
geofence:
enabled: true
# Fence type
type: "polygon" # Options: "polygon", "circle", "cylinder"
# Polygon fence (GPS coordinates for safety boundary)
polygon_fence:
points:
- latitude: 47.397742
longitude: 8.545594
- latitude: 47.398242
longitude: 8.545594
- latitude: 47.398242
longitude: 8.546094
- latitude: 47.397742
longitude: 8.546094
# Close the polygon automatically
auto_close: true
# Circular fence (alternative to polygon)
circle_fence:
center_latitude: 47.397742
center_longitude: 8.545594
radius_meters: 100
# Cylinder fence (circle + altitude limits)
cylinder_fence:
center_latitude: 47.397742
center_longitude: 8.545594
radius_meters: 100
min_altitude: 0 # meters AGL
max_altitude: 50 # meters AGL
# Altitude limits (applies to all fence types)
altitude_limits:
min_altitude: 0.0 # meters AGL
max_altitude: 50.0 # meters AGL
enable_floor: true
enable_ceiling: true
# Fence actions
actions:
on_breach: "RTL" # Options: "RTL", "LAND", "HOLD", "ALERT_ONLY"
warning_distance: 10.0 # meters from fence
warning_action: "ALERT" # Options: "ALERT", "SLOW_DOWN"
# Monitoring
check_rate: 10 # Hz
consecutive_breaches_required: 3 # Require 3 consecutive breaches before action
# Visualization
publish_markers: true
marker_topic: "/geofence/markers"
marker_color:
safe: {r: 0.0, g: 1.0, b: 0.0, a: 0.5}
warning: {r: 1.0, g: 1.0, b: 0.0, a: 0.7}
breach: {r: 1.0, g: 0.0, b: 0.0, a: 0.9}
# Multi-vehicle support
vehicles:
uav:
enabled: true
namespace: "/uav"
gps_topic: "/uav/mavros/global_position/global"
ugv:
enabled: true
namespace: "/ugv"
gps_topic: "/ugv/gps/fix"

19
config/gui.config Normal file
View File

@@ -0,0 +1,19 @@
<!-- Camera feed plugins (added via --gui-config on top of defaults) -->
<plugin filename="ImageDisplay" name="Gimbal Camera">
<gz-gui>
<title>Gimbal Camera</title>
<property type="string" key="state">docked</property>
</gz-gui>
<topic>/world/uav_ugv_search/model/iris_with_gimbal/model/gimbal/link/pitch_link/sensor/camera/image</topic>
<topic_picker>true</topic_picker>
</plugin>
<plugin filename="ImageDisplay" name="Downward Camera">
<gz-gui>
<title>Downward Camera</title>
<property type="string" key="state">docked</property>
</gz-gui>
<topic>/uav/camera/downward</topic>
<topic_picker>true</topic_picker>
</plugin>

72
config/mavros_params.yaml
View File

@@ -1,72 +0,0 @@
# MAVROS Configuration for GPS-Denied Navigation
# GPS is DISABLED for navigation, ENABLED only for geofencing
/**:
ros__parameters:
use_sim_time: true
# Connection
fcu_protocol: "v2.0"
# System
system_id: 1
component_id: 1
# Time sync
time:
time_ref_source: "fcu"
timesync_mode: MAVLINK
timesync_avg_alpha: 0.6
# GPS Settings - ONLY for geofencing
gps:
# Disable GPS for navigation
use_gps_for_navigation: false
# Enable GPS only for geofence
enable_gps_geofence: true
# Local position (PRIMARY navigation source)
local_position:
frame_id: "map"
tf:
send: true
frame_id: "map"
child_frame_id: "base_link"
# Use local odometry, NOT GPS
use_vision: true # Will use our visual odometry
# Vision pose input (from visual odometry)
vision_pose:
tf:
listen: false
frame_id: "odom"
child_frame_id: "base_link"
# Vision speed input (from optical flow)
vision_speed:
listen_twist: true
# Setpoint configuration
setpoint_raw:
thrust_scaling: 1.0
setpoint_position:
tf:
listen: false
# Use LOCAL_NED frame (relative positioning)
mav_frame: 1 # MAV_FRAME_LOCAL_NED
setpoint_velocity:
mav_frame: 8 # MAV_FRAME_BODY_NED (body-relative)
# Disable GPS-based modes
# Only allow: MANUAL, STABILIZE, ALT_HOLD, GUIDED (local), LOITER (local)
allowed_modes:
- "MANUAL"
- "STABILIZE"
- "ALT_HOLD"
- "GUIDED"
- "LOITER"
- "RTL" # Returns to LOCAL origin, not GPS home

12
config/missions/hover.yaml Normal file
View File

@@ -0,0 +1,12 @@
name: hover
description: Take off and hold position
altitude: 5.0
duration: 10.0
steps:
- action: takeoff
altitude: 5.0
- action: hover
duration: 10.0
- action: land

21
config/missions/search.yaml Normal file
View File

@@ -0,0 +1,21 @@
name: search
description: Spiral search pattern to find and land on UGV
altitude: 5.0
search:
pattern: spiral
initial_leg: 3.0
leg_increment: 2.0
max_legs: 12
detection_radius: 2.0
steps:
- action: takeoff
altitude: 5.0
- action: search_spiral
initial_leg: 3.0
leg_increment: 2.0
max_legs: 12
- action: land_on_ugv
detection_radius: 2.0

26
config/missions/square.yaml Normal file
View File

@@ -0,0 +1,26 @@
name: square
description: Fly a square pattern
altitude: 5.0
side_length: 5.0
steps:
- action: takeoff
altitude: 5.0
- action: move_rel
x: 5.0
y: 0.0
z: 0.0
- action: move_rel
x: 0.0
y: 5.0
z: 0.0
- action: move_rel
x: -5.0
y: 0.0
z: 0.0
- action: move_rel
x: 0.0
y: -5.0
z: 0.0
- action: land

36
config/properties.yaml Normal file
View File

@@ -0,0 +1,36 @@
simulation:
world: uav_ugv_search.sdf
software_render: auto
physics:
max_step_size: 0.002
real_time_factor: 1.0
sensor_noise:
enabled: true
position_stddev: 0.05
velocity_stddev: 0.1
gyro_stddev: 0.0002
accel_stddev: 0.017
gps:
enabled: true
update_rate: 5
noise_stddev: 2.0
purpose: geofence_only
initial_positions:
uav:
x: 0.0
y: 0.0
z: 0.195
yaw: 90.0
ugv:
x: 5.0
y: 5.0
z: 0.0
yaw: 0.0
logging:
enabled: true
log_directory: logs

86
config/simulation_config.yaml
View File

@@ -1,86 +0,0 @@
# General Simulation Configuration
/**:
ros__parameters:
simulation:
# Use Gazebo ground truth for "perfect" position estimation
# In real world, this would come from visual odometry
use_gazebo_ground_truth: true # Set false for realistic VO errors
# Add noise to simulate real sensors
add_sensor_noise: true
# Noise parameters
noise:
position_stddev: 0.05 # meters
velocity_stddev: 0.1 # m/s
imu_gyro_stddev: 0.0002 # rad/s
imu_accel_stddev: 0.017 # m/s^2
# GPS simulation (for geofence only)
gps:
enabled: true # Only for geofencing
update_rate: 5 # Hz
noise_stddev: 2.0 # meters (realistic GPS error)
# Real-time factor (1.0 = real-time)
real_time_factor: 1.0
# Physics settings
physics:
engine: "ode"
max_step_size: 0.001
real_time_update_rate: 1000
# World configuration
world:
name: "empty_custom"
gravity: 9.81
# Lighting (affects vision)
lighting:
ambient_level: 0.4
shadows: true
sun_angle: 45.0 # degrees
# Ground plane
ground:
size: [100.0, 100.0] # meters
texture: "ground_plane"
friction: 1.0
# Initial positions (LOCAL coordinates)
initial_positions:
uav:
x: 0.0
y: 0.0
z: 0.2 # Start slightly above ground
roll: 0.0
pitch: 0.0
yaw: 0.0
ugv:
x: 5.0
y: 0.0
z: 0.0
roll: 0.0
pitch: 0.0
yaw: 1.57 # 90 degrees (facing +Y)
# Visualization
visualization:
show_trajectory: true
trajectory_length: 1000 # points
show_local_frame: true
show_velocity_vector: true
# Data logging
logging:
enabled: true
log_directory: "~/.ros/uav_ugv_logs"
log_topics:
- "/uav/visual_odometry/pose"
- "/uav/mavros/local_position/pose"
- "/geofence/status"
bag_recording: false

74
config/uav.yaml Normal file
View File

@@ -0,0 +1,74 @@
connection:
sim: "tcp:127.0.0.1:5760"
real: "/dev/ttyAMA0"
baud: 57600
flight:
takeoff_altitude: 5.0
max_altitude: 15.0
min_altitude: 3.0
max_velocity: 2.0
max_acceleration: 1.0
max_climb_rate: 1.0
max_yaw_rate: 45.0
navigation:
frame: LOCAL_NED
waypoint_radius: 0.5
position_hold_radius: 0.2
home_mode: local
vision:
forward_camera:
enabled: true
frame_rate: 30
resolution: [640, 480]
downward_camera:
enabled: true
frame_rate: 30
resolution: [320, 240]
visual_odometry:
enabled: true
method: ORB
min_features: 100
max_features: 500
optical_flow:
enabled: true
method: Lucas-Kanade
window_size: 15
min_altitude: 0.3
max_altitude: 10.0
landmark_detection:
enabled: true
method: ArUco
marker_size: 0.15
position_estimation:
method: EKF
update_rate: 50
weights:
visual_odometry: 0.6
optical_flow: 0.3
imu: 0.1
process_noise:
position: 0.1
velocity: 0.5
measurement_noise:
visual_odom: 0.05
optical_flow: 0.1
imu: 0.2
obstacle_avoidance:
enabled: true
detection_range: 5.0
safety_margin: 1.0
safety:
geofence:
enabled: true
action_on_breach: RTL
max_altitude: 10
radius: 20
failsafe:
vision_loss_timeout: 5.0
action_on_vision_loss: HOLD

105
config/uav_params.yaml
View File

@@ -1,105 +0,0 @@
# UAV Configuration - GPS-Denied Navigation
/**:
ros__parameters:
# Vision parameters for navigation
vision:
# Forward camera (for visual odometry)
forward_camera:
enabled: true
topic: "/uav/camera/forward/image_raw"
info_topic: "/uav/camera/forward/camera_info"
frame_rate: 30
# Downward camera (for optical flow)
downward_camera:
enabled: true
topic: "/uav/camera/downward/image_raw"
info_topic: "/uav/camera/downward/camera_info"
frame_rate: 30
# Visual odometry settings
visual_odometry:
enabled: true
method: "ORB" # Options: ORB, SIFT, SURF
min_features: 100
max_features: 500
feature_quality: 0.01
min_distance: 10
# Optical flow settings
optical_flow:
enabled: true
method: "Lucas-Kanade"
window_size: 15
max_level: 3
min_altitude: 0.3 # meters
max_altitude: 10.0 # meters
# Landmark detection
landmarks:
enabled: true
detection_method: "ArUco" # Options: ArUco, AprilTag, ORB
marker_size: 0.15 # meters
# Position estimation (sensor fusion)
position_estimator:
fusion_method: "EKF" # Extended Kalman Filter
# Sensor weights (trust levels)
weights:
visual_odometry: 0.6
optical_flow: 0.3
imu: 0.1
# Update rates
update_rate: 50 # Hz
# Covariance matrices
process_noise:
position: 0.1
velocity: 0.5
measurement_noise:
visual_odom: 0.05
optical_flow: 0.1
imu: 0.2
# Navigation parameters (RELATIVE coordinates only)
navigation:
frame: "LOCAL_NED" # Never use GPS frame
# Control parameters
max_velocity: 2.0 # m/s
max_acceleration: 1.0 # m/s^2
max_climb_rate: 1.0 # m/s
# Waypoint following
waypoint_radius: 0.5 # meters
position_hold_radius: 0.2 # meters
# Obstacle avoidance (vision-based)
obstacle_avoidance:
enabled: true
detection_range: 5.0 # meters
safety_margin: 1.0 # meters
# Mission parameters
mission:
takeoff_altitude: 5.0 # meters (relative)
loiter_radius: 2.0 # meters
rtl_altitude: 10.0 # meters (relative)
# Home position is LOCAL (0,0,0), not GPS
home_mode: "local" # Never "gps"
# Safety (geofencing uses GPS, but navigation doesn't)
safety:
geofence:
enabled: true
use_gps: true # ONLY for geofence
action_on_breach: "RTL" # Return to LOCAL origin
failsafe:
vision_loss_timeout: 5.0 # seconds
action_on_vision_loss: "HOLD" # or "RTL" or "LAND"

46
config/ugv.yaml Normal file
View File

@@ -0,0 +1,46 @@
vehicle:
wheelbase: 0.3
track_width: 0.25
wheel_radius: 0.05
connection:
sim: null
real: null
position:
x: 5.0
y: 5.0
z: 0.0
yaw: 0.0
navigation:
max_linear_velocity: 1.0
max_angular_velocity: 1.5
linear_acceleration: 0.5
angular_acceleration: 1.0
waypoint_radius: 0.3
position_tolerance: 0.3
landing_pad:
marker_type: ArUco
marker_id: 0
marker_size: 0.3
pad_diameter: 1.0
color: [255, 255, 0]
vision:
detection_method: ArUco
camera:
enabled: true
frame_rate: 30
resolution: [320, 240]
safety:
geofence:
enabled: true
action_on_breach: STOP
failsafe:
vision_loss_timeout: 3.0
action_on_vision_loss: STOP
collision_distance: 0.2
action_on_collision: STOP

110
config/ugv_params.yaml
View File

@@ -1,110 +0,0 @@
# UGV Configuration - GPS-Denied Navigation
/**:
ros__parameters:
# Vehicle physical parameters
vehicle:
wheelbase: 0.3 # meters
track_width: 0.25 # meters
wheel_radius: 0.05 # meters
max_speed: 1.0 # m/s
max_turn_rate: 1.5 # rad/s
# Vision parameters for navigation
vision:
# Forward camera (for visual odometry and obstacle detection)
forward_camera:
enabled: true
topic: "/ugv/camera/forward/image_raw"
info_topic: "/ugv/camera/forward/camera_info"
frame_rate: 30
fov_horizontal: 90 # degrees
fov_vertical: 60 # degrees
# Visual odometry settings
visual_odometry:
enabled: true
method: "ORB" # Options: ORB, SIFT
min_features: 150
max_features: 600
feature_quality: 0.02
min_distance: 8
# Wheel odometry (primary odometry for ground vehicle)
wheel_odometry:
enabled: true
topic: "/ugv/odom"
covariance_scale: 0.05
# Obstacle detection
obstacle_detection:
enabled: true
method: "depth_camera" # or "stereo"
min_distance: 0.3 # meters
max_distance: 10.0 # meters
# Position estimation (sensor fusion)
position_estimator:
fusion_method: "EKF"
# Sensor weights (trust levels)
weights:
visual_odometry: 0.3
wheel_odometry: 0.5
imu: 0.2
# Update rates
update_rate: 50 # Hz
# Covariance matrices
process_noise:
position: 0.05
velocity: 0.2
heading: 0.1
measurement_noise:
visual_odom: 0.08
wheel_odom: 0.03
imu: 0.15
# Navigation parameters (RELATIVE coordinates only)
navigation:
frame: "LOCAL_NED" # Never use GPS frame
# Control parameters
max_linear_velocity: 1.0 # m/s
max_angular_velocity: 1.5 # rad/s
linear_acceleration: 0.5 # m/s^2
angular_acceleration: 1.0 # rad/s^2
# Path following
lookahead_distance: 0.5 # meters
waypoint_radius: 0.3 # meters
# Obstacle avoidance
obstacle_avoidance:
enabled: true
detection_range: 3.0 # meters
safety_margin: 0.5 # meters
avoidance_method: "VFH" # Vector Field Histogram
# Mission parameters
mission:
default_speed: 0.5 # m/s
turn_in_place_threshold: 0.5 # radians
# Home position is LOCAL (0,0,0), not GPS
home_mode: "local"
# Safety
safety:
geofence:
enabled: true
use_gps: true # ONLY for geofence
action_on_breach: "STOP"
failsafe:
vision_loss_timeout: 3.0 # seconds
action_on_vision_loss: "STOP"
collision_distance: 0.2 # meters
action_on_collision: "STOP"

45
models/iris_with_gimbal/model.config Executable file
View File

@@ -0,0 +1,45 @@
<?xml version="1.0"?>
<model>
<name>Iris with Gimbal</name>
<version>2.0</version>
<sdf version="1.9">model.sdf</sdf>
<author>
<name>Fadri Furrer</name>
<email>fadri.furrer@mavt.ethz.ch</email>
</author>
<author>
<name>Michael Burri</name>
</author>
<author>
<name>Mina Kamel</name>
</author>
<author>
<name>Janosch Nikolic</name>
</author>
<author>
<name>Markus Achtelik</name>
</author>
<author>
<name>Rhys Mainwaring</name>
</author>
<description>
Starting with iris_with_standoffs
add LiftDragPlugin
add ArduCopterPlugin
add gimbal_small_2d
</description>
<depend>
<model>
<uri>model://gimbal_small_2d</uri>
<version>2.0</version>
</model>
<model>
<uri>model://iris_with_standoffs</uri>
<version>2.0</version>
</model>
</depend>
</model>

383
models/iris_with_gimbal/model.sdf Executable file
View File

@@ -0,0 +1,383 @@
<?xml version='1.0'?>
<sdf version="1.9">
<model name="iris_with_gimbal">
<include>
<uri>model://iris_with_standoffs</uri>
</include>
<include>
<uri>model://gimbal_small_3d</uri>
<name>gimbal</name>
<pose degrees="true">0 -0.01 -0.124923 90 0 90</pose>
</include>
<joint name="gimbal_joint" type="revolute">
<parent>iris_with_standoffs::base_link</parent>
<child>gimbal::base_link</child>
<axis>
<limit>
<lower>0</lower>
<upper>0</upper>
</limit>
<xyz>0 0 1</xyz>
</axis>
</joint>
<!-- Downward-facing camera for ArUco / landing pad detection -->
<link name="downward_cam_link">
<pose>0 0 -0.05 0 0 0</pose>
<inertial>
<mass>0.01</mass>
<inertia>
<ixx>0.00001</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.00001</iyy>
<iyz>0</iyz>
<izz>0.00001</izz>
</inertia>
</inertial>
<sensor name="downward_camera" type="camera">
<pose>0 0 0 0 1.5708 0</pose>
<always_on>1</always_on>
<update_rate>10</update_rate>
<visualize>1</visualize>
<topic>/uav/camera/downward</topic>
<camera>
<horizontal_fov>1.3962634</horizontal_fov>
<image>
<width>640</width>
<height>480</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.1</near>
<far>100</far>
</clip>
</camera>
</sensor>
</link>
<joint name="downward_cam_joint" type="fixed">
<parent>iris_with_standoffs::base_link</parent>
<child>downward_cam_link</child>
</joint>
<!-- plugins -->
<plugin filename="gz-sim-joint-state-publisher-system"
name="gz::sim::systems::JointStatePublisher">
</plugin>
<plugin filename="gz-sim-lift-drag-system"
name="gz::sim::systems::LiftDrag">
<a0>0.3</a0>
<alpha_stall>1.4</alpha_stall>
<cla>4.2500</cla>
<cda>0.10</cda>
<cma>0.0</cma>
<cla_stall>-0.025</cla_stall>
<cda_stall>0.0</cda_stall>
<cma_stall>0.0</cma_stall>
<area>0.002</area>
<air_density>1.2041</air_density>
<cp>0.084 0 0</cp>
<forward>0 1 0</forward>
<upward>0 0 1</upward>
<link_name>iris_with_standoffs::rotor_0</link_name>
</plugin>
<plugin filename="gz-sim-lift-drag-system"
name="gz::sim::systems::LiftDrag">
<a0>0.3</a0>
<alpha_stall>1.4</alpha_stall>
<cla>4.2500</cla>
<cda>0.10</cda>
<cma>0.0</cma>
<cla_stall>-0.025</cla_stall>
<cda_stall>0.0</cda_stall>
<cma_stall>0.0</cma_stall>
<area>0.002</area>
<air_density>1.2041</air_density>
<cp>-0.084 0 0</cp>
<forward>0 -1 0</forward>
<upward>0 0 1</upward>
<link_name>iris_with_standoffs::rotor_0</link_name>
</plugin>
<plugin filename="gz-sim-lift-drag-system"
name="gz::sim::systems::LiftDrag">
<a0>0.3</a0>
<alpha_stall>1.4</alpha_stall>
<cla>4.2500</cla>
<cda>0.10</cda>
<cma>0.0</cma>
<cla_stall>-0.025</cla_stall>
<cda_stall>0.0</cda_stall>
<cma_stall>0.0</cma_stall>
<area>0.002</area>
<air_density>1.2041</air_density>
<cp>0.084 0 0</cp>
<forward>0 1 0</forward>
<upward>0 0 1</upward>
<link_name>iris_with_standoffs::rotor_1</link_name>
</plugin>
<plugin filename="gz-sim-lift-drag-system"
name="gz::sim::systems::LiftDrag">
<a0>0.3</a0>
<alpha_stall>1.4</alpha_stall>
<cla>4.2500</cla>
<cda>0.10</cda>
<cma>0.0</cma>
<cla_stall>-0.025</cla_stall>
<cda_stall>0.0</cda_stall>
<cma_stall>0.0</cma_stall>
<area>0.002</area>
<air_density>1.2041</air_density>
<cp>-0.084 0 0</cp>
<forward>0 -1 0</forward>
<upward>0 0 1</upward>
<link_name>iris_with_standoffs::rotor_1</link_name>
</plugin>
<plugin filename="gz-sim-lift-drag-system"
name="gz::sim::systems::LiftDrag">
<a0>0.3</a0>
<alpha_stall>1.4</alpha_stall>
<cla>4.2500</cla>
<cda>0.10</cda>
<cma>0.0</cma>
<cla_stall>-0.025</cla_stall>
<cda_stall>0.0</cda_stall>
<cma_stall>0.0</cma_stall>
<area>0.002</area>
<air_density>1.2041</air_density>
<cp>0.084 0 0</cp>
<forward>0 -1 0</forward>
<upward>0 0 1</upward>
<link_name>iris_with_standoffs::rotor_2</link_name>
</plugin>
<plugin filename="gz-sim-lift-drag-system"
name="gz::sim::systems::LiftDrag">
<a0>0.3</a0>
<alpha_stall>1.4</alpha_stall>
<cla>4.2500</cla>
<cda>0.10</cda>
<cma>0.0</cma>
<cla_stall>-0.025</cla_stall>
<cda_stall>0.0</cda_stall>
<cma_stall>0.0</cma_stall>
<area>0.002</area>
<air_density>1.2041</air_density>
<cp>-0.084 0 0</cp>
<forward>0 1 0</forward>
<upward>0 0 1</upward>
<link_name>iris_with_standoffs::rotor_2</link_name>
</plugin>
<plugin filename="gz-sim-lift-drag-system"
name="gz::sim::systems::LiftDrag">
<a0>0.3</a0>
<alpha_stall>1.4</alpha_stall>
<cla>4.2500</cla>
<cda>0.10</cda>
<cma>0.0</cma>
<cla_stall>-0.025</cla_stall>
<cda_stall>0.0</cda_stall>
<cma_stall>0.0</cma_stall>
<area>0.002</area>
<air_density>1.2041</air_density>
<cp>0.084 0 0</cp>
<forward>0 -1 0</forward>
<upward>0 0 1</upward>
<link_name>iris_with_standoffs::rotor_3</link_name>
</plugin>
<plugin filename="gz-sim-lift-drag-system"
name="gz::sim::systems::LiftDrag">
<a0>0.3</a0>
<alpha_stall>1.4</alpha_stall>
<cla>4.2500</cla>
<cda>0.10</cda>
<cma>0.0</cma>
<cla_stall>-0.025</cla_stall>
<cda_stall>0.0</cda_stall>
<cma_stall>0.0</cma_stall>
<area>0.002</area>
<air_density>1.2041</air_density>
<cp>-0.084 0 0</cp>
<forward>0 1 0</forward>
<upward>0 0 1</upward>
<link_name>iris_with_standoffs::rotor_3</link_name>
</plugin>
<plugin filename="gz-sim-apply-joint-force-system"
name="gz::sim::systems::ApplyJointForce">
<joint_name>iris_with_standoffs::rotor_0_joint</joint_name>
</plugin>
<plugin filename="gz-sim-apply-joint-force-system"
name="gz::sim::systems::ApplyJointForce">
<joint_name>iris_with_standoffs::rotor_1_joint</joint_name>
</plugin>
<plugin filename="gz-sim-apply-joint-force-system"
name="gz::sim::systems::ApplyJointForce">
<joint_name>iris_with_standoffs::rotor_2_joint</joint_name>
</plugin>
<plugin filename="gz-sim-apply-joint-force-system"
name="gz::sim::systems::ApplyJointForce">
<joint_name>iris_with_standoffs::rotor_3_joint</joint_name>
</plugin>
<plugin name="ArduPilotPlugin"
filename="ArduPilotPlugin">
<!-- Port settings -->
<fdm_addr>127.0.0.1</fdm_addr>
<fdm_port_in>9002</fdm_port_in>
<connectionTimeoutMaxCount>5</connectionTimeoutMaxCount>
<lock_step>1</lock_step>
<!-- Frame conventions
Require by ArduPilot: change model and gazebo from XYZ to XY-Z coordinates
-->
<modelXYZToAirplaneXForwardZDown degrees="true">0 0 0 180 0 0</modelXYZToAirplaneXForwardZDown>
<gazeboXYZToNED degrees="true">0 0 0 180 0 90</gazeboXYZToNED>
<!-- Sensors -->
<imuName>iris_with_standoffs::imu_link::imu_sensor</imuName>
<!--
incoming control command [0, 1]
so offset it by 0 to get [0, 1]
and divide max target by 1.
offset = 0
multiplier = 838 max rpm / 1 = 838
-->
<control channel="0">
<jointName>iris_with_standoffs::rotor_0_joint</jointName>
<useForce>1</useForce>
<multiplier>838</multiplier>
<offset>0</offset>
<servo_min>1100</servo_min>
<servo_max>1900</servo_max>
<type>VELOCITY</type>
<p_gain>0.20</p_gain>
<i_gain>0</i_gain>
<d_gain>0</d_gain>
<i_max>0</i_max>
<i_min>0</i_min>
<cmd_max>2.5</cmd_max>
<cmd_min>-2.5</cmd_min>
<controlVelocitySlowdownSim>1</controlVelocitySlowdownSim>
</control>
<control channel="1">
<jointName>iris_with_standoffs::rotor_1_joint</jointName>
<useForce>1</useForce>
<multiplier>838</multiplier>
<offset>0</offset>
<servo_min>1100</servo_min>
<servo_max>1900</servo_max>
<type>VELOCITY</type>
<p_gain>0.20</p_gain>
<i_gain>0</i_gain>
<d_gain>0</d_gain>
<i_max>0</i_max>
<i_min>0</i_min>
<cmd_max>2.5</cmd_max>
<cmd_min>-2.5</cmd_min>
<controlVelocitySlowdownSim>1</controlVelocitySlowdownSim>
</control>
<control channel="2">
<jointName>iris_with_standoffs::rotor_2_joint</jointName>
<useForce>1</useForce>
<multiplier>-838</multiplier>
<offset>0</offset>
<servo_min>1100</servo_min>
<servo_max>1900</servo_max>
<type>VELOCITY</type>
<p_gain>0.20</p_gain>
<i_gain>0</i_gain>
<d_gain>0</d_gain>
<i_max>0</i_max>
<i_min>0</i_min>
<cmd_max>2.5</cmd_max>
<cmd_min>-2.5</cmd_min>
<controlVelocitySlowdownSim>1</controlVelocitySlowdownSim>
</control>
<control channel="3">
<jointName>iris_with_standoffs::rotor_3_joint</jointName>
<useForce>1</useForce>
<multiplier>-838</multiplier>
<offset>0</offset>
<servo_min>1100</servo_min>
<servo_max>1900</servo_max>
<type>VELOCITY</type>
<p_gain>0.20</p_gain>
<i_gain>0</i_gain>
<d_gain>0</d_gain>
<i_max>0</i_max>
<i_min>0</i_min>
<cmd_max>2.5</cmd_max>
<cmd_min>-2.5</cmd_min>
<controlVelocitySlowdownSim>1</controlVelocitySlowdownSim>
</control>
<!-- roll range is -30 to +30 deg -->
<control channel="8">
<jointName>gimbal::roll_joint</jointName>
<multiplier>1.047197551196</multiplier>
<offset>-0.5</offset>
<servo_min>1100</servo_min>
<servo_max>1900</servo_max>
<type>COMMAND</type>
<cmd_topic>/gimbal/cmd_roll</cmd_topic>
<p_gain>2</p_gain>
</control>
<!-- pitch range is -135 to +45 deg -->
<control channel="9">
<jointName>gimbal::pitch_joint</jointName>
<multiplier>-3.14159265</multiplier>
<offset>-0.75</offset>
<servo_min>1100</servo_min>
<servo_max>1900</servo_max>
<type>COMMAND</type>
<cmd_topic>/gimbal/cmd_pitch</cmd_topic>
<p_gain>2</p_gain>
</control>
<!-- yaw range is -160 to +160 deg -->
<control channel="10">
<jointName>gimbal::yaw_joint</jointName>
<multiplier>-5.5850536</multiplier>
<offset>-0.5</offset>
<servo_min>1100</servo_min>
<servo_max>1900</servo_max>
<type>COMMAND</type>
<cmd_topic>/gimbal/cmd_yaw</cmd_topic>
<p_gain>2</p_gain>
</control>
</plugin>
<plugin
filename="gz-sim-joint-position-controller-system"
name="gz::sim::systems::JointPositionController">
<joint_name>gimbal::roll_joint</joint_name>
<topic>/gimbal/cmd_roll</topic>
<p_gain>2</p_gain>
</plugin>
<plugin
filename="gz-sim-joint-position-controller-system"
name="gz::sim::systems::JointPositionController">
<joint_name>gimbal::pitch_joint</joint_name>
<topic>/gimbal/cmd_pitch</topic>
<p_gain>2</p_gain>
</plugin>
<plugin
filename="gz-sim-joint-position-controller-system"
name="gz::sim::systems::JointPositionController">
<joint_name>gimbal::yaw_joint</joint_name>
<topic>/gimbal/cmd_yaw</topic>
<p_gain>2</p_gain>
</plugin>
</model>
</sdf>

74
scripts/run_autonomous.sh
View File

@@ -18,8 +18,8 @@ print_error() { echo -e "${RED}[ERROR]${NC} $1"; }
SOFTWARE_RENDER=auto
WORLD="uav_ugv_search.sdf"
MISSION="hover"
ALTITUDE=5.0
DURATION=30.0
ALTITUDE=""
DURATION=""
while [[ $# -gt 0 ]]; do
case $1 in
@@ -35,8 +35,8 @@ while [[ $# -gt 0 ]]; do
echo " --no-software-render Disable software rendering"
echo " --world <file> World file (default: uav_ugv_search.sdf)"
echo " --mission <type> hover, square, search (default: hover)"
echo " --altitude <m> Takeoff altitude (default: 5.0)"
echo " --duration <s> Hover duration (default: 30.0)"
echo " --altitude <m> Override altitude from config"
echo " --duration <s> Override duration from config"
exit 0
;;
*) shift ;;
@@ -45,6 +45,8 @@ done
cleanup_all() {
print_info "Cleaning up ..."
pkill -f "camera_viewer.py" 2>/dev/null || true
pkill -f "camera_processor" 2>/dev/null || true
pkill -f "gz sim" 2>/dev/null || true
pkill -f "arducopter" 2>/dev/null || true
pkill -f "sim_vehicle.py" 2>/dev/null || true
@@ -64,6 +66,13 @@ export PATH=$PATH:$HOME/ardupilot/Tools/autotest:$HOME/.local/bin
export GZ_SIM_RESOURCE_PATH="$PROJECT_DIR/models:$PROJECT_DIR/worlds:$HOME/ardupilot_gazebo/models:$HOME/ardupilot_gazebo/worlds"
export GZ_SIM_SYSTEM_PLUGIN_PATH=$HOME/ardupilot_gazebo/build
print_info "Model path: $GZ_SIM_RESOURCE_PATH"
if [ -f "$PROJECT_DIR/models/iris_with_gimbal/model.sdf" ]; then
print_info "Using LOCAL iris_with_gimbal (with downward camera)"
else
print_info "Using ardupilot_gazebo iris_with_gimbal (NO downward camera)"
fi
if [ "$SOFTWARE_RENDER" = "auto" ]; then
if grep -qi microsoft /proc/version 2>/dev/null; then
print_info "WSL detected -> software rendering"
@@ -93,15 +102,44 @@ else
exit 1
fi
UGV_CONFIG="$PROJECT_DIR/config/ugv.yaml"
if [ -f "$UGV_CONFIG" ] && [ -f "$WORLD_FILE" ]; then
python3 -c "
import yaml, re
cfg = yaml.safe_load(open('$UGV_CONFIG'))
pos = cfg.get('position', {})
x, y = pos.get('x', 5.0), pos.get('y', 5.0)
with open('$WORLD_FILE', 'r') as f:
sdf = f.read()
sdf = re.sub(
r'(<model name=\"ugv_target\">\s*<static>true</static>\s*)<pose>[^<]*</pose>',
rf'\1<pose>{x} {y} 0 0 0 0</pose>',
sdf, count=1)
with open('$WORLD_FILE', 'w') as f:
f.write(sdf)
print(f'[INFO] UGV position synced from config: ({x}, {y})')
" 2>/dev/null || print_info "UGV position sync skipped"
fi
print_info "==================================="
print_info " UAV-UGV Simulation"
print_info "==================================="
print_info "World: $WORLD_FILE"
print_info "Mission: $MISSION"
print_info "Altitude: ${ALTITUDE}m"
echo ""
print_info "[1/3] Starting Gazebo ..."
GZ_DEFAULT_GUI="/usr/share/gz/gz-sim8/gui/gui.config"
GZ_USER_GUI="$HOME/.gz/sim/8/gui.config"
CAMERA_PLUGINS="$PROJECT_DIR/config/gui.config"
rm -rf /tmp/gz-* /tmp/gazebo-* 2>/dev/null
mkdir -p "$HOME/.gz/sim/8"
cp "$GZ_DEFAULT_GUI" "$GZ_USER_GUI"
sed -i 's|<start_paused>true</start_paused>|<start_paused>false</start_paused>|' "$GZ_USER_GUI"
sed -i 's|show_again="true"|show_again="false"|' "$GZ_USER_GUI"
print_info "[1/4] Starting Gazebo ..."
gz sim -v4 -r "$WORLD_FILE" &
GZ_PID=$!
sleep 10
@@ -112,7 +150,7 @@ if ! kill -0 $GZ_PID 2>/dev/null; then
fi
print_success "Gazebo running (PID: $GZ_PID)"
print_info "[2/3] Starting ArduPilot SITL ..."
print_info "[2/4] Starting ArduPilot SITL ..."
cd ~/ardupilot
SITL_ARGS="-v ArduCopter -f gazebo-iris --model JSON -I0"
@@ -136,19 +174,23 @@ if ! pgrep -f "arducopter" > /dev/null 2>&1; then
fi
print_success "ArduPilot SITL running (TCP 5760)"
print_info "[3/3] Starting main.py ..."
print_info "[3/4] Starting main.py ..."
cd "$PROJECT_DIR"
sleep 3
python3 src/main.py \
--device sim \
--connection "tcp:127.0.0.1:5760" \
--mission "$MISSION" \
--altitude "$ALTITUDE" \
--duration "$DURATION" &
MAIN_ARGS="--device sim --connection tcp:127.0.0.1:5760 --mission $MISSION"
[ -n "$ALTITUDE" ] && MAIN_ARGS="$MAIN_ARGS --altitude $ALTITUDE"
[ -n "$DURATION" ] && MAIN_ARGS="$MAIN_ARGS --duration $DURATION"
python3 src/main.py $MAIN_ARGS &
MAIN_PID=$!
print_success "main.py running (PID: $MAIN_PID)"
print_info "[4/4] Starting camera viewer ..."
python3 -W ignore:RuntimeWarning -m src.vision.camera_processor down &
CAM_PID=$!
print_success "Camera viewer running (PID: $CAM_PID)"
print_info ""
print_info "==================================="
print_info " Simulation Running"
@@ -156,8 +198,10 @@ print_info "==================================="
print_info " Gazebo -> ArduPilot SITL (TCP:5760)"
print_info " |"
print_info " main.py (pymavlink)"
print_info " |"
print_info " camera_viewer.py (OpenCV)"
print_info ""
print_info " Mission: $MISSION Alt: ${ALTITUDE}m"
print_info " Mission: $MISSION (config from YAML)"
print_info " Press Ctrl+C to stop"
print_info "==================================="

31
setup.sh
View File

@@ -247,6 +247,11 @@ if sudo apt-get install -y gz-harmonic 2>/dev/null; then
libgz-msgs10-dev \
rapidjson-dev \
2>/dev/null || print_warning "Some Gazebo dev packages may be missing"
# Install Gazebo Python bindings (needed for camera/vision scripts)
sudo apt-get install -y \
python3-gz-transport13 \
python3-gz-msgs10 \
2>/dev/null || print_warning "Gazebo Python bindings not available via apt"
elif sudo apt-get install -y gz-garden 2>/dev/null; then
GZ_VERSION="garden"
# Install Garden development packages
@@ -262,6 +267,11 @@ elif sudo apt-get install -y gz-garden 2>/dev/null; then
libgz-msgs9-dev \
rapidjson-dev \
2>/dev/null || print_warning "Some Gazebo dev packages may be missing"
# Install Gazebo Python bindings for Garden
sudo apt-get install -y \
python3-gz-transport12 \
python3-gz-msgs9 \
2>/dev/null || print_warning "Gazebo Python bindings not available via apt"
else
print_warning "Could not install Gazebo Harmonic/Garden"
GZ_VERSION="none"
@@ -283,7 +293,10 @@ if [ -d "$SCRIPT_DIR/venv" ]; then
rm -rf "$SCRIPT_DIR/venv"
fi
python3 -m venv "$SCRIPT_DIR/venv"
# --system-site-packages is required so the venv can access
# Gazebo Python bindings (gz.transport13, gz.msgs10) which are
# installed as system packages and cannot be pip-installed.
python3 -m venv --system-site-packages "$SCRIPT_DIR/venv"
source "$SCRIPT_DIR/venv/bin/activate"
pip install --upgrade pip
@@ -357,7 +370,7 @@ fi
echo -e "\033[0;32mEnvironment activated (ROS 2 $ROS_VER)\033[0m"
echo ""
echo "Run simulation: bash scripts/run_simulation.sh"
echo "Run simulation: bash scripts/run_autonomous.sh --mission hover"
ACTIVATE_EOF
chmod +x "$SCRIPT_DIR/activate_venv.sh"
@@ -478,6 +491,18 @@ if [ "$INSTALL_ARDUPILOT" = true ]; then
command -v gz &> /dev/null && echo "[OK] Gazebo (gz)" || echo "[WARN] Gazebo not found"
command -v mavproxy.py &> /dev/null && echo "[OK] MAVProxy" || echo "[WARN] MAVProxy not in PATH"
[ -f "$ARDUPILOT_GZ/build/libArduPilotPlugin.so" ] && echo "[OK] ArduPilot Gazebo plugin" || echo "[WARN] Plugin not built"
# Verify Gazebo Python bindings are accessible from venv
source "$SCRIPT_DIR/venv/bin/activate"
python3 -c "from gz.transport13 import Node; print('[OK] gz.transport13 Python bindings')" 2>/dev/null || \
echo "[WARN] gz.transport13 Python bindings not found"
python3 -c "from gz.msgs10.image_pb2 import Image; print('[OK] gz.msgs10 Python bindings')" 2>/dev/null || \
echo "[WARN] gz.msgs10 Python bindings not found"
python3 -c "import pymavlink; print('[OK] pymavlink')" 2>/dev/null || \
echo "[WARN] pymavlink not found in venv"
python3 -c "import cv2; print(f'[OK] OpenCV {cv2.__version__}')" 2>/dev/null || \
echo "[WARN] OpenCV not found in venv"
deactivate
((STEP++))
fi
@@ -513,6 +538,6 @@ echo -e "${CYAN}To run the simulation:${NC}"
echo ""
echo " cd $SCRIPT_DIR"
echo " source activate_venv.sh"
echo " bash scripts/run_simulation.sh"
echo " bash scripts/run_autonomous.sh --mission hover"
echo ""
echo -e "${GREEN}==========================================${NC}"

48
src/control/uav_controller.py
View File

@@ -123,22 +123,40 @@ class Controller:
self._drain_messages()
print("[UAV] Mode -> GUIDED_NOGPS")
def arm(self, retries: int = 10):
def arm(self, retries: int = 30):
self.set_mode_guided()
for attempt in range(1, retries + 1):
print(f"[UAV] Arm attempt {attempt}/{retries}...")
self.conn.arducopter_arm()
for i in range(retries):
print(f"[UAV] Arm attempt {i+1}/{retries}...")
t0 = time.time()
while time.time() - t0 < 5:
# Force arm: param2=21196 bypasses pre-arm checks
# (equivalent to MAVProxy's "arm throttle force")
self.conn.mav.command_long_send(
self.conn.target_system,
self.conn.target_component,
mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
0,
1, # param1: 1 = arm
21196, # param2: force arm magic number
0, 0, 0, 0, 0)
# Wait up to 4 seconds for armed status instead of
# blocking forever with motors_armed_wait()
armed = False
for _ in range(20):
self._drain_messages()
if self.armed:
print("[UAV] Armed")
return True
sleep(0.2)
if self.conn.motors_armed():
armed = True
break
time.sleep(0.2)
self._drain_messages()
if armed:
self.armed = True
print("[UAV] Armed")
return True
print(f"[UAV] Arm attempt {i+1} failed, retrying...")
time.sleep(2)
print("[UAV] FAILED to arm after all retries")
return False
@@ -156,8 +174,12 @@ class Controller:
print(f"[UAV] Takeoff -> {altitude}m")
def land(self):
self.conn.set_mode_rtl()
print("[UAV] Landing (RTL)")
self.conn.mav.command_long_send(
self.conn.target_system,
self.conn.target_component,
MAV_CMD_DO_SET_MODE,
0, 89, 9, 0, 0, 0, 0, 0)
print("[UAV] Landing (LAND mode)")
def land_at(self, lat: int, lon: int):
self.conn.mav.command_long_send(

173
src/main.py
View File

@@ -4,28 +4,56 @@ import sys
import os
import time
import argparse
import yaml
from time import sleep
from pathlib import Path
sys.path.insert(0, os.path.dirname(os.path.abspath(__file__)))
from control.uav_controller import Controller, HOLD_ALT
from control.uav_controller import Controller
from control.ugv_controller import UGVController
from utils.helpers import clamp, distance_2d, PIDController, LowPassFilter
from utils.transforms import normalize_angle, body_to_world, world_to_body
PROJECT_DIR = Path(__file__).resolve().parent.parent
CONFIG_DIR = PROJECT_DIR / "config"
def mission_hover(ctrl: Controller, altitude: float = HOLD_ALT,
duration: float = 30.0):
print("\n" + "=" * 50)
print(f" HOVER MISSION: {altitude}m for {duration}s")
print("=" * 50 + "\n")
def load_config(name: str) -> dict:
path = CONFIG_DIR / name
if not path.exists():
print(f"[WARN] Config not found: {path}")
return {}
with open(path, 'r') as f:
return yaml.safe_load(f) or {}
def load_mission(name: str) -> dict:
path = CONFIG_DIR / "missions" / f"{name}.yaml"
if not path.exists():
print(f"[WARN] Mission not found: {path}")
return {}
with open(path, 'r') as f:
return yaml.safe_load(f) or {}
def setup_ardupilot(ctrl: Controller):
ctrl.set_param('ARMING_CHECK', 0)
ctrl.set_param('SCHED_LOOP_RATE', 200)
ctrl.set_param('FS_THR_ENABLE', 0)
ctrl.set_param('FS_GCS_ENABLE', 0)
sleep(2)
def mission_hover(ctrl: Controller, uav_cfg: dict, mission_cfg: dict):
altitude = mission_cfg.get('altitude', uav_cfg['flight']['takeoff_altitude'])
duration = mission_cfg.get('duration', 30.0)
print("\n" + "=" * 50)
print(f" HOVER MISSION: {altitude}m for {duration}s")
print("=" * 50 + "\n")
setup_ardupilot(ctrl)
ctrl.wait_for_gps()
if not ctrl.arm():
@@ -46,29 +74,18 @@ def mission_hover(ctrl: Controller, altitude: float = HOLD_ALT,
print("\n[UAV] Hover complete")
ctrl.land()
print("[UAV] Waiting for landing ...")
t0 = time.time()
while time.time() - t0 < 30:
ctrl.update_state()
if ctrl.altitude < 0.3:
break
sleep(0.5)
print("[UAV] Landed!")
wait_for_landing(ctrl)
def mission_square(ctrl: Controller, altitude: float = HOLD_ALT,
side: float = 5.0):
def mission_square(ctrl: Controller, uav_cfg: dict, mission_cfg: dict):
altitude = mission_cfg.get('altitude', uav_cfg['flight']['takeoff_altitude'])
side = mission_cfg.get('side_length', 5.0)
print("\n" + "=" * 50)
print(f" SQUARE MISSION: {side}m sides at {altitude}m")
print("=" * 50 + "\n")
ctrl.set_param('ARMING_CHECK', 0)
ctrl.set_param('SCHED_LOOP_RATE', 200)
ctrl.set_param('FS_THR_ENABLE', 0)
ctrl.set_param('FS_GCS_ENABLE', 0)
sleep(2)
setup_ardupilot(ctrl)
ctrl.wait_for_gps()
if not ctrl.arm():
@@ -92,29 +109,23 @@ def mission_square(ctrl: Controller, altitude: float = HOLD_ALT,
print("[UAV] Square complete")
ctrl.land()
print("[UAV] Waiting for landing ...")
t0 = time.time()
while time.time() - t0 < 30:
ctrl.update_state()
if ctrl.altitude < 0.3:
break
sleep(0.5)
print("[UAV] Landed!")
wait_for_landing(ctrl)
def mission_search(ctrl: Controller, ugv: UGVController,
altitude: float = HOLD_ALT):
uav_cfg: dict, mission_cfg: dict):
altitude = mission_cfg.get('altitude', uav_cfg['flight']['takeoff_altitude'])
search_cfg = mission_cfg.get('search', {})
initial_leg = search_cfg.get('initial_leg', 3.0)
leg_increment = search_cfg.get('leg_increment', 2.0)
max_legs = search_cfg.get('max_legs', 12)
detection_radius = search_cfg.get('detection_radius', 2.0)
print("\n" + "=" * 50)
print(f" SEARCH MISSION at {altitude}m")
print("=" * 50 + "\n")
ctrl.set_param('ARMING_CHECK', 0)
ctrl.set_param('SCHED_LOOP_RATE', 200)
ctrl.set_param('FS_THR_ENABLE', 0)
ctrl.set_param('FS_GCS_ENABLE', 0)
sleep(2)
setup_ardupilot(ctrl)
ctrl.wait_for_gps()
if not ctrl.arm():
@@ -127,30 +138,23 @@ def mission_search(ctrl: Controller, ugv: UGVController,
ugv_pos = ugv.get_position()
print(f"[UAV] UGV target at ({ugv_pos['x']:.1f}, {ugv_pos['y']:.1f})")
distance_step = 3.0
increment = 2.0
distance_step = initial_leg
travel_x = True
direction = 1
MAX_LEGS = 12
for leg in range(MAX_LEGS):
for leg in range(max_legs):
ctrl.update_state()
uav_pos = ctrl.get_local_position()
dist_to_ugv = distance_2d(
(uav_pos['x'], uav_pos['y']),
(ugv_pos['x'], ugv_pos['y'])
)
print(f"[UAV] Spiral leg {leg+1}/{MAX_LEGS} dist_to_ugv: {dist_to_ugv:.1f}m")
print(f"[UAV] Spiral leg {leg+1}/{max_legs} dist_to_ugv: {dist_to_ugv:.1f}m")
if dist_to_ugv < 2.0:
print("[UAV] UGV found! Landing nearby.")
if dist_to_ugv < detection_radius:
print("[UAV] UGV found! Landing.")
ctrl.land()
t0 = time.time()
while time.time() - t0 < 30:
ctrl.update_state()
if ctrl.altitude < 0.3:
break
sleep(0.5)
wait_for_landing(ctrl)
print("[UAV] Landed on UGV!")
return
@@ -159,55 +163,82 @@ def mission_search(ctrl: Controller, ugv: UGVController,
else:
ctrl.move_pos_rel(0, distance_step * direction, 0)
direction *= -1
distance_step += increment
distance_step += leg_increment
travel_x = not travel_x
sleep(4)
print("[UAV] Search complete - UGV not found, landing")
ctrl.land()
wait_for_landing(ctrl)
def wait_for_landing(ctrl: Controller, timeout: float = 60.0):
print("[UAV] Waiting for landing ...")
t0 = time.time()
while time.time() - t0 < 30:
while time.time() - t0 < timeout:
ctrl.update_state()
elapsed = int(time.time() - t0)
print(f"\r[UAV] Descending: {ctrl.altitude:.1f}m ({elapsed}s) ",
end='', flush=True)
if ctrl.altitude < 0.5 and not ctrl.armed:
break
if ctrl.altitude < 0.3:
break
sleep(0.5)
print("[UAV] Landed!")
print(f"\n[UAV] Landed! (alt: {ctrl.altitude:.2f}m)")
def main():
parser = argparse.ArgumentParser(description='UAV-UGV Simulation')
parser.add_argument('--device', default='sim', choices=['sim', 'real'])
parser.add_argument('--connection', default=None)
parser.add_argument('--mission', default='hover', choices=['hover', 'square', 'search'])
parser.add_argument('--altitude', type=float, default=HOLD_ALT)
parser.add_argument('--duration', type=float, default=30.0)
parser.add_argument('--ugv-x', type=float, default=10.0)
parser.add_argument('--ugv-y', type=float, default=5.0)
parser.add_argument('--ugv-connection', default=None)
parser.add_argument('--mission', default='hover')
parser.add_argument('--altitude', type=float, default=None)
parser.add_argument('--duration', type=float, default=None)
args = parser.parse_args()
uav_cfg = load_config('uav.yaml')
ugv_cfg = load_config('ugv.yaml')
props = load_config('properties.yaml')
mission_cfg = load_mission(args.mission)
if args.altitude is not None:
mission_cfg['altitude'] = args.altitude
if args.duration is not None:
mission_cfg['duration'] = args.duration
if args.connection:
conn_str = args.connection
elif args.device == 'real':
conn_str = '/dev/ttyAMA0'
conn_str = uav_cfg.get('connection', {}).get('real', '/dev/ttyAMA0')
else:
conn_str = 'tcp:127.0.0.1:5760'
conn_str = uav_cfg.get('connection', {}).get('sim', 'tcp:127.0.0.1:5760')
ugv_pos = ugv_cfg.get('position', {})
ugv = UGVController(
connection_string=args.ugv_connection,
static_pos=(args.ugv_x, args.ugv_y),
connection_string=ugv_cfg.get('connection', {}).get('sim'),
static_pos=(ugv_pos.get('x', 10.0), ugv_pos.get('y', 5.0)),
)
ctrl = Controller(conn_str)
if args.mission == 'hover':
mission_hover(ctrl, altitude=args.altitude, duration=args.duration)
elif args.mission == 'square':
mission_square(ctrl, altitude=args.altitude)
elif args.mission == 'search':
mission_search(ctrl, ugv, altitude=args.altitude)
print(f"[MAIN] Config loaded from {CONFIG_DIR}")
print(f"[MAIN] Mission: {args.mission}")
missions = {
'hover': lambda: mission_hover(ctrl, uav_cfg, mission_cfg),
'square': lambda: mission_square(ctrl, uav_cfg, mission_cfg),
'search': lambda: mission_search(ctrl, ugv, uav_cfg, mission_cfg),
}
runner = missions.get(args.mission)
if runner:
runner()
else:
print(f"[MAIN] Unknown mission: {args.mission}")
print(f"[MAIN] Available: {list(missions.keys())}")
return
print("[MAIN] Mission finished.")

327
src/vision/camera_processor.py
View File

@@ -1,227 +1,152 @@
#!/usr/bin/env python3
"""
Camera Processor Node
Handles camera feed processing for GPS-denied navigation
Camera Processor - Handles Gazebo camera feeds for GPS-denied navigation.
Subscribes to gz-transport topics, processes frames, and displays via OpenCV.
Downstream modules (ArUco detector, optical flow) register as callbacks.
"""
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image, CameraInfo
from std_msgs.msg import Header
from cv_bridge import CvBridge
import cv2
import sys
import signal
import time
import numpy as np
import cv2
from gz.transport13 import Node
from gz.msgs10.image_pb2 import Image
# PixelFormatType enum values (from gz.msgs10 Image proto)
PF_RGB_INT8 = 3
PF_BGR_INT8 = 8
PF_R_FLOAT32 = 13
class CameraProcessor(Node):
"""
Processes camera feeds for visual odometry and obstacle detection.
Outputs processed images for downstream nodes.
"""
class CameraProcessor:
def __init__(self, topics=None, show_gui=True):
self.node = Node()
self.show_gui = show_gui
self.frames = {}
self.callbacks = {}
self.running = True
def __init__(self):
super().__init__('camera_processor')
if topics is None:
topics = {
"downward": "/uav/camera/downward",
"gimbal": "/world/uav_ugv_search/model/iris_with_gimbal/model/gimbal/link/pitch_link/sensor/camera/image",
}
self.bridge = CvBridge()
self.topics = topics
# Camera parameters
self.camera_matrix = None
self.dist_coeffs = None
self.image_size = None
# Image processing parameters
self.declare_parameter('undistort', True)
self.declare_parameter('grayscale_output', True)
self.declare_parameter('histogram_equalization', True)
self.declare_parameter('resize_factor', 1.0)
self.undistort = self.get_parameter('undistort').value
self.grayscale_output = self.get_parameter('grayscale_output').value
self.histogram_eq = self.get_parameter('histogram_equalization').value
self.resize_factor = self.get_parameter('resize_factor').value
# Undistort maps (computed once)
self.map1 = None
self.map2 = None
# Subscribers - Forward camera
self.forward_image_sub = self.create_subscription(
Image,
'/uav/camera/forward/image_raw',
self.forward_image_callback,
10
)
self.forward_info_sub = self.create_subscription(
CameraInfo,
'/uav/camera/forward/camera_info',
self.camera_info_callback,
10
)
# Subscribers - Downward camera
self.downward_image_sub = self.create_subscription(
Image,
'/uav/camera/downward/image_raw',
self.downward_image_callback,
10
)
# Publishers - Processed images
self.forward_processed_pub = self.create_publisher(
Image,
'/uav/camera/forward/image_processed',
10
)
self.downward_processed_pub = self.create_publisher(
Image,
'/uav/camera/downward/image_processed',
10
)
# Debug visualization
self.debug_pub = self.create_publisher(
Image,
'/uav/camera/debug',
10
)
self.get_logger().info('Camera Processor Node Started')
def camera_info_callback(self, msg: CameraInfo):
"""Extract and store camera calibration parameters."""
if self.camera_matrix is None:
self.camera_matrix = np.array(msg.k).reshape(3, 3)
self.dist_coeffs = np.array(msg.d)
self.image_size = (msg.width, msg.height)
# Compute undistortion maps
if self.undistort and self.dist_coeffs is not None:
new_camera_matrix, _ = cv2.getOptimalNewCameraMatrix(
self.camera_matrix,
self.dist_coeffs,
self.image_size,
alpha=0
)
self.map1, self.map2 = cv2.initUndistortRectifyMap(
self.camera_matrix,
self.dist_coeffs,
None,
new_camera_matrix,
self.image_size,
cv2.CV_16SC2
)
self.get_logger().info(
f'Camera calibration received: {self.image_size[0]}x{self.image_size[1]}'
)
def process_image(self, image: np.ndarray) -> np.ndarray:
"""
Apply image processing pipeline.
Args:
image: Input BGR image
Returns:
Processed image
"""
processed = image.copy()
# 1. Undistort if calibration available
if self.undistort and self.map1 is not None:
processed = cv2.remap(processed, self.map1, self.map2, cv2.INTER_LINEAR)
# 2. Resize if needed
if self.resize_factor != 1.0:
new_size = (
int(processed.shape[1] * self.resize_factor),
int(processed.shape[0] * self.resize_factor)
)
processed = cv2.resize(processed, new_size, interpolation=cv2.INTER_AREA)
# 3. Convert to grayscale if requested
if self.grayscale_output:
if len(processed.shape) == 3:
processed = cv2.cvtColor(processed, cv2.COLOR_BGR2GRAY)
# 4. Histogram equalization for better feature detection
if self.histogram_eq:
if len(processed.shape) == 2:
# CLAHE for better results than standard histogram equalization
clahe = cv2.createCLAHE(clipLimit=2.0, tileGridSize=(8, 8))
processed = clahe.apply(processed)
for name, topic in self.topics.items():
self.callbacks[name] = []
ok = self.node.subscribe(Image, topic, self._make_gz_callback(name))
if ok:
print(f"[CAM] Subscribed: {name} -> {topic}")
else:
# For color images, apply to L channel in LAB
lab = cv2.cvtColor(processed, cv2.COLOR_BGR2LAB)
clahe = cv2.createCLAHE(clipLimit=2.0, tileGridSize=(8, 8))
lab[:, :, 0] = clahe.apply(lab[:, :, 0])
processed = cv2.cvtColor(lab, cv2.COLOR_LAB2BGR)
print(f"[CAM] Failed: {topic}")
signal.signal(signal.SIGINT, lambda s, f: self.stop())
def _make_gz_callback(self, name):
def cb(msg):
fmt = msg.pixel_format_type
if fmt == PF_RGB_INT8:
arr = np.frombuffer(msg.data, dtype=np.uint8).reshape(
msg.height, msg.width, 3
)
bgr = cv2.cvtColor(arr, cv2.COLOR_RGB2BGR)
elif fmt == PF_BGR_INT8:
arr = np.frombuffer(msg.data, dtype=np.uint8).reshape(
msg.height, msg.width, 3
)
bgr = arr
elif fmt == PF_R_FLOAT32:
arr = np.frombuffer(msg.data, dtype=np.float32).reshape(
msg.height, msg.width
)
normalized = cv2.normalize(arr, None, 0, 255, cv2.NORM_MINMAX)
bgr = cv2.cvtColor(normalized.astype(np.uint8), cv2.COLOR_GRAY2BGR)
else:
return
processed = self.process_image(bgr)
self.frames[name] = processed
for fn in self.callbacks.get(name, []):
fn(name, processed)
return cb
def process_image(self, image):
processed = image.copy()
clahe = cv2.createCLAHE(clipLimit=2.0, tileGridSize=(8, 8))
lab = cv2.cvtColor(processed, cv2.COLOR_BGR2LAB)
lab[:, :, 0] = clahe.apply(lab[:, :, 0])
processed = cv2.cvtColor(lab, cv2.COLOR_LAB2BGR)
return processed
def forward_image_callback(self, msg: Image):
"""Process forward camera images for visual odometry."""
try:
# Convert ROS Image to OpenCV
cv_image = self.bridge.imgmsg_to_cv2(msg, 'bgr8')
def register_callback(self, camera_name, fn):
if camera_name in self.callbacks:
self.callbacks[camera_name].append(fn)
print(f"[CAM] Registered callback for {camera_name}: {fn.__name__}")
# Process image
processed = self.process_image(cv_image)
def stop(self):
self.running = False
# Convert back to ROS Image
if len(processed.shape) == 2:
encoding = 'mono8'
else:
encoding = 'bgr8'
def spin(self):
print("[CAM] Running. Press 'q' to quit, 's' to screenshot.")
while self.running:
for name, frame in list(self.frames.items()):
cv2.imshow(name, frame)
processed_msg = self.bridge.cv2_to_imgmsg(processed, encoding)
processed_msg.header = msg.header
key = cv2.waitKey(33) & 0xFF
if key == ord("q"):
break
elif key == ord("s"):
for name, frame in self.frames.items():
fname = f"{name}_{int(time.time())}.png"
cv2.imwrite(fname, frame)
print(f"[CAM] Saved {fname}")
# Publish processed image
self.forward_processed_pub.publish(processed_msg)
cv2.destroyAllWindows()
print("[CAM] Stopped.")
except Exception as e:
self.get_logger().error(f'Forward image processing error: {e}')
def spin_headless(self):
print("[CAM] Running headless (no GUI).")
while self.running:
time.sleep(0.1)
print("[CAM] Stopped.")
def downward_image_callback(self, msg: Image):
"""Process downward camera images for optical flow."""
try:
# Convert ROS Image to OpenCV
cv_image = self.bridge.imgmsg_to_cv2(msg, 'bgr8')
# Process image
processed = self.process_image(cv_image)
# Convert back to ROS Image
if len(processed.shape) == 2:
encoding = 'mono8'
else:
encoding = 'bgr8'
processed_msg = self.bridge.cv2_to_imgmsg(processed, encoding)
processed_msg.header = msg.header
# Publish processed image
self.downward_processed_pub.publish(processed_msg)
except Exception as e:
self.get_logger().error(f'Downward image processing error: {e}')
def get_frame(self, camera_name):
return self.frames.get(camera_name)
def main(args=None):
rclpy.init(args=args)
node = CameraProcessor()
def main():
cameras = "both"
show_gui = True
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
finally:
node.destroy_node()
rclpy.shutdown()
if len(sys.argv) > 1:
cameras = sys.argv[1].lower()
if "--headless" in sys.argv:
show_gui = False
all_topics = {
"downward": "/uav/camera/downward",
"gimbal": "/world/uav_ugv_search/model/iris_with_gimbal/model/gimbal/link/pitch_link/sensor/camera/image",
}
if cameras == "down":
topics = {"downward": all_topics["downward"]}
elif cameras == "gimbal":
topics = {"gimbal": all_topics["gimbal"]}
else:
topics = all_topics
proc = CameraProcessor(topics=topics, show_gui=show_gui)
if show_gui:
proc.spin()
else:
proc.spin_headless()
if __name__ == '__main__':
if __name__ == "__main__":
main()

176
src/vision/object_detector.py
View File

@@ -1,132 +1,126 @@
#!/usr/bin/env python3
"""Object Detector - Visual landmark and obstacle detection."""
"""
Object Detector - ArUco marker and feature detection.
Registers as a callback on CameraProcessor to receive processed frames.
"""
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from geometry_msgs.msg import PoseArray, Pose
from std_msgs.msg import Float32MultiArray
from cv_bridge import CvBridge
import cv2
import numpy as np
from scipy.spatial.transform import Rotation
class ObjectDetector(Node):
class ObjectDetector:
def __init__(self, marker_size=0.15, camera_matrix=None, detection_method="ArUco"):
self.detection_method = detection_method
self.marker_size = marker_size
def __init__(self):
super().__init__('object_detector')
self.bridge = CvBridge()
self.declare_parameter('detection_method', 'ArUco')
self.declare_parameter('marker_size', 0.15)
self.declare_parameter('camera_matrix', [500.0, 0.0, 320.0, 0.0, 500.0, 240.0, 0.0, 0.0, 1.0])
self.detection_method = self.get_parameter('detection_method').value
self.marker_size = self.get_parameter('marker_size').value
camera_matrix_flat = self.get_parameter('camera_matrix').value
self.camera_matrix = np.array(camera_matrix_flat).reshape(3, 3)
if camera_matrix is not None:
self.camera_matrix = np.array(camera_matrix).reshape(3, 3)
else:
self.camera_matrix = np.array(
[[500.0, 0.0, 320.0], [0.0, 500.0, 240.0], [0.0, 0.0, 1.0]]
)
self.dist_coeffs = np.zeros(5)
if self.detection_method == 'ArUco':
if self.detection_method == "ArUco":
self.aruco_dict = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_50)
self.aruco_params = cv2.aruco.DetectorParameters()
self.aruco_detector = cv2.aruco.ArucoDetector(self.aruco_dict, self.aruco_params)
self.aruco_detector = cv2.aruco.ArucoDetector(
self.aruco_dict, self.aruco_params
)
self.image_sub = self.create_subscription(
Image, '/uav/camera/forward/image_raw', self.image_callback, 10)
self.last_detections = []
self.on_detection = None
self.landmarks_pub = self.create_publisher(PoseArray, '/uav/landmarks/poses', 10)
self.marker_ids_pub = self.create_publisher(Float32MultiArray, '/uav/landmarks/ids', 10)
self.debug_image_pub = self.create_publisher(Image, '/uav/landmarks/debug', 10)
print(f"[DET] Object Detector initialized ({self.detection_method})")
self.get_logger().info(f'Object Detector Started - {self.detection_method}')
def detect(self, camera_name, frame):
if self.detection_method == "ArUco":
detections, annotated = self.detect_aruco(frame)
else:
detections, annotated = self.detect_orb(frame)
def image_callback(self, msg):
try:
frame = self.bridge.imgmsg_to_cv2(msg, 'bgr8')
self.last_detections = detections
if self.detection_method == 'ArUco':
self.detect_aruco(frame, msg.header)
else:
self.detect_orb_features(frame, msg.header)
if detections and self.on_detection:
self.on_detection(detections)
except Exception as e:
self.get_logger().error(f'Detection error: {e}')
if annotated is not None:
cv2.imshow(f"{camera_name} [detections]", annotated)
def detect_aruco(self, frame, header):
return detections
def detect_aruco(self, frame):
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
corners, ids, rejected = self.aruco_detector.detectMarkers(gray)
pose_array = PoseArray()
pose_array.header = header
pose_array.header.frame_id = 'camera_link'
id_array = Float32MultiArray()
detections = []
annotated = frame.copy()
if ids is not None:
rvecs, tvecs, _ = cv2.aruco.estimatePoseSingleMarkers(
corners, self.marker_size, self.camera_matrix, self.dist_coeffs)
corners, self.marker_size, self.camera_matrix, self.dist_coeffs
)
for i, marker_id in enumerate(ids.flatten()):
pose = Pose()
pose.position.x = float(tvecs[i][0][0])
pose.position.y = float(tvecs[i][0][1])
pose.position.z = float(tvecs[i][0][2])
tvec = tvecs[i][0]
rvec = rvecs[i][0]
rotation_matrix, _ = cv2.Rodrigues(rvecs[i])
from scipy.spatial.transform import Rotation
rotation_matrix, _ = cv2.Rodrigues(rvec)
r = Rotation.from_matrix(rotation_matrix)
quat = r.as_quat()
pose.orientation.x = quat[0]
pose.orientation.y = quat[1]
pose.orientation.z = quat[2]
pose.orientation.w = quat[3]
pose_array.poses.append(pose)
id_array.data.append(float(marker_id))
center = np.mean(corners[i][0], axis=0)
debug_frame = cv2.aruco.drawDetectedMarkers(frame.copy(), corners, ids)
detection = {
"id": int(marker_id),
"position": tvec.tolist(),
"orientation_quat": quat.tolist(),
"rvec": rvec.tolist(),
"tvec": tvec.tolist(),
"corners": corners[i][0].tolist(),
"center_px": center.tolist(),
"distance": float(np.linalg.norm(tvec)),
}
detections.append(detection)
cv2.aruco.drawDetectedMarkers(annotated, corners, ids)
for i in range(len(rvecs)):
cv2.drawFrameAxes(debug_frame, self.camera_matrix, self.dist_coeffs,
rvecs[i], tvecs[i], self.marker_size * 0.5)
cv2.drawFrameAxes(
annotated,
self.camera_matrix,
self.dist_coeffs,
rvecs[i],
tvecs[i],
self.marker_size * 0.5,
)
debug_msg = self.bridge.cv2_to_imgmsg(debug_frame, 'bgr8')
debug_msg.header = header
self.debug_image_pub.publish(debug_msg)
d = detections[i]
label = f"ID:{d['id']} d:{d['distance']:.2f}m"
pos = (int(d["center_px"][0]), int(d["center_px"][1]) - 10)
cv2.putText(
annotated, label, pos, cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2
)
self.landmarks_pub.publish(pose_array)
self.marker_ids_pub.publish(id_array)
return detections, annotated
def detect_orb_features(self, frame, header):
def detect_orb(self, frame):
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
orb = cv2.ORB_create(nfeatures=500)
keypoints, descriptors = orb.detectAndCompute(gray, None)
pose_array = PoseArray()
pose_array.header = header
detections = []
for kp in keypoints[:50]:
pose = Pose()
pose.position.x = float(kp.pt[0])
pose.position.y = float(kp.pt[1])
pose.position.z = 0.0
pose_array.poses.append(pose)
detections.append(
{
"type": "feature",
"center_px": [kp.pt[0], kp.pt[1]],
"size": kp.size,
"response": kp.response,
}
)
self.landmarks_pub.publish(pose_array)
def main(args=None):
rclpy.init(args=args)
node = ObjectDetector()
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
finally:
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
annotated = cv2.drawKeypoints(
frame, keypoints[:50], None, color=(0, 255, 0), flags=0
)
return detections, annotated

155
src/vision/optical_flow.py
View File

@@ -1,128 +1,85 @@
#!/usr/bin/env python3
"""Optical Flow - Velocity estimation from downward camera."""
"""
Optical Flow - Velocity estimation from downward camera.
Lucas-Kanade sparse optical flow for GPS-denied velocity estimation.
"""
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image, Range
from geometry_msgs.msg import TwistStamped
from cv_bridge import CvBridge
import time
import cv2
import numpy as np
class OpticalFlowNode(Node):
def __init__(self):
super().__init__('optical_flow_node')
self.bridge = CvBridge()
class OpticalFlowEstimator:
def __init__(self, focal_length=500.0, min_altitude=0.3, max_altitude=10.0):
self.focal_length = focal_length
self.min_altitude = min_altitude
self.max_altitude = max_altitude
self.prev_frame = None
self.prev_points = None
self.prev_time = None
self.current_altitude = 1.0
self.declare_parameter('window_size', 15)
self.declare_parameter('max_level', 3)
self.declare_parameter('min_altitude', 0.3)
self.declare_parameter('max_altitude', 10.0)
self.declare_parameter('focal_length', 500.0)
self.window_size = self.get_parameter('window_size').value
self.max_level = self.get_parameter('max_level').value
self.min_altitude = self.get_parameter('min_altitude').value
self.max_altitude = self.get_parameter('max_altitude').value
self.focal_length = self.get_parameter('focal_length').value
self.velocity = np.zeros(2)
self.lk_params = dict(
winSize=(self.window_size, self.window_size),
maxLevel=self.max_level,
criteria=(cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 10, 0.03)
winSize=(15, 15),
maxLevel=3,
criteria=(cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 10, 0.03),
)
self.feature_params = dict(
maxCorners=100,
qualityLevel=0.3,
minDistance=7,
blockSize=7
maxCorners=100, qualityLevel=0.3, minDistance=7, blockSize=7
)
self.velocity = np.zeros(2)
self.prev_time = None
self.on_velocity = None
self.image_sub = self.create_subscription(
Image, '/uav/camera/downward/image_raw', self.image_callback, 10)
print("[OF] Optical Flow Estimator initialized")
self.altitude_sub = self.create_subscription(
Range, '/uav/rangefinder/range', self.altitude_callback, 10)
def set_altitude(self, altitude):
if self.min_altitude < altitude < self.max_altitude:
self.current_altitude = altitude
self.velocity_pub = self.create_publisher(
TwistStamped, '/uav/optical_flow/velocity', 10)
def process_frame(self, camera_name, frame):
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
current_time = time.time()
self.get_logger().info('Optical Flow Node Started')
if self.prev_frame is not None and self.prev_points is not None:
new_points, status, error = cv2.calcOpticalFlowPyrLK(
self.prev_frame, gray, self.prev_points, None, **self.lk_params
)
def altitude_callback(self, msg):
if msg.range > self.min_altitude and msg.range < self.max_altitude:
self.current_altitude = msg.range
if new_points is not None:
good_new = new_points[status.flatten() == 1]
good_old = self.prev_points[status.flatten() == 1]
def image_callback(self, msg):
try:
frame = self.bridge.imgmsg_to_cv2(msg, 'bgr8')
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
if len(good_new) > 10:
flow = good_new - good_old
avg_flow = np.mean(flow, axis=0)
current_time = self.get_clock().now()
if self.prev_time is not None:
dt = current_time - self.prev_time
if dt > 0:
pixel_velocity = avg_flow / dt
self.velocity[0] = (
pixel_velocity[0]
* self.current_altitude
/ self.focal_length
)
self.velocity[1] = (
pixel_velocity[1]
* self.current_altitude
/ self.focal_length
)
if self.prev_frame is not None and self.prev_points is not None:
new_points, status, error = cv2.calcOpticalFlowPyrLK(
self.prev_frame, gray, self.prev_points, None, **self.lk_params)
if self.on_velocity:
self.on_velocity(self.velocity)
if new_points is not None:
good_new = new_points[status == 1]
good_old = self.prev_points[status == 1]
self.prev_points = cv2.goodFeaturesToTrack(gray, **self.feature_params)
self.prev_frame = gray
self.prev_time = current_time
if len(good_new) > 10:
flow = good_new - good_old
avg_flow = np.mean(flow, axis=0)
return self.velocity
if self.prev_time is not None:
dt = (current_time - self.prev_time).nanoseconds / 1e9
if dt > 0:
pixel_velocity = avg_flow / dt
self.velocity[0] = pixel_velocity[0] * self.current_altitude / self.focal_length
self.velocity[1] = pixel_velocity[1] * self.current_altitude / self.focal_length
self.publish_velocity()
self.prev_points = cv2.goodFeaturesToTrack(gray, **self.feature_params)
self.prev_frame = gray
self.prev_time = current_time
except Exception as e:
self.get_logger().error(f'Optical flow error: {e}')
def publish_velocity(self):
vel_msg = TwistStamped()
vel_msg.header.stamp = self.get_clock().now().to_msg()
vel_msg.header.frame_id = 'base_link'
vel_msg.twist.linear.x = float(self.velocity[0])
vel_msg.twist.linear.y = float(self.velocity[1])
vel_msg.twist.linear.z = 0.0
self.velocity_pub.publish(vel_msg)
def main(args=None):
rclpy.init(args=args)
node = OpticalFlowNode()
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
finally:
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
def get_velocity(self):
return self.velocity.copy()

184
src/vision/visual_odometry.py
View File

@@ -1,117 +1,95 @@
#!/usr/bin/env python3
"""Visual Odometry - Camera-based position estimation without GPS."""
"""
Visual Odometry - Camera-based position estimation without GPS.
ORB/SIFT feature matching with essential matrix decomposition.
"""
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image, CameraInfo
from geometry_msgs.msg import PoseStamped, TwistStamped
from cv_bridge import CvBridge
import time
import cv2
import numpy as np
from scipy.spatial.transform import Rotation
class VisualOdometryNode(Node):
class VisualOdometry:
def __init__(self, camera_matrix=None, detector_type="ORB", min_features=100):
self.detector_type = detector_type
self.min_features = min_features
def __init__(self):
super().__init__('visual_odometry_node')
if camera_matrix is not None:
self.camera_matrix = np.array(camera_matrix).reshape(3, 3)
else:
self.camera_matrix = np.array(
[[500.0, 0.0, 320.0], [0.0, 500.0, 240.0], [0.0, 0.0, 1.0]]
)
self.dist_coeffs = np.zeros(5)
self.bridge = CvBridge()
self.prev_frame = None
self.prev_keypoints = None
self.prev_descriptors = None
self.camera_matrix = None
self.dist_coeffs = None
self.current_pose = np.eye(4)
self.position = np.zeros(3)
self.orientation = np.eye(3)
self.prev_time = None
self.velocity = np.zeros(3)
self.detector_type = self.declare_parameter('detector_type', 'ORB').value
self.min_features = self.declare_parameter('min_features', 100).value
self.feature_quality = self.declare_parameter('feature_quality', 0.01).value
if self.detector_type == 'ORB':
if detector_type == "ORB":
self.detector = cv2.ORB_create(nfeatures=500)
self.matcher = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=False)
elif self.detector_type == 'SIFT':
elif detector_type == "SIFT":
self.detector = cv2.SIFT_create(nfeatures=500)
self.matcher = cv2.BFMatcher(cv2.NORM_L2, crossCheck=False)
else:
self.detector = cv2.ORB_create(nfeatures=500)
self.matcher = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=False)
self.image_sub = self.create_subscription(
Image, '/uav/camera/forward/image_raw', self.image_callback, 10)
self.prev_frame = None
self.prev_keypoints = None
self.prev_descriptors = None
self.prev_time = None
self.camera_info_sub = self.create_subscription(
CameraInfo, '/uav/camera/forward/camera_info', self.camera_info_callback, 10)
self.position = np.zeros(3)
self.orientation = np.eye(3)
self.velocity = np.zeros(3)
self.pose_pub = self.create_publisher(PoseStamped, '/uav/visual_odometry/pose', 10)
self.velocity_pub = self.create_publisher(TwistStamped, '/uav/visual_odometry/velocity', 10)
self.on_pose = None
self.timer = self.create_timer(0.033, self.publish_pose)
print(f"[VO] Visual Odometry initialized ({detector_type})")
self.get_logger().info(f'Visual Odometry Node Started - {self.detector_type} detector')
def process_frame(self, camera_name, frame):
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
keypoints, descriptors = self.detector.detectAndCompute(gray, None)
current_time = time.time()
def camera_info_callback(self, msg):
if self.camera_matrix is None:
self.camera_matrix = np.array(msg.k).reshape(3, 3)
self.dist_coeffs = np.array(msg.d)
self.get_logger().info('Camera calibration received')
if (
self.prev_frame is not None
and self.prev_descriptors is not None
and len(keypoints) >= self.min_features
):
matches = self._match_features(self.prev_descriptors, descriptors)
if len(matches) >= self.min_features:
self._estimate_motion(
self.prev_keypoints, keypoints, matches, current_time
)
def image_callback(self, msg):
try:
frame = self.bridge.imgmsg_to_cv2(msg, 'bgr8')
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
self.prev_frame = gray
self.prev_keypoints = keypoints
self.prev_descriptors = descriptors
self.prev_time = current_time
keypoints, descriptors = self.detector.detectAndCompute(gray, None)
current_time = self.get_clock().now()
if self.prev_frame is not None and len(keypoints) >= self.min_features:
matches = self.match_features(self.prev_descriptors, descriptors)
if len(matches) >= self.min_features:
self.estimate_motion(self.prev_keypoints, keypoints, matches, current_time)
self.prev_frame = gray
self.prev_keypoints = keypoints
self.prev_descriptors = descriptors
self.prev_time = current_time
except Exception as e:
self.get_logger().error(f'Visual odometry error: {e}')
def match_features(self, desc1, desc2):
def _match_features(self, desc1, desc2):
if desc1 is None or desc2 is None:
return []
matches = self.matcher.knnMatch(desc1, desc2, k=2)
good_matches = []
for match_pair in matches:
if len(match_pair) == 2:
m, n = match_pair
good = []
for pair in matches:
if len(pair) == 2:
m, n = pair
if m.distance < 0.7 * n.distance:
good_matches.append(m)
good.append(m)
return good
return good_matches
def estimate_motion(self, prev_kp, curr_kp, matches, current_time):
if self.camera_matrix is None or len(matches) < 5:
def _estimate_motion(self, prev_kp, curr_kp, matches, current_time):
if len(matches) < 5:
return
pts1 = np.float32([prev_kp[m.queryIdx].pt for m in matches])
pts2 = np.float32([curr_kp[m.trainIdx].pt for m in matches])
E, mask = cv2.findEssentialMat(
pts1, pts2, self.camera_matrix,
method=cv2.RANSAC, prob=0.999, threshold=1.0)
pts1, pts2, self.camera_matrix, method=cv2.RANSAC, prob=0.999, threshold=1.0
)
if E is None:
return
@@ -122,52 +100,20 @@ class VisualOdometryNode(Node):
translation = scale * t.flatten()
if self.prev_time is not None:
dt = (current_time - self.prev_time).nanoseconds / 1e9
dt = current_time - self.prev_time
if dt > 0:
self.velocity = translation / dt
self.position += self.orientation @ translation
self.orientation = R @ self.orientation
def publish_pose(self):
pose_msg = PoseStamped()
pose_msg.header.stamp = self.get_clock().now().to_msg()
pose_msg.header.frame_id = 'odom'
if self.on_pose:
r = Rotation.from_matrix(self.orientation)
self.on_pose(self.position.copy(), r.as_quat())
pose_msg.pose.position.x = float(self.position[0])
pose_msg.pose.position.y = float(self.position[1])
pose_msg.pose.position.z = float(self.position[2])
def get_pose(self):
r = Rotation.from_matrix(self.orientation)
return self.position.copy(), r.as_quat()
rotation = Rotation.from_matrix(self.orientation)
quat = rotation.as_quat()
pose_msg.pose.orientation.x = quat[0]
pose_msg.pose.orientation.y = quat[1]
pose_msg.pose.orientation.z = quat[2]
pose_msg.pose.orientation.w = quat[3]
self.pose_pub.publish(pose_msg)
vel_msg = TwistStamped()
vel_msg.header.stamp = self.get_clock().now().to_msg()
vel_msg.header.frame_id = 'odom'
vel_msg.twist.linear.x = float(self.velocity[0])
vel_msg.twist.linear.y = float(self.velocity[1])
vel_msg.twist.linear.z = float(self.velocity[2])
self.velocity_pub.publish(vel_msg)
def main(args=None):
rclpy.init(args=args)
node = VisualOdometryNode()
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
finally:
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
def get_velocity(self):
return self.velocity.copy()

163
src/vision/visual_servoing.py
View File

@@ -1,135 +1,76 @@
#!/usr/bin/env python3
"""Visual Servoing - Vision-based control for precision positioning."""
"""
Visual Servoing - Vision-based control for precision landing on ArUco marker.
Uses ObjectDetector detections + pymavlink to send velocity commands.
"""
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from geometry_msgs.msg import PoseStamped, TwistStamped, Pose
from std_msgs.msg import Bool
from cv_bridge import CvBridge
import cv2
import numpy as np
class VisualServoing(Node):
class VisualServoing:
def __init__(self, controller, target_marker_id=0, desired_distance=1.0):
self.controller = controller
self.target_marker_id = target_marker_id
self.desired_distance = desired_distance
def __init__(self):
super().__init__('visual_servoing')
self.bridge = CvBridge()
self.declare_parameter('target_marker_id', 0)
self.declare_parameter('desired_distance', 1.0)
self.declare_parameter('kp_linear', 0.5)
self.declare_parameter('kp_angular', 0.3)
self.declare_parameter('max_velocity', 1.0)
self.target_marker_id = self.get_parameter('target_marker_id').value
self.desired_distance = self.get_parameter('desired_distance').value
self.kp_linear = self.get_parameter('kp_linear').value
self.kp_angular = self.get_parameter('kp_angular').value
self.max_velocity = self.get_parameter('max_velocity').value
self.kp_xy = 0.5
self.kp_z = 0.3
self.max_velocity = 1.0
self.enabled = False
self.target_pose = None
self.target_acquired = False
self.image_center = (320, 240)
self.camera_matrix = np.array([
[500.0, 0.0, 320.0],
[0.0, 500.0, 240.0],
[0.0, 0.0, 1.0]
])
self.dist_coeffs = np.zeros(5)
self.last_detection = None
self.aruco_dict = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_50)
self.aruco_params = cv2.aruco.DetectorParameters()
self.aruco_detector = cv2.aruco.ArucoDetector(self.aruco_dict, self.aruco_params)
print(f"[VS] Visual Servoing initialized (target marker: {target_marker_id})")
self.image_sub = self.create_subscription(
Image, '/uav/camera/forward/image_raw', self.image_callback, 10)
def enable(self):
self.enabled = True
print("[VS] Enabled")
self.enable_sub = self.create_subscription(
Bool, '/visual_servoing/enable', self.enable_callback, 10)
def disable(self):
self.enabled = False
print("[VS] Disabled")
self.target_sub = self.create_subscription(
Pose, '/visual_servoing/target', self.target_callback, 10)
self.velocity_pub = self.create_publisher(
TwistStamped, '/uav/visual_servoing/cmd_vel', 10)
self.status_pub = self.create_publisher(
Bool, '/visual_servoing/target_acquired', 10)
self.get_logger().info('Visual Servoing Node Started')
def enable_callback(self, msg):
self.enabled = msg.data
self.get_logger().info(f'Visual servoing {"enabled" if self.enabled else "disabled"}')
def target_callback(self, msg):
self.target_pose = msg
def image_callback(self, msg):
def on_detections(self, detections):
if not self.enabled:
return
try:
frame = self.bridge.imgmsg_to_cv2(msg, 'bgr8')
self.image_center = (frame.shape[1] // 2, frame.shape[0] // 2)
target = None
for d in detections:
if d.get("id") == self.target_marker_id:
target = d
break
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
corners, ids, _ = self.aruco_detector.detectMarkers(gray)
if target is None:
self.target_acquired = False
return
target_acquired = Bool()
target_acquired.data = False
self.target_acquired = True
self.last_detection = target
self.compute_control(target)
if ids is not None and self.target_marker_id in ids:
idx = np.where(ids == self.target_marker_id)[0][0]
target_corners = corners[idx]
def compute_control(self, detection):
center_px = detection["center_px"]
distance = detection["distance"]
position = detection["position"]
rvecs, tvecs, _ = cv2.aruco.estimatePoseSingleMarkers(
[target_corners], 0.15, self.camera_matrix, self.dist_coeffs)
error_x = self.image_center[0] - center_px[0]
error_y = self.image_center[1] - center_px[1]
error_z = distance - self.desired_distance
target_pos = tvecs[0][0]
self.compute_control(target_pos, target_corners)
vx = np.clip(self.kp_xy * error_y / 100.0, -self.max_velocity, self.max_velocity)
vy = np.clip(self.kp_xy * error_x / 100.0, -self.max_velocity, self.max_velocity)
vz = np.clip(-self.kp_z * error_z, -self.max_velocity, self.max_velocity)
target_acquired.data = True
self.controller.send_velocity(vx, vy, vz)
self.status_pub.publish(target_acquired)
except Exception as e:
self.get_logger().error(f'Visual servoing error: {e}')
def compute_control(self, target_pos, corners):
marker_center = np.mean(corners[0], axis=0)
error_x = self.image_center[0] - marker_center[0]
error_y = self.image_center[1] - marker_center[1]
error_z = target_pos[2] - self.desired_distance
vel_msg = TwistStamped()
vel_msg.header.stamp = self.get_clock().now().to_msg()
vel_msg.header.frame_id = 'base_link'
vel_msg.twist.linear.x = np.clip(self.kp_linear * error_z, -self.max_velocity, self.max_velocity)
vel_msg.twist.linear.y = np.clip(self.kp_linear * error_x / 100.0, -self.max_velocity, self.max_velocity)
vel_msg.twist.linear.z = np.clip(self.kp_linear * error_y / 100.0, -self.max_velocity, self.max_velocity)
vel_msg.twist.angular.z = np.clip(self.kp_angular * error_x / 100.0, -1.0, 1.0)
self.velocity_pub.publish(vel_msg)
def main(args=None):
rclpy.init(args=args)
node = VisualServoing()
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
finally:
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
print(
f"\r[VS] Target ID:{detection['id']} "
f"d:{distance:.2f}m "
f"err:({error_x:.0f},{error_y:.0f}) "
f"vel:({vx:.2f},{vy:.2f},{vz:.2f})",
end="",
flush=True,
)

3
worlds/uav_ugv_search.sdf
View File

@@ -153,11 +153,12 @@
<pose degrees="true">0 0 0.195 0 0 90</pose>
</include>
<!-- ===================== UGV: Target Vehicle ===================== -->
<!-- The drone should search for this vehicle and land on it -->
<model name="ugv_target">
<static>true</static>
<pose>10 5 0 0 0 0</pose>
<pose>5.0 5.0 0 0 0 0</pose>
<!-- Main body (blue box) -->
<link name="base_link">

13
wsl_env.sh
View File

@@ -1,13 +0,0 @@
#!/bin/bash
# WSL Environment for UAV-UGV Simulation
if [ -d "/mnt/wslg" ]; then
export DISPLAY=:0
else
export DISPLAY=$(cat /etc/resolv.conf 2>/dev/null | grep nameserver | awk '{print $2}'):0
fi
export LIBGL_ALWAYS_INDIRECT=0
export MESA_GL_VERSION_OVERRIDE=3.3
export MESA_GLSL_VERSION_OVERRIDE=330
export OGRE_RTT_MODE=Copy