Aruco Code and UVG/UAV Logic Fixes
This commit is contained in:
@@ -68,7 +68,7 @@ FS_GCS_ENABLE 0
|
|||||||
# ====================
|
# ====================
|
||||||
# Logging
|
# Logging
|
||||||
# ====================
|
# ====================
|
||||||
LOG_BITMASK 176126
|
LOG_BITMASK 0
|
||||||
LOG_DISARMED 0
|
LOG_DISARMED 0
|
||||||
|
|
||||||
# ====================
|
# ====================
|
||||||
@@ -76,3 +76,16 @@ LOG_DISARMED 0
|
|||||||
# ====================
|
# ====================
|
||||||
LAND_SPEED 150 # Final descent cm/s (default 50)
|
LAND_SPEED 150 # Final descent cm/s (default 50)
|
||||||
LAND_SPEED_HIGH 250 # Initial descent cm/s (default 0=WPNAV_SPEED_DN)
|
LAND_SPEED_HIGH 250 # Initial descent cm/s (default 0=WPNAV_SPEED_DN)
|
||||||
|
|
||||||
|
# ====================
|
||||||
|
# SITL Fast Startup
|
||||||
|
# ====================
|
||||||
|
# These MUST be in the .parm file (loaded at boot), not set via MAVLink
|
||||||
|
GPS_HDOP_GOOD 250 # Accept worse GPS quality (default 140)
|
||||||
|
EK3_CHECK_SCALE 200 # Relax EKF health checks (default 100)
|
||||||
|
EK3_GPS_CHECK 0 # Disable GPS preflight checks entirely in SITL
|
||||||
|
EK3_POSNE_M_NSE 0.5 # Trust GPS position more (default 0.5)
|
||||||
|
EK3_VELD_M_NSE 0.2 # Trust GPS vertical vel more (default 0.4)
|
||||||
|
EK3_VELNE_M_NSE 0.1 # Trust GPS horizontal vel more (default 0.3)
|
||||||
|
INS_GYRO_CAL 0 # Skip gyro calibration on boot (SITL only)
|
||||||
|
AHRS_GPS_MINSATS 6 # Minimum satellites
|
||||||
|
|||||||
@@ -8,7 +8,8 @@ altitude: 4.0 # Search altitude (meters)
|
|||||||
marker:
|
marker:
|
||||||
dictionary: DICT_4X4_50
|
dictionary: DICT_4X4_50
|
||||||
size: 0.5 # Physical marker size in meters
|
size: 0.5 # Physical marker size in meters
|
||||||
landing_ids: [0] # Marker IDs that trigger landing
|
landing_ids: [0] # Marker IDs that trigger landing (on UGV)
|
||||||
|
target_ids: [1] # Marker IDs to find and report to UGV
|
||||||
|
|
||||||
# ── Search Patterns ──────────────────────────────────────────
|
# ── Search Patterns ──────────────────────────────────────────
|
||||||
spiral:
|
spiral:
|
||||||
|
|||||||
@@ -2,7 +2,12 @@
|
|||||||
# UGV-specific settings. Mission config is in search.yaml.
|
# UGV-specific settings. Mission config is in search.yaml.
|
||||||
|
|
||||||
# UGV spawn position in local NED (meters)
|
# UGV spawn position in local NED (meters)
|
||||||
# Also used by run_autonomous.sh to place the UGV model in Gazebo
|
# The UAV starts on top of this — run_autonomous.sh syncs both
|
||||||
position:
|
position:
|
||||||
x: 5.0
|
x: 0.0
|
||||||
y: 5.0
|
y: 0.0
|
||||||
|
|
||||||
|
# Gazebo transport topics
|
||||||
|
topics:
|
||||||
|
cmd_vel: /ugv/cmd_vel
|
||||||
|
odometry: /ugv/odometry
|
||||||
|
|||||||
@@ -1,159 +1,161 @@
|
|||||||
<?xml version="1.0" ?>
|
<?xml version="1.0" ?>
|
||||||
<sdf version="1.6">
|
<sdf version="1.9">
|
||||||
<model name="custom_ugv">
|
<model name="custom_ugv">
|
||||||
<static>false</static>
|
<static>false</static>
|
||||||
|
|
||||||
<link name="base_link">
|
<link name="base_link">
|
||||||
<pose>0 0 0.1 0 0 0</pose>
|
<pose>0 0 0.12 0 0 0</pose>
|
||||||
<inertial>
|
<inertial>
|
||||||
<mass>5.0</mass>
|
<mass>80.0</mass>
|
||||||
<inertia>
|
<inertia>
|
||||||
<ixx>0.1</ixx><ixy>0</ixy><ixz>0</ixz>
|
<ixx>3.0</ixx><ixy>0</ixy><ixz>0</ixz>
|
||||||
<iyy>0.15</iyy><iyz>0</iyz><izz>0.1</izz>
|
<iyy>4.0</iyy><iyz>0</iyz><izz>3.0</izz>
|
||||||
</inertia>
|
</inertia>
|
||||||
</inertial>
|
</inertial>
|
||||||
<collision name="collision">
|
<collision name="collision">
|
||||||
<geometry><box><size>0.4 0.3 0.15</size></box></geometry>
|
<geometry><box><size>0.8 0.6 0.12</size></box></geometry>
|
||||||
</collision>
|
|
||||||
<visual name="visual">
|
|
||||||
<geometry><box><size>0.4 0.3 0.15</size></box></geometry>
|
|
||||||
<material><ambient>0.3 0.3 0.8 1</ambient></material>
|
|
||||||
</visual>
|
|
||||||
</link>
|
|
||||||
|
|
||||||
<link name="left_wheel">
|
|
||||||
<pose>0 0.175 0.05 -1.5708 0 0</pose>
|
|
||||||
<inertial>
|
|
||||||
<mass>0.5</mass>
|
|
||||||
<inertia>
|
|
||||||
<ixx>0.001</ixx><ixy>0</ixy><ixz>0</ixz>
|
|
||||||
<iyy>0.001</iyy><iyz>0</iyz><izz>0.001</izz>
|
|
||||||
</inertia>
|
|
||||||
</inertial>
|
|
||||||
<collision name="collision">
|
|
||||||
<geometry><cylinder><radius>0.05</radius><length>0.04</length></cylinder></geometry>
|
|
||||||
<surface>
|
<surface>
|
||||||
<friction><ode><mu>1.0</mu><mu2>1.0</mu2></ode></friction>
|
<friction>
|
||||||
|
<ode><mu>10.0</mu><mu2>10.0</mu2></ode>
|
||||||
|
</friction>
|
||||||
</surface>
|
</surface>
|
||||||
</collision>
|
</collision>
|
||||||
<visual name="visual">
|
<visual name="visual">
|
||||||
<geometry><cylinder><radius>0.05</radius><length>0.04</length></cylinder></geometry>
|
<geometry><box><size>0.8 0.6 0.12</size></box></geometry>
|
||||||
<material><ambient>0.1 0.1 0.1 1</ambient></material>
|
<material><ambient>0.2 0.2 0.7 1</ambient><diffuse>0.2 0.2 0.8 1</diffuse></material>
|
||||||
</visual>
|
</visual>
|
||||||
</link>
|
</link>
|
||||||
|
|
||||||
<link name="right_wheel">
|
<link name="front_left_wheel">
|
||||||
<pose>0 -0.175 0.05 -1.5708 0 0</pose>
|
<pose>0.25 0.35 0.08 -1.5708 0 0</pose>
|
||||||
<inertial>
|
<inertial>
|
||||||
<mass>0.5</mass>
|
<mass>2.0</mass>
|
||||||
<inertia>
|
<inertia>
|
||||||
<ixx>0.001</ixx><ixy>0</ixy><ixz>0</ixz>
|
<ixx>0.005</ixx><ixy>0</ixy><ixz>0</ixz>
|
||||||
<iyy>0.001</iyy><iyz>0</iyz><izz>0.001</izz>
|
<iyy>0.005</iyy><iyz>0</iyz><izz>0.008</izz>
|
||||||
</inertia>
|
</inertia>
|
||||||
</inertial>
|
</inertial>
|
||||||
<collision name="collision">
|
<collision name="collision">
|
||||||
<geometry><cylinder><radius>0.05</radius><length>0.04</length></cylinder></geometry>
|
<geometry><cylinder><radius>0.08</radius><length>0.05</length></cylinder></geometry>
|
||||||
<surface>
|
<surface><friction><ode><mu>5.0</mu><mu2>5.0</mu2></ode></friction></surface>
|
||||||
<friction><ode><mu>1.0</mu><mu2>1.0</mu2></ode></friction>
|
|
||||||
</surface>
|
|
||||||
</collision>
|
</collision>
|
||||||
<visual name="visual">
|
<visual name="visual">
|
||||||
<geometry><cylinder><radius>0.05</radius><length>0.04</length></cylinder></geometry>
|
<geometry><cylinder><radius>0.08</radius><length>0.05</length></cylinder></geometry>
|
||||||
<material><ambient>0.1 0.1 0.1 1</ambient></material>
|
<material><ambient>0.15 0.15 0.15 1</ambient></material>
|
||||||
</visual>
|
</visual>
|
||||||
</link>
|
</link>
|
||||||
|
|
||||||
<link name="caster">
|
<link name="front_right_wheel">
|
||||||
<pose>-0.15 0 0.025 0 0 0</pose>
|
<pose>0.25 -0.35 0.08 -1.5708 0 0</pose>
|
||||||
<inertial>
|
<inertial>
|
||||||
<mass>0.1</mass>
|
<mass>2.0</mass>
|
||||||
<inertia>
|
<inertia>
|
||||||
<ixx>0.0001</ixx><ixy>0</ixy><ixz>0</ixz>
|
<ixx>0.005</ixx><ixy>0</ixy><ixz>0</ixz>
|
||||||
<iyy>0.0001</iyy><iyz>0</iyz><izz>0.0001</izz>
|
<iyy>0.005</iyy><iyz>0</iyz><izz>0.008</izz>
|
||||||
</inertia>
|
</inertia>
|
||||||
</inertial>
|
</inertial>
|
||||||
<collision name="collision">
|
<collision name="collision">
|
||||||
<geometry><sphere><radius>0.025</radius></sphere></geometry>
|
<geometry><cylinder><radius>0.08</radius><length>0.05</length></cylinder></geometry>
|
||||||
<surface>
|
<surface><friction><ode><mu>5.0</mu><mu2>5.0</mu2></ode></friction></surface>
|
||||||
<friction><ode><mu>0.0</mu><mu2>0.0</mu2></ode></friction>
|
|
||||||
</surface>
|
|
||||||
</collision>
|
</collision>
|
||||||
<visual name="visual">
|
<visual name="visual">
|
||||||
<geometry><sphere><radius>0.025</radius></sphere></geometry>
|
<geometry><cylinder><radius>0.08</radius><length>0.05</length></cylinder></geometry>
|
||||||
<material><ambient>0.1 0.1 0.1 1</ambient></material>
|
<material><ambient>0.15 0.15 0.15 1</ambient></material>
|
||||||
</visual>
|
</visual>
|
||||||
</link>
|
</link>
|
||||||
|
|
||||||
<joint name="left_wheel_joint" type="revolute">
|
<link name="rear_left_wheel">
|
||||||
|
<pose>-0.25 0.35 0.08 -1.5708 0 0</pose>
|
||||||
|
<inertial>
|
||||||
|
<mass>2.0</mass>
|
||||||
|
<inertia>
|
||||||
|
<ixx>0.005</ixx><ixy>0</ixy><ixz>0</ixz>
|
||||||
|
<iyy>0.005</iyy><iyz>0</iyz><izz>0.008</izz>
|
||||||
|
</inertia>
|
||||||
|
</inertial>
|
||||||
|
<collision name="collision">
|
||||||
|
<geometry><cylinder><radius>0.08</radius><length>0.05</length></cylinder></geometry>
|
||||||
|
<surface><friction><ode><mu>5.0</mu><mu2>5.0</mu2></ode></friction></surface>
|
||||||
|
</collision>
|
||||||
|
<visual name="visual">
|
||||||
|
<geometry><cylinder><radius>0.08</radius><length>0.05</length></cylinder></geometry>
|
||||||
|
<material><ambient>0.15 0.15 0.15 1</ambient></material>
|
||||||
|
</visual>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<link name="rear_right_wheel">
|
||||||
|
<pose>-0.25 -0.35 0.08 -1.5708 0 0</pose>
|
||||||
|
<inertial>
|
||||||
|
<mass>2.0</mass>
|
||||||
|
<inertia>
|
||||||
|
<ixx>0.005</ixx><ixy>0</ixy><ixz>0</ixz>
|
||||||
|
<iyy>0.005</iyy><iyz>0</iyz><izz>0.008</izz>
|
||||||
|
</inertia>
|
||||||
|
</inertial>
|
||||||
|
<collision name="collision">
|
||||||
|
<geometry><cylinder><radius>0.08</radius><length>0.05</length></cylinder></geometry>
|
||||||
|
<surface><friction><ode><mu>5.0</mu><mu2>5.0</mu2></ode></friction></surface>
|
||||||
|
</collision>
|
||||||
|
<visual name="visual">
|
||||||
|
<geometry><cylinder><radius>0.08</radius><length>0.05</length></cylinder></geometry>
|
||||||
|
<material><ambient>0.15 0.15 0.15 1</ambient></material>
|
||||||
|
</visual>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<!-- ArUco landing tag (ID 0) on top -->
|
||||||
|
<link name="aruco_tag">
|
||||||
|
<pose>0 0 0.185 0 0 0</pose>
|
||||||
|
<visual name="aruco_visual">
|
||||||
|
<geometry><box><size>0.5 0.5 0.005</size></box></geometry>
|
||||||
|
<material>
|
||||||
|
<ambient>1 1 1 1</ambient>
|
||||||
|
<diffuse>1 1 1 1</diffuse>
|
||||||
|
<pbr><metal><albedo_map>tags/land_tag.png</albedo_map></metal></pbr>
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<joint name="aruco_joint" type="fixed">
|
||||||
<parent>base_link</parent>
|
<parent>base_link</parent>
|
||||||
<child>left_wheel</child>
|
<child>aruco_tag</child>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<joint name="front_left_wheel_joint" type="revolute">
|
||||||
|
<parent>base_link</parent>
|
||||||
|
<child>front_left_wheel</child>
|
||||||
|
<axis><xyz>0 0 1</xyz></axis>
|
||||||
|
</joint>
|
||||||
|
<joint name="front_right_wheel_joint" type="revolute">
|
||||||
|
<parent>base_link</parent>
|
||||||
|
<child>front_right_wheel</child>
|
||||||
|
<axis><xyz>0 0 1</xyz></axis>
|
||||||
|
</joint>
|
||||||
|
<joint name="rear_left_wheel_joint" type="revolute">
|
||||||
|
<parent>base_link</parent>
|
||||||
|
<child>rear_left_wheel</child>
|
||||||
|
<axis><xyz>0 0 1</xyz></axis>
|
||||||
|
</joint>
|
||||||
|
<joint name="rear_right_wheel_joint" type="revolute">
|
||||||
|
<parent>base_link</parent>
|
||||||
|
<child>rear_right_wheel</child>
|
||||||
<axis><xyz>0 0 1</xyz></axis>
|
<axis><xyz>0 0 1</xyz></axis>
|
||||||
</joint>
|
</joint>
|
||||||
|
|
||||||
<joint name="right_wheel_joint" type="revolute">
|
<plugin filename="gz-sim-diff-drive-system"
|
||||||
<parent>base_link</parent>
|
name="gz::sim::systems::DiffDrive">
|
||||||
<child>right_wheel</child>
|
<left_joint>front_left_wheel_joint</left_joint>
|
||||||
<axis><xyz>0 0 1</xyz></axis>
|
<left_joint>rear_left_wheel_joint</left_joint>
|
||||||
</joint>
|
<right_joint>front_right_wheel_joint</right_joint>
|
||||||
|
<right_joint>rear_right_wheel_joint</right_joint>
|
||||||
<joint name="caster_joint" type="ball">
|
<wheel_separation>0.70</wheel_separation>
|
||||||
<parent>base_link</parent>
|
<wheel_radius>0.08</wheel_radius>
|
||||||
<child>caster</child>
|
<max_linear_acceleration>1.0</max_linear_acceleration>
|
||||||
</joint>
|
<min_linear_acceleration>-1.0</min_linear_acceleration>
|
||||||
|
<max_angular_acceleration>2.0</max_angular_acceleration>
|
||||||
<link name="camera_link">
|
<min_angular_acceleration>-2.0</min_angular_acceleration>
|
||||||
<pose>0.22 0 0.15 0 0 0</pose>
|
<topic>/ugv/cmd_vel</topic>
|
||||||
<inertial>
|
<odom_topic>/ugv/odometry</odom_topic>
|
||||||
<mass>0.01</mass>
|
<odom_publisher_frequency>20</odom_publisher_frequency>
|
||||||
<inertia>
|
|
||||||
<ixx>0.00001</ixx><ixy>0</ixy><ixz>0</ixz>
|
|
||||||
<iyy>0.00001</iyy><iyz>0</iyz><izz>0.00001</izz>
|
|
||||||
</inertia>
|
|
||||||
</inertial>
|
|
||||||
<visual name="visual">
|
|
||||||
<geometry><box><size>0.02 0.04 0.02</size></box></geometry>
|
|
||||||
<material><ambient>0 0 0 1</ambient></material>
|
|
||||||
</visual>
|
|
||||||
<sensor name="camera" type="camera">
|
|
||||||
<camera>
|
|
||||||
<horizontal_fov>1.57</horizontal_fov>
|
|
||||||
<image><width>640</width><height>480</height><format>R8G8B8</format></image>
|
|
||||||
<clip><near>0.1</near><far>50</far></clip>
|
|
||||||
</camera>
|
|
||||||
<always_on>1</always_on>
|
|
||||||
<update_rate>30</update_rate>
|
|
||||||
<plugin name="camera_plugin" filename="libgazebo_ros_camera.so">
|
|
||||||
<ros>
|
|
||||||
<namespace>/ugv</namespace>
|
|
||||||
<remapping>image_raw:=camera/forward/image_raw</remapping>
|
|
||||||
<remapping>camera_info:=camera/forward/camera_info</remapping>
|
|
||||||
</ros>
|
|
||||||
<camera_name>forward_camera</camera_name>
|
|
||||||
<frame_name>camera_link</frame_name>
|
|
||||||
</plugin>
|
|
||||||
</sensor>
|
|
||||||
</link>
|
|
||||||
|
|
||||||
<joint name="camera_joint" type="fixed">
|
|
||||||
<parent>base_link</parent>
|
|
||||||
<child>camera_link</child>
|
|
||||||
</joint>
|
|
||||||
|
|
||||||
<plugin name="diff_drive" filename="libgazebo_ros_diff_drive.so">
|
|
||||||
<ros><namespace>/ugv</namespace></ros>
|
|
||||||
<left_joint>left_wheel_joint</left_joint>
|
|
||||||
<right_joint>right_wheel_joint</right_joint>
|
|
||||||
<wheel_separation>0.35</wheel_separation>
|
|
||||||
<wheel_diameter>0.1</wheel_diameter>
|
|
||||||
<max_wheel_torque>5</max_wheel_torque>
|
|
||||||
<max_wheel_acceleration>2.0</max_wheel_acceleration>
|
|
||||||
<command_topic>cmd_vel</command_topic>
|
|
||||||
<odometry_topic>odom</odometry_topic>
|
|
||||||
<odometry_frame>odom</odometry_frame>
|
|
||||||
<robot_base_frame>base_link</robot_base_frame>
|
|
||||||
<publish_odom>true</publish_odom>
|
|
||||||
<publish_odom_tf>true</publish_odom_tf>
|
|
||||||
<publish_wheel_tf>true</publish_wheel_tf>
|
|
||||||
</plugin>
|
</plugin>
|
||||||
</model>
|
</model>
|
||||||
</sdf>
|
</sdf>
|
||||||
|
|||||||
BIN
models/custom_ugv/tags/land_tag.png
Normal file
BIN
models/custom_ugv/tags/land_tag.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 6.0 KiB |
@@ -59,6 +59,14 @@ cleanup_sitl() {
|
|||||||
}
|
}
|
||||||
trap cleanup_all EXIT
|
trap cleanup_all EXIT
|
||||||
|
|
||||||
|
if [ -f "$PROJECT_DIR/venv/bin/activate" ]; then
|
||||||
|
print_info "Using venv: $PROJECT_DIR/venv"
|
||||||
|
source "$PROJECT_DIR/venv/bin/activate"
|
||||||
|
elif [ -f "$PROJECT_DIR/.venv/bin/activate" ]; then
|
||||||
|
print_info "Using .venv: $PROJECT_DIR/.venv"
|
||||||
|
source "$PROJECT_DIR/.venv/bin/activate"
|
||||||
|
fi
|
||||||
|
|
||||||
export PATH=$PATH:$HOME/ardupilot/Tools/autotest:$HOME/.local/bin
|
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_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
|
export GZ_SIM_SYSTEM_PLUGIN_PATH=$HOME/ardupilot_gazebo/build
|
||||||
@@ -105,17 +113,24 @@ if [ -f "$UGV_CONFIG" ] && [ -f "$WORLD_FILE" ]; then
|
|||||||
import yaml, re
|
import yaml, re
|
||||||
cfg = yaml.safe_load(open('$UGV_CONFIG'))
|
cfg = yaml.safe_load(open('$UGV_CONFIG'))
|
||||||
pos = cfg.get('position', {})
|
pos = cfg.get('position', {})
|
||||||
x, y = pos.get('x', 5.0), pos.get('y', 5.0)
|
x, y = pos.get('x', 0.0), pos.get('y', 0.0)
|
||||||
with open('$WORLD_FILE', 'r') as f:
|
with open('$WORLD_FILE', 'r') as f:
|
||||||
sdf = f.read()
|
sdf = f.read()
|
||||||
|
# Sync UGV include position
|
||||||
sdf = re.sub(
|
sdf = re.sub(
|
||||||
r'(<model name=\"ugv_target\">\s*<static>true</static>\s*)<pose>[^<]*</pose>',
|
r'(<name>ugv</name>\s*)<pose>[^<]*</pose>',
|
||||||
rf'\1<pose>{x} {y} 0 0 0 0</pose>',
|
rf'\1<pose>{x} {y} 0 0 0 0</pose>',
|
||||||
sdf, count=1)
|
sdf, count=1)
|
||||||
|
# Sync UAV on top of UGV (body top=0.18 + iris legs=0.195 ≈ 0.40)
|
||||||
|
uav_z = 0.40
|
||||||
|
sdf = re.sub(
|
||||||
|
r'(<uri>model://iris_with_gimbal</uri>\s*)<pose[^>]*>[^<]*</pose>',
|
||||||
|
rf'\1<pose degrees=\"true\">{x} {y} {uav_z} 0 0 90</pose>',
|
||||||
|
sdf, count=1)
|
||||||
with open('$WORLD_FILE', 'w') as f:
|
with open('$WORLD_FILE', 'w') as f:
|
||||||
f.write(sdf)
|
f.write(sdf)
|
||||||
print(f'[INFO] UGV position synced from config: ({x}, {y})')
|
print(f'[INFO] UGV at ({x}, {y}) — UAV on top at z={uav_z}')
|
||||||
" 2>/dev/null || print_info "UGV position sync skipped"
|
" 2>/dev/null || print_info "Position sync skipped"
|
||||||
fi
|
fi
|
||||||
|
|
||||||
print_info "==================================="
|
print_info "==================================="
|
||||||
@@ -136,6 +151,27 @@ 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|<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"
|
sed -i 's|show_again="true"|show_again="false"|' "$GZ_USER_GUI"
|
||||||
|
|
||||||
|
cat << 'EOF' >> "$GZ_USER_GUI"
|
||||||
|
<plugin filename="VideoRecorder" name="VideoRecorder">
|
||||||
|
<gz-gui>
|
||||||
|
<property key="resizable" type="bool">false</property>
|
||||||
|
<property key="x" type="double">300</property>
|
||||||
|
<property key="y" type="double">50</property>
|
||||||
|
<property key="width" type="double">50</property>
|
||||||
|
<property key="height" type="double">50</property>
|
||||||
|
<property key="state" type="string">floating</property>
|
||||||
|
<property key="showTitleBar" type="bool">false</property>
|
||||||
|
<property key="cardBackground" type="string">#777777</property>
|
||||||
|
</gz-gui>
|
||||||
|
|
||||||
|
<record_video>
|
||||||
|
<use_sim_time>true</use_sim_time>
|
||||||
|
<lockstep>true</lockstep>
|
||||||
|
<bitrate>4000000</bitrate>
|
||||||
|
</record_video>
|
||||||
|
</plugin>
|
||||||
|
EOF
|
||||||
|
|
||||||
print_info "[1/4] Starting Gazebo ..."
|
print_info "[1/4] Starting Gazebo ..."
|
||||||
gz sim -v4 -r "$WORLD_FILE" &
|
gz sim -v4 -r "$WORLD_FILE" &
|
||||||
GZ_PID=$!
|
GZ_PID=$!
|
||||||
@@ -193,10 +229,12 @@ print_info " Simulation Running"
|
|||||||
print_info "==================================="
|
print_info "==================================="
|
||||||
print_info " Gazebo -> ArduPilot SITL (TCP:5760)"
|
print_info " Gazebo -> ArduPilot SITL (TCP:5760)"
|
||||||
print_info " |"
|
print_info " |"
|
||||||
print_info " main.py (pymavlink)"
|
print_info " main.py (pymavlink + gz.transport)"
|
||||||
print_info " |"
|
print_info " | |"
|
||||||
print_info " camera_viewer.py (OpenCV)"
|
print_info " UAV ctrl UGV ctrl (/ugv/cmd_vel)"
|
||||||
print_info ""
|
print_info ""
|
||||||
|
print_info " UAV starts ON UGV, takes off, searches for"
|
||||||
|
print_info " target tag, dispatches UGV, lands on UGV"
|
||||||
print_info " Search: $SEARCH"
|
print_info " Search: $SEARCH"
|
||||||
print_info " Press Ctrl+C to stop"
|
print_info " Press Ctrl+C to stop"
|
||||||
print_info "==================================="
|
print_info "==================================="
|
||||||
|
|||||||
@@ -36,9 +36,13 @@ class Search:
|
|||||||
self.running = True
|
self.running = True
|
||||||
self.landed = False
|
self.landed = False
|
||||||
self._landing = False
|
self._landing = False
|
||||||
|
self.landing_enabled = False
|
||||||
|
|
||||||
self.actions = actions or {}
|
self.actions = actions or {}
|
||||||
self.land_ids = set(self.actions.get("land", []))
|
self.land_ids = set(self.actions.get("land", []))
|
||||||
|
self.target_ids = set(self.actions.get("target", []))
|
||||||
|
self.on_target_found = None
|
||||||
|
self._dispatched_targets = set()
|
||||||
|
|
||||||
self.spiral_max_legs = 12
|
self.spiral_max_legs = 12
|
||||||
self.spiral_initial_leg = self.CAM_FOV_METERS
|
self.spiral_initial_leg = self.CAM_FOV_METERS
|
||||||
@@ -281,7 +285,15 @@ class Search:
|
|||||||
f"distance:{d['distance']:.2f}m "
|
f"distance:{d['distance']:.2f}m "
|
||||||
f"UAV at ({pos['x']:.1f}, {pos['y']:.1f})")
|
f"UAV at ({pos['x']:.1f}, {pos['y']:.1f})")
|
||||||
|
|
||||||
if marker_id in self.land_ids and not self._landing:
|
if marker_id in self.target_ids and marker_id not in self._dispatched_targets:
|
||||||
|
self._dispatched_targets.add(marker_id)
|
||||||
|
pos = self.found_markers[marker_id]['uav_position']
|
||||||
|
print(f"\n[SEARCH] Target ID:{marker_id} found! "
|
||||||
|
f"Dispatching UGV to ({pos['x']:.1f}, {pos['y']:.1f})")
|
||||||
|
if self.on_target_found:
|
||||||
|
self.on_target_found(marker_id, pos['x'], pos['y'])
|
||||||
|
|
||||||
|
if marker_id in self.land_ids and not self._landing and self.landing_enabled:
|
||||||
print(f"\n[SEARCH] Landing target ID:{marker_id} found! "
|
print(f"\n[SEARCH] Landing target ID:{marker_id} found! "
|
||||||
f"Starting approach.")
|
f"Starting approach.")
|
||||||
self._landing = True
|
self._landing = True
|
||||||
|
|||||||
@@ -105,9 +105,24 @@ class Controller:
|
|||||||
name.encode('utf-8'),
|
name.encode('utf-8'),
|
||||||
value,
|
value,
|
||||||
mavutil.mavlink.MAV_PARAM_TYPE_REAL32)
|
mavutil.mavlink.MAV_PARAM_TYPE_REAL32)
|
||||||
sleep(0.5)
|
sleep(0.1)
|
||||||
self._drain_messages()
|
self._drain_messages()
|
||||||
|
|
||||||
|
def configure_ekf_fast_converge(self):
|
||||||
|
"""Tune EKF parameters for faster initial convergence in SITL."""
|
||||||
|
params = {
|
||||||
|
'EK3_CHECK_SCALE': 200, # Relax EKF health checks (default 100)
|
||||||
|
'EK3_GPS_CHECK': 0, # Disable GPS preflight checks in SITL
|
||||||
|
'EK3_POSNE_M_NSE': 0.5, # Trust GPS position more
|
||||||
|
'EK3_VELD_M_NSE': 0.2, # Trust GPS vertical vel more (default 0.4)
|
||||||
|
'EK3_VELNE_M_NSE': 0.1, # Trust GPS horizontal vel more (default 0.3)
|
||||||
|
'INS_GYRO_CAL': 0, # Skip gyro cal on boot (SITL doesn't need it)
|
||||||
|
'ARMING_CHECK': 0, # Disable ALL preflight checks (SITL only)
|
||||||
|
}
|
||||||
|
for name, value in params.items():
|
||||||
|
self.set_param(name, value)
|
||||||
|
print("[UAV] EKF fast-converge params applied")
|
||||||
|
|
||||||
def set_mode_guided(self):
|
def set_mode_guided(self):
|
||||||
self.conn.mav.command_long_send(
|
self.conn.mav.command_long_send(
|
||||||
self.conn.target_system,
|
self.conn.target_system,
|
||||||
@@ -128,8 +143,33 @@ class Controller:
|
|||||||
self._drain_messages()
|
self._drain_messages()
|
||||||
print("[UAV] Mode -> GUIDED_NOGPS")
|
print("[UAV] Mode -> GUIDED_NOGPS")
|
||||||
|
|
||||||
def arm(self, retries: int = 30):
|
def wait_for_ekf(self, timeout: float = 120.0) -> bool:
|
||||||
|
"""Wait until EKF has actual GPS-fused position estimate."""
|
||||||
|
print("[UAV] Waiting for EKF position estimate ...")
|
||||||
|
t0 = time.time()
|
||||||
|
while time.time() - t0 < timeout:
|
||||||
|
self._drain_messages()
|
||||||
|
msg = self.conn.recv_match(type='EKF_STATUS_REPORT', blocking=True, timeout=2)
|
||||||
|
if msg is not None:
|
||||||
|
# bit 0x08 = pos_horiz_abs (actual GPS-fused position)
|
||||||
|
# bit 0x20 = pred_pos_horiz_abs (predicted, not enough for arming)
|
||||||
|
pos_horiz_abs = bool(msg.flags & 0x08)
|
||||||
|
if pos_horiz_abs:
|
||||||
|
print(f"\n[UAV] EKF has position estimate (flags=0x{msg.flags:04x})")
|
||||||
|
return True
|
||||||
|
elapsed = int(time.time() - t0)
|
||||||
|
print(f"\r[UAV] Waiting for EKF ... {elapsed}s ", end='', flush=True)
|
||||||
|
print("\n[UAV] EKF wait timed out")
|
||||||
|
return False
|
||||||
|
|
||||||
|
def arm(self, retries: int = 15):
|
||||||
self.set_mode_guided()
|
self.set_mode_guided()
|
||||||
|
|
||||||
|
# Wait for EKF before burning through arm attempts
|
||||||
|
if not self.wait_for_ekf(timeout=120.0):
|
||||||
|
print("[UAV] Aborting arm: EKF never became healthy")
|
||||||
|
return False
|
||||||
|
|
||||||
for i in range(retries):
|
for i in range(retries):
|
||||||
print(f"[UAV] Arm attempt {i+1}/{retries}...")
|
print(f"[UAV] Arm attempt {i+1}/{retries}...")
|
||||||
self.conn.mav.command_long_send(
|
self.conn.mav.command_long_send(
|
||||||
@@ -149,7 +189,7 @@ class Controller:
|
|||||||
self.armed = True
|
self.armed = True
|
||||||
print("[UAV] Armed")
|
print("[UAV] Armed")
|
||||||
return True
|
return True
|
||||||
print(f"[UAV] Arm attempt {i+1} failed, retrying...")
|
print(f"[UAV] Arm attempt {i+1} failed, retrying in 2s...")
|
||||||
time.sleep(2)
|
time.sleep(2)
|
||||||
return False
|
return False
|
||||||
|
|
||||||
@@ -259,7 +299,8 @@ class Controller:
|
|||||||
if msg and msg.fix_type >= 3:
|
if msg and msg.fix_type >= 3:
|
||||||
print(f"\n[UAV] GPS fix: {msg.fix_type} sats: {msg.satellites_visible}")
|
print(f"\n[UAV] GPS fix: {msg.fix_type} sats: {msg.satellites_visible}")
|
||||||
print("[UAV] Waiting for EKF to settle ...")
|
print("[UAV] Waiting for EKF to settle ...")
|
||||||
for _ in range(15):
|
# Wait briefly; let the arming loop handle the rest
|
||||||
|
for _ in range(2):
|
||||||
self._drain_messages()
|
self._drain_messages()
|
||||||
sleep(1)
|
sleep(1)
|
||||||
return True
|
return True
|
||||||
|
|||||||
@@ -1,94 +1,149 @@
|
|||||||
#!/usr/bin/env python3
|
#!/usr/bin/env python3
|
||||||
|
"""UGV controller using Gazebo transport (gz.transport13).
|
||||||
|
|
||||||
|
Publishes Twist to /ugv/cmd_vel, subscribes to /ugv/odometry for
|
||||||
|
position feedback. Drives a proportional turn-then-drive controller.
|
||||||
|
"""
|
||||||
|
|
||||||
import math
|
import math
|
||||||
import numpy as np
|
import time
|
||||||
from time import sleep, perf_counter
|
import threading
|
||||||
from typing import Tuple, Optional
|
from time import sleep
|
||||||
|
|
||||||
|
from gz.transport13 import Node
|
||||||
|
from gz.msgs10.twist_pb2 import Twist
|
||||||
|
from gz.msgs10.odometry_pb2 import Odometry
|
||||||
|
|
||||||
try:
|
|
||||||
from pymavlink import mavutil
|
|
||||||
import pymavlink.dialects.v20.ardupilotmega as mavlink
|
|
||||||
from pymavlink.dialects.v20.ardupilotmega import (
|
|
||||||
MAVLink_set_position_target_local_ned_message,
|
|
||||||
MAV_FRAME_BODY_OFFSET_NED,
|
|
||||||
MAV_FRAME_LOCAL_NED,
|
|
||||||
)
|
|
||||||
HAS_MAVLINK = True
|
|
||||||
except ImportError:
|
|
||||||
HAS_MAVLINK = False
|
|
||||||
|
|
||||||
MAX_LINEAR_VEL = 1.0
|
MAX_LINEAR_VEL = 1.0
|
||||||
MAX_ANGULAR_VEL = 1.5
|
MAX_ANGULAR_VEL = 2.0
|
||||||
POSITION_TOL = 0.3
|
POSITION_TOL = 0.5
|
||||||
|
YAW_TOL = 0.15
|
||||||
|
|
||||||
|
|
||||||
class UGVController:
|
class UGVController:
|
||||||
|
def __init__(self, cmd_topic="/ugv/cmd_vel", odom_topic="/ugv/odometry"):
|
||||||
def __init__(self, connection_string: Optional[str] = None,
|
self.node = Node()
|
||||||
static_pos: Tuple[float, float] = (10.0, 5.0)):
|
self.pub = self.node.advertise(cmd_topic, Twist)
|
||||||
self.conn = None
|
self.position = {'x': 0.0, 'y': 0.0, 'z': 0.0}
|
||||||
self.backend = 'passive'
|
|
||||||
self.position = {'x': static_pos[0], 'y': static_pos[1], 'z': 0.0}
|
|
||||||
self.yaw = 0.0
|
self.yaw = 0.0
|
||||||
|
self._lock = threading.Lock()
|
||||||
|
self._driving = False
|
||||||
|
self._drive_thread = None
|
||||||
|
|
||||||
if connection_string and HAS_MAVLINK:
|
ok = self.node.subscribe(Odometry, odom_topic, self._odom_cb)
|
||||||
try:
|
if ok:
|
||||||
print(f"[UGV] Connecting via pymavlink: {connection_string}")
|
print(f"[UGV] Subscribed to {odom_topic}")
|
||||||
self.conn = mavutil.mavlink_connection(connection_string)
|
else:
|
||||||
self.conn.wait_heartbeat(timeout=10)
|
print(f"[UGV] WARNING: Failed to subscribe to {odom_topic}")
|
||||||
print("[UGV] Heartbeat received (system %u component %u)" %
|
|
||||||
(self.conn.target_system, self.conn.target_component))
|
|
||||||
self.backend = 'mavlink'
|
|
||||||
except Exception as e:
|
|
||||||
print(f"[UGV] MAVLink connection failed: {e}")
|
|
||||||
self.conn = None
|
|
||||||
|
|
||||||
if self.backend == 'passive':
|
print(f"[UGV] Publishing commands to {cmd_topic}")
|
||||||
print(f"[UGV] Passive mode - static at ({static_pos[0]}, {static_pos[1]})")
|
|
||||||
|
|
||||||
def update_state(self):
|
def _odom_cb(self, msg):
|
||||||
if self.backend == 'mavlink' and self.conn:
|
with self._lock:
|
||||||
while True:
|
self.position['x'] = msg.pose.position.x
|
||||||
msg = self.conn.recv_match(blocking=False)
|
self.position['y'] = msg.pose.position.y
|
||||||
if msg is None:
|
self.position['z'] = msg.pose.position.z
|
||||||
break
|
q = msg.pose.orientation
|
||||||
mtype = msg.get_type()
|
siny_cosp = 2.0 * (q.w * q.z + q.x * q.y)
|
||||||
if mtype == 'LOCAL_POSITION_NED':
|
cosy_cosp = 1.0 - 2.0 * (q.y * q.y + q.z * q.z)
|
||||||
self.position = {'x': msg.x, 'y': msg.y, 'z': msg.z}
|
self.yaw = math.atan2(siny_cosp, cosy_cosp)
|
||||||
elif mtype == 'ATTITUDE':
|
|
||||||
self.yaw = msg.yaw
|
|
||||||
|
|
||||||
def get_position(self) -> dict:
|
def get_position(self):
|
||||||
self.update_state()
|
with self._lock:
|
||||||
return self.position.copy()
|
return self.position.copy()
|
||||||
|
|
||||||
def move_to(self, x: float, y: float):
|
def get_yaw(self):
|
||||||
if self.backend == 'mavlink' and self.conn:
|
with self._lock:
|
||||||
move_msg = MAVLink_set_position_target_local_ned_message(
|
return self.yaw
|
||||||
int(perf_counter() * 1000),
|
|
||||||
self.conn.target_system,
|
|
||||||
self.conn.target_component,
|
|
||||||
MAV_FRAME_LOCAL_NED,
|
|
||||||
3576, x, y, 0,
|
|
||||||
0, 0, 0, 0, 0, 0, 0, 0)
|
|
||||||
self.conn.mav.send(move_msg)
|
|
||||||
print(f"[UGV] Moving to ({x:.1f}, {y:.1f})")
|
|
||||||
|
|
||||||
def send_velocity(self, vx: float, vy: float, yaw_rate: float = 0.0):
|
def send_cmd_vel(self, linear_x, angular_z):
|
||||||
if self.backend == 'mavlink' and self.conn:
|
msg = Twist()
|
||||||
move_msg = MAVLink_set_position_target_local_ned_message(
|
msg.linear.x = max(-MAX_LINEAR_VEL, min(MAX_LINEAR_VEL, linear_x))
|
||||||
int(perf_counter() * 1000),
|
msg.angular.z = max(-MAX_ANGULAR_VEL, min(MAX_ANGULAR_VEL, angular_z))
|
||||||
self.conn.target_system,
|
self.pub.publish(msg)
|
||||||
self.conn.target_component,
|
|
||||||
MAV_FRAME_BODY_OFFSET_NED,
|
|
||||||
3527, 0, 0, 0,
|
|
||||||
vx, vy, 0, 0, 0, 0, 0, yaw_rate)
|
|
||||||
self.conn.mav.send(move_msg)
|
|
||||||
|
|
||||||
def stop(self):
|
def stop(self):
|
||||||
self.send_velocity(0, 0, 0)
|
self.send_cmd_vel(0.0, 0.0)
|
||||||
|
|
||||||
def distance_to(self, x: float, y: float) -> float:
|
def distance_to(self, x, y):
|
||||||
dx = x - self.position['x']
|
pos = self.get_position()
|
||||||
dy = y - self.position['y']
|
dx = x - pos['x']
|
||||||
|
dy = y - pos['y']
|
||||||
return math.sqrt(dx**2 + dy**2)
|
return math.sqrt(dx**2 + dy**2)
|
||||||
|
|
||||||
|
def drive_to(self, target_x, target_y, callback=None):
|
||||||
|
"""Start driving to (target_x, target_y) in a background thread."""
|
||||||
|
if self._driving:
|
||||||
|
print("[UGV] Already driving, ignoring new target")
|
||||||
|
return
|
||||||
|
self._driving = True
|
||||||
|
self._drive_thread = threading.Thread(
|
||||||
|
target=self._drive_loop,
|
||||||
|
args=(target_x, target_y, callback),
|
||||||
|
daemon=True
|
||||||
|
)
|
||||||
|
self._drive_thread.start()
|
||||||
|
|
||||||
|
def _drive_loop(self, target_x, target_y, callback):
|
||||||
|
print(f"[UGV] Driving to ({target_x:.1f}, {target_y:.1f})")
|
||||||
|
t0 = time.time()
|
||||||
|
timeout = 120.0
|
||||||
|
|
||||||
|
while self._driving and (time.time() - t0) < timeout:
|
||||||
|
pos = self.get_position()
|
||||||
|
yaw = self.get_yaw()
|
||||||
|
dx = target_x - pos['x']
|
||||||
|
dy = target_y - pos['y']
|
||||||
|
dist = math.sqrt(dx**2 + dy**2)
|
||||||
|
|
||||||
|
if dist < POSITION_TOL:
|
||||||
|
self.stop()
|
||||||
|
elapsed = int(time.time() - t0)
|
||||||
|
print(f"\n[UGV] Arrived at ({target_x:.1f}, {target_y:.1f}) "
|
||||||
|
f"in {elapsed}s")
|
||||||
|
self._driving = False
|
||||||
|
if callback:
|
||||||
|
callback(target_x, target_y)
|
||||||
|
return
|
||||||
|
|
||||||
|
target_yaw = math.atan2(dy, dx)
|
||||||
|
yaw_error = target_yaw - yaw
|
||||||
|
while yaw_error > math.pi:
|
||||||
|
yaw_error -= 2 * math.pi
|
||||||
|
while yaw_error < -math.pi:
|
||||||
|
yaw_error += 2 * math.pi
|
||||||
|
|
||||||
|
if abs(yaw_error) > YAW_TOL:
|
||||||
|
angular = max(-MAX_ANGULAR_VEL,
|
||||||
|
min(MAX_ANGULAR_VEL, yaw_error * 2.0))
|
||||||
|
linear = 0.0 if abs(yaw_error) > 0.5 else 0.3
|
||||||
|
else:
|
||||||
|
linear = min(MAX_LINEAR_VEL, dist * 0.5)
|
||||||
|
angular = yaw_error * 1.0
|
||||||
|
|
||||||
|
self.send_cmd_vel(linear, angular)
|
||||||
|
|
||||||
|
elapsed = int(time.time() - t0)
|
||||||
|
print(f"\r[UGV] dist:{dist:.1f}m yaw_err:"
|
||||||
|
f"{math.degrees(yaw_error):+.0f}° "
|
||||||
|
f"v:{linear:.2f} ω:{angular:.2f} ({elapsed}s) ",
|
||||||
|
end='', flush=True)
|
||||||
|
|
||||||
|
sleep(0.1)
|
||||||
|
|
||||||
|
self.stop()
|
||||||
|
self._driving = False
|
||||||
|
print(f"\n[UGV] Drive timeout or stopped")
|
||||||
|
|
||||||
|
def cancel_drive(self):
|
||||||
|
self._driving = False
|
||||||
|
self.stop()
|
||||||
|
|
||||||
|
@property
|
||||||
|
def is_driving(self):
|
||||||
|
return self._driving
|
||||||
|
|
||||||
|
@property
|
||||||
|
def has_arrived(self):
|
||||||
|
return not self._driving and self._drive_thread is not None
|
||||||
|
|||||||
@@ -1 +0,0 @@
|
|||||||
"""Localization module for sensor fusion and position estimation."""
|
|
||||||
@@ -1,202 +0,0 @@
|
|||||||
#!/usr/bin/env python3
|
|
||||||
"""EKF Fusion - Extended Kalman Filter for sensor fusion."""
|
|
||||||
|
|
||||||
import rclpy
|
|
||||||
from rclpy.node import Node
|
|
||||||
from geometry_msgs.msg import PoseStamped, TwistStamped, PoseWithCovarianceStamped
|
|
||||||
from sensor_msgs.msg import Imu
|
|
||||||
from nav_msgs.msg import Odometry
|
|
||||||
import numpy as np
|
|
||||||
from filterpy.kalman import ExtendedKalmanFilter
|
|
||||||
|
|
||||||
|
|
||||||
class EKFFusionNode(Node):
|
|
||||||
|
|
||||||
def __init__(self):
|
|
||||||
super().__init__('ekf_fusion_node')
|
|
||||||
|
|
||||||
self.declare_parameter('process_noise_pos', 0.1)
|
|
||||||
self.declare_parameter('process_noise_vel', 0.5)
|
|
||||||
self.declare_parameter('measurement_noise_vo', 0.05)
|
|
||||||
self.declare_parameter('measurement_noise_of', 0.1)
|
|
||||||
self.declare_parameter('update_rate', 50.0)
|
|
||||||
|
|
||||||
self.ekf = ExtendedKalmanFilter(dim_x=9, dim_z=6)
|
|
||||||
|
|
||||||
self.ekf.x = np.zeros(9)
|
|
||||||
|
|
||||||
self.ekf.P = np.eye(9) * 1.0
|
|
||||||
|
|
||||||
process_noise_pos = self.get_parameter('process_noise_pos').value
|
|
||||||
process_noise_vel = self.get_parameter('process_noise_vel').value
|
|
||||||
|
|
||||||
self.ekf.Q = np.diag([
|
|
||||||
process_noise_pos, process_noise_pos, process_noise_pos,
|
|
||||||
process_noise_vel, process_noise_vel, process_noise_vel,
|
|
||||||
0.1, 0.1, 0.1
|
|
||||||
])
|
|
||||||
|
|
||||||
meas_noise_vo = self.get_parameter('measurement_noise_vo').value
|
|
||||||
meas_noise_of = self.get_parameter('measurement_noise_of').value
|
|
||||||
|
|
||||||
self.ekf.R = np.diag([
|
|
||||||
meas_noise_vo, meas_noise_vo, meas_noise_vo,
|
|
||||||
meas_noise_of, meas_noise_of, meas_noise_of
|
|
||||||
])
|
|
||||||
|
|
||||||
self.prev_time = None
|
|
||||||
self.orientation = np.array([0.0, 0.0, 0.0, 1.0])
|
|
||||||
|
|
||||||
self.vo_sub = self.create_subscription(
|
|
||||||
PoseStamped, '/uav/visual_odometry/pose', self.vo_callback, 10)
|
|
||||||
|
|
||||||
self.of_sub = self.create_subscription(
|
|
||||||
TwistStamped, '/uav/optical_flow/velocity', self.of_callback, 10)
|
|
||||||
|
|
||||||
self.imu_sub = self.create_subscription(
|
|
||||||
Imu, '/uav/imu/data', self.imu_callback, 10)
|
|
||||||
|
|
||||||
self.pose_pub = self.create_publisher(
|
|
||||||
PoseWithCovarianceStamped, '/uav/ekf/pose', 10)
|
|
||||||
|
|
||||||
self.odom_pub = self.create_publisher(
|
|
||||||
Odometry, '/uav/ekf/odom', 10)
|
|
||||||
|
|
||||||
update_rate = self.get_parameter('update_rate').value
|
|
||||||
self.timer = self.create_timer(1.0 / update_rate, self.predict_step)
|
|
||||||
|
|
||||||
self.get_logger().info('EKF Fusion Node Started')
|
|
||||||
|
|
||||||
def state_transition(self, x, dt):
|
|
||||||
F = np.eye(9)
|
|
||||||
F[0, 3] = dt
|
|
||||||
F[1, 4] = dt
|
|
||||||
F[2, 5] = dt
|
|
||||||
F[3, 6] = dt
|
|
||||||
F[4, 7] = dt
|
|
||||||
F[5, 8] = dt
|
|
||||||
return F @ x
|
|
||||||
|
|
||||||
def jacobian_F(self, dt):
|
|
||||||
F = np.eye(9)
|
|
||||||
F[0, 3] = dt
|
|
||||||
F[1, 4] = dt
|
|
||||||
F[2, 5] = dt
|
|
||||||
F[3, 6] = dt
|
|
||||||
F[4, 7] = dt
|
|
||||||
F[5, 8] = dt
|
|
||||||
return F
|
|
||||||
|
|
||||||
def predict_step(self):
|
|
||||||
current_time = self.get_clock().now()
|
|
||||||
|
|
||||||
if self.prev_time is not None:
|
|
||||||
dt = (current_time.nanoseconds - self.prev_time.nanoseconds) / 1e9
|
|
||||||
if dt <= 0 or dt > 1.0:
|
|
||||||
dt = 0.02
|
|
||||||
else:
|
|
||||||
dt = 0.02
|
|
||||||
|
|
||||||
self.prev_time = current_time
|
|
||||||
|
|
||||||
F = self.jacobian_F(dt)
|
|
||||||
self.ekf.F = F
|
|
||||||
self.ekf.predict()
|
|
||||||
|
|
||||||
self.publish_state()
|
|
||||||
|
|
||||||
def vo_callback(self, msg):
|
|
||||||
z = np.array([
|
|
||||||
msg.pose.position.x,
|
|
||||||
msg.pose.position.y,
|
|
||||||
msg.pose.position.z,
|
|
||||||
0.0, 0.0, 0.0
|
|
||||||
])
|
|
||||||
|
|
||||||
self.orientation = np.array([
|
|
||||||
msg.pose.orientation.x,
|
|
||||||
msg.pose.orientation.y,
|
|
||||||
msg.pose.orientation.z,
|
|
||||||
msg.pose.orientation.w
|
|
||||||
])
|
|
||||||
|
|
||||||
H = np.zeros((6, 9))
|
|
||||||
H[0, 0] = 1.0
|
|
||||||
H[1, 1] = 1.0
|
|
||||||
H[2, 2] = 1.0
|
|
||||||
|
|
||||||
self.ekf.update(z, HJacobian=lambda x: H, Hx=lambda x: H @ x)
|
|
||||||
|
|
||||||
def of_callback(self, msg):
|
|
||||||
z = np.array([
|
|
||||||
self.ekf.x[0],
|
|
||||||
self.ekf.x[1],
|
|
||||||
self.ekf.x[2],
|
|
||||||
msg.twist.linear.x,
|
|
||||||
msg.twist.linear.y,
|
|
||||||
0.0
|
|
||||||
])
|
|
||||||
|
|
||||||
H = np.zeros((6, 9))
|
|
||||||
H[0, 0] = 1.0
|
|
||||||
H[1, 1] = 1.0
|
|
||||||
H[2, 2] = 1.0
|
|
||||||
H[3, 3] = 1.0
|
|
||||||
H[4, 4] = 1.0
|
|
||||||
|
|
||||||
self.ekf.update(z, HJacobian=lambda x: H, Hx=lambda x: H @ x)
|
|
||||||
|
|
||||||
def imu_callback(self, msg):
|
|
||||||
accel = np.array([
|
|
||||||
msg.linear_acceleration.x,
|
|
||||||
msg.linear_acceleration.y,
|
|
||||||
msg.linear_acceleration.z - 9.81
|
|
||||||
])
|
|
||||||
|
|
||||||
self.ekf.x[6:9] = accel
|
|
||||||
|
|
||||||
def publish_state(self):
|
|
||||||
pose_msg = PoseWithCovarianceStamped()
|
|
||||||
pose_msg.header.stamp = self.get_clock().now().to_msg()
|
|
||||||
pose_msg.header.frame_id = 'odom'
|
|
||||||
|
|
||||||
pose_msg.pose.pose.position.x = float(self.ekf.x[0])
|
|
||||||
pose_msg.pose.pose.position.y = float(self.ekf.x[1])
|
|
||||||
pose_msg.pose.pose.position.z = float(self.ekf.x[2])
|
|
||||||
|
|
||||||
pose_msg.pose.pose.orientation.x = float(self.orientation[0])
|
|
||||||
pose_msg.pose.pose.orientation.y = float(self.orientation[1])
|
|
||||||
pose_msg.pose.pose.orientation.z = float(self.orientation[2])
|
|
||||||
pose_msg.pose.pose.orientation.w = float(self.orientation[3])
|
|
||||||
|
|
||||||
cov = self.ekf.P[:6, :6].flatten().tolist()
|
|
||||||
pose_msg.pose.covariance = cov
|
|
||||||
|
|
||||||
self.pose_pub.publish(pose_msg)
|
|
||||||
|
|
||||||
odom_msg = Odometry()
|
|
||||||
odom_msg.header = pose_msg.header
|
|
||||||
odom_msg.child_frame_id = 'base_link'
|
|
||||||
odom_msg.pose = pose_msg.pose
|
|
||||||
|
|
||||||
odom_msg.twist.twist.linear.x = float(self.ekf.x[3])
|
|
||||||
odom_msg.twist.twist.linear.y = float(self.ekf.x[4])
|
|
||||||
odom_msg.twist.twist.linear.z = float(self.ekf.x[5])
|
|
||||||
|
|
||||||
self.odom_pub.publish(odom_msg)
|
|
||||||
|
|
||||||
|
|
||||||
def main(args=None):
|
|
||||||
rclpy.init(args=args)
|
|
||||||
node = EKFFusionNode()
|
|
||||||
try:
|
|
||||||
rclpy.spin(node)
|
|
||||||
except KeyboardInterrupt:
|
|
||||||
pass
|
|
||||||
finally:
|
|
||||||
node.destroy_node()
|
|
||||||
rclpy.shutdown()
|
|
||||||
|
|
||||||
|
|
||||||
if __name__ == '__main__':
|
|
||||||
main()
|
|
||||||
@@ -1,165 +0,0 @@
|
|||||||
#!/usr/bin/env python3
|
|
||||||
"""Landmark Tracker - Tracks visual landmarks for position correction."""
|
|
||||||
|
|
||||||
import rclpy
|
|
||||||
from rclpy.node import Node
|
|
||||||
from geometry_msgs.msg import PoseArray, PoseStamped, Point
|
|
||||||
from visualization_msgs.msg import Marker, MarkerArray
|
|
||||||
from std_msgs.msg import Float32MultiArray
|
|
||||||
import numpy as np
|
|
||||||
|
|
||||||
|
|
||||||
class LandmarkTracker(Node):
|
|
||||||
|
|
||||||
def __init__(self):
|
|
||||||
super().__init__('landmark_tracker')
|
|
||||||
|
|
||||||
self.declare_parameter('max_landmarks', 50)
|
|
||||||
self.declare_parameter('matching_threshold', 0.5)
|
|
||||||
self.declare_parameter('landmark_timeout', 5.0)
|
|
||||||
|
|
||||||
self.max_landmarks = self.get_parameter('max_landmarks').value
|
|
||||||
self.matching_threshold = self.get_parameter('matching_threshold').value
|
|
||||||
self.landmark_timeout = self.get_parameter('landmark_timeout').value
|
|
||||||
|
|
||||||
self.landmarks = {}
|
|
||||||
self.landmark_id_counter = 0
|
|
||||||
|
|
||||||
self.current_pose = None
|
|
||||||
|
|
||||||
self.detection_sub = self.create_subscription(
|
|
||||||
PoseArray, '/uav/landmarks/poses', self.detection_callback, 10)
|
|
||||||
|
|
||||||
self.marker_ids_sub = self.create_subscription(
|
|
||||||
Float32MultiArray, '/uav/landmarks/ids', self.marker_ids_callback, 10)
|
|
||||||
|
|
||||||
self.pose_sub = self.create_subscription(
|
|
||||||
PoseStamped, '/uav/local_position/pose', self.pose_callback, 10)
|
|
||||||
|
|
||||||
self.landmark_map_pub = self.create_publisher(
|
|
||||||
MarkerArray, '/landmarks/map', 10)
|
|
||||||
|
|
||||||
self.position_correction_pub = self.create_publisher(
|
|
||||||
PoseStamped, '/landmarks/position_correction', 10)
|
|
||||||
|
|
||||||
self.timer = self.create_timer(1.0, self.cleanup_landmarks)
|
|
||||||
self.viz_timer = self.create_timer(0.5, self.publish_landmark_map)
|
|
||||||
|
|
||||||
self.get_logger().info('Landmark Tracker Started')
|
|
||||||
|
|
||||||
def pose_callback(self, msg):
|
|
||||||
self.current_pose = msg
|
|
||||||
|
|
||||||
def marker_ids_callback(self, msg):
|
|
||||||
pass
|
|
||||||
|
|
||||||
def detection_callback(self, msg):
|
|
||||||
if self.current_pose is None:
|
|
||||||
return
|
|
||||||
|
|
||||||
robot_pos = np.array([
|
|
||||||
self.current_pose.pose.position.x,
|
|
||||||
self.current_pose.pose.position.y,
|
|
||||||
self.current_pose.pose.position.z
|
|
||||||
])
|
|
||||||
|
|
||||||
for pose in msg.poses:
|
|
||||||
local_pos = np.array([pose.position.x, pose.position.y, pose.position.z])
|
|
||||||
|
|
||||||
global_pos = robot_pos + local_pos
|
|
||||||
|
|
||||||
matched_id = self.match_landmark(global_pos)
|
|
||||||
|
|
||||||
if matched_id is not None:
|
|
||||||
self.update_landmark(matched_id, global_pos)
|
|
||||||
else:
|
|
||||||
self.add_landmark(global_pos)
|
|
||||||
|
|
||||||
def match_landmark(self, position):
|
|
||||||
for lm_id, lm_data in self.landmarks.items():
|
|
||||||
distance = np.linalg.norm(position - lm_data['position'])
|
|
||||||
if distance < self.matching_threshold:
|
|
||||||
return lm_id
|
|
||||||
return None
|
|
||||||
|
|
||||||
def add_landmark(self, position):
|
|
||||||
if len(self.landmarks) >= self.max_landmarks:
|
|
||||||
oldest_id = min(self.landmarks.keys(),
|
|
||||||
key=lambda k: self.landmarks[k]['last_seen'])
|
|
||||||
del self.landmarks[oldest_id]
|
|
||||||
|
|
||||||
self.landmarks[self.landmark_id_counter] = {
|
|
||||||
'position': position.copy(),
|
|
||||||
'observations': 1,
|
|
||||||
'last_seen': self.get_clock().now(),
|
|
||||||
'covariance': np.eye(3) * 0.5
|
|
||||||
}
|
|
||||||
self.landmark_id_counter += 1
|
|
||||||
|
|
||||||
def update_landmark(self, lm_id, position):
|
|
||||||
lm = self.landmarks[lm_id]
|
|
||||||
|
|
||||||
alpha = 1.0 / (lm['observations'] + 1)
|
|
||||||
lm['position'] = (1 - alpha) * lm['position'] + alpha * position
|
|
||||||
lm['observations'] += 1
|
|
||||||
lm['last_seen'] = self.get_clock().now()
|
|
||||||
lm['covariance'] *= 0.95
|
|
||||||
|
|
||||||
def cleanup_landmarks(self):
|
|
||||||
current_time = self.get_clock().now()
|
|
||||||
expired = []
|
|
||||||
|
|
||||||
for lm_id, lm_data in self.landmarks.items():
|
|
||||||
dt = (current_time - lm_data['last_seen']).nanoseconds / 1e9
|
|
||||||
if dt > self.landmark_timeout:
|
|
||||||
expired.append(lm_id)
|
|
||||||
|
|
||||||
for lm_id in expired:
|
|
||||||
del self.landmarks[lm_id]
|
|
||||||
|
|
||||||
def publish_landmark_map(self):
|
|
||||||
marker_array = MarkerArray()
|
|
||||||
|
|
||||||
for lm_id, lm_data in self.landmarks.items():
|
|
||||||
marker = Marker()
|
|
||||||
marker.header.stamp = self.get_clock().now().to_msg()
|
|
||||||
marker.header.frame_id = 'odom'
|
|
||||||
marker.ns = 'landmarks'
|
|
||||||
marker.id = lm_id
|
|
||||||
marker.type = Marker.SPHERE
|
|
||||||
marker.action = Marker.ADD
|
|
||||||
|
|
||||||
marker.pose.position.x = float(lm_data['position'][0])
|
|
||||||
marker.pose.position.y = float(lm_data['position'][1])
|
|
||||||
marker.pose.position.z = float(lm_data['position'][2])
|
|
||||||
marker.pose.orientation.w = 1.0
|
|
||||||
|
|
||||||
scale = 0.1 + 0.02 * min(lm_data['observations'], 20)
|
|
||||||
marker.scale.x = scale
|
|
||||||
marker.scale.y = scale
|
|
||||||
marker.scale.z = scale
|
|
||||||
|
|
||||||
marker.color.r = 0.0
|
|
||||||
marker.color.g = 0.8
|
|
||||||
marker.color.b = 0.2
|
|
||||||
marker.color.a = 0.8
|
|
||||||
|
|
||||||
marker_array.markers.append(marker)
|
|
||||||
|
|
||||||
self.landmark_map_pub.publish(marker_array)
|
|
||||||
|
|
||||||
|
|
||||||
def main(args=None):
|
|
||||||
rclpy.init(args=args)
|
|
||||||
node = LandmarkTracker()
|
|
||||||
try:
|
|
||||||
rclpy.spin(node)
|
|
||||||
except KeyboardInterrupt:
|
|
||||||
pass
|
|
||||||
finally:
|
|
||||||
node.destroy_node()
|
|
||||||
rclpy.shutdown()
|
|
||||||
|
|
||||||
|
|
||||||
if __name__ == '__main__':
|
|
||||||
main()
|
|
||||||
@@ -1,188 +0,0 @@
|
|||||||
#!/usr/bin/env python3
|
|
||||||
"""Position Estimator - Fuses sensors for local position estimate."""
|
|
||||||
|
|
||||||
import rclpy
|
|
||||||
from rclpy.node import Node
|
|
||||||
from geometry_msgs.msg import PoseStamped, TwistStamped, PoseWithCovarianceStamped
|
|
||||||
from sensor_msgs.msg import Imu
|
|
||||||
from nav_msgs.msg import Odometry
|
|
||||||
import numpy as np
|
|
||||||
from scipy.spatial.transform import Rotation
|
|
||||||
|
|
||||||
|
|
||||||
class PositionEstimator(Node):
|
|
||||||
|
|
||||||
def __init__(self):
|
|
||||||
super().__init__('position_estimator')
|
|
||||||
|
|
||||||
self.declare_parameter('fusion_method', 'weighted_average')
|
|
||||||
self.declare_parameter('vo_weight', 0.6)
|
|
||||||
self.declare_parameter('optical_flow_weight', 0.3)
|
|
||||||
self.declare_parameter('imu_weight', 0.1)
|
|
||||||
self.declare_parameter('update_rate', 50.0)
|
|
||||||
|
|
||||||
self.fusion_method = self.get_parameter('fusion_method').value
|
|
||||||
self.vo_weight = self.get_parameter('vo_weight').value
|
|
||||||
self.of_weight = self.get_parameter('optical_flow_weight').value
|
|
||||||
self.imu_weight = self.get_parameter('imu_weight').value
|
|
||||||
|
|
||||||
self.position = np.zeros(3)
|
|
||||||
self.velocity = np.zeros(3)
|
|
||||||
self.orientation = np.array([0.0, 0.0, 0.0, 1.0])
|
|
||||||
|
|
||||||
self.vo_pose = None
|
|
||||||
self.of_velocity = None
|
|
||||||
self.imu_data = None
|
|
||||||
|
|
||||||
self.prev_time = None
|
|
||||||
|
|
||||||
self.vo_sub = self.create_subscription(
|
|
||||||
PoseStamped, '/uav/visual_odometry/pose', self.vo_callback, 10)
|
|
||||||
|
|
||||||
self.of_sub = self.create_subscription(
|
|
||||||
TwistStamped, '/uav/optical_flow/velocity', self.of_callback, 10)
|
|
||||||
|
|
||||||
self.imu_sub = self.create_subscription(
|
|
||||||
Imu, '/uav/imu/data', self.imu_callback, 10)
|
|
||||||
|
|
||||||
self.pose_pub = self.create_publisher(
|
|
||||||
PoseWithCovarianceStamped, '/uav/local_position/pose', 10)
|
|
||||||
|
|
||||||
self.odom_pub = self.create_publisher(
|
|
||||||
Odometry, '/uav/local_position/odom', 10)
|
|
||||||
|
|
||||||
update_rate = self.get_parameter('update_rate').value
|
|
||||||
self.timer = self.create_timer(1.0 / update_rate, self.update_estimate)
|
|
||||||
|
|
||||||
self.get_logger().info(f'Position Estimator Started - {self.fusion_method}')
|
|
||||||
|
|
||||||
def vo_callback(self, msg):
|
|
||||||
self.vo_pose = msg
|
|
||||||
|
|
||||||
def of_callback(self, msg):
|
|
||||||
self.of_velocity = msg
|
|
||||||
|
|
||||||
def imu_callback(self, msg):
|
|
||||||
self.imu_data = msg
|
|
||||||
|
|
||||||
def update_estimate(self):
|
|
||||||
current_time = self.get_clock().now()
|
|
||||||
|
|
||||||
if self.prev_time is not None:
|
|
||||||
dt = (current_time.nanoseconds - self.prev_time.nanoseconds) / 1e9
|
|
||||||
if dt <= 0:
|
|
||||||
dt = 0.02
|
|
||||||
else:
|
|
||||||
dt = 0.02
|
|
||||||
|
|
||||||
self.prev_time = current_time
|
|
||||||
|
|
||||||
if self.fusion_method == 'weighted_average':
|
|
||||||
self.weighted_average_fusion(dt)
|
|
||||||
else:
|
|
||||||
self.simple_fusion(dt)
|
|
||||||
|
|
||||||
self.publish_estimate()
|
|
||||||
|
|
||||||
def weighted_average_fusion(self, dt):
|
|
||||||
total_weight = 0.0
|
|
||||||
weighted_pos = np.zeros(3)
|
|
||||||
weighted_vel = np.zeros(3)
|
|
||||||
|
|
||||||
if self.vo_pose is not None:
|
|
||||||
vo_pos = np.array([
|
|
||||||
self.vo_pose.pose.position.x,
|
|
||||||
self.vo_pose.pose.position.y,
|
|
||||||
self.vo_pose.pose.position.z
|
|
||||||
])
|
|
||||||
weighted_pos += self.vo_weight * vo_pos
|
|
||||||
total_weight += self.vo_weight
|
|
||||||
|
|
||||||
self.orientation = np.array([
|
|
||||||
self.vo_pose.pose.orientation.x,
|
|
||||||
self.vo_pose.pose.orientation.y,
|
|
||||||
self.vo_pose.pose.orientation.z,
|
|
||||||
self.vo_pose.pose.orientation.w
|
|
||||||
])
|
|
||||||
|
|
||||||
if self.of_velocity is not None:
|
|
||||||
of_vel = np.array([
|
|
||||||
self.of_velocity.twist.linear.x,
|
|
||||||
self.of_velocity.twist.linear.y,
|
|
||||||
0.0
|
|
||||||
])
|
|
||||||
weighted_vel += self.of_weight * of_vel
|
|
||||||
|
|
||||||
if self.imu_data is not None:
|
|
||||||
imu_accel = np.array([
|
|
||||||
self.imu_data.linear_acceleration.x,
|
|
||||||
self.imu_data.linear_acceleration.y,
|
|
||||||
self.imu_data.linear_acceleration.z - 9.81
|
|
||||||
])
|
|
||||||
self.velocity += imu_accel * dt * self.imu_weight
|
|
||||||
|
|
||||||
if total_weight > 0:
|
|
||||||
self.position = weighted_pos / total_weight
|
|
||||||
|
|
||||||
if self.of_velocity is not None:
|
|
||||||
self.velocity = weighted_vel
|
|
||||||
|
|
||||||
def simple_fusion(self, dt):
|
|
||||||
if self.vo_pose is not None:
|
|
||||||
self.position = np.array([
|
|
||||||
self.vo_pose.pose.position.x,
|
|
||||||
self.vo_pose.pose.position.y,
|
|
||||||
self.vo_pose.pose.position.z
|
|
||||||
])
|
|
||||||
|
|
||||||
def publish_estimate(self):
|
|
||||||
pose_msg = PoseWithCovarianceStamped()
|
|
||||||
pose_msg.header.stamp = self.get_clock().now().to_msg()
|
|
||||||
pose_msg.header.frame_id = 'odom'
|
|
||||||
|
|
||||||
pose_msg.pose.pose.position.x = float(self.position[0])
|
|
||||||
pose_msg.pose.pose.position.y = float(self.position[1])
|
|
||||||
pose_msg.pose.pose.position.z = float(self.position[2])
|
|
||||||
|
|
||||||
pose_msg.pose.pose.orientation.x = float(self.orientation[0])
|
|
||||||
pose_msg.pose.pose.orientation.y = float(self.orientation[1])
|
|
||||||
pose_msg.pose.pose.orientation.z = float(self.orientation[2])
|
|
||||||
pose_msg.pose.pose.orientation.w = float(self.orientation[3])
|
|
||||||
|
|
||||||
pose_msg.pose.covariance = [
|
|
||||||
0.1, 0, 0, 0, 0, 0,
|
|
||||||
0, 0.1, 0, 0, 0, 0,
|
|
||||||
0, 0, 0.1, 0, 0, 0,
|
|
||||||
0, 0, 0, 0.05, 0, 0,
|
|
||||||
0, 0, 0, 0, 0.05, 0,
|
|
||||||
0, 0, 0, 0, 0, 0.05
|
|
||||||
]
|
|
||||||
|
|
||||||
self.pose_pub.publish(pose_msg)
|
|
||||||
|
|
||||||
odom_msg = Odometry()
|
|
||||||
odom_msg.header = pose_msg.header
|
|
||||||
odom_msg.child_frame_id = 'base_link'
|
|
||||||
odom_msg.pose = pose_msg.pose
|
|
||||||
|
|
||||||
odom_msg.twist.twist.linear.x = float(self.velocity[0])
|
|
||||||
odom_msg.twist.twist.linear.y = float(self.velocity[1])
|
|
||||||
odom_msg.twist.twist.linear.z = float(self.velocity[2])
|
|
||||||
|
|
||||||
self.odom_pub.publish(odom_msg)
|
|
||||||
|
|
||||||
|
|
||||||
def main(args=None):
|
|
||||||
rclpy.init(args=args)
|
|
||||||
node = PositionEstimator()
|
|
||||||
try:
|
|
||||||
rclpy.spin(node)
|
|
||||||
except KeyboardInterrupt:
|
|
||||||
pass
|
|
||||||
finally:
|
|
||||||
node.destroy_node()
|
|
||||||
rclpy.shutdown()
|
|
||||||
|
|
||||||
|
|
||||||
if __name__ == '__main__':
|
|
||||||
main()
|
|
||||||
151
src/main.py
151
src/main.py
@@ -12,6 +12,7 @@ sys.path.insert(0, os.path.dirname(os.path.abspath(__file__)))
|
|||||||
|
|
||||||
from control.uav_controller import Controller
|
from control.uav_controller import Controller
|
||||||
from control.search import Search
|
from control.search import Search
|
||||||
|
from control.ugv_controller import UGVController
|
||||||
from vision.object_detector import ObjectDetector
|
from vision.object_detector import ObjectDetector
|
||||||
from vision.camera_processor import CameraProcessor
|
from vision.camera_processor import CameraProcessor
|
||||||
from navigation.flight_tracker import FlightTracker
|
from navigation.flight_tracker import FlightTracker
|
||||||
@@ -21,6 +22,19 @@ PROJECT_DIR = Path(__file__).resolve().parent.parent
|
|||||||
CONFIG_DIR = PROJECT_DIR / "config"
|
CONFIG_DIR = PROJECT_DIR / "config"
|
||||||
|
|
||||||
|
|
||||||
|
# ── Coordinate frame conversion ──────────────────────────────
|
||||||
|
# ArduPilot LOCAL_POSITION_NED: X=North, Y=East, Z=Down
|
||||||
|
# Gazebo Harmonic (ENU default): X=East, Y=North, Z=Up
|
||||||
|
# The axes are swapped in the horizontal plane.
|
||||||
|
def ned_to_gz(ned_x, ned_y):
|
||||||
|
"""Convert ArduPilot NED (North,East) → Gazebo (East,North)."""
|
||||||
|
return ned_y, ned_x
|
||||||
|
|
||||||
|
def gz_to_ned(gz_x, gz_y):
|
||||||
|
"""Convert Gazebo (East,North) → ArduPilot NED (North,East)."""
|
||||||
|
return gz_y, gz_x
|
||||||
|
|
||||||
|
|
||||||
def load_config(name: str) -> dict:
|
def load_config(name: str) -> dict:
|
||||||
path = CONFIG_DIR / name
|
path = CONFIG_DIR / name
|
||||||
if not path.exists():
|
if not path.exists():
|
||||||
@@ -81,9 +95,14 @@ def main():
|
|||||||
marker_dict = marker_cfg.get('dictionary', 'DICT_4X4_50')
|
marker_dict = marker_cfg.get('dictionary', 'DICT_4X4_50')
|
||||||
marker_size = marker_cfg.get('size', 0.5)
|
marker_size = marker_cfg.get('size', 0.5)
|
||||||
landing_ids = marker_cfg.get('landing_ids', [0])
|
landing_ids = marker_cfg.get('landing_ids', [0])
|
||||||
|
target_ids = marker_cfg.get('target_ids', [])
|
||||||
|
|
||||||
ugv_pos_cfg = ugv_cfg.get('position', {})
|
ugv_pos_cfg = ugv_cfg.get('position', {})
|
||||||
ugv_position = (ugv_pos_cfg.get('x', 5.0), ugv_pos_cfg.get('y', 5.0))
|
ugv_position = (ugv_pos_cfg.get('x', 0.0), ugv_pos_cfg.get('y', 0.0))
|
||||||
|
|
||||||
|
ugv_topics = ugv_cfg.get('topics', {})
|
||||||
|
ugv_cmd_topic = ugv_topics.get('cmd_vel', '/ugv/cmd_vel')
|
||||||
|
ugv_odom_topic = ugv_topics.get('odometry', '/ugv/odometry')
|
||||||
|
|
||||||
if args.connection:
|
if args.connection:
|
||||||
conn_str = args.connection
|
conn_str = args.connection
|
||||||
@@ -94,6 +113,8 @@ def main():
|
|||||||
|
|
||||||
ctrl = Controller(conn_str)
|
ctrl = Controller(conn_str)
|
||||||
|
|
||||||
|
ugv = UGVController(cmd_topic=ugv_cmd_topic, odom_topic=ugv_odom_topic)
|
||||||
|
|
||||||
detector = ObjectDetector(
|
detector = ObjectDetector(
|
||||||
marker_size=marker_size,
|
marker_size=marker_size,
|
||||||
aruco_dict_name=marker_dict,
|
aruco_dict_name=marker_dict,
|
||||||
@@ -117,7 +138,7 @@ def main():
|
|||||||
print(f"[MAIN] Camera unavailable: {e}")
|
print(f"[MAIN] Camera unavailable: {e}")
|
||||||
camera = None
|
camera = None
|
||||||
|
|
||||||
actions = {'land': landing_ids}
|
actions = {'land': landing_ids, 'target': target_ids}
|
||||||
search = Search(ctrl, detector, camera=camera, mode=search_mode,
|
search = Search(ctrl, detector, camera=camera, mode=search_mode,
|
||||||
altitude=altitude, actions=actions)
|
altitude=altitude, actions=actions)
|
||||||
|
|
||||||
@@ -137,7 +158,9 @@ def main():
|
|||||||
|
|
||||||
print(f"[MAIN] Config loaded from {CONFIG_DIR}")
|
print(f"[MAIN] Config loaded from {CONFIG_DIR}")
|
||||||
print(f"[MAIN] Search: {search_mode} Altitude: {altitude}m")
|
print(f"[MAIN] Search: {search_mode} Altitude: {altitude}m")
|
||||||
print(f"[MAIN] Marker: {marker_dict} size:{marker_size}m land:{landing_ids}")
|
print(f"[MAIN] Marker: {marker_dict} size:{marker_size}m "
|
||||||
|
f"land:{landing_ids} target:{target_ids}")
|
||||||
|
print(f"[MAIN] UAV starts on UGV at ({ugv_position[0]}, {ugv_position[1]})")
|
||||||
|
|
||||||
tracker = FlightTracker(
|
tracker = FlightTracker(
|
||||||
window_size=600,
|
window_size=600,
|
||||||
@@ -148,7 +171,10 @@ def main():
|
|||||||
tracker.set_plan(waypoints)
|
tracker.set_plan(waypoints)
|
||||||
tracker.start()
|
tracker.start()
|
||||||
|
|
||||||
|
ctrl.configure_ekf_fast_converge()
|
||||||
setup_ardupilot(ctrl)
|
setup_ardupilot(ctrl)
|
||||||
|
if recorder:
|
||||||
|
recorder.set_phase("gps_wait")
|
||||||
ctrl.wait_for_gps()
|
ctrl.wait_for_gps()
|
||||||
|
|
||||||
if not ctrl.arm():
|
if not ctrl.arm():
|
||||||
@@ -158,13 +184,18 @@ def main():
|
|||||||
recorder.stop()
|
recorder.stop()
|
||||||
return
|
return
|
||||||
|
|
||||||
|
if recorder:
|
||||||
|
recorder.set_phase("takeoff")
|
||||||
|
|
||||||
ctrl.takeoff(altitude)
|
ctrl.takeoff(altitude)
|
||||||
ctrl.wait_altitude(altitude, tolerance=1.0, timeout=30)
|
ctrl.wait_altitude(altitude, tolerance=1.0, timeout=30)
|
||||||
|
|
||||||
if recorder:
|
if recorder:
|
||||||
|
recorder.set_phase("search")
|
||||||
recorder.start_recording(tracker=tracker, camera=camera)
|
recorder.start_recording(tracker=tracker, camera=camera)
|
||||||
recorder.snapshot_camera("pre_search")
|
recorder.snapshot_camera("pre_search")
|
||||||
|
|
||||||
|
# --- Wire tracker updates into search loop ---
|
||||||
original_check = search.check_for_markers
|
original_check = search.check_for_markers
|
||||||
def tracked_check():
|
def tracked_check():
|
||||||
result = original_check()
|
result = original_check()
|
||||||
@@ -173,14 +204,45 @@ def main():
|
|||||||
tracker.update_uav(pos['x'], pos['y'],
|
tracker.update_uav(pos['x'], pos['y'],
|
||||||
altitude=ctrl.altitude,
|
altitude=ctrl.altitude,
|
||||||
heading=ctrl.current_yaw)
|
heading=ctrl.current_yaw)
|
||||||
|
# Convert UGV Gazebo odom → NED for the tracker
|
||||||
|
ugv_pos = ugv.get_position()
|
||||||
|
ugv_ned_x, ugv_ned_y = gz_to_ned(ugv_pos['x'], ugv_pos['y'])
|
||||||
|
tracker.update_ugv(ugv_ned_x, ugv_ned_y)
|
||||||
|
# Log position data
|
||||||
|
if recorder:
|
||||||
|
recorder.log_position(
|
||||||
|
uav_x=pos['x'], uav_y=pos['y'],
|
||||||
|
uav_alt=ctrl.altitude, uav_heading=ctrl.current_yaw,
|
||||||
|
ugv_x=ugv_ned_x, ugv_y=ugv_ned_y)
|
||||||
for mid, info in search.found_markers.items():
|
for mid, info in search.found_markers.items():
|
||||||
p = info['uav_position']
|
p = info['uav_position']
|
||||||
tracker.add_marker(mid, p['x'], p['y'])
|
tracker.add_marker(mid, p['x'], p['y'])
|
||||||
return result
|
return result
|
||||||
search.check_for_markers = tracked_check
|
search.check_for_markers = tracked_check
|
||||||
|
|
||||||
search._on_waypoint_change = lambda idx: tracker.set_active_waypoint(idx)
|
search._on_waypoint_change = lambda idx: tracker.set_active_waypoint(idx)
|
||||||
|
|
||||||
|
# --- Target found → dispatch UGV ---
|
||||||
|
# Shared state: set by callback, read by post-search logic
|
||||||
|
ugv_target_pos = [None, None] # [x, y] — set when target found
|
||||||
|
|
||||||
|
def on_target_found(marker_id, x, y):
|
||||||
|
# x, y are in NED (North, East) from the UAV
|
||||||
|
# Convert to Gazebo frame for the UGV
|
||||||
|
gz_x, gz_y = ned_to_gz(x, y)
|
||||||
|
ugv_target_pos[0] = gz_x
|
||||||
|
ugv_target_pos[1] = gz_y
|
||||||
|
print(f"\n[MAIN] Target ID:{marker_id} found at NED({x:.1f}, {y:.1f})")
|
||||||
|
print(f"[MAIN] Dispatching UGV to Gazebo({gz_x:.1f}, {gz_y:.1f}) ...")
|
||||||
|
if recorder:
|
||||||
|
recorder.set_phase("ugv_dispatch")
|
||||||
|
recorder.snapshot_camera("target_found")
|
||||||
|
recorder.snapshot_tracker("target_found")
|
||||||
|
ugv.drive_to(gz_x, gz_y)
|
||||||
|
search.stop()
|
||||||
|
|
||||||
|
search.on_target_found = on_target_found
|
||||||
|
|
||||||
|
# --- Run search (drone searches for target tag ID 1) ---
|
||||||
results = search.run()
|
results = search.run()
|
||||||
search_results = search.get_results()
|
search_results = search.get_results()
|
||||||
|
|
||||||
@@ -196,10 +258,69 @@ def main():
|
|||||||
pos = info['uav_position']
|
pos = info['uav_position']
|
||||||
print(f" ID:{marker_id} at ({pos['x']:.1f}, {pos['y']:.1f}) "
|
print(f" ID:{marker_id} at ({pos['x']:.1f}, {pos['y']:.1f}) "
|
||||||
f"distance:{info['distance']:.2f}m")
|
f"distance:{info['distance']:.2f}m")
|
||||||
if search_results.get('landed'):
|
|
||||||
print(f" Landed on target: YES")
|
|
||||||
print(f"{'=' * 50}\n")
|
print(f"{'=' * 50}\n")
|
||||||
|
|
||||||
|
# --- Wait for UGV to arrive, THEN land on it ---
|
||||||
|
if ugv_target_pos[0] is not None:
|
||||||
|
print("[MAIN] Waiting for UGV to arrive at target ...")
|
||||||
|
if recorder:
|
||||||
|
recorder.set_phase("wait_ugv")
|
||||||
|
while ugv.is_driving:
|
||||||
|
ugv_pos = ugv.get_position()
|
||||||
|
ugv_ned_x, ugv_ned_y = gz_to_ned(ugv_pos['x'], ugv_pos['y'])
|
||||||
|
ctrl.update_state()
|
||||||
|
pos = ctrl.get_local_position()
|
||||||
|
tracker.update_uav(pos['x'], pos['y'],
|
||||||
|
altitude=ctrl.altitude,
|
||||||
|
heading=ctrl.current_yaw)
|
||||||
|
tracker.update_ugv(ugv_ned_x, ugv_ned_y)
|
||||||
|
sleep(0.5)
|
||||||
|
|
||||||
|
# UGV has arrived — fly to above UGV, then land
|
||||||
|
ugv_pos = ugv.get_position()
|
||||||
|
ugv_ned_x, ugv_ned_y = gz_to_ned(ugv_pos['x'], ugv_pos['y'])
|
||||||
|
print(f"\n[MAIN] UGV arrived at Gazebo({ugv_pos['x']:.1f}, {ugv_pos['y']:.1f})")
|
||||||
|
print(f"[MAIN] Flying UAV to NED({ugv_ned_x:.1f}, {ugv_ned_y:.1f}) ...")
|
||||||
|
|
||||||
|
if recorder:
|
||||||
|
recorder.set_phase("fly_to_ugv")
|
||||||
|
|
||||||
|
ctrl.move_local_ned(ugv_ned_x, ugv_ned_y, -altitude)
|
||||||
|
# Wait until UAV is roughly above UGV
|
||||||
|
for _ in range(60):
|
||||||
|
ctrl.update_state()
|
||||||
|
pos = ctrl.get_local_position()
|
||||||
|
dx = ugv_ned_x - pos['x']
|
||||||
|
dy = ugv_ned_y - pos['y']
|
||||||
|
dist = (dx**2 + dy**2) ** 0.5
|
||||||
|
tracker.update_uav(pos['x'], pos['y'],
|
||||||
|
altitude=ctrl.altitude,
|
||||||
|
heading=ctrl.current_yaw)
|
||||||
|
if dist < 1.5:
|
||||||
|
break
|
||||||
|
sleep(0.5)
|
||||||
|
|
||||||
|
print(f"[MAIN] Above UGV — initiating landing")
|
||||||
|
if recorder:
|
||||||
|
recorder.set_phase("landing")
|
||||||
|
|
||||||
|
search.landing_enabled = True
|
||||||
|
search.running = True
|
||||||
|
|
||||||
|
# Attempt visual servoing landing
|
||||||
|
for _ in range(20):
|
||||||
|
search.check_for_markers()
|
||||||
|
if search.landed:
|
||||||
|
break
|
||||||
|
sleep(0.5)
|
||||||
|
|
||||||
|
if not search.landed:
|
||||||
|
ctrl.land()
|
||||||
|
wait_for_landing(ctrl)
|
||||||
|
else:
|
||||||
|
# No target found, just land in place
|
||||||
|
if recorder:
|
||||||
|
recorder.set_phase("landing")
|
||||||
if not search_results.get('landed'):
|
if not search_results.get('landed'):
|
||||||
ctrl.land()
|
ctrl.land()
|
||||||
wait_for_landing(ctrl)
|
wait_for_landing(ctrl)
|
||||||
@@ -207,27 +328,43 @@ def main():
|
|||||||
wait_for_landing(ctrl)
|
wait_for_landing(ctrl)
|
||||||
|
|
||||||
if recorder:
|
if recorder:
|
||||||
|
recorder.set_phase("complete")
|
||||||
recorder.snapshot_camera("post_landing")
|
recorder.snapshot_camera("post_landing")
|
||||||
|
recorder.snapshot_tracker("final")
|
||||||
|
ugv_target_info = None
|
||||||
|
if ugv_target_pos[0] is not None:
|
||||||
|
ugv_target_info = {
|
||||||
|
"gz_x": round(ugv_target_pos[0], 2),
|
||||||
|
"gz_y": round(ugv_target_pos[1], 2),
|
||||||
|
}
|
||||||
recorder.save_summary(
|
recorder.save_summary(
|
||||||
search_mode=search_mode,
|
search_mode=search_mode,
|
||||||
altitude=altitude,
|
altitude=altitude,
|
||||||
markers=results,
|
markers=results,
|
||||||
landed=search_results.get('landed', False),
|
landed=True,
|
||||||
|
ugv_dispatched=ugv_target_pos[0] is not None,
|
||||||
|
ugv_target=ugv_target_info,
|
||||||
)
|
)
|
||||||
recorder.stop()
|
recorder.stop()
|
||||||
|
|
||||||
print(f"\n{'=' * 50}")
|
print(f"\n{'=' * 50}")
|
||||||
print(f" Simulation finished.")
|
print(f" Simulation finished.")
|
||||||
print(f" Tracker and Gazebo windows are still open.")
|
print(f" Tracker and Gazebo windows are still open.")
|
||||||
|
if ugv.is_driving:
|
||||||
|
print(f" UGV is still driving to target...")
|
||||||
print(f" Press Ctrl+C to exit.")
|
print(f" Press Ctrl+C to exit.")
|
||||||
print(f"{'=' * 50}\n")
|
print(f"{'=' * 50}\n")
|
||||||
|
|
||||||
try:
|
try:
|
||||||
while True:
|
while True:
|
||||||
|
ugv_pos = ugv.get_position()
|
||||||
|
ugv_ned_x, ugv_ned_y = gz_to_ned(ugv_pos['x'], ugv_pos['y'])
|
||||||
|
tracker.update_ugv(ugv_ned_x, ugv_ned_y)
|
||||||
sleep(1)
|
sleep(1)
|
||||||
except KeyboardInterrupt:
|
except KeyboardInterrupt:
|
||||||
pass
|
pass
|
||||||
finally:
|
finally:
|
||||||
|
ugv.cancel_drive()
|
||||||
tracker.stop()
|
tracker.stop()
|
||||||
print("[MAIN] Done.")
|
print("[MAIN] Done.")
|
||||||
|
|
||||||
|
|||||||
@@ -1 +0,0 @@
|
|||||||
"""ROS 2 Node implementations."""
|
|
||||||
@@ -1,7 +0,0 @@
|
|||||||
#!/usr/bin/env python3
|
|
||||||
"""Geofence Node - ROS 2 wrapper."""
|
|
||||||
|
|
||||||
from src.safety.geofence_monitor import GeofenceMonitor, main
|
|
||||||
|
|
||||||
if __name__ == '__main__':
|
|
||||||
main()
|
|
||||||
@@ -1,112 +0,0 @@
|
|||||||
#!/usr/bin/env python3
|
|
||||||
"""Multi-Vehicle Coordination Node."""
|
|
||||||
|
|
||||||
import rclpy
|
|
||||||
from rclpy.node import Node
|
|
||||||
from geometry_msgs.msg import PoseStamped, TwistStamped
|
|
||||||
from std_msgs.msg import String, Bool
|
|
||||||
import numpy as np
|
|
||||||
|
|
||||||
|
|
||||||
class MultiVehicleCoordinator(Node):
|
|
||||||
|
|
||||||
def __init__(self):
|
|
||||||
super().__init__('multi_vehicle_coord')
|
|
||||||
|
|
||||||
self.declare_parameter('min_separation', 3.0)
|
|
||||||
self.declare_parameter('coordination_mode', 'leader_follower')
|
|
||||||
|
|
||||||
self.min_separation = self.get_parameter('min_separation').value
|
|
||||||
self.coord_mode = self.get_parameter('coordination_mode').value
|
|
||||||
|
|
||||||
self.uav_pose = None
|
|
||||||
self.ugv_pose = None
|
|
||||||
|
|
||||||
self.uav_pose_sub = self.create_subscription(
|
|
||||||
PoseStamped, '/uav/local_position/pose', self.uav_pose_callback, 10)
|
|
||||||
|
|
||||||
self.ugv_pose_sub = self.create_subscription(
|
|
||||||
PoseStamped, '/ugv/local_position/pose', self.ugv_pose_callback, 10)
|
|
||||||
|
|
||||||
self.uav_cmd_pub = self.create_publisher(String, '/uav/controller/command', 10)
|
|
||||||
self.ugv_cmd_pub = self.create_publisher(String, '/ugv/controller/command', 10)
|
|
||||||
self.uav_goal_pub = self.create_publisher(PoseStamped, '/uav/setpoint_position', 10)
|
|
||||||
self.ugv_goal_pub = self.create_publisher(PoseStamped, '/ugv/goal_pose', 10)
|
|
||||||
|
|
||||||
self.collision_warning_pub = self.create_publisher(Bool, '/coordination/collision_warning', 10)
|
|
||||||
self.status_pub = self.create_publisher(String, '/coordination/status', 10)
|
|
||||||
|
|
||||||
self.timer = self.create_timer(0.1, self.coordination_loop)
|
|
||||||
|
|
||||||
self.get_logger().info(f'Multi-Vehicle Coordinator Started - Mode: {self.coord_mode}')
|
|
||||||
|
|
||||||
def uav_pose_callback(self, msg):
|
|
||||||
self.uav_pose = msg
|
|
||||||
|
|
||||||
def ugv_pose_callback(self, msg):
|
|
||||||
self.ugv_pose = msg
|
|
||||||
|
|
||||||
def coordination_loop(self):
|
|
||||||
if self.uav_pose is None or self.ugv_pose is None:
|
|
||||||
return
|
|
||||||
|
|
||||||
uav_pos = np.array([
|
|
||||||
self.uav_pose.pose.position.x,
|
|
||||||
self.uav_pose.pose.position.y,
|
|
||||||
self.uav_pose.pose.position.z
|
|
||||||
])
|
|
||||||
|
|
||||||
ugv_pos = np.array([
|
|
||||||
self.ugv_pose.pose.position.x,
|
|
||||||
self.ugv_pose.pose.position.y,
|
|
||||||
0.0
|
|
||||||
])
|
|
||||||
|
|
||||||
horizontal_distance = np.linalg.norm(uav_pos[:2] - ugv_pos[:2])
|
|
||||||
|
|
||||||
collision_warning = Bool()
|
|
||||||
collision_warning.data = horizontal_distance < self.min_separation
|
|
||||||
self.collision_warning_pub.publish(collision_warning)
|
|
||||||
|
|
||||||
if collision_warning.data:
|
|
||||||
self.get_logger().warn(f'Vehicles too close: {horizontal_distance:.2f}m')
|
|
||||||
|
|
||||||
status = String()
|
|
||||||
status.data = f'separation:{horizontal_distance:.2f}|mode:{self.coord_mode}'
|
|
||||||
self.status_pub.publish(status)
|
|
||||||
|
|
||||||
if self.coord_mode == 'leader_follower':
|
|
||||||
self.leader_follower_control(uav_pos, ugv_pos)
|
|
||||||
elif self.coord_mode == 'formation':
|
|
||||||
self.formation_control(uav_pos, ugv_pos)
|
|
||||||
|
|
||||||
def leader_follower_control(self, uav_pos, ugv_pos):
|
|
||||||
offset = np.array([0.0, 0.0, 5.0])
|
|
||||||
target_uav_pos = ugv_pos + offset
|
|
||||||
|
|
||||||
goal = PoseStamped()
|
|
||||||
goal.header.stamp = self.get_clock().now().to_msg()
|
|
||||||
goal.header.frame_id = 'odom'
|
|
||||||
goal.pose.position.x = float(target_uav_pos[0])
|
|
||||||
goal.pose.position.y = float(target_uav_pos[1])
|
|
||||||
goal.pose.position.z = float(target_uav_pos[2])
|
|
||||||
goal.pose.orientation.w = 1.0
|
|
||||||
|
|
||||||
def formation_control(self, uav_pos, ugv_pos):
|
|
||||||
pass
|
|
||||||
|
|
||||||
|
|
||||||
def main(args=None):
|
|
||||||
rclpy.init(args=args)
|
|
||||||
node = MultiVehicleCoordinator()
|
|
||||||
try:
|
|
||||||
rclpy.spin(node)
|
|
||||||
except KeyboardInterrupt:
|
|
||||||
pass
|
|
||||||
finally:
|
|
||||||
node.destroy_node()
|
|
||||||
rclpy.shutdown()
|
|
||||||
|
|
||||||
|
|
||||||
if __name__ == '__main__':
|
|
||||||
main()
|
|
||||||
@@ -1,134 +0,0 @@
|
|||||||
#!/usr/bin/env python3
|
|
||||||
"""Vision-Based Navigation Node."""
|
|
||||||
|
|
||||||
import rclpy
|
|
||||||
from rclpy.node import Node
|
|
||||||
from sensor_msgs.msg import Image
|
|
||||||
from geometry_msgs.msg import PoseStamped, TwistStamped
|
|
||||||
from mavros_msgs.srv import CommandBool, SetMode
|
|
||||||
from mavros_msgs.msg import State
|
|
||||||
from cv_bridge import CvBridge
|
|
||||||
import numpy as np
|
|
||||||
|
|
||||||
|
|
||||||
class VisionNavNode(Node):
|
|
||||||
|
|
||||||
def __init__(self):
|
|
||||||
super().__init__('vision_nav_node')
|
|
||||||
|
|
||||||
self.bridge = CvBridge()
|
|
||||||
|
|
||||||
self.current_image = None
|
|
||||||
self.current_local_pose = None
|
|
||||||
self.current_vo_pose = None
|
|
||||||
self.is_armed = False
|
|
||||||
self.current_mode = ""
|
|
||||||
|
|
||||||
self.waypoints = [
|
|
||||||
{'x': 0.0, 'y': 0.0, 'z': 5.0},
|
|
||||||
{'x': 10.0, 'y': 0.0, 'z': 5.0},
|
|
||||||
{'x': 10.0, 'y': 10.0, 'z': 5.0},
|
|
||||||
{'x': 0.0, 'y': 10.0, 'z': 5.0},
|
|
||||||
{'x': 0.0, 'y': 0.0, 'z': 5.0},
|
|
||||||
{'x': 0.0, 'y': 0.0, 'z': 0.0},
|
|
||||||
]
|
|
||||||
|
|
||||||
self.current_waypoint_idx = 0
|
|
||||||
self.waypoint_tolerance = 0.5
|
|
||||||
|
|
||||||
self.image_sub = self.create_subscription(
|
|
||||||
Image, '/uav/camera/forward/image_raw', self.image_callback, 10)
|
|
||||||
|
|
||||||
self.local_pose_sub = self.create_subscription(
|
|
||||||
PoseStamped, '/uav/mavros/local_position/pose', self.local_pose_callback, 10)
|
|
||||||
|
|
||||||
self.vo_pose_sub = self.create_subscription(
|
|
||||||
PoseStamped, '/uav/visual_odometry/pose', self.vo_pose_callback, 10)
|
|
||||||
|
|
||||||
self.state_sub = self.create_subscription(
|
|
||||||
State, '/uav/mavros/state', self.state_callback, 10)
|
|
||||||
|
|
||||||
self.setpoint_local_pub = self.create_publisher(
|
|
||||||
PoseStamped, '/uav/mavros/setpoint_position/local', 10)
|
|
||||||
|
|
||||||
self.setpoint_vel_pub = self.create_publisher(
|
|
||||||
TwistStamped, '/uav/mavros/setpoint_velocity/cmd_vel', 10)
|
|
||||||
|
|
||||||
self.arming_client = self.create_client(CommandBool, '/uav/mavros/cmd/arming')
|
|
||||||
self.set_mode_client = self.create_client(SetMode, '/uav/mavros/set_mode')
|
|
||||||
|
|
||||||
self.control_timer = self.create_timer(0.05, self.control_loop)
|
|
||||||
self.setpoint_timer = self.create_timer(0.1, self.publish_setpoint)
|
|
||||||
|
|
||||||
self.get_logger().info('Vision Navigation Node Started - GPS-DENIED mode')
|
|
||||||
|
|
||||||
def image_callback(self, msg):
|
|
||||||
try:
|
|
||||||
self.current_image = self.bridge.imgmsg_to_cv2(msg, 'bgr8')
|
|
||||||
except Exception as e:
|
|
||||||
self.get_logger().error(f'Image conversion failed: {e}')
|
|
||||||
|
|
||||||
def local_pose_callback(self, msg):
|
|
||||||
self.current_local_pose = msg
|
|
||||||
|
|
||||||
def vo_pose_callback(self, msg):
|
|
||||||
self.current_vo_pose = msg
|
|
||||||
|
|
||||||
def state_callback(self, msg):
|
|
||||||
self.is_armed = msg.armed
|
|
||||||
self.current_mode = msg.mode
|
|
||||||
|
|
||||||
def publish_setpoint(self):
|
|
||||||
if self.current_waypoint_idx >= len(self.waypoints):
|
|
||||||
return
|
|
||||||
|
|
||||||
waypoint = self.waypoints[self.current_waypoint_idx]
|
|
||||||
|
|
||||||
setpoint = PoseStamped()
|
|
||||||
setpoint.header.stamp = self.get_clock().now().to_msg()
|
|
||||||
setpoint.header.frame_id = 'map'
|
|
||||||
|
|
||||||
setpoint.pose.position.x = waypoint['x']
|
|
||||||
setpoint.pose.position.y = waypoint['y']
|
|
||||||
setpoint.pose.position.z = waypoint['z']
|
|
||||||
setpoint.pose.orientation.w = 1.0
|
|
||||||
|
|
||||||
self.setpoint_local_pub.publish(setpoint)
|
|
||||||
|
|
||||||
def control_loop(self):
|
|
||||||
if self.current_local_pose is None:
|
|
||||||
return
|
|
||||||
|
|
||||||
if self.current_waypoint_idx < len(self.waypoints):
|
|
||||||
waypoint = self.waypoints[self.current_waypoint_idx]
|
|
||||||
|
|
||||||
dx = waypoint['x'] - self.current_local_pose.pose.position.x
|
|
||||||
dy = waypoint['y'] - self.current_local_pose.pose.position.y
|
|
||||||
dz = waypoint['z'] - self.current_local_pose.pose.position.z
|
|
||||||
|
|
||||||
distance = np.sqrt(dx**2 + dy**2 + dz**2)
|
|
||||||
|
|
||||||
if distance < self.waypoint_tolerance:
|
|
||||||
self.get_logger().info(
|
|
||||||
f'Reached waypoint {self.current_waypoint_idx}: '
|
|
||||||
f'[{waypoint["x"]}, {waypoint["y"]}, {waypoint["z"]}] (LOCAL coordinates)')
|
|
||||||
self.current_waypoint_idx += 1
|
|
||||||
|
|
||||||
if self.current_waypoint_idx >= len(self.waypoints):
|
|
||||||
self.get_logger().info('Mission complete!')
|
|
||||||
|
|
||||||
|
|
||||||
def main(args=None):
|
|
||||||
rclpy.init(args=args)
|
|
||||||
node = VisionNavNode()
|
|
||||||
try:
|
|
||||||
rclpy.spin(node)
|
|
||||||
except KeyboardInterrupt:
|
|
||||||
pass
|
|
||||||
finally:
|
|
||||||
node.destroy_node()
|
|
||||||
rclpy.shutdown()
|
|
||||||
|
|
||||||
|
|
||||||
if __name__ == '__main__':
|
|
||||||
main()
|
|
||||||
@@ -1,7 +0,0 @@
|
|||||||
#!/usr/bin/env python3
|
|
||||||
"""Visual Odometry Node - ROS 2 wrapper."""
|
|
||||||
|
|
||||||
from src.vision.visual_odometry import VisualOdometryNode, main
|
|
||||||
|
|
||||||
if __name__ == '__main__':
|
|
||||||
main()
|
|
||||||
@@ -1,21 +1,37 @@
|
|||||||
#!/usr/bin/env python3
|
#!/usr/bin/env python3
|
||||||
|
"""Run recorder — captures simulation video and logs to a results folder.
|
||||||
|
|
||||||
|
Each run gets a folder like results/run_1/ containing:
|
||||||
|
- log.txt Process logs (stdout/stderr)
|
||||||
|
- flight_path.avi Flight tracker video
|
||||||
|
- flight_path.png Final flight path image
|
||||||
|
- camera.avi Downward camera video
|
||||||
|
- camera_*.png Camera snapshots at key moments
|
||||||
|
- gazebo.avi Gazebo simulation window recording (via xwd)
|
||||||
|
"""
|
||||||
|
|
||||||
import os
|
import os
|
||||||
import sys
|
import sys
|
||||||
import io
|
import io
|
||||||
import json
|
|
||||||
import time
|
import time
|
||||||
import threading
|
import threading
|
||||||
import cv2
|
import cv2
|
||||||
import numpy as np
|
import numpy as np
|
||||||
|
import subprocess
|
||||||
|
import re
|
||||||
from pathlib import Path
|
from pathlib import Path
|
||||||
from datetime import datetime
|
from datetime import datetime
|
||||||
|
|
||||||
|
try:
|
||||||
|
import PIL.Image
|
||||||
|
HAS_PIL = True
|
||||||
|
except ImportError:
|
||||||
|
HAS_PIL = False
|
||||||
|
|
||||||
class RunRecorder:
|
class RunRecorder:
|
||||||
def __init__(self, results_dir=None, fps=5):
|
def __init__(self, results_dir=None, fps=5):
|
||||||
if results_dir is None:
|
if results_dir is None:
|
||||||
project_dir = Path(__file__).resolve().parent.parent
|
project_dir = Path(__file__).resolve().parent.parent.parent
|
||||||
results_dir = project_dir / "results"
|
results_dir = project_dir / "results"
|
||||||
|
|
||||||
results_dir = Path(results_dir)
|
results_dir = Path(results_dir)
|
||||||
@@ -47,11 +63,15 @@ class RunRecorder:
|
|||||||
|
|
||||||
self._tracker_writer = None
|
self._tracker_writer = None
|
||||||
self._camera_writer = None
|
self._camera_writer = None
|
||||||
|
self._gazebo_writer = None
|
||||||
|
|
||||||
self._tracker_size = None
|
self._tracker_size = None
|
||||||
self._camera_size = None
|
self._camera_size = None
|
||||||
|
self._gazebo_size = None
|
||||||
|
|
||||||
self._tracker_frames = 0
|
self._tracker_frames = 0
|
||||||
self._camera_frames = 0
|
self._camera_frames = 0
|
||||||
|
self._gazebo_frames = 0
|
||||||
|
|
||||||
self._last_tracker_frame = None
|
self._last_tracker_frame = None
|
||||||
self._last_camera_frame = None
|
self._last_camera_frame = None
|
||||||
@@ -63,6 +83,10 @@ class RunRecorder:
|
|||||||
self._record_thread = None
|
self._record_thread = None
|
||||||
self._lock = threading.Lock()
|
self._lock = threading.Lock()
|
||||||
|
|
||||||
|
# Gazebo window capture via xwd
|
||||||
|
self._gazebo_wid = None
|
||||||
|
self._find_gazebo_window()
|
||||||
|
|
||||||
self.metadata = {
|
self.metadata = {
|
||||||
"run": run_num,
|
"run": run_num,
|
||||||
"start_time": datetime.now().isoformat(),
|
"start_time": datetime.now().isoformat(),
|
||||||
@@ -71,6 +95,37 @@ class RunRecorder:
|
|||||||
|
|
||||||
print(f"[REC] Recording to: {self.run_dir}")
|
print(f"[REC] Recording to: {self.run_dir}")
|
||||||
|
|
||||||
|
def _find_gazebo_window(self):
|
||||||
|
"""Attempts to find the Gazebo window ID using xwininfo."""
|
||||||
|
try:
|
||||||
|
# Find window ID
|
||||||
|
cmd = "xwininfo -root -tree | grep -i 'gazebo\|gz sim'"
|
||||||
|
output = subprocess.check_output(cmd, shell=True).decode('utf-8')
|
||||||
|
lines = output.strip().split('\n')
|
||||||
|
if not lines:
|
||||||
|
print("[REC] Gazebo window not found")
|
||||||
|
return
|
||||||
|
|
||||||
|
# Pick the first one
|
||||||
|
wid_line = lines[0]
|
||||||
|
wid_match = re.search(r'(0x[0-9a-fA-F]+)', wid_line)
|
||||||
|
if not wid_match:
|
||||||
|
return
|
||||||
|
self._gazebo_wid = wid_match.group(1)
|
||||||
|
print(f"[REC] Found Gazebo window ID: {self._gazebo_wid}")
|
||||||
|
|
||||||
|
except Exception as e:
|
||||||
|
print(f"[REC] Error finding Gazebo window: {e}")
|
||||||
|
|
||||||
|
def set_phase(self, phase: str):
|
||||||
|
# No-op since CSV logging is removed
|
||||||
|
pass
|
||||||
|
|
||||||
|
def log_position(self, uav_x=0, uav_y=0, uav_alt=0, uav_heading=0,
|
||||||
|
ugv_x=0, ugv_y=0):
|
||||||
|
# No-op since CSV logging is removed
|
||||||
|
pass
|
||||||
|
|
||||||
def start_logging(self):
|
def start_logging(self):
|
||||||
sys.stdout = self._tee_stdout
|
sys.stdout = self._tee_stdout
|
||||||
sys.stderr = self._tee_stderr
|
sys.stderr = self._tee_stderr
|
||||||
@@ -83,6 +138,11 @@ class RunRecorder:
|
|||||||
self._tracker_ref = tracker
|
self._tracker_ref = tracker
|
||||||
self._camera_ref = camera
|
self._camera_ref = camera
|
||||||
self._recording = True
|
self._recording = True
|
||||||
|
|
||||||
|
# Re-check Gazebo window
|
||||||
|
if not self._gazebo_wid:
|
||||||
|
self._find_gazebo_window()
|
||||||
|
|
||||||
self._record_thread = threading.Thread(
|
self._record_thread = threading.Thread(
|
||||||
target=self._record_loop, daemon=True
|
target=self._record_loop, daemon=True
|
||||||
)
|
)
|
||||||
@@ -111,6 +171,33 @@ class RunRecorder:
|
|||||||
except Exception:
|
except Exception:
|
||||||
pass
|
pass
|
||||||
|
|
||||||
|
# Gazebo Capture via xwd
|
||||||
|
if self._gazebo_wid and HAS_PIL:
|
||||||
|
try:
|
||||||
|
# Capture window to stdout using xwd
|
||||||
|
proc = subprocess.run(
|
||||||
|
['xwd', '-id', self._gazebo_wid, '-out', '/dev/stdout'],
|
||||||
|
stdout=subprocess.PIPE,
|
||||||
|
stderr=subprocess.PIPE
|
||||||
|
)
|
||||||
|
if proc.returncode == 0:
|
||||||
|
# Parse XWD using PIL
|
||||||
|
img_pil = PIL.Image.open(io.BytesIO(proc.stdout))
|
||||||
|
# Convert to OpenCV (RGB -> BGR)
|
||||||
|
img_np = np.array(img_pil)
|
||||||
|
# PIL opens XWD as RGB usually, check mode
|
||||||
|
if img_pil.mode == 'RGB':
|
||||||
|
img_bgr = cv2.cvtColor(img_np, cv2.COLOR_RGB2BGR)
|
||||||
|
elif img_pil.mode == 'RGBA':
|
||||||
|
img_bgr = cv2.cvtColor(img_np, cv2.COLOR_RGBA2BGR)
|
||||||
|
else:
|
||||||
|
img_bgr = cv2.cvtColor(img_np, cv2.COLOR_RGB2BGR) # Fallback
|
||||||
|
|
||||||
|
self._write_gazebo_frame(img_bgr)
|
||||||
|
except Exception:
|
||||||
|
# xwd failed or window closed
|
||||||
|
pass
|
||||||
|
|
||||||
elapsed = time.time() - t0
|
elapsed = time.time() - t0
|
||||||
sleep_time = max(0, interval - elapsed)
|
sleep_time = max(0, interval - elapsed)
|
||||||
time.sleep(sleep_time)
|
time.sleep(sleep_time)
|
||||||
@@ -135,6 +222,25 @@ class RunRecorder:
|
|||||||
self._camera_writer.write(frame)
|
self._camera_writer.write(frame)
|
||||||
self._camera_frames += 1
|
self._camera_frames += 1
|
||||||
|
|
||||||
|
def _write_gazebo_frame(self, frame):
|
||||||
|
h, w = frame.shape[:2]
|
||||||
|
# Ensure dimensions are even
|
||||||
|
if w % 2 != 0: w -= 1
|
||||||
|
if h % 2 != 0: h -= 1
|
||||||
|
frame = frame[:h, :w]
|
||||||
|
|
||||||
|
if self._gazebo_writer is None:
|
||||||
|
self._gazebo_size = (w, h)
|
||||||
|
fourcc = cv2.VideoWriter_fourcc(*'XVID')
|
||||||
|
path = str(self.run_dir / "gazebo.avi")
|
||||||
|
self._gazebo_writer = cv2.VideoWriter(path, fourcc, self.fps, (w, h))
|
||||||
|
elif (w, h) != self._gazebo_size:
|
||||||
|
# Handle resize? Just skip for now or resize
|
||||||
|
frame = cv2.resize(frame, self._gazebo_size)
|
||||||
|
|
||||||
|
self._gazebo_writer.write(frame)
|
||||||
|
self._gazebo_frames += 1
|
||||||
|
|
||||||
def snapshot_camera(self, label="snapshot"):
|
def snapshot_camera(self, label="snapshot"):
|
||||||
if self._camera_ref is None:
|
if self._camera_ref is None:
|
||||||
return
|
return
|
||||||
@@ -148,50 +254,20 @@ class RunRecorder:
|
|||||||
self._camera_snapshots.append(filename)
|
self._camera_snapshots.append(filename)
|
||||||
print(f"[REC] Snapshot: {filename}")
|
print(f"[REC] Snapshot: {filename}")
|
||||||
|
|
||||||
|
def snapshot_tracker(self, label="tracker"):
|
||||||
|
"""Save a snapshot of the current flight tracker."""
|
||||||
|
if self._last_tracker_frame is not None:
|
||||||
|
filename = f"tracker_{label}.png"
|
||||||
|
path = self.run_dir / filename
|
||||||
|
cv2.imwrite(str(path), self._last_tracker_frame)
|
||||||
|
print(f"[REC] Tracker snapshot: {filename}")
|
||||||
|
|
||||||
def save_summary(self, search_mode="", altitude=0, markers=None,
|
def save_summary(self, search_mode="", altitude=0, markers=None,
|
||||||
landed=False, extra=None):
|
landed=False, ugv_dispatched=False,
|
||||||
duration = time.time() - self.start_time
|
ugv_target=None, extra=None):
|
||||||
mins = int(duration // 60)
|
# Summary JSON file saving moved/removed as per request
|
||||||
secs = int(duration % 60)
|
# Still useful to see basic stats in terminal?
|
||||||
|
pass
|
||||||
summary = {
|
|
||||||
**self.metadata,
|
|
||||||
"end_time": datetime.now().isoformat(),
|
|
||||||
"duration_seconds": round(duration, 1),
|
|
||||||
"duration_display": f"{mins}m {secs}s",
|
|
||||||
"search_mode": search_mode,
|
|
||||||
"altitude": altitude,
|
|
||||||
"landed": landed,
|
|
||||||
"markers_found": {},
|
|
||||||
"recordings": {
|
|
||||||
"log": "log.txt",
|
|
||||||
"flight_path_video": "flight_path.avi",
|
|
||||||
"flight_path_image": "flight_path.png",
|
|
||||||
"camera_video": "camera.avi",
|
|
||||||
"camera_snapshots": self._camera_snapshots,
|
|
||||||
},
|
|
||||||
"frame_counts": {
|
|
||||||
"tracker": self._tracker_frames,
|
|
||||||
"camera": self._camera_frames,
|
|
||||||
},
|
|
||||||
}
|
|
||||||
|
|
||||||
if markers:
|
|
||||||
for mid, info in markers.items():
|
|
||||||
pos = info.get('uav_position', {})
|
|
||||||
summary["markers_found"][str(mid)] = {
|
|
||||||
"x": round(pos.get('x', 0), 2),
|
|
||||||
"y": round(pos.get('y', 0), 2),
|
|
||||||
"distance": round(info.get('distance', 0), 2),
|
|
||||||
}
|
|
||||||
|
|
||||||
if extra:
|
|
||||||
summary.update(extra)
|
|
||||||
|
|
||||||
path = self.run_dir / "summary.json"
|
|
||||||
with open(path, "w") as f:
|
|
||||||
json.dump(summary, f, indent=2)
|
|
||||||
print(f"[REC] Summary saved: {path}")
|
|
||||||
|
|
||||||
def stop(self):
|
def stop(self):
|
||||||
self._recording = False
|
self._recording = False
|
||||||
@@ -211,6 +287,8 @@ class RunRecorder:
|
|||||||
self._tracker_writer.release()
|
self._tracker_writer.release()
|
||||||
if self._camera_writer:
|
if self._camera_writer:
|
||||||
self._camera_writer.release()
|
self._camera_writer.release()
|
||||||
|
if self._gazebo_writer:
|
||||||
|
self._gazebo_writer.release()
|
||||||
|
|
||||||
self.stop_logging()
|
self.stop_logging()
|
||||||
self._log_file.close()
|
self._log_file.close()
|
||||||
@@ -220,10 +298,14 @@ class RunRecorder:
|
|||||||
secs = int(duration % 60)
|
secs = int(duration % 60)
|
||||||
|
|
||||||
self._original_stdout.write(
|
self._original_stdout.write(
|
||||||
f"\n[REC] Run recorded: {self.run_dir}\n"
|
f"\n[REC] ═══════════════════════════════════════\n"
|
||||||
f"[REC] Duration: {mins}m {secs}s | "
|
f"[REC] Run #{self.run_num} recorded\n"
|
||||||
f"Tracker: {self._tracker_frames} frames | "
|
f"[REC] Dir: {self.run_dir}\n"
|
||||||
f"Camera: {self._camera_frames} frames\n"
|
f"[REC] Duration: {mins}m {secs}s\n"
|
||||||
|
f"[REC] Tracker: {self._tracker_frames} frames\n"
|
||||||
|
f"[REC] Camera: {self._camera_frames} frames\n"
|
||||||
|
f"[REC] Gazebo: {self._gazebo_frames} frames\n"
|
||||||
|
f"[REC] ═══════════════════════════════════════\n"
|
||||||
)
|
)
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
BIN
worlds/tags/aruco_DICT_4X4_50_1.png
Normal file
BIN
worlds/tags/aruco_DICT_4X4_50_1.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 5.7 KiB |
@@ -29,6 +29,7 @@
|
|||||||
</plugin>
|
</plugin>
|
||||||
<plugin filename="gz-sim-scene-broadcaster-system"
|
<plugin filename="gz-sim-scene-broadcaster-system"
|
||||||
name="gz::sim::systems::SceneBroadcaster">
|
name="gz::sim::systems::SceneBroadcaster">
|
||||||
|
<state_hertz>25</state_hertz>
|
||||||
</plugin>
|
</plugin>
|
||||||
<plugin filename="gz-sim-imu-system"
|
<plugin filename="gz-sim-imu-system"
|
||||||
name="gz::sim::systems::Imu">
|
name="gz::sim::systems::Imu">
|
||||||
@@ -142,81 +143,35 @@
|
|||||||
</model>
|
</model>
|
||||||
|
|
||||||
<!-- ===================== UAV: ArduPilot Iris ===================== -->
|
<!-- ===================== UAV: ArduPilot Iris ===================== -->
|
||||||
<!-- Uses the ArduPilot iris_with_gimbal model which includes:
|
<!-- Starts on top of the UGV. Body top z=0.18 + iris legs 0.195 = 0.375 -->
|
||||||
- ArduPilotPlugin (connects to SITL)
|
|
||||||
- IMU sensor
|
|
||||||
- Gimbal with camera
|
|
||||||
- Rotor dynamics / LiftDrag
|
|
||||||
-->
|
|
||||||
<include>
|
<include>
|
||||||
<uri>model://iris_with_gimbal</uri>
|
<uri>model://iris_with_gimbal</uri>
|
||||||
<pose degrees="true">0 0 0.195 0 0 90</pose>
|
<pose degrees="true">0.0 0.0 0.4 0 0 90</pose>
|
||||||
</include>
|
</include>
|
||||||
|
|
||||||
|
<!-- ===================== UGV: Driveable Rover ===================== -->
|
||||||
|
<!-- Dynamic 4-wheel skid-steer, drone takes off from it, then it drives -->
|
||||||
|
<include>
|
||||||
|
<uri>model://custom_ugv</uri>
|
||||||
|
<name>ugv</name>
|
||||||
|
<pose>0.0 0.0 0 0 0 0</pose>
|
||||||
|
</include>
|
||||||
|
|
||||||
<!-- ===================== UGV: Target Vehicle ===================== -->
|
<!-- ===================== TARGET ArUco Tag (ID 1) ===================== -->
|
||||||
<!-- The drone should search for this vehicle and land on it -->
|
<!-- On the ground — the UAV searches for this, then UGV drives here -->
|
||||||
<model name="ugv_target">
|
<model name="target_tag_1">
|
||||||
<static>true</static>
|
<static>true</static>
|
||||||
<pose>5.0 5.0 0 0 0 0</pose>
|
<pose>8 -6 0.005 0 0 0</pose>
|
||||||
|
<link name="link">
|
||||||
<!-- Main body (blue box) -->
|
<visual name="v">
|
||||||
<link name="base_link">
|
<geometry><box><size>0.5 0.5 0.01</size></box></geometry>
|
||||||
<pose>0 0 0.2 0 0 0</pose>
|
|
||||||
<visual name="body">
|
|
||||||
<geometry><box><size>1.0 0.7 0.3</size></box></geometry>
|
|
||||||
<material>
|
|
||||||
<ambient>0.2 0.2 0.7 1</ambient>
|
|
||||||
<diffuse>0.2 0.2 0.8 1</diffuse>
|
|
||||||
</material>
|
|
||||||
</visual>
|
|
||||||
<collision name="body_collision">
|
|
||||||
<geometry><box><size>1.0 0.7 0.3</size></box></geometry>
|
|
||||||
</collision>
|
|
||||||
</link>
|
|
||||||
|
|
||||||
<!-- ArUco landing tag on top -->
|
|
||||||
<link name="aruco_tag">
|
|
||||||
<pose>0 0 0.36 0 0 0</pose>
|
|
||||||
<visual name="aruco_visual">
|
|
||||||
<geometry><box><size>0.5 0.5 0.005</size></box></geometry>
|
|
||||||
<material>
|
<material>
|
||||||
<ambient>1 1 1 1</ambient>
|
<ambient>1 1 1 1</ambient>
|
||||||
<diffuse>1 1 1 1</diffuse>
|
<diffuse>1 1 1 1</diffuse>
|
||||||
<pbr>
|
<pbr><metal><albedo_map>tags/aruco_DICT_4X4_50_1.png</albedo_map></metal></pbr>
|
||||||
<metal>
|
|
||||||
<albedo_map>tags/land_tag.png</albedo_map>
|
|
||||||
</metal>
|
|
||||||
</pbr>
|
|
||||||
</material>
|
</material>
|
||||||
</visual>
|
</visual>
|
||||||
</link>
|
</link>
|
||||||
|
|
||||||
<!-- Wheels (visual only since static) -->
|
|
||||||
<link name="wheel_fl">
|
|
||||||
<pose>0.35 0.38 0.1 1.5708 0 0</pose>
|
|
||||||
<visual name="v"><geometry><cylinder><radius>0.1</radius><length>0.06</length></cylinder></geometry>
|
|
||||||
<material><ambient>0.15 0.15 0.15 1</ambient><diffuse>0.15 0.15 0.15 1</diffuse></material>
|
|
||||||
</visual>
|
|
||||||
</link>
|
|
||||||
<link name="wheel_fr">
|
|
||||||
<pose>0.35 -0.38 0.1 1.5708 0 0</pose>
|
|
||||||
<visual name="v"><geometry><cylinder><radius>0.1</radius><length>0.06</length></cylinder></geometry>
|
|
||||||
<material><ambient>0.15 0.15 0.15 1</ambient><diffuse>0.15 0.15 0.15 1</diffuse></material>
|
|
||||||
</visual>
|
|
||||||
</link>
|
|
||||||
<link name="wheel_rl">
|
|
||||||
<pose>-0.35 0.38 0.1 1.5708 0 0</pose>
|
|
||||||
<visual name="v"><geometry><cylinder><radius>0.1</radius><length>0.06</length></cylinder></geometry>
|
|
||||||
<material><ambient>0.15 0.15 0.15 1</ambient><diffuse>0.15 0.15 0.15 1</diffuse></material>
|
|
||||||
</visual>
|
|
||||||
</link>
|
|
||||||
<link name="wheel_rr">
|
|
||||||
<pose>-0.35 -0.38 0.1 1.5708 0 0</pose>
|
|
||||||
<visual name="v"><geometry><cylinder><radius>0.1</radius><length>0.06</length></cylinder></geometry>
|
|
||||||
<material><ambient>0.15 0.15 0.15 1</ambient><diffuse>0.15 0.15 0.15 1</diffuse></material>
|
|
||||||
</visual>
|
|
||||||
</link>
|
|
||||||
</model>
|
</model>
|
||||||
|
|
||||||
<!-- ===================== VISUAL MARKERS ===================== -->
|
<!-- ===================== VISUAL MARKERS ===================== -->
|
||||||
|
|||||||
Reference in New Issue
Block a user