Code reorganization. Display recording fixes. Search Flight Planner Fixes. Bug Fixes
This commit is contained in:
@@ -18,6 +18,7 @@ A ROS 2 simulation environment for UAV and UGV development using GPS-denied navi
|
||||
- 16GB RAM recommended
|
||||
- 50GB disk space
|
||||
- NVIDIA GPU recommended (software rendering available)
|
||||
- `ffmpeg`, `x11-utils`, `xdotool` (for simulation recording)
|
||||
|
||||
## Installation
|
||||
|
||||
|
||||
@@ -1,37 +1,23 @@
|
||||
# ─── Mission / Search Configuration ──────────────────────────
|
||||
# Single source of truth for all mission parameters.
|
||||
|
||||
# ── Flight ───────────────────────────────────────────────────
|
||||
altitude: 5.0 # Search altitude (feet)
|
||||
|
||||
# ── ArUco Marker ─────────────────────────────────────────────
|
||||
altitude: 5.0
|
||||
marker:
|
||||
dictionary: DICT_4X4_50
|
||||
size: 0.5 # Physical marker size in meters
|
||||
landing_ids: [0] # Marker IDs that trigger landing (on UGV)
|
||||
target_ids: [1] # Marker IDs to find and report to UGV
|
||||
target_position: [5.0, 3.0] # Initial X, Y location of the target Aruco map in the map
|
||||
|
||||
# ── Search Patterns ──────────────────────────────────────────
|
||||
size: 0.5
|
||||
landing_ids: [0]
|
||||
target_ids: [1]
|
||||
target_position: [5.0, 3.0]
|
||||
spiral:
|
||||
max_legs: 35
|
||||
|
||||
lawnmower:
|
||||
width: 30.0
|
||||
height: 30.0
|
||||
|
||||
levy:
|
||||
max_steps: 40
|
||||
field_size: 50.0
|
||||
|
||||
# ── Geofence ─────────────────────────────────────────────────
|
||||
geofence:
|
||||
enabled: true
|
||||
warning_distance: 3.0
|
||||
min_altitude: 0.0
|
||||
max_altitude: 15.0
|
||||
# Polygon vertices in local NED (x=North, y=East) meters
|
||||
# Centered on UAV start position (0, 0)
|
||||
points:
|
||||
- [-15, -15]
|
||||
- [-15, 15]
|
||||
|
||||
@@ -1,11 +1,7 @@
|
||||
# ─── UAV Configuration ───────────────────────────────────────
|
||||
# UAV-specific settings. Mission config is in search.yaml.
|
||||
|
||||
connection:
|
||||
sim: "tcp:127.0.0.1:5760"
|
||||
real: "/dev/ttyAMA0"
|
||||
baud: 57600
|
||||
|
||||
speed:
|
||||
min_mph: 0.2
|
||||
max_mph: 1.0
|
||||
|
||||
@@ -1,17 +1,9 @@
|
||||
# ─── UGV Configuration ───────────────────────────────────────
|
||||
# UGV-specific settings. Mission config is in search.yaml.
|
||||
|
||||
# UGV spawn position in local NED (meters)
|
||||
# The UAV starts on top of this — run_autonomous.sh syncs both
|
||||
position:
|
||||
x: 0.0
|
||||
y: 0.0
|
||||
|
||||
# Gazebo transport topics
|
||||
topics:
|
||||
cmd_vel: /ugv/cmd_vel
|
||||
odometry: /ugv/odometry
|
||||
|
||||
speed:
|
||||
min_mph: 0.5
|
||||
max_mph: 3.5
|
||||
|
||||
@@ -6,6 +6,7 @@
|
||||
- 16GB RAM minimum
|
||||
- 50GB free disk space
|
||||
- Internet connection
|
||||
- `ffmpeg`, `x11-utils`, `xdotool` (installed automatically by `setup.sh`)
|
||||
|
||||
## Automatic Installation
|
||||
|
||||
@@ -33,11 +34,11 @@ bash setup.sh
|
||||
|
||||
## Manual Installation
|
||||
|
||||
### Step 1: Install ROS 2
|
||||
### Step 1: Install ROS 2 and System Dependencies
|
||||
|
||||
```bash
|
||||
# Ubuntu 22.04
|
||||
sudo apt install software-properties-common
|
||||
sudo apt install software-properties-common x11-utils xdotool ffmpeg
|
||||
sudo add-apt-repository universe
|
||||
sudo apt update && sudo apt install curl -y
|
||||
sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg
|
||||
|
||||
@@ -10,6 +10,7 @@ This guide covers running the UAV-UGV simulation on Windows Subsystem for Linux
|
||||
- WSL2 with Ubuntu 22.04
|
||||
- 16GB RAM minimum
|
||||
- Optional: NVIDIA GPU with WSL drivers
|
||||
- `ffmpeg`, `x11-utils`, `xdotool` (installed via `setup.sh` or `apt`)
|
||||
|
||||
## WSL2 Installation
|
||||
|
||||
|
||||
@@ -50,7 +50,6 @@ DICT_SIZES = {
|
||||
"DICT_7X7_1000": 1000,
|
||||
}
|
||||
|
||||
|
||||
def generate_marker(dict_name, marker_id, pixel_size=600, border_bits=1):
|
||||
dict_id = ARUCO_DICTS[dict_name]
|
||||
aruco_dict = cv2.aruco.getPredefinedDictionary(dict_id)
|
||||
@@ -65,7 +64,6 @@ def generate_marker(dict_name, marker_id, pixel_size=600, border_bits=1):
|
||||
padded[border_px:border_px + pixel_size, border_px:border_px + pixel_size] = marker_img
|
||||
return padded
|
||||
|
||||
|
||||
def get_landing_id():
|
||||
search_cfg = CONFIG_DIR / "search.yaml"
|
||||
if search_cfg.exists():
|
||||
@@ -76,7 +74,6 @@ def get_landing_id():
|
||||
return land_ids[0]
|
||||
return 0
|
||||
|
||||
|
||||
def main():
|
||||
parser = argparse.ArgumentParser(description="Generate ArUco marker tags for Gazebo simulation")
|
||||
parser.add_argument("--dict", default="DICT_4X4_50", choices=list(ARUCO_DICTS.keys()))
|
||||
@@ -113,7 +110,7 @@ def main():
|
||||
params = cv2.aruco.DetectorParameters()
|
||||
except AttributeError:
|
||||
params = cv2.aruco.DetectorParameters_create()
|
||||
|
||||
|
||||
params.minMarkerPerimeterRate = 0.01
|
||||
try:
|
||||
params.cornerRefinementMethod = cv2.aruco.CORNER_REFINE_SUBPIX
|
||||
@@ -132,9 +129,8 @@ def main():
|
||||
_, detected_ids, _ = _detect(img)
|
||||
if detected_ids is not None and marker_id in detected_ids.flatten():
|
||||
ok += 1
|
||||
|
||||
print(f"Done. {ok}/{len(ids)} markers verified.")
|
||||
|
||||
print(f"Done. {ok}/{len(ids)} markers verified.")
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
|
||||
@@ -22,10 +22,10 @@ def generate_world(base_filename="uav_ugv_search_base.sdf", output_filename="uav
|
||||
|
||||
ugv_x = ugv_cfg.get("position", {}).get("x", 0.0)
|
||||
ugv_y = ugv_cfg.get("position", {}).get("y", 0.0)
|
||||
|
||||
|
||||
uav_x = ugv_x
|
||||
uav_y = ugv_y
|
||||
uav_z = 0.40 # Start on top of UGV (0.18 top + 0.195 legs)
|
||||
uav_z = 0.40
|
||||
|
||||
marker = search_cfg.get("marker", {})
|
||||
target_pos = marker.get("target_position", [8.0, -6.0])
|
||||
@@ -42,19 +42,19 @@ def generate_world(base_filename="uav_ugv_search_base.sdf", output_filename="uav
|
||||
try:
|
||||
tree = ET.parse(base_path)
|
||||
root = tree.getroot()
|
||||
|
||||
|
||||
world = root.find('world')
|
||||
if world is None:
|
||||
print("[ERROR] No <world> tag found in SDF.")
|
||||
sys.exit(1)
|
||||
|
||||
|
||||
for include in world.findall('include'):
|
||||
uri = include.find('uri')
|
||||
if uri is not None and uri.text == 'model://iris_with_gimbal':
|
||||
pose = include.find('pose')
|
||||
if pose is not None:
|
||||
pose.text = f"{uav_x} {uav_y} {uav_z} 0 0 90"
|
||||
|
||||
|
||||
name = include.find('name')
|
||||
if name is not None and name.text == 'ugv':
|
||||
pose = include.find('pose')
|
||||
@@ -72,7 +72,7 @@ def generate_world(base_filename="uav_ugv_search_base.sdf", output_filename="uav
|
||||
if geofence_cfg.get("enabled", False):
|
||||
points = geofence_cfg.get("points", [])
|
||||
if len(points) >= 3:
|
||||
# Remove old geofence visual if it exists
|
||||
|
||||
for old_model in world.findall('model'):
|
||||
if old_model.get('name') == 'geofence_visual':
|
||||
world.remove(old_model)
|
||||
@@ -80,29 +80,29 @@ def generate_world(base_filename="uav_ugv_search_base.sdf", output_filename="uav
|
||||
gf_model = ET.SubElement(world, "model", name="geofence_visual")
|
||||
ET.SubElement(gf_model, "static").text = "true"
|
||||
link = ET.SubElement(gf_model, "link", name="link")
|
||||
|
||||
|
||||
import math
|
||||
for i in range(len(points)):
|
||||
p1 = points[i]
|
||||
p2 = points[(i + 1) % len(points)]
|
||||
x1, y1 = float(p1[0]), float(p1[1])
|
||||
x2, y2 = float(p2[0]), float(p2[1])
|
||||
|
||||
|
||||
dx = x2 - x1
|
||||
dy = y2 - y1
|
||||
length = math.sqrt(dx*dx + dy*dy)
|
||||
cx = x1 + dx / 2.0
|
||||
cy = y1 + dy / 2.0
|
||||
yaw = math.atan2(dy, dx)
|
||||
|
||||
|
||||
visual = ET.SubElement(link, "visual", name=f"edge_{i}")
|
||||
ET.SubElement(visual, "pose").text = f"{cx} {cy} 0.01 0 0 {yaw}"
|
||||
|
||||
|
||||
geometry = ET.SubElement(visual, "geometry")
|
||||
box = ET.SubElement(geometry, "box")
|
||||
# size is Length(X), Width(Y), Thickness(Z)
|
||||
|
||||
ET.SubElement(box, "size").text = f"{length} 0.2 0.02"
|
||||
|
||||
|
||||
material = ET.SubElement(visual, "material")
|
||||
ET.SubElement(material, "ambient").text = "1 0 0 1"
|
||||
ET.SubElement(material, "diffuse").text = "1 0 0 1"
|
||||
@@ -112,7 +112,7 @@ def generate_world(base_filename="uav_ugv_search_base.sdf", output_filename="uav
|
||||
print(f"[INFO] Generated world file: {output_path}")
|
||||
print(f"[INFO] UGV set to ({ugv_x}, {ugv_y})")
|
||||
print(f"[INFO] Target Marker set to ({target_x}, {target_y})")
|
||||
|
||||
|
||||
except Exception as e:
|
||||
print(f"[ERROR] Failed to parse or write XML: {e}")
|
||||
sys.exit(1)
|
||||
|
||||
@@ -1,9 +1,7 @@
|
||||
#!/bin/bash
|
||||
|
||||
echo "Killing all simulation processes..."
|
||||
|
||||
pkill -9 -f "gz sim" 2>/dev/null || true
|
||||
pkill -9 -f "ruby" 2>/dev/null || true # gz sim uses ruby
|
||||
pkill -9 -f "ruby" 2>/dev/null || true
|
||||
pkill -9 -f "gazebo" 2>/dev/null || true
|
||||
pkill -9 -f "gzserver" 2>/dev/null || true
|
||||
pkill -9 -f "gzclient" 2>/dev/null || true
|
||||
@@ -15,9 +13,7 @@ pkill -9 -f "ArduRover" 2>/dev/null || true
|
||||
pkill -9 -f "arducopter" 2>/dev/null || true
|
||||
pkill -9 -f "autonomous_controller" 2>/dev/null || true
|
||||
pkill -9 -f "ros2" 2>/dev/null || true
|
||||
|
||||
sleep 1
|
||||
|
||||
echo "All processes killed."
|
||||
echo "Remaining ROS nodes:"
|
||||
ros2 node list 2>/dev/null || echo "No ROS nodes running"
|
||||
|
||||
@@ -1,25 +1,19 @@
|
||||
#!/bin/bash
|
||||
|
||||
set -e
|
||||
|
||||
SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
|
||||
PROJECT_DIR="$(dirname "$SCRIPT_DIR")"
|
||||
|
||||
RED='\033[0;31m'
|
||||
GREEN='\033[0;32m'
|
||||
YELLOW='\033[1;33m'
|
||||
CYAN='\033[0;36m'
|
||||
NC='\033[0m'
|
||||
|
||||
print_info() { echo -e "${CYAN}[INFO]${NC} $1"; }
|
||||
print_success() { echo -e "${GREEN}[SUCCESS]${NC} $1"; }
|
||||
print_error() { echo -e "${RED}[ERROR]${NC} $1"; }
|
||||
|
||||
SOFTWARE_RENDER=auto
|
||||
WORLD="uav_ugv_search.sdf"
|
||||
SEARCH="spiral"
|
||||
ALTITUDE=""
|
||||
|
||||
while [[ $# -gt 0 ]]; do
|
||||
case $1 in
|
||||
--software-render) SOFTWARE_RENDER=true; shift ;;
|
||||
@@ -39,7 +33,6 @@ while [[ $# -gt 0 ]]; do
|
||||
*) shift ;;
|
||||
esac
|
||||
done
|
||||
|
||||
cleanup_all() {
|
||||
print_info "Cleaning up ..."
|
||||
pkill -f "camera_viewer.py" 2>/dev/null || true
|
||||
@@ -50,7 +43,6 @@ cleanup_all() {
|
||||
pkill -f "mavproxy" 2>/dev/null || true
|
||||
pkill -f "main.py" 2>/dev/null || true
|
||||
}
|
||||
|
||||
cleanup_sitl() {
|
||||
pkill -f "arducopter" 2>/dev/null || true
|
||||
pkill -f "sim_vehicle.py" 2>/dev/null || true
|
||||
@@ -58,7 +50,6 @@ cleanup_sitl() {
|
||||
pkill -f "main.py" 2>/dev/null || true
|
||||
}
|
||||
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"
|
||||
@@ -66,18 +57,15 @@ 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 GZ_SIM_RESOURCE_PATH="$PROJECT_DIR/models:$PROJECT_DIR/worlds:$HOME/ardupilot_gazebo/models:$HOME/ardupilot_gazebo/worlds"
|
||||
export GZ_SIM_SYSTEM_PLUGIN_PATH=$HOME/ardupilot_gazebo/build
|
||||
|
||||
print_info "Model path: $GZ_SIM_RESOURCE_PATH"
|
||||
if [ -f "$PROJECT_DIR/models/iris_with_gimbal/model.sdf" ]; then
|
||||
print_info "Using LOCAL iris_with_gimbal (with downward camera)"
|
||||
else
|
||||
print_info "Using ardupilot_gazebo iris_with_gimbal (NO downward camera)"
|
||||
fi
|
||||
|
||||
if [ "$SOFTWARE_RENDER" = "auto" ]; then
|
||||
if grep -qi microsoft /proc/version 2>/dev/null; then
|
||||
print_info "WSL detected -> software rendering"
|
||||
@@ -86,15 +74,12 @@ if [ "$SOFTWARE_RENDER" = "auto" ]; then
|
||||
SOFTWARE_RENDER=false
|
||||
fi
|
||||
fi
|
||||
|
||||
if [ "$SOFTWARE_RENDER" = "true" ]; then
|
||||
export LIBGL_ALWAYS_SOFTWARE=1
|
||||
export GALLIUM_DRIVER=llvmpipe
|
||||
export MESA_GL_VERSION_OVERRIDE=3.3
|
||||
fi
|
||||
|
||||
cleanup_all 2>/dev/null
|
||||
|
||||
WORLD_FILE=""
|
||||
if [ -f "$PROJECT_DIR/worlds/$WORLD" ]; then
|
||||
WORLD_FILE="$PROJECT_DIR/worlds/$WORLD"
|
||||
@@ -106,29 +91,23 @@ else
|
||||
print_error "World file not found: $WORLD"
|
||||
exit 1
|
||||
fi
|
||||
|
||||
if [ -f "$PROJECT_DIR/scripts/generate_world.py" ]; then
|
||||
python3 "$PROJECT_DIR/scripts/generate_world.py"
|
||||
fi
|
||||
|
||||
print_info "==================================="
|
||||
print_info " UAV-UGV Simulation"
|
||||
print_info "==================================="
|
||||
print_info "World: $WORLD_FILE"
|
||||
print_info "Mission: $MISSION"
|
||||
echo ""
|
||||
|
||||
GZ_DEFAULT_GUI="/usr/share/gz/gz-sim8/gui/gui.config"
|
||||
GZ_USER_GUI="$HOME/.gz/sim/8/gui.config"
|
||||
CAMERA_PLUGINS="$PROJECT_DIR/config/gui.config"
|
||||
|
||||
rm -rf /tmp/gz-* /tmp/gazebo-* 2>/dev/null
|
||||
|
||||
mkdir -p "$HOME/.gz/sim/8"
|
||||
cp "$GZ_DEFAULT_GUI" "$GZ_USER_GUI"
|
||||
sed -i 's|<start_paused>true</start_paused>|<start_paused>false</start_paused>|' "$GZ_USER_GUI"
|
||||
sed -i 's|show_again="true"|show_again="false"|' "$GZ_USER_GUI"
|
||||
|
||||
cat << 'EOF' >> "$GZ_USER_GUI"
|
||||
<plugin filename="VideoRecorder" name="VideoRecorder">
|
||||
<gz-gui>
|
||||
@@ -141,7 +120,6 @@ cat << 'EOF' >> "$GZ_USER_GUI"
|
||||
<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>
|
||||
@@ -149,58 +127,45 @@ cat << 'EOF' >> "$GZ_USER_GUI"
|
||||
</record_video>
|
||||
</plugin>
|
||||
EOF
|
||||
|
||||
print_info "[1/4] Starting Gazebo ..."
|
||||
gz sim -v4 -r "$WORLD_FILE" &
|
||||
GZ_PID=$!
|
||||
sleep 10
|
||||
|
||||
if ! kill -0 $GZ_PID 2>/dev/null; then
|
||||
print_error "Gazebo failed to start"
|
||||
exit 1
|
||||
fi
|
||||
print_success "Gazebo running (PID: $GZ_PID)"
|
||||
|
||||
print_info "[2/4] Starting ArduPilot SITL ..."
|
||||
cd ~/ardupilot
|
||||
|
||||
SITL_ARGS="-v ArduCopter -f gazebo-iris --model JSON -I0"
|
||||
SITL_ARGS="$SITL_ARGS --no-mavproxy"
|
||||
|
||||
PARAM_FILE="$PROJECT_DIR/config/ardupilot_gps_denied.parm"
|
||||
if [ -f "$PARAM_FILE" ]; then
|
||||
print_info "Loading params: $PARAM_FILE"
|
||||
SITL_ARGS="$SITL_ARGS --add-param-file $PARAM_FILE"
|
||||
fi
|
||||
|
||||
sim_vehicle.py $SITL_ARGS &
|
||||
SITL_PID=$!
|
||||
|
||||
print_info "Waiting for SITL (~20s) ..."
|
||||
sleep 20
|
||||
|
||||
if ! pgrep -f "arducopter" > /dev/null 2>&1; then
|
||||
print_error "ArduPilot SITL failed to start"
|
||||
exit 1
|
||||
fi
|
||||
print_success "ArduPilot SITL running (TCP 5760)"
|
||||
|
||||
print_info "[3/4] Starting main.py ..."
|
||||
cd "$PROJECT_DIR"
|
||||
sleep 3
|
||||
|
||||
MAIN_ARGS="--device sim --connection tcp:127.0.0.1:5760 --search $SEARCH"
|
||||
[ -n "$ALTITUDE" ] && MAIN_ARGS="$MAIN_ARGS --altitude $ALTITUDE"
|
||||
|
||||
python3 src/main.py $MAIN_ARGS &
|
||||
MAIN_PID=$!
|
||||
print_success "main.py running (PID: $MAIN_PID)"
|
||||
|
||||
print_info "[4/4] Starting camera viewer ..."
|
||||
python3 -W ignore:RuntimeWarning -m src.vision.camera_processor down &
|
||||
CAM_PID=$!
|
||||
print_success "Camera viewer running (PID: $CAM_PID)"
|
||||
|
||||
print_info ""
|
||||
print_info "==================================="
|
||||
print_info " Simulation Running"
|
||||
@@ -216,11 +181,8 @@ print_info " target tag, dispatches UGV, lands on UGV"
|
||||
print_info " Search: $SEARCH"
|
||||
print_info " Press Ctrl+C to stop"
|
||||
print_info "==================================="
|
||||
|
||||
wait $MAIN_PID 2>/dev/null
|
||||
|
||||
cleanup_sitl
|
||||
|
||||
print_info ""
|
||||
print_info "=================================="
|
||||
print_info " Search Complete!"
|
||||
@@ -229,5 +191,4 @@ print_info "Gazebo GUI still open."
|
||||
print_info "Press Ctrl+C to exit."
|
||||
print_info "=================================="
|
||||
print_info ""
|
||||
|
||||
wait $GZ_PID 2>/dev/null
|
||||
|
||||
@@ -1,40 +1,22 @@
|
||||
#!/bin/bash
|
||||
# =============================================================================
|
||||
# UAV-UGV Simulation - Run Script
|
||||
# =============================================================================
|
||||
# Launches Gazebo with ArduPilot SITL for GPS-denied navigation testing
|
||||
#
|
||||
# Usage:
|
||||
# ./scripts/run_simulation.sh # Default world
|
||||
# ./scripts/run_simulation.sh --world runway # Runway world
|
||||
# ./scripts/run_simulation.sh --software-render # For WSL/no GPU
|
||||
# =============================================================================
|
||||
|
||||
set -e
|
||||
|
||||
RED='\033[0;31m'
|
||||
GREEN='\033[0;32m'
|
||||
YELLOW='\033[1;33m'
|
||||
BLUE='\033[0;34m'
|
||||
NC='\033[0m'
|
||||
|
||||
SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
|
||||
PROJECT_DIR="$(dirname "$SCRIPT_DIR")"
|
||||
|
||||
echo -e "${BLUE}==========================================${NC}"
|
||||
echo -e "${BLUE} UAV-UGV Simulation${NC}"
|
||||
echo -e "${BLUE} GPS-Denied Navigation with Geofencing${NC}"
|
||||
echo -e "${BLUE}==========================================${NC}"
|
||||
echo ""
|
||||
|
||||
# Detect WSL
|
||||
IS_WSL=false
|
||||
if grep -qEi "(microsoft|wsl)" /proc/version 2>/dev/null; then
|
||||
IS_WSL=true
|
||||
echo -e "${YELLOW}Running in WSL environment${NC}"
|
||||
fi
|
||||
|
||||
# Detect ROS distro
|
||||
ROS_DISTRO=""
|
||||
for distro in jazzy humble iron galactic; do
|
||||
if [ -d "/opt/ros/$distro" ]; then
|
||||
@@ -42,22 +24,16 @@ for distro in jazzy humble iron galactic; do
|
||||
break
|
||||
fi
|
||||
done
|
||||
|
||||
if [ -n "$ROS_DISTRO" ]; then
|
||||
source /opt/ros/${ROS_DISTRO}/setup.bash
|
||||
echo -e "${GREEN}ROS 2: $ROS_DISTRO${NC}"
|
||||
fi
|
||||
|
||||
# ArduPilot paths
|
||||
ARDUPILOT_HOME="${ARDUPILOT_HOME:-$HOME/ardupilot}"
|
||||
ARDUPILOT_GZ="${ARDUPILOT_GZ:-$HOME/ardupilot_gazebo}"
|
||||
|
||||
# Parse arguments
|
||||
WORLD_NAME="iris_runway"
|
||||
VEHICLE="ArduCopter"
|
||||
USE_SOFTWARE_RENDER=false
|
||||
INSTANCE=0
|
||||
|
||||
while [[ $# -gt 0 ]]; do
|
||||
case $1 in
|
||||
--world)
|
||||
@@ -96,16 +72,11 @@ while [[ $# -gt 0 ]]; do
|
||||
;;
|
||||
esac
|
||||
done
|
||||
|
||||
# Setup Gazebo paths
|
||||
export GZ_SIM_RESOURCE_PATH="$PROJECT_DIR/models:$PROJECT_DIR/worlds:${GZ_SIM_RESOURCE_PATH:-}"
|
||||
|
||||
if [ -d "$ARDUPILOT_GZ" ]; then
|
||||
export GZ_SIM_SYSTEM_PLUGIN_PATH="$ARDUPILOT_GZ/build:${GZ_SIM_SYSTEM_PLUGIN_PATH:-}"
|
||||
export GZ_SIM_RESOURCE_PATH="$ARDUPILOT_GZ/models:$ARDUPILOT_GZ/worlds:$GZ_SIM_RESOURCE_PATH"
|
||||
fi
|
||||
|
||||
# Find world file
|
||||
WORLD_FILE=""
|
||||
if [ -f "$PROJECT_DIR/worlds/${WORLD_NAME}.sdf" ]; then
|
||||
WORLD_FILE="$PROJECT_DIR/worlds/${WORLD_NAME}.sdf"
|
||||
@@ -123,8 +94,6 @@ else
|
||||
ls "$ARDUPILOT_GZ/worlds/"*.sdf 2>/dev/null | xargs -I {} basename {} .sdf || echo " (none)"
|
||||
exit 1
|
||||
fi
|
||||
|
||||
# WSL/Display setup
|
||||
if $IS_WSL; then
|
||||
if [ -z "$DISPLAY" ]; then
|
||||
if [ -d "/mnt/wslg" ]; then
|
||||
@@ -135,75 +104,52 @@ if $IS_WSL; then
|
||||
fi
|
||||
echo -e "${BLUE}DISPLAY: $DISPLAY${NC}"
|
||||
fi
|
||||
|
||||
if $USE_SOFTWARE_RENDER; then
|
||||
echo -e "${YELLOW}Using software rendering${NC}"
|
||||
export LIBGL_ALWAYS_SOFTWARE=1
|
||||
export MESA_GL_VERSION_OVERRIDE=3.3
|
||||
fi
|
||||
|
||||
# Cleanup function
|
||||
cleanup() {
|
||||
echo ""
|
||||
echo -e "${YELLOW}Shutting down simulation...${NC}"
|
||||
|
||||
pkill -f "sim_vehicle.py" 2>/dev/null || true
|
||||
pkill -f "mavproxy" 2>/dev/null || true
|
||||
pkill -f "arducopter" 2>/dev/null || true
|
||||
pkill -f "ardurover" 2>/dev/null || true
|
||||
pkill -f "ruby" 2>/dev/null || true # gz sim uses ruby
|
||||
pkill -f "ruby" 2>/dev/null || true
|
||||
pkill -f "gz sim" 2>/dev/null || true
|
||||
|
||||
sleep 2
|
||||
echo -e "${GREEN}Cleanup complete.${NC}"
|
||||
}
|
||||
|
||||
trap cleanup EXIT INT TERM
|
||||
|
||||
# Check ArduPilot installation
|
||||
if [ ! -d "$ARDUPILOT_HOME" ]; then
|
||||
echo -e "${RED}ERROR: ArduPilot not found at $ARDUPILOT_HOME${NC}"
|
||||
echo "Run setup.sh first to install ArduPilot"
|
||||
exit 1
|
||||
fi
|
||||
|
||||
# Check Gazebo plugin
|
||||
if [ ! -f "$ARDUPILOT_GZ/build/libArduPilotPlugin.so" ]; then
|
||||
echo -e "${RED}ERROR: ArduPilot Gazebo plugin not found${NC}"
|
||||
echo "Run setup.sh first to build the plugin"
|
||||
exit 1
|
||||
fi
|
||||
|
||||
echo ""
|
||||
echo -e "${GREEN}Configuration:${NC}"
|
||||
echo " Vehicle: $VEHICLE"
|
||||
echo " World: $(basename $WORLD_FILE .sdf)"
|
||||
echo " Instance: $INSTANCE"
|
||||
echo ""
|
||||
|
||||
# Start Gazebo in background
|
||||
echo -e "${GREEN}Starting Gazebo...${NC}"
|
||||
gz sim -v4 -r "$WORLD_FILE" &
|
||||
GZ_PID=$!
|
||||
|
||||
sleep 5
|
||||
|
||||
# Check if Gazebo started
|
||||
if ! kill -0 $GZ_PID 2>/dev/null; then
|
||||
echo -e "${RED}ERROR: Gazebo failed to start${NC}"
|
||||
exit 1
|
||||
fi
|
||||
|
||||
echo -e "${GREEN}Gazebo running (PID: $GZ_PID)${NC}"
|
||||
|
||||
# Start ArduPilot SITL
|
||||
echo -e "${GREEN}Starting ArduPilot SITL...${NC}"
|
||||
cd "$ARDUPILOT_HOME"
|
||||
|
||||
export PATH=$PATH:$ARDUPILOT_HOME/Tools/autotest:$HOME/.local/bin:$HOME/.local/pipx/venvs/mavproxy/bin
|
||||
|
||||
# Run sim_vehicle.py without --console to keep it running
|
||||
# The --no-mavproxy flag runs just the SITL, then we start mavproxy separately
|
||||
sim_vehicle.py \
|
||||
-v $VEHICLE \
|
||||
-f gazebo-iris \
|
||||
@@ -212,11 +158,7 @@ sim_vehicle.py \
|
||||
--no-mavproxy \
|
||||
&
|
||||
SITL_PID=$!
|
||||
|
||||
# Wait for SITL to initialize
|
||||
sleep 8
|
||||
|
||||
# Start MAVProxy in foreground so user can interact with it
|
||||
echo ""
|
||||
echo -e "${GREEN}==========================================${NC}"
|
||||
echo -e "${GREEN} Simulation Running${NC}"
|
||||
@@ -227,17 +169,15 @@ echo " - Gazebo: PID $GZ_PID"
|
||||
echo " - ArduPilot SITL: PID $SITL_PID"
|
||||
echo ""
|
||||
echo -e "${BLUE}MAVProxy Commands:${NC}"
|
||||
echo " mode guided # Switch to GUIDED mode"
|
||||
echo " arm throttle # Arm the drone"
|
||||
echo " takeoff 5 # Takeoff to 5 meters"
|
||||
echo " guided 10 5 -10 # Fly to position (N, E, D)"
|
||||
echo " rtl # Return to launch"
|
||||
echo " land # Land"
|
||||
echo " mode guided
|
||||
echo " arm throttle
|
||||
echo " takeoff 5
|
||||
echo " guided 10 5 -10
|
||||
echo " rtl
|
||||
echo " land
|
||||
echo ""
|
||||
echo -e "${YELLOW}MAVProxy starting... (Ctrl+C to exit)${NC}"
|
||||
echo ""
|
||||
|
||||
# Start MAVProxy in foreground - user can type commands directly
|
||||
mavproxy.py \
|
||||
--master tcp:127.0.0.1:5760 \
|
||||
--sitl 127.0.0.1:5501 \
|
||||
|
||||
@@ -1,34 +1,19 @@
|
||||
#!/bin/bash
|
||||
# Setup script for NVIDIA GPU with Gazebo
|
||||
|
||||
set -e
|
||||
|
||||
echo "Setting up NVIDIA GPU for Gazebo..."
|
||||
|
||||
# Check if NVIDIA GPU is available
|
||||
if ! command -v nvidia-smi &> /dev/null; then
|
||||
echo "NVIDIA driver not found. Please install NVIDIA drivers first."
|
||||
exit 1
|
||||
fi
|
||||
|
||||
nvidia-smi
|
||||
|
||||
# Set environment variables for NVIDIA
|
||||
export __NV_PRIME_RENDER_OFFLOAD=1
|
||||
export __GLX_VENDOR_LIBRARY_NAME=nvidia
|
||||
|
||||
# For Gazebo Harmonic
|
||||
export LIBGL_ALWAYS_SOFTWARE=0
|
||||
|
||||
# Create persistent config
|
||||
cat >> ~/.bashrc << 'EOF'
|
||||
|
||||
# NVIDIA GPU for Gazebo
|
||||
export __NV_PRIME_RENDER_OFFLOAD=1
|
||||
export __GLX_VENDOR_LIBRARY_NAME=nvidia
|
||||
export LIBGL_ALWAYS_SOFTWARE=0
|
||||
EOF
|
||||
|
||||
echo ""
|
||||
echo "NVIDIA setup complete!"
|
||||
echo "Please restart your terminal or run: source ~/.bashrc"
|
||||
|
||||
@@ -1,38 +1,20 @@
|
||||
#!/bin/bash
|
||||
# =============================================================================
|
||||
# UAV-UGV Simulation - Uninstall Script
|
||||
# =============================================================================
|
||||
# Removes ArduPilot, ardupilot_gazebo, and project files
|
||||
# Does NOT remove ROS 2 or Gazebo (system packages)
|
||||
#
|
||||
# Usage:
|
||||
# ./scripts/uninstall.sh # Remove ArduPilot and plugin only
|
||||
# ./scripts/uninstall.sh --all # Remove everything including project
|
||||
# =============================================================================
|
||||
|
||||
set -e
|
||||
|
||||
RED='\033[0;31m'
|
||||
GREEN='\033[0;32m'
|
||||
YELLOW='\033[1;33m'
|
||||
BLUE='\033[0;34m'
|
||||
NC='\033[0m'
|
||||
|
||||
SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
|
||||
PROJECT_DIR="$(dirname "$SCRIPT_DIR")"
|
||||
|
||||
ARDUPILOT_HOME="$HOME/ardupilot"
|
||||
ARDUPILOT_GZ="$HOME/ardupilot_gazebo"
|
||||
|
||||
echo -e "${BLUE}==========================================${NC}"
|
||||
echo -e "${BLUE} UAV-UGV Simulation - Uninstall${NC}"
|
||||
echo -e "${BLUE}==========================================${NC}"
|
||||
echo ""
|
||||
|
||||
# Parse arguments
|
||||
REMOVE_ALL=false
|
||||
FORCE=false
|
||||
|
||||
for arg in "$@"; do
|
||||
case $arg in
|
||||
--all)
|
||||
@@ -54,8 +36,6 @@ for arg in "$@"; do
|
||||
;;
|
||||
esac
|
||||
done
|
||||
|
||||
# Confirmation
|
||||
if [ "$FORCE" = false ]; then
|
||||
echo "This will remove:"
|
||||
echo " - ArduPilot SITL ($ARDUPILOT_HOME)"
|
||||
@@ -73,10 +53,7 @@ if [ "$FORCE" = false ]; then
|
||||
exit 0
|
||||
fi
|
||||
fi
|
||||
|
||||
echo ""
|
||||
|
||||
# Kill running processes
|
||||
echo -e "${YELLOW}Stopping running processes...${NC}"
|
||||
pkill -f "sim_vehicle.py" 2>/dev/null || true
|
||||
pkill -f "mavproxy" 2>/dev/null || true
|
||||
@@ -86,8 +63,6 @@ pkill -f "gz sim" 2>/dev/null || true
|
||||
pkill -f "gzserver" 2>/dev/null || true
|
||||
pkill -f "gzclient" 2>/dev/null || true
|
||||
sleep 1
|
||||
|
||||
# Remove ArduPilot
|
||||
if [ -d "$ARDUPILOT_HOME" ]; then
|
||||
echo -e "${YELLOW}Removing ArduPilot ($ARDUPILOT_HOME)...${NC}"
|
||||
rm -rf "$ARDUPILOT_HOME"
|
||||
@@ -95,8 +70,6 @@ if [ -d "$ARDUPILOT_HOME" ]; then
|
||||
else
|
||||
echo "ArduPilot not found at $ARDUPILOT_HOME"
|
||||
fi
|
||||
|
||||
# Remove ardupilot_gazebo
|
||||
if [ -d "$ARDUPILOT_GZ" ]; then
|
||||
echo -e "${YELLOW}Removing ardupilot_gazebo ($ARDUPILOT_GZ)...${NC}"
|
||||
rm -rf "$ARDUPILOT_GZ"
|
||||
@@ -104,33 +77,22 @@ if [ -d "$ARDUPILOT_GZ" ]; then
|
||||
else
|
||||
echo "ardupilot_gazebo not found at $ARDUPILOT_GZ"
|
||||
fi
|
||||
|
||||
# Remove Python venv
|
||||
if [ -d "$PROJECT_DIR/venv" ]; then
|
||||
echo -e "${YELLOW}Removing Python virtual environment...${NC}"
|
||||
rm -rf "$PROJECT_DIR/venv"
|
||||
echo -e "${GREEN}Removed venv${NC}"
|
||||
fi
|
||||
|
||||
# Remove generated files
|
||||
rm -f "$PROJECT_DIR/activate_venv.sh" 2>/dev/null || true
|
||||
rm -f "$PROJECT_DIR/wsl_env.sh" 2>/dev/null || true
|
||||
|
||||
# Remove ArduPilot environment files
|
||||
rm -f "$HOME/.ardupilot_env" 2>/dev/null || true
|
||||
|
||||
# Remove pip user packages
|
||||
echo -e "${YELLOW}Removing user pip packages (mavproxy, pymavlink)...${NC}"
|
||||
pip3 uninstall -y mavproxy pymavlink pexpect 2>/dev/null || true
|
||||
|
||||
# Remove project directory if --all
|
||||
if [ "$REMOVE_ALL" = true ]; then
|
||||
echo -e "${YELLOW}Removing project directory ($PROJECT_DIR)...${NC}"
|
||||
cd "$HOME"
|
||||
rm -rf "$PROJECT_DIR"
|
||||
echo -e "${GREEN}Removed project directory${NC}"
|
||||
fi
|
||||
|
||||
echo ""
|
||||
echo -e "${GREEN}==========================================${NC}"
|
||||
echo -e "${GREEN} Uninstall Complete${NC}"
|
||||
|
||||
177
setup.sh
177
setup.sh
@@ -1,24 +1,11 @@
|
||||
#!/bin/bash
|
||||
# =============================================================================
|
||||
# UAV-UGV Simulation - Complete Installation Script
|
||||
# =============================================================================
|
||||
# Installs everything needed for GPS-denied navigation simulation
|
||||
# Compatible with Ubuntu 22.04/24.04 and WSL2
|
||||
#
|
||||
# Usage:
|
||||
# ./setup.sh # Full installation
|
||||
# ./setup.sh --skip-ardupilot # Skip ArduPilot (Gazebo only)
|
||||
# =============================================================================
|
||||
|
||||
set -e
|
||||
|
||||
RED='\033[0;31m'
|
||||
GREEN='\033[0;32m'
|
||||
YELLOW='\033[1;33m'
|
||||
BLUE='\033[0;34m'
|
||||
CYAN='\033[0;36m'
|
||||
NC='\033[0m'
|
||||
|
||||
print_header() {
|
||||
echo ""
|
||||
echo -e "${BLUE}==========================================${NC}"
|
||||
@@ -26,49 +13,34 @@ print_header() {
|
||||
echo -e "${BLUE}==========================================${NC}"
|
||||
echo ""
|
||||
}
|
||||
|
||||
print_step() {
|
||||
echo -e "${GREEN}[$1/$TOTAL_STEPS] $2${NC}"
|
||||
}
|
||||
|
||||
print_info() {
|
||||
echo -e "${CYAN}INFO: $1${NC}"
|
||||
}
|
||||
|
||||
print_warning() {
|
||||
echo -e "${YELLOW}WARNING: $1${NC}"
|
||||
}
|
||||
|
||||
print_error() {
|
||||
echo -e "${RED}ERROR: $1${NC}"
|
||||
}
|
||||
|
||||
# Get script directory
|
||||
SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
|
||||
cd "$SCRIPT_DIR"
|
||||
|
||||
# Deactivate existing venv if active to avoid conflicts
|
||||
if [ -n "$VIRTUAL_ENV" ]; then
|
||||
deactivate 2>/dev/null || true
|
||||
fi
|
||||
|
||||
# ArduPilot directories
|
||||
ARDUPILOT_HOME="$HOME/ardupilot"
|
||||
ARDUPILOT_GZ="$HOME/ardupilot_gazebo"
|
||||
|
||||
# Detect environment
|
||||
detect_environment() {
|
||||
IS_WSL=false
|
||||
IS_WSL2=false
|
||||
|
||||
if grep -qEi "(microsoft|wsl)" /proc/version 2>/dev/null; then
|
||||
IS_WSL=true
|
||||
if grep -qi "wsl2" /proc/version 2>/dev/null || [ -f /run/WSL ]; then
|
||||
IS_WSL2=true
|
||||
fi
|
||||
fi
|
||||
|
||||
# Detect Ubuntu version
|
||||
if [ -f /etc/os-release ]; then
|
||||
. /etc/os-release
|
||||
UBUNTU_VERSION="$VERSION_ID"
|
||||
@@ -77,8 +49,6 @@ detect_environment() {
|
||||
UBUNTU_VERSION="22.04"
|
||||
UBUNTU_CODENAME="jammy"
|
||||
fi
|
||||
|
||||
# Determine ROS distro and compatible repository codename
|
||||
if [ "$UBUNTU_VERSION" = "24.04" ]; then
|
||||
ROS_DISTRO="jazzy"
|
||||
ROS_UBUNTU_CODENAME="noble"
|
||||
@@ -86,7 +56,6 @@ detect_environment() {
|
||||
ROS_DISTRO="humble"
|
||||
ROS_UBUNTU_CODENAME="jammy"
|
||||
else
|
||||
# For newer versions, use latest available
|
||||
UBUNTU_MAJOR=$(echo "$UBUNTU_VERSION" | cut -d. -f1)
|
||||
if [ "$UBUNTU_MAJOR" -ge 24 ] 2>/dev/null; then
|
||||
ROS_DISTRO="jazzy"
|
||||
@@ -97,40 +66,29 @@ detect_environment() {
|
||||
fi
|
||||
fi
|
||||
}
|
||||
|
||||
# Parse arguments
|
||||
INSTALL_ARDUPILOT=true
|
||||
for arg in "$@"; do
|
||||
if [ "$arg" = "--skip-ardupilot" ]; then
|
||||
INSTALL_ARDUPILOT=false
|
||||
fi
|
||||
done
|
||||
|
||||
detect_environment
|
||||
|
||||
print_header "UAV-UGV Simulation - Complete Setup"
|
||||
echo "GPS-Denied Navigation with Geofencing"
|
||||
echo ""
|
||||
|
||||
echo -e "${CYAN}Detected Environment:${NC}"
|
||||
echo " Ubuntu: $UBUNTU_VERSION ($UBUNTU_CODENAME)"
|
||||
echo " WSL: $IS_WSL | WSL2: $IS_WSL2"
|
||||
echo " ROS 2 Target: $ROS_DISTRO"
|
||||
echo " ArduPilot: $INSTALL_ARDUPILOT"
|
||||
echo ""
|
||||
|
||||
if [ "$INSTALL_ARDUPILOT" = true ]; then
|
||||
TOTAL_STEPS=10
|
||||
else
|
||||
TOTAL_STEPS=7
|
||||
fi
|
||||
STEP=1
|
||||
|
||||
# ============================================================================
|
||||
# STEP 1: System Update & Dependencies
|
||||
# ============================================================================
|
||||
print_step $STEP "Installing system dependencies"
|
||||
|
||||
sudo apt-get update
|
||||
sudo apt-get install -y \
|
||||
curl \
|
||||
@@ -165,9 +123,10 @@ sudo apt-get install -y \
|
||||
ccache \
|
||||
gawk \
|
||||
libtool-bin \
|
||||
netcat-openbsd
|
||||
|
||||
# WSL-specific packages
|
||||
netcat-openbsd \
|
||||
ffmpeg \
|
||||
xdotool \
|
||||
x11-utils
|
||||
if $IS_WSL; then
|
||||
print_info "Installing WSL GUI support packages"
|
||||
sudo apt-get install -y \
|
||||
@@ -178,24 +137,13 @@ if $IS_WSL; then
|
||||
libgl1-mesa-glx
|
||||
fi
|
||||
((STEP++))
|
||||
|
||||
# ============================================================================
|
||||
# STEP 2: Install ROS 2
|
||||
# ============================================================================
|
||||
print_step $STEP "Installing ROS 2 $ROS_DISTRO"
|
||||
|
||||
# Add ROS 2 GPG key
|
||||
sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key \
|
||||
-o /usr/share/keyrings/ros-archive-keyring.gpg
|
||||
|
||||
# Add repository using compatible Ubuntu codename
|
||||
echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] \
|
||||
http://packages.ros.org/ros2/ubuntu $ROS_UBUNTU_CODENAME main" | \
|
||||
sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null
|
||||
|
||||
sudo apt-get update
|
||||
|
||||
# Install ROS 2
|
||||
sudo apt-get install -y \
|
||||
ros-${ROS_DISTRO}-ros-base \
|
||||
ros-${ROS_DISTRO}-geometry-msgs \
|
||||
@@ -206,35 +154,21 @@ sudo apt-get install -y \
|
||||
print_error "Failed to install ROS 2 $ROS_DISTRO"
|
||||
exit 1
|
||||
}
|
||||
|
||||
# Install ros-gz bridge
|
||||
sudo apt-get install -y ros-${ROS_DISTRO}-ros-gz || \
|
||||
print_warning "Could not install ros-gz bridge"
|
||||
|
||||
print_info "ROS 2 $ROS_DISTRO installed"
|
||||
((STEP++))
|
||||
|
||||
# ============================================================================
|
||||
# STEP 3: Install Gazebo (Harmonic or Garden)
|
||||
# ============================================================================
|
||||
print_step $STEP "Installing Gazebo"
|
||||
|
||||
# Add Gazebo repository
|
||||
GZ_UBUNTU_CODENAME="$ROS_UBUNTU_CODENAME"
|
||||
sudo wget -q https://packages.osrfoundation.org/gazebo.gpg \
|
||||
-O /usr/share/keyrings/pkgs-osrf-archive-keyring.gpg 2>/dev/null || true
|
||||
|
||||
echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/pkgs-osrf-archive-keyring.gpg] \
|
||||
http://packages.osrfoundation.org/gazebo/ubuntu-stable $GZ_UBUNTU_CODENAME main" | \
|
||||
sudo tee /etc/apt/sources.list.d/gazebo-stable.list > /dev/null
|
||||
|
||||
sudo apt-get update
|
||||
|
||||
# Try Gazebo Harmonic first, then Garden
|
||||
GZ_VERSION=""
|
||||
if sudo apt-get install -y gz-harmonic 2>/dev/null; then
|
||||
GZ_VERSION="harmonic"
|
||||
# Install Harmonic development packages for building plugins
|
||||
sudo apt-get install -y \
|
||||
libgz-cmake3-dev \
|
||||
libgz-sim8-dev \
|
||||
@@ -247,14 +181,12 @@ if sudo apt-get install -y gz-harmonic 2>/dev/null; then
|
||||
libgz-msgs10-dev \
|
||||
rapidjson-dev \
|
||||
2>/dev/null || print_warning "Some Gazebo dev packages may be missing"
|
||||
# Install Gazebo Python bindings (needed for camera/vision scripts)
|
||||
sudo apt-get install -y \
|
||||
python3-gz-transport13 \
|
||||
python3-gz-msgs10 \
|
||||
2>/dev/null || print_warning "Gazebo Python bindings not available via apt"
|
||||
elif sudo apt-get install -y gz-garden 2>/dev/null; then
|
||||
GZ_VERSION="garden"
|
||||
# Install Garden development packages
|
||||
sudo apt-get install -y \
|
||||
libgz-cmake3-dev \
|
||||
libgz-sim7-dev \
|
||||
@@ -267,7 +199,6 @@ elif sudo apt-get install -y gz-garden 2>/dev/null; then
|
||||
libgz-msgs9-dev \
|
||||
rapidjson-dev \
|
||||
2>/dev/null || print_warning "Some Gazebo dev packages may be missing"
|
||||
# Install Gazebo Python bindings for Garden
|
||||
sudo apt-get install -y \
|
||||
python3-gz-transport12 \
|
||||
python3-gz-msgs9 \
|
||||
@@ -276,52 +207,29 @@ else
|
||||
print_warning "Could not install Gazebo Harmonic/Garden"
|
||||
GZ_VERSION="none"
|
||||
fi
|
||||
|
||||
if command -v gz &> /dev/null; then
|
||||
print_info "Gazebo $GZ_VERSION installed"
|
||||
else
|
||||
print_warning "Gazebo command not found"
|
||||
fi
|
||||
((STEP++))
|
||||
|
||||
# ============================================================================
|
||||
# STEP 4: Python Virtual Environment
|
||||
# ============================================================================
|
||||
print_step $STEP "Setting up Python environment"
|
||||
|
||||
if [ -d "$SCRIPT_DIR/venv" ]; then
|
||||
rm -rf "$SCRIPT_DIR/venv"
|
||||
fi
|
||||
|
||||
# --system-site-packages is required so the venv can access
|
||||
# Gazebo Python bindings (gz.transport13, gz.msgs10) which are
|
||||
# installed as system packages and cannot be pip-installed.
|
||||
python3 -m venv --system-site-packages "$SCRIPT_DIR/venv"
|
||||
source "$SCRIPT_DIR/venv/bin/activate"
|
||||
pip install --upgrade pip
|
||||
|
||||
if [ -f "$SCRIPT_DIR/requirements.txt" ]; then
|
||||
pip install -r "$SCRIPT_DIR/requirements.txt" || print_warning "Some Python packages failed"
|
||||
else
|
||||
pip install numpy opencv-python scipy shapely filterpy transforms3d pymavlink pexpect future
|
||||
fi
|
||||
|
||||
deactivate
|
||||
((STEP++))
|
||||
|
||||
# ============================================================================
|
||||
# STEP 5: Create Activation Script
|
||||
# ============================================================================
|
||||
print_step $STEP "Creating activation script"
|
||||
|
||||
cat > "$SCRIPT_DIR/activate_venv.sh" << 'ACTIVATE_EOF'
|
||||
#!/bin/bash
|
||||
# UAV-UGV Simulation - Environment Activation
|
||||
# Usage: source activate_venv.sh
|
||||
|
||||
SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
|
||||
|
||||
# ROS 2 Setup
|
||||
if [ -f "/opt/ros/jazzy/setup.bash" ]; then
|
||||
source /opt/ros/jazzy/setup.bash
|
||||
ROS_VER="jazzy"
|
||||
@@ -332,32 +240,20 @@ else
|
||||
echo "[WARN] ROS 2 not found in /opt/ros/"
|
||||
ROS_VER="none"
|
||||
fi
|
||||
|
||||
# ArduPilot Environment
|
||||
export ARDUPILOT_HOME="$HOME/ardupilot"
|
||||
export PATH="$ARDUPILOT_HOME/Tools/autotest:$PATH"
|
||||
export PATH="$HOME/.local/bin:$PATH"
|
||||
|
||||
# Deactivate existing venv
|
||||
if [ -n "$VIRTUAL_ENV" ]; then
|
||||
deactivate 2>/dev/null || true
|
||||
fi
|
||||
|
||||
# Activate project venv
|
||||
if [ -f "$SCRIPT_DIR/venv/bin/activate" ]; then
|
||||
source "$SCRIPT_DIR/venv/bin/activate"
|
||||
fi
|
||||
|
||||
# Gazebo paths (new Gazebo - Ignition/Harmonic)
|
||||
export GZ_SIM_RESOURCE_PATH="$SCRIPT_DIR/models:$SCRIPT_DIR/worlds:${GZ_SIM_RESOURCE_PATH:-}"
|
||||
|
||||
# ArduPilot Gazebo plugin
|
||||
if [ -d "$HOME/ardupilot_gazebo/build" ]; then
|
||||
export GZ_SIM_SYSTEM_PLUGIN_PATH="$HOME/ardupilot_gazebo/build:${GZ_SIM_SYSTEM_PLUGIN_PATH:-}"
|
||||
export GZ_SIM_RESOURCE_PATH="$HOME/ardupilot_gazebo/models:$HOME/ardupilot_gazebo/worlds:$GZ_SIM_RESOURCE_PATH"
|
||||
fi
|
||||
|
||||
# WSL environment
|
||||
if grep -qEi "(microsoft|wsl)" /proc/version 2>/dev/null; then
|
||||
if [ -d "/mnt/wslg" ]; then
|
||||
export DISPLAY=:0
|
||||
@@ -367,31 +263,20 @@ if grep -qEi "(microsoft|wsl)" /proc/version 2>/dev/null; then
|
||||
export LIBGL_ALWAYS_INDIRECT=0
|
||||
export MESA_GL_VERSION_OVERRIDE=3.3
|
||||
fi
|
||||
|
||||
echo -e "\033[0;32mEnvironment activated (ROS 2 $ROS_VER)\033[0m"
|
||||
echo ""
|
||||
echo "Run simulation: bash scripts/run_autonomous.sh --search spiral"
|
||||
ACTIVATE_EOF
|
||||
|
||||
chmod +x "$SCRIPT_DIR/activate_venv.sh"
|
||||
((STEP++))
|
||||
|
||||
# ============================================================================
|
||||
# STEP 6: Create WSL Environment (if applicable)
|
||||
# ============================================================================
|
||||
print_step $STEP "Configuring environment files"
|
||||
|
||||
if $IS_WSL; then
|
||||
cat > "$SCRIPT_DIR/wsl_env.sh" << 'WSLEOF'
|
||||
#!/bin/bash
|
||||
# WSL Environment for UAV-UGV Simulation
|
||||
|
||||
if [ -d "/mnt/wslg" ]; then
|
||||
export DISPLAY=:0
|
||||
else
|
||||
export DISPLAY=$(cat /etc/resolv.conf 2>/dev/null | grep nameserver | awk '{print $2}'):0
|
||||
fi
|
||||
|
||||
export LIBGL_ALWAYS_INDIRECT=0
|
||||
export MESA_GL_VERSION_OVERRIDE=3.3
|
||||
export MESA_GLSL_VERSION_OVERRIDE=330
|
||||
@@ -400,56 +285,30 @@ WSLEOF
|
||||
chmod +x "$SCRIPT_DIR/wsl_env.sh"
|
||||
fi
|
||||
((STEP++))
|
||||
|
||||
# ============================================================================
|
||||
# ARDUPILOT INSTALLATION (if not skipped)
|
||||
# ============================================================================
|
||||
if [ "$INSTALL_ARDUPILOT" = true ]; then
|
||||
|
||||
# ========================================================================
|
||||
# STEP 7: Clone and Setup ArduPilot
|
||||
# ========================================================================
|
||||
print_step $STEP "Setting up ArduPilot SITL"
|
||||
|
||||
if [ ! -d "$ARDUPILOT_HOME" ]; then
|
||||
print_info "Cloning ArduPilot repository..."
|
||||
git clone --recurse-submodules https://github.com/ArduPilot/ardupilot.git "$ARDUPILOT_HOME"
|
||||
else
|
||||
print_info "ArduPilot directory already exists"
|
||||
fi
|
||||
|
||||
cd "$ARDUPILOT_HOME"
|
||||
git submodule update --init --recursive
|
||||
|
||||
# Install ArduPilot prerequisites
|
||||
print_info "Installing ArduPilot prerequisites (this may take a while)..."
|
||||
Tools/environment_install/install-prereqs-ubuntu.sh -y || true
|
||||
. ~/.profile 2>/dev/null || true
|
||||
|
||||
# Source ArduPilot environment
|
||||
[ -f "$HOME/.ardupilot_env" ] && source "$HOME/.ardupilot_env"
|
||||
|
||||
print_info "ArduPilot prerequisites installed"
|
||||
((STEP++))
|
||||
|
||||
# ========================================================================
|
||||
# STEP 8: Build ArduCopter SITL
|
||||
# ========================================================================
|
||||
print_step $STEP "Building ArduPilot SITL (this takes several minutes)"
|
||||
|
||||
cd "$ARDUPILOT_HOME"
|
||||
./waf configure --board sitl
|
||||
./waf copter
|
||||
./waf rover
|
||||
|
||||
print_info "ArduPilot SITL built"
|
||||
((STEP++))
|
||||
|
||||
# ========================================================================
|
||||
# STEP 9: Build ArduPilot Gazebo Plugin
|
||||
# ========================================================================
|
||||
print_step $STEP "Building ArduPilot Gazebo plugin"
|
||||
|
||||
if [ ! -d "$ARDUPILOT_GZ" ]; then
|
||||
print_info "Cloning ardupilot_gazebo repository..."
|
||||
git clone https://github.com/ArduPilot/ardupilot_gazebo.git "$ARDUPILOT_GZ"
|
||||
@@ -458,46 +317,30 @@ if [ "$INSTALL_ARDUPILOT" = true ]; then
|
||||
cd "$ARDUPILOT_GZ"
|
||||
git pull origin main || true
|
||||
fi
|
||||
|
||||
cd "$ARDUPILOT_GZ"
|
||||
mkdir -p build && cd build
|
||||
|
||||
# Deactivate venv so cmake finds system Python with ROS 2 ament packages
|
||||
deactivate 2>/dev/null || true
|
||||
cmake .. -DCMAKE_BUILD_TYPE=Release
|
||||
make -j$(nproc)
|
||||
# Reactivate venv
|
||||
source "$SCRIPT_DIR/venv/bin/activate"
|
||||
|
||||
print_info "ArduPilot Gazebo plugin built"
|
||||
|
||||
# Install MAVProxy using pipx (required for Ubuntu 23.04+ PEP 668)
|
||||
print_info "Installing MAVProxy..."
|
||||
if pipx install MAVProxy --include-deps; then
|
||||
print_info "Injecting dependencies into MAVProxy venv..."
|
||||
pipx inject mavproxy future pexpect
|
||||
else
|
||||
# Fallback: try pip with --break-system-packages
|
||||
pip3 install --user --break-system-packages pymavlink mavproxy 2>/dev/null || \
|
||||
pip3 install --user pymavlink mavproxy 2>/dev/null || \
|
||||
print_warning "MAVProxy installation failed - install manually"
|
||||
fi
|
||||
((STEP++))
|
||||
|
||||
# ========================================================================
|
||||
# STEP 10: Verify Installation
|
||||
# ========================================================================
|
||||
print_step $STEP "Verifying installation"
|
||||
|
||||
export PATH=$PATH:$ARDUPILOT_HOME/Tools/autotest:$HOME/.local/bin
|
||||
|
||||
echo ""
|
||||
command -v sim_vehicle.py &> /dev/null && echo "[OK] sim_vehicle.py" || echo "[WARN] sim_vehicle.py not found"
|
||||
command -v gz &> /dev/null && echo "[OK] Gazebo (gz)" || echo "[WARN] Gazebo not found"
|
||||
command -v mavproxy.py &> /dev/null && echo "[OK] MAVProxy" || echo "[WARN] MAVProxy not in PATH"
|
||||
[ -f "$ARDUPILOT_GZ/build/libArduPilotPlugin.so" ] && echo "[OK] ArduPilot Gazebo plugin" || echo "[WARN] Plugin not built"
|
||||
|
||||
# Verify Gazebo Python bindings are accessible from venv
|
||||
source "$SCRIPT_DIR/venv/bin/activate"
|
||||
python3 -c "from gz.transport13 import Node; print('[OK] gz.transport13 Python bindings')" 2>/dev/null || \
|
||||
echo "[WARN] gz.transport13 Python bindings not found"
|
||||
@@ -509,19 +352,9 @@ if [ "$INSTALL_ARDUPILOT" = true ]; then
|
||||
echo "[WARN] OpenCV not found in venv"
|
||||
deactivate
|
||||
((STEP++))
|
||||
|
||||
fi
|
||||
|
||||
# ============================================================================
|
||||
# Make scripts executable
|
||||
# ============================================================================
|
||||
chmod +x "$SCRIPT_DIR/scripts/"*.sh 2>/dev/null || true
|
||||
|
||||
# ============================================================================
|
||||
# COMPLETE
|
||||
# ============================================================================
|
||||
print_header "Installation Complete!"
|
||||
|
||||
echo -e "${GREEN}Components installed:${NC}"
|
||||
echo " - ROS 2 $ROS_DISTRO"
|
||||
echo " - Gazebo $GZ_VERSION"
|
||||
@@ -531,14 +364,12 @@ if [ "$INSTALL_ARDUPILOT" = true ]; then
|
||||
fi
|
||||
echo " - Python dependencies"
|
||||
echo ""
|
||||
|
||||
if $IS_WSL; then
|
||||
echo -e "${YELLOW}WSL Notes:${NC}"
|
||||
echo " - GUI requires WSLg (Win11) or VcXsrv (Win10)"
|
||||
echo " - Use --software-render if graphics are slow"
|
||||
echo ""
|
||||
fi
|
||||
|
||||
echo -e "${CYAN}To run the simulation:${NC}"
|
||||
echo ""
|
||||
echo " cd $SCRIPT_DIR"
|
||||
|
||||
@@ -0,0 +1 @@
|
||||
|
||||
|
||||
@@ -0,0 +1 @@
|
||||
|
||||
|
||||
@@ -1,236 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Mission Planner - Coordinates missions using local waypoints."""
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from geometry_msgs.msg import PoseStamped
|
||||
from nav_msgs.msg import Path
|
||||
from std_msgs.msg import String, Bool
|
||||
import yaml
|
||||
from enum import Enum
|
||||
|
||||
|
||||
class MissionState(Enum):
|
||||
IDLE = 0
|
||||
LOADING = 1
|
||||
EXECUTING = 2
|
||||
PAUSED = 3
|
||||
COMPLETED = 4
|
||||
ABORTED = 5
|
||||
|
||||
|
||||
class MissionPlanner(Node):
|
||||
|
||||
def __init__(self):
|
||||
super().__init__('mission_planner')
|
||||
|
||||
self.declare_parameter('mission_file', '')
|
||||
|
||||
self.mission = []
|
||||
self.current_action_idx = 0
|
||||
self.state = MissionState.IDLE
|
||||
|
||||
self.uav_goal_reached = False
|
||||
self.ugv_goal_reached = False
|
||||
|
||||
self.command_sub = self.create_subscription(
|
||||
String, '/mission/command', self.command_callback, 10)
|
||||
|
||||
self.uav_status_sub = self.create_subscription(
|
||||
String, '/uav/controller/status', self.uav_status_callback, 10)
|
||||
|
||||
self.ugv_status_sub = self.create_subscription(
|
||||
String, '/ugv/controller/status', self.ugv_status_callback, 10)
|
||||
|
||||
self.uav_reached_sub = self.create_subscription(
|
||||
Bool, '/uav/waypoint_follower/waypoint_reached', self.uav_reached_callback, 10)
|
||||
|
||||
self.ugv_reached_sub = self.create_subscription(
|
||||
Bool, '/ugv/goal_reached', self.ugv_reached_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_waypoint_pub = self.create_publisher(PoseStamped, '/uav/setpoint_position', 10)
|
||||
self.ugv_goal_pub = self.create_publisher(PoseStamped, '/ugv/goal_pose', 10)
|
||||
self.uav_path_pub = self.create_publisher(Path, '/waypoint_follower/set_path', 10)
|
||||
self.status_pub = self.create_publisher(String, '/mission/status', 10)
|
||||
|
||||
self.mission_timer = self.create_timer(0.5, self.mission_loop)
|
||||
|
||||
self.get_logger().info('Mission Planner Started - LOCAL coordinates only')
|
||||
|
||||
def command_callback(self, msg):
|
||||
cmd = msg.data.lower().split(':')
|
||||
action = cmd[0]
|
||||
|
||||
if action == 'load':
|
||||
if len(cmd) > 1:
|
||||
self.load_mission_file(cmd[1])
|
||||
else:
|
||||
self.load_demo_mission()
|
||||
elif action == 'start':
|
||||
self.start_mission()
|
||||
elif action == 'pause':
|
||||
self.state = MissionState.PAUSED
|
||||
elif action == 'resume':
|
||||
self.state = MissionState.EXECUTING
|
||||
elif action == 'abort':
|
||||
self.abort_mission()
|
||||
elif action == 'clear':
|
||||
self.mission = []
|
||||
self.current_action_idx = 0
|
||||
self.state = MissionState.IDLE
|
||||
|
||||
def uav_status_callback(self, msg):
|
||||
pass
|
||||
|
||||
def ugv_status_callback(self, msg):
|
||||
pass
|
||||
|
||||
def uav_reached_callback(self, msg):
|
||||
if msg.data:
|
||||
self.uav_goal_reached = True
|
||||
|
||||
def ugv_reached_callback(self, msg):
|
||||
if msg.data:
|
||||
self.ugv_goal_reached = True
|
||||
|
||||
def load_demo_mission(self):
|
||||
self.mission = [
|
||||
{'type': 'uav_command', 'command': 'arm'},
|
||||
{'type': 'uav_command', 'command': 'takeoff'},
|
||||
{'type': 'wait', 'duration': 3.0},
|
||||
{'type': 'uav_waypoint', 'x': 10.0, 'y': 0.0, 'z': 5.0},
|
||||
{'type': 'uav_waypoint', 'x': 10.0, 'y': 10.0, 'z': 5.0},
|
||||
{'type': 'ugv_goal', 'x': 5.0, 'y': 5.0},
|
||||
{'type': 'wait_for_vehicles'},
|
||||
{'type': 'uav_waypoint', 'x': 0.0, 'y': 0.0, 'z': 5.0},
|
||||
{'type': 'ugv_goal', 'x': 0.0, 'y': 0.0},
|
||||
{'type': 'wait_for_vehicles'},
|
||||
{'type': 'uav_command', 'command': 'land'},
|
||||
]
|
||||
self.current_action_idx = 0
|
||||
self.state = MissionState.IDLE
|
||||
self.get_logger().info(f'Demo mission loaded: {len(self.mission)} actions')
|
||||
|
||||
def load_mission_file(self, filepath):
|
||||
try:
|
||||
with open(filepath, 'r') as f:
|
||||
data = yaml.safe_load(f)
|
||||
self.mission = data.get('mission', [])
|
||||
self.current_action_idx = 0
|
||||
self.state = MissionState.IDLE
|
||||
self.get_logger().info(f'Mission loaded from {filepath}')
|
||||
except Exception as e:
|
||||
self.get_logger().error(f'Failed to load mission: {e}')
|
||||
|
||||
def start_mission(self):
|
||||
if len(self.mission) == 0:
|
||||
self.get_logger().warn('No mission loaded')
|
||||
return
|
||||
|
||||
self.current_action_idx = 0
|
||||
self.state = MissionState.EXECUTING
|
||||
self.uav_goal_reached = False
|
||||
self.ugv_goal_reached = False
|
||||
self.get_logger().info('Mission started')
|
||||
|
||||
def abort_mission(self):
|
||||
self.state = MissionState.ABORTED
|
||||
|
||||
stop_msg = String()
|
||||
stop_msg.data = 'hold'
|
||||
self.uav_cmd_pub.publish(stop_msg)
|
||||
|
||||
stop_msg.data = 'stop'
|
||||
self.ugv_cmd_pub.publish(stop_msg)
|
||||
|
||||
self.get_logger().warn('Mission aborted')
|
||||
|
||||
def mission_loop(self):
|
||||
status_msg = String()
|
||||
status_msg.data = f'{self.state.name}|{self.current_action_idx}/{len(self.mission)}'
|
||||
self.status_pub.publish(status_msg)
|
||||
|
||||
if self.state != MissionState.EXECUTING:
|
||||
return
|
||||
|
||||
if self.current_action_idx >= len(self.mission):
|
||||
self.state = MissionState.COMPLETED
|
||||
self.get_logger().info('Mission completed!')
|
||||
return
|
||||
|
||||
action = self.mission[self.current_action_idx]
|
||||
completed = self.execute_action(action)
|
||||
|
||||
if completed:
|
||||
self.current_action_idx += 1
|
||||
self.uav_goal_reached = False
|
||||
self.ugv_goal_reached = False
|
||||
|
||||
def execute_action(self, action):
|
||||
action_type = action.get('type', '')
|
||||
|
||||
if action_type == 'uav_command':
|
||||
cmd_msg = String()
|
||||
cmd_msg.data = action['command']
|
||||
self.uav_cmd_pub.publish(cmd_msg)
|
||||
return True
|
||||
|
||||
elif action_type == 'ugv_command':
|
||||
cmd_msg = String()
|
||||
cmd_msg.data = action['command']
|
||||
self.ugv_cmd_pub.publish(cmd_msg)
|
||||
return True
|
||||
|
||||
elif action_type == 'uav_waypoint':
|
||||
pose = PoseStamped()
|
||||
pose.header.stamp = self.get_clock().now().to_msg()
|
||||
pose.header.frame_id = 'odom'
|
||||
pose.pose.position.x = float(action['x'])
|
||||
pose.pose.position.y = float(action['y'])
|
||||
pose.pose.position.z = float(action['z'])
|
||||
pose.pose.orientation.w = 1.0
|
||||
self.uav_waypoint_pub.publish(pose)
|
||||
return self.uav_goal_reached
|
||||
|
||||
elif action_type == 'ugv_goal':
|
||||
pose = PoseStamped()
|
||||
pose.header.stamp = self.get_clock().now().to_msg()
|
||||
pose.header.frame_id = 'odom'
|
||||
pose.pose.position.x = float(action['x'])
|
||||
pose.pose.position.y = float(action['y'])
|
||||
pose.pose.orientation.w = 1.0
|
||||
self.ugv_goal_pub.publish(pose)
|
||||
return self.ugv_goal_reached
|
||||
|
||||
elif action_type == 'wait':
|
||||
if not hasattr(self, '_wait_start'):
|
||||
self._wait_start = self.get_clock().now()
|
||||
|
||||
elapsed = (self.get_clock().now() - self._wait_start).nanoseconds / 1e9
|
||||
if elapsed >= action['duration']:
|
||||
delattr(self, '_wait_start')
|
||||
return True
|
||||
return False
|
||||
|
||||
elif action_type == 'wait_for_vehicles':
|
||||
return self.uav_goal_reached and self.ugv_goal_reached
|
||||
|
||||
return True
|
||||
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
node = MissionPlanner()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -29,7 +29,6 @@ DEFAULT_WPNAV_SPEED_DN = 75
|
||||
DEFAULT_WPNAV_ACCEL_Z = 75
|
||||
DEFAULT_LOIT_SPEED = 150
|
||||
|
||||
|
||||
class Controller:
|
||||
HOLD_ALT = HOLD_ALT
|
||||
LOWEST_ALT = LOWEST_ALT
|
||||
@@ -81,10 +80,6 @@ class Controller:
|
||||
self.update_state()
|
||||
return self.position.copy()
|
||||
|
||||
def get_altitude(self) -> float:
|
||||
self.update_state()
|
||||
return self.altitude
|
||||
|
||||
def set_param(self, name: str, value: float):
|
||||
self.conn.mav.param_set_send(
|
||||
self.conn.target_system,
|
||||
@@ -96,15 +91,15 @@ class Controller:
|
||||
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)
|
||||
'EK3_CHECK_SCALE': 200,
|
||||
'EK3_GPS_CHECK': 0,
|
||||
'EK3_POSNE_M_NSE': 0.5,
|
||||
'EK3_VELD_M_NSE': 0.2,
|
||||
'EK3_VELNE_M_NSE': 0.1,
|
||||
'INS_GYRO_CAL': 0,
|
||||
'ARMING_CHECK': 0,
|
||||
}
|
||||
for name, value in params.items():
|
||||
self.set_param(name, value)
|
||||
@@ -131,15 +126,14 @@ class Controller:
|
||||
print("[UAV] Mode -> GUIDED_NOGPS")
|
||||
|
||||
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:
|
||||
elapsed = int(time.time() - t0)
|
||||
@@ -151,7 +145,6 @@ class Controller:
|
||||
def arm(self, retries: int = 15):
|
||||
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
|
||||
@@ -179,10 +172,6 @@ class Controller:
|
||||
time.sleep(2)
|
||||
return False
|
||||
|
||||
def disarm(self):
|
||||
self.conn.arducopter_disarm()
|
||||
print("[UAV] Disarmed")
|
||||
|
||||
def takeoff(self, altitude: float = HOLD_ALT):
|
||||
self.conn.mav.command_long_send(
|
||||
self.conn.target_system,
|
||||
@@ -199,36 +188,6 @@ class Controller:
|
||||
0, 89, 9, 0, 0, 0, 0, 0)
|
||||
print("[UAV] Landing (LAND mode)")
|
||||
|
||||
def move_pos_rel(self, x: float, y: float, z: float):
|
||||
move_msg = MAVLink_set_position_target_local_ned_message(
|
||||
int(perf_counter() * 1000),
|
||||
self.conn.target_system,
|
||||
self.conn.target_component,
|
||||
MAV_FRAME_BODY_OFFSET_NED,
|
||||
3576, x, y, z,
|
||||
0, 0, 0, 0, 0, 0, 0, 0)
|
||||
self.conn.mav.send(move_msg)
|
||||
|
||||
def move_pos_rel_yaw(self, x: float, y: float, z: float, yaw: float):
|
||||
move_msg = MAVLink_set_position_target_local_ned_message(
|
||||
int(perf_counter() * 1000),
|
||||
self.conn.target_system,
|
||||
self.conn.target_component,
|
||||
MAV_FRAME_BODY_OFFSET_NED,
|
||||
3576, x, y, z,
|
||||
0, 0, 0, 0, 0, 0, yaw, 0)
|
||||
self.conn.mav.send(move_msg)
|
||||
|
||||
def move_vel_rel_alt(self, vx: float, vy: float, z: float):
|
||||
move_msg = MAVLink_set_position_target_local_ned_message(
|
||||
int(perf_counter() * 1000),
|
||||
self.conn.target_system,
|
||||
self.conn.target_component,
|
||||
MAV_FRAME_BODY_OFFSET_NED,
|
||||
3555, 0, 0, z,
|
||||
vx, vy, 0, 0, 0, 0, 0, 0)
|
||||
self.conn.mav.send(move_msg)
|
||||
|
||||
def move_local_ned(self, x: float, y: float, z: float):
|
||||
move_msg = MAVLink_set_position_target_local_ned_message(
|
||||
int(perf_counter() * 1000),
|
||||
@@ -239,9 +198,6 @@ class Controller:
|
||||
0, 0, 0, 0, 0, 0, 0, 0)
|
||||
self.conn.mav.send(move_msg)
|
||||
|
||||
def distance(self, p1: Tuple, p2: Tuple) -> float:
|
||||
return float(np.linalg.norm(np.asarray(p1) - np.asarray(p2)))
|
||||
|
||||
def wait_altitude(self, target_alt: float, tolerance: float = 0.5,
|
||||
timeout: float = 30.0):
|
||||
t0 = time.time()
|
||||
@@ -276,9 +232,24 @@ class Controller:
|
||||
f"accel={wpnav_accel}cm/s² up={wpnav_speed_up}cm/s "
|
||||
f"dn={wpnav_speed_dn}cm/s")
|
||||
|
||||
def set_max_velocity(self, speed):
|
||||
self.conn.mav.command_long_send(
|
||||
self.conn.target_system,
|
||||
self.conn.target_component,
|
||||
mavlink.MAV_CMD_DO_CHANGE_SPEED,
|
||||
0, 1, speed, -1, 0, 0, 0, 0)
|
||||
def setup_ardupilot(ctrl: Controller):
|
||||
ctrl.set_param('ARMING_CHECK', 0)
|
||||
ctrl.set_param('SCHED_LOOP_RATE', 200)
|
||||
ctrl.set_param('FS_THR_ENABLE', 0)
|
||||
ctrl.set_param('FS_GCS_ENABLE', 0)
|
||||
sleep(2)
|
||||
|
||||
def wait_for_landing(ctrl: Controller, timeout: float = 60.0):
|
||||
print("[UAV] Waiting for landing ...")
|
||||
t0 = time.time()
|
||||
while time.time() - t0 < timeout:
|
||||
ctrl.update_state()
|
||||
elapsed = int(time.time() - t0)
|
||||
print(f"\r[UAV] Descending: {ctrl.altitude:.1f}m ({elapsed}s) ",
|
||||
end='', flush=True)
|
||||
if ctrl.altitude < 0.5 and not ctrl.armed:
|
||||
break
|
||||
if ctrl.altitude < 0.3:
|
||||
break
|
||||
sleep(0.5)
|
||||
print(f"\n[UAV] Landed! (alt: {ctrl.altitude:.2f}m)")
|
||||
|
||||
@@ -1,9 +1,4 @@
|
||||
#!/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 time
|
||||
@@ -14,13 +9,11 @@ from gz.transport13 import Node
|
||||
from gz.msgs10.twist_pb2 import Twist
|
||||
from gz.msgs10.odometry_pb2 import Odometry
|
||||
|
||||
|
||||
MAX_LINEAR_VEL = 1.0
|
||||
MAX_ANGULAR_VEL = 2.0
|
||||
POSITION_TOL = 0.5
|
||||
YAW_TOL = 0.15
|
||||
|
||||
|
||||
class UGVController:
|
||||
def __init__(self, cmd_topic="/ugv/cmd_vel", odom_topic="/ugv/odometry",
|
||||
min_speed=0.5, max_speed=1.0):
|
||||
@@ -69,14 +62,8 @@ class UGVController:
|
||||
def stop(self):
|
||||
self.send_cmd_vel(0.0, 0.0)
|
||||
|
||||
def distance_to(self, x, y):
|
||||
pos = self.get_position()
|
||||
dx = x - pos['x']
|
||||
dy = y - pos['y']
|
||||
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
|
||||
@@ -147,6 +134,3 @@ class UGVController:
|
||||
def is_driving(self):
|
||||
return self._driving
|
||||
|
||||
@property
|
||||
def has_arrived(self):
|
||||
return not self._driving and self._drive_thread is not None
|
||||
|
||||
102
src/main.py
102
src/main.py
@@ -10,63 +10,19 @@ from pathlib import Path
|
||||
|
||||
sys.path.insert(0, os.path.dirname(os.path.abspath(__file__)))
|
||||
|
||||
from control.uav_controller import Controller
|
||||
from control.search import Search
|
||||
from control.uav_controller import Controller, setup_ardupilot, wait_for_landing
|
||||
from navigation.search import Search
|
||||
from control.ugv_controller import UGVController
|
||||
from vision.object_detector import ObjectDetector
|
||||
from vision.camera_processor import CameraProcessor
|
||||
from navigation.flight_tracker import FlightTracker
|
||||
from utils.recorder import RunRecorder
|
||||
|
||||
from utils.convert import ned_to_gz, gz_to_ned, feet_to_meters, mph_to_ms
|
||||
from utils.config import load_config
|
||||
PROJECT_DIR = Path(__file__).resolve().parent.parent
|
||||
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:
|
||||
path = CONFIG_DIR / name
|
||||
if not path.exists():
|
||||
print(f"[WARN] Config not found: {path}")
|
||||
return {}
|
||||
with open(path, 'r') as f:
|
||||
return yaml.safe_load(f) or {}
|
||||
|
||||
|
||||
def setup_ardupilot(ctrl: Controller):
|
||||
ctrl.set_param('ARMING_CHECK', 0)
|
||||
ctrl.set_param('SCHED_LOOP_RATE', 200)
|
||||
ctrl.set_param('FS_THR_ENABLE', 0)
|
||||
ctrl.set_param('FS_GCS_ENABLE', 0)
|
||||
sleep(2)
|
||||
|
||||
|
||||
def wait_for_landing(ctrl: Controller, timeout: float = 60.0):
|
||||
print("[UAV] Waiting for landing ...")
|
||||
t0 = time.time()
|
||||
while time.time() - t0 < timeout:
|
||||
ctrl.update_state()
|
||||
elapsed = int(time.time() - t0)
|
||||
print(f"\r[UAV] Descending: {ctrl.altitude:.1f}m ({elapsed}s) ",
|
||||
end='', flush=True)
|
||||
if ctrl.altitude < 0.5 and not ctrl.armed:
|
||||
break
|
||||
if ctrl.altitude < 0.3:
|
||||
break
|
||||
sleep(0.5)
|
||||
print(f"\n[UAV] Landed! (alt: {ctrl.altitude:.2f}m)")
|
||||
|
||||
|
||||
def main():
|
||||
parser = argparse.ArgumentParser(description='UAV-UGV Simulation')
|
||||
@@ -83,11 +39,10 @@ def main():
|
||||
recorder = RunRecorder()
|
||||
recorder.start_logging()
|
||||
|
||||
uav_cfg = load_config('uav.yaml')
|
||||
search_cfg = load_config('search.yaml')
|
||||
ugv_cfg = load_config('ugv.yaml')
|
||||
|
||||
from utils.convert import feet_to_meters, mph_to_ms
|
||||
uav_cfg = load_config('uav.yaml', CONFIG_DIR)
|
||||
search_cfg = load_config('search.yaml', CONFIG_DIR)
|
||||
ugv_cfg = load_config('ugv.yaml', CONFIG_DIR)
|
||||
|
||||
raw_altitude = args.altitude or search_cfg.get('altitude', 4.0)
|
||||
altitude = feet_to_meters(raw_altitude)
|
||||
@@ -123,7 +78,6 @@ def main():
|
||||
|
||||
ctrl = Controller(conn_str)
|
||||
|
||||
# Apply configuration speed bounds
|
||||
ugv_min_ms = mph_to_ms(ugv_min_mph)
|
||||
ugv_max_ms = mph_to_ms(ugv_max_mph)
|
||||
ugv = UGVController(cmd_topic=ugv_cmd_topic, odom_topic=ugv_odom_topic,
|
||||
@@ -188,12 +142,11 @@ def main():
|
||||
tracker.start()
|
||||
|
||||
ctrl.configure_ekf_fast_converge()
|
||||
|
||||
# Configure AP speeds in cm/s from max_mph
|
||||
|
||||
uav_max_ms = mph_to_ms(uav_max_mph)
|
||||
uav_max_cms = int(uav_max_ms * 100)
|
||||
ctrl.configure_speed_limits(wpnav_speed=uav_max_cms, loit_speed=uav_max_cms)
|
||||
|
||||
|
||||
setup_ardupilot(ctrl)
|
||||
if recorder:
|
||||
recorder.set_phase("ekf_wait")
|
||||
@@ -211,17 +164,15 @@ def main():
|
||||
|
||||
ctrl.takeoff(altitude)
|
||||
tracker.reset_timer()
|
||||
|
||||
|
||||
if recorder:
|
||||
recorder.set_phase("search")
|
||||
|
||||
ctrl.wait_altitude(altitude, tolerance=1.0, timeout=30)
|
||||
|
||||
|
||||
if recorder:
|
||||
recorder.start_recording(tracker=tracker, camera=camera)
|
||||
recorder.snapshot_camera("pre_search")
|
||||
|
||||
# --- Wire tracker updates into search loop ---
|
||||
original_check = search.check_for_markers
|
||||
def tracked_check():
|
||||
result = original_check()
|
||||
@@ -230,11 +181,11 @@ def main():
|
||||
tracker.update_uav(pos['x'], pos['y'],
|
||||
altitude=ctrl.altitude,
|
||||
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'],
|
||||
@@ -247,13 +198,10 @@ def main():
|
||||
search.check_for_markers = tracked_check
|
||||
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
|
||||
ugv_target_pos = [None, None]
|
||||
|
||||
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
|
||||
@@ -261,20 +209,14 @@ def main():
|
||||
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()
|
||||
search_results = search.get_results()
|
||||
|
||||
if recorder and results:
|
||||
recorder.snapshot_camera("marker_detected")
|
||||
|
||||
print(f"\n{'=' * 50}")
|
||||
print(f" SEARCH COMPLETE")
|
||||
print(f"{'=' * 50}")
|
||||
@@ -286,7 +228,6 @@ def main():
|
||||
f"distance:{info['distance']:.2f}m")
|
||||
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:
|
||||
@@ -302,7 +243,6 @@ def main():
|
||||
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})")
|
||||
@@ -312,7 +252,7 @@ def main():
|
||||
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()
|
||||
@@ -329,11 +269,10 @@ def main():
|
||||
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:
|
||||
@@ -344,7 +283,7 @@ def main():
|
||||
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'):
|
||||
@@ -355,8 +294,6 @@ def main():
|
||||
|
||||
if recorder:
|
||||
recorder.set_phase("complete")
|
||||
recorder.snapshot_camera("post_landing")
|
||||
recorder.snapshot_tracker("final")
|
||||
ugv_target_info = None
|
||||
if ugv_target_pos[0] is not None:
|
||||
ugv_target_info = {
|
||||
@@ -394,6 +331,5 @@ def main():
|
||||
tracker.stop()
|
||||
print("[MAIN] Done.")
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
|
||||
@@ -1 +1 @@
|
||||
"""Navigation module for local path planning and waypoint following."""
|
||||
|
||||
|
||||
@@ -10,7 +10,7 @@ import cv2
|
||||
from collections import deque
|
||||
|
||||
sys.path.insert(0, os.path.join(os.path.dirname(__file__), ".."))
|
||||
|
||||
from safety.geofence import point_in_polygon, distance_to_polygon_edge
|
||||
|
||||
class FlightTracker:
|
||||
BG_COLOR = (30, 30, 30)
|
||||
@@ -101,36 +101,6 @@ class FlightTracker:
|
||||
with self._lock:
|
||||
self.active_waypoint = idx
|
||||
|
||||
def _point_in_polygon(self, px, py, polygon):
|
||||
n = len(polygon)
|
||||
inside = False
|
||||
j = n - 1
|
||||
for i in range(n):
|
||||
xi, yi = polygon[i]
|
||||
xj, yj = polygon[j]
|
||||
if ((yi > py) != (yj > py)) and (px < (xj - xi) * (py - yi) / (yj - yi) + xi):
|
||||
inside = not inside
|
||||
j = i
|
||||
return inside
|
||||
|
||||
def _distance_to_polygon_edge(self, px, py, polygon):
|
||||
min_dist = float('inf')
|
||||
n = len(polygon)
|
||||
for i in range(n):
|
||||
x1, y1 = polygon[i]
|
||||
x2, y2 = polygon[(i + 1) % n]
|
||||
dx, dy = x2 - x1, y2 - y1
|
||||
length_sq = dx * dx + dy * dy
|
||||
if length_sq == 0:
|
||||
dist = math.sqrt((px - x1) ** 2 + (py - y1) ** 2)
|
||||
else:
|
||||
t = max(0, min(1, ((px - x1) * dx + (py - y1) * dy) / length_sq))
|
||||
proj_x = x1 + t * dx
|
||||
proj_y = y1 + t * dy
|
||||
dist = math.sqrt((px - proj_x) ** 2 + (py - proj_y) ** 2)
|
||||
min_dist = min(min_dist, dist)
|
||||
return min_dist
|
||||
|
||||
def draw(self):
|
||||
frame = np.full((self.window_size, self.total_width, 3),
|
||||
self.BG_COLOR, dtype=np.uint8)
|
||||
@@ -331,9 +301,9 @@ class FlightTracker:
|
||||
y += 30
|
||||
|
||||
if self.fence_points:
|
||||
inside = self._point_in_polygon(uav[0], uav[1], self.fence_points)
|
||||
inside = point_in_polygon(uav[0], uav[1], self.fence_points)
|
||||
alt_ok = self.fence_alt_min <= alt <= self.fence_alt_max
|
||||
fence_dist = self._distance_to_polygon_edge(uav[0], uav[1], self.fence_points)
|
||||
fence_dist = distance_to_polygon_edge(uav[0], uav[1], self.fence_points)
|
||||
if not inside or not alt_ok:
|
||||
status, status_color = "BREACHED", self.FENCE_BREACH_COLOR
|
||||
elif fence_dist < self.fence_warn_dist:
|
||||
|
||||
@@ -1,149 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Local Planner - Path planning using relative coordinates only."""
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from geometry_msgs.msg import PoseStamped, Twist, Point
|
||||
from nav_msgs.msg import Path, Odometry
|
||||
from visualization_msgs.msg import Marker
|
||||
import numpy as np
|
||||
|
||||
|
||||
class LocalPlanner(Node):
|
||||
|
||||
def __init__(self):
|
||||
super().__init__('local_planner')
|
||||
|
||||
self.declare_parameter('max_velocity', 2.0)
|
||||
self.declare_parameter('max_acceleration', 1.0)
|
||||
self.declare_parameter('lookahead_distance', 1.0)
|
||||
self.declare_parameter('goal_tolerance', 0.3)
|
||||
|
||||
self.max_velocity = self.get_parameter('max_velocity').value
|
||||
self.max_acceleration = self.get_parameter('max_acceleration').value
|
||||
self.lookahead_distance = self.get_parameter('lookahead_distance').value
|
||||
self.goal_tolerance = self.get_parameter('goal_tolerance').value
|
||||
|
||||
self.current_pose = None
|
||||
self.current_velocity = np.zeros(3)
|
||||
self.path = []
|
||||
self.current_waypoint_idx = 0
|
||||
|
||||
self.odom_sub = self.create_subscription(
|
||||
Odometry, '/uav/local_position/odom', self.odom_callback, 10)
|
||||
|
||||
self.goal_sub = self.create_subscription(
|
||||
PoseStamped, '/uav/goal_pose', self.goal_callback, 10)
|
||||
|
||||
self.path_sub = self.create_subscription(
|
||||
Path, '/uav/planned_path', self.path_callback, 10)
|
||||
|
||||
self.cmd_vel_pub = self.create_publisher(Twist, '/uav/cmd_vel', 10)
|
||||
self.setpoint_pub = self.create_publisher(PoseStamped, '/uav/setpoint_position', 10)
|
||||
self.path_viz_pub = self.create_publisher(Path, '/uav/local_path', 10)
|
||||
|
||||
self.timer = self.create_timer(0.05, self.control_loop)
|
||||
|
||||
self.get_logger().info('Local Planner Started - GPS-denied navigation')
|
||||
|
||||
def odom_callback(self, msg):
|
||||
self.current_pose = msg.pose.pose
|
||||
self.current_velocity = np.array([
|
||||
msg.twist.twist.linear.x,
|
||||
msg.twist.twist.linear.y,
|
||||
msg.twist.twist.linear.z
|
||||
])
|
||||
|
||||
def goal_callback(self, msg):
|
||||
goal = np.array([
|
||||
msg.pose.position.x,
|
||||
msg.pose.position.y,
|
||||
msg.pose.position.z
|
||||
])
|
||||
self.path = [goal]
|
||||
self.current_waypoint_idx = 0
|
||||
self.get_logger().info(f'New goal received: [{goal[0]:.2f}, {goal[1]:.2f}, {goal[2]:.2f}]')
|
||||
|
||||
def path_callback(self, msg):
|
||||
self.path = []
|
||||
for pose_stamped in msg.poses:
|
||||
waypoint = np.array([
|
||||
pose_stamped.pose.position.x,
|
||||
pose_stamped.pose.position.y,
|
||||
pose_stamped.pose.position.z
|
||||
])
|
||||
self.path.append(waypoint)
|
||||
self.current_waypoint_idx = 0
|
||||
self.get_logger().info(f'Path received with {len(self.path)} waypoints')
|
||||
|
||||
def control_loop(self):
|
||||
if self.current_pose is None or len(self.path) == 0:
|
||||
return
|
||||
|
||||
if self.current_waypoint_idx >= len(self.path):
|
||||
self.stop()
|
||||
return
|
||||
|
||||
current_pos = np.array([
|
||||
self.current_pose.position.x,
|
||||
self.current_pose.position.y,
|
||||
self.current_pose.position.z
|
||||
])
|
||||
|
||||
target = self.path[self.current_waypoint_idx]
|
||||
distance = np.linalg.norm(target - current_pos)
|
||||
|
||||
if distance < self.goal_tolerance:
|
||||
self.current_waypoint_idx += 1
|
||||
if self.current_waypoint_idx >= len(self.path):
|
||||
self.get_logger().info('Path complete!')
|
||||
self.stop()
|
||||
return
|
||||
target = self.path[self.current_waypoint_idx]
|
||||
|
||||
direction = target - current_pos
|
||||
distance = np.linalg.norm(direction)
|
||||
|
||||
if distance > 0:
|
||||
direction = direction / distance
|
||||
|
||||
speed = min(self.max_velocity, distance * 0.5)
|
||||
velocity = direction * speed
|
||||
|
||||
self.publish_command(velocity, target)
|
||||
|
||||
def publish_command(self, velocity, target):
|
||||
cmd = Twist()
|
||||
cmd.linear.x = float(velocity[0])
|
||||
cmd.linear.y = float(velocity[1])
|
||||
cmd.linear.z = float(velocity[2])
|
||||
self.cmd_vel_pub.publish(cmd)
|
||||
|
||||
setpoint = PoseStamped()
|
||||
setpoint.header.stamp = self.get_clock().now().to_msg()
|
||||
setpoint.header.frame_id = 'odom'
|
||||
setpoint.pose.position.x = float(target[0])
|
||||
setpoint.pose.position.y = float(target[1])
|
||||
setpoint.pose.position.z = float(target[2])
|
||||
setpoint.pose.orientation.w = 1.0
|
||||
self.setpoint_pub.publish(setpoint)
|
||||
|
||||
def stop(self):
|
||||
cmd = Twist()
|
||||
self.cmd_vel_pub.publish(cmd)
|
||||
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
node = LocalPlanner()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -1,160 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Obstacle Avoidance - Vision-based obstacle detection and avoidance."""
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from sensor_msgs.msg import Image, LaserScan
|
||||
from geometry_msgs.msg import Twist, TwistStamped, Vector3
|
||||
from std_msgs.msg import Bool
|
||||
from cv_bridge import CvBridge
|
||||
import cv2
|
||||
import numpy as np
|
||||
|
||||
|
||||
class ObstacleAvoidance(Node):
|
||||
|
||||
def __init__(self):
|
||||
super().__init__('obstacle_avoidance')
|
||||
|
||||
self.bridge = CvBridge()
|
||||
|
||||
self.declare_parameter('detection_range', 5.0)
|
||||
self.declare_parameter('safety_margin', 1.0)
|
||||
self.declare_parameter('avoidance_gain', 1.0)
|
||||
self.declare_parameter('enable', True)
|
||||
|
||||
self.detection_range = self.get_parameter('detection_range').value
|
||||
self.safety_margin = self.get_parameter('safety_margin').value
|
||||
self.avoidance_gain = self.get_parameter('avoidance_gain').value
|
||||
self.enabled = self.get_parameter('enable').value
|
||||
|
||||
self.obstacle_detected = False
|
||||
self.obstacle_direction = np.zeros(3)
|
||||
self.obstacle_distance = float('inf')
|
||||
|
||||
self.depth_sub = self.create_subscription(
|
||||
Image, '/uav/camera/depth/image_raw', self.depth_callback, 10)
|
||||
|
||||
self.scan_sub = self.create_subscription(
|
||||
LaserScan, '/uav/scan', self.scan_callback, 10)
|
||||
|
||||
self.cmd_vel_sub = self.create_subscription(
|
||||
Twist, '/uav/cmd_vel', self.cmd_vel_callback, 10)
|
||||
|
||||
self.enable_sub = self.create_subscription(
|
||||
Bool, '/obstacle_avoidance/enable', self.enable_callback, 10)
|
||||
|
||||
self.cmd_vel_pub = self.create_publisher(Twist, '/uav/cmd_vel_safe', 10)
|
||||
self.obstacle_pub = self.create_publisher(Bool, '/obstacle_avoidance/obstacle_detected', 10)
|
||||
self.avoidance_vec_pub = self.create_publisher(Vector3, '/obstacle_avoidance/avoidance_vector', 10)
|
||||
|
||||
self.current_cmd = Twist()
|
||||
|
||||
self.timer = self.create_timer(0.05, self.avoidance_loop)
|
||||
|
||||
self.get_logger().info('Obstacle Avoidance Node Started')
|
||||
|
||||
def enable_callback(self, msg):
|
||||
self.enabled = msg.data
|
||||
|
||||
def depth_callback(self, msg):
|
||||
try:
|
||||
depth_image = self.bridge.imgmsg_to_cv2(msg, 'passthrough')
|
||||
|
||||
height, width = depth_image.shape
|
||||
center_region = depth_image[height//3:2*height//3, width//3:2*width//3]
|
||||
|
||||
valid_depths = center_region[center_region > 0]
|
||||
if len(valid_depths) > 0:
|
||||
min_distance = np.min(valid_depths)
|
||||
|
||||
if min_distance < self.detection_range:
|
||||
self.obstacle_detected = True
|
||||
self.obstacle_distance = min_distance
|
||||
|
||||
left_region = depth_image[:, :width//3]
|
||||
right_region = depth_image[:, 2*width//3:]
|
||||
|
||||
left_clear = np.mean(left_region[left_region > 0]) if np.any(left_region > 0) else float('inf')
|
||||
right_clear = np.mean(right_region[right_region > 0]) if np.any(right_region > 0) else float('inf')
|
||||
|
||||
if left_clear > right_clear:
|
||||
self.obstacle_direction = np.array([0.0, 1.0, 0.0])
|
||||
else:
|
||||
self.obstacle_direction = np.array([0.0, -1.0, 0.0])
|
||||
else:
|
||||
self.obstacle_detected = False
|
||||
self.obstacle_distance = float('inf')
|
||||
|
||||
except Exception as e:
|
||||
self.get_logger().error(f'Depth processing error: {e}')
|
||||
|
||||
def scan_callback(self, msg):
|
||||
ranges = np.array(msg.ranges)
|
||||
valid_ranges = ranges[np.isfinite(ranges)]
|
||||
|
||||
if len(valid_ranges) > 0:
|
||||
min_range = np.min(valid_ranges)
|
||||
min_idx = np.argmin(ranges)
|
||||
|
||||
if min_range < self.detection_range:
|
||||
self.obstacle_detected = True
|
||||
self.obstacle_distance = min_range
|
||||
|
||||
angle = msg.angle_min + min_idx * msg.angle_increment
|
||||
self.obstacle_direction = np.array([
|
||||
-np.cos(angle),
|
||||
-np.sin(angle),
|
||||
0.0
|
||||
])
|
||||
else:
|
||||
self.obstacle_detected = False
|
||||
|
||||
def cmd_vel_callback(self, msg):
|
||||
self.current_cmd = msg
|
||||
|
||||
def avoidance_loop(self):
|
||||
obstacle_msg = Bool()
|
||||
obstacle_msg.data = self.obstacle_detected
|
||||
self.obstacle_pub.publish(obstacle_msg)
|
||||
|
||||
if not self.enabled:
|
||||
self.cmd_vel_pub.publish(self.current_cmd)
|
||||
return
|
||||
|
||||
safe_cmd = Twist()
|
||||
|
||||
if self.obstacle_detected and self.obstacle_distance < self.safety_margin:
|
||||
repulsion = self.avoidance_gain * (self.safety_margin / self.obstacle_distance)
|
||||
avoidance = self.obstacle_direction * repulsion
|
||||
|
||||
safe_cmd.linear.x = self.current_cmd.linear.x + float(avoidance[0])
|
||||
safe_cmd.linear.y = self.current_cmd.linear.y + float(avoidance[1])
|
||||
safe_cmd.linear.z = self.current_cmd.linear.z + float(avoidance[2])
|
||||
safe_cmd.angular = self.current_cmd.angular
|
||||
|
||||
vec_msg = Vector3()
|
||||
vec_msg.x = float(avoidance[0])
|
||||
vec_msg.y = float(avoidance[1])
|
||||
vec_msg.z = float(avoidance[2])
|
||||
self.avoidance_vec_pub.publish(vec_msg)
|
||||
else:
|
||||
safe_cmd = self.current_cmd
|
||||
|
||||
self.cmd_vel_pub.publish(safe_cmd)
|
||||
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
node = ObstacleAvoidance()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
21
src/navigation/patterns/lawnmower.py
Normal file
21
src/navigation/patterns/lawnmower.py
Normal file
@@ -0,0 +1,21 @@
|
||||
def plan_lawnmower(start, width, height, lane_spacing):
|
||||
waypoints = [start]
|
||||
sx, sy = start
|
||||
num_lanes = max(1, int(width / lane_spacing) + 1)
|
||||
|
||||
start_x = sx - width / 2.0
|
||||
start_y = sy - height / 2.0
|
||||
|
||||
for lane in range(num_lanes):
|
||||
lane_x = start_x + lane * lane_spacing
|
||||
if lane % 2 == 0:
|
||||
current_y = start_y
|
||||
target_y = start_y + height
|
||||
else:
|
||||
current_y = start_y + height
|
||||
target_y = start_y
|
||||
|
||||
waypoints.append((lane_x, current_y))
|
||||
waypoints.append((lane_x, target_y))
|
||||
|
||||
return waypoints
|
||||
42
src/navigation/patterns/levy.py
Normal file
42
src/navigation/patterns/levy.py
Normal file
@@ -0,0 +1,42 @@
|
||||
import math
|
||||
from scipy.stats import levy, uniform
|
||||
|
||||
def plan_levy(start, max_steps, field_size, step_size, angle_dist=None):
|
||||
if angle_dist is None:
|
||||
angle_dist = uniform(loc=-180, scale=360)
|
||||
|
||||
waypoints = [start]
|
||||
x, y = start
|
||||
|
||||
half_size = field_size / 2.0
|
||||
min_x, max_x = start[0] - half_size, start[0] + half_size
|
||||
min_y, max_y = start[1] - half_size, start[1] + half_size
|
||||
|
||||
for _ in range(max_steps):
|
||||
angle_deg = angle_dist.rvs()
|
||||
angle_rad = math.radians(angle_deg)
|
||||
raw_distance = levy.rvs(loc=step_size, scale=step_size)
|
||||
distance = max(raw_distance, step_size)
|
||||
|
||||
nx = x + distance * math.cos(angle_rad)
|
||||
ny = y + distance * math.sin(angle_rad)
|
||||
|
||||
width = max_x - min_x
|
||||
height = max_y - min_y
|
||||
|
||||
if width > 0:
|
||||
nx_shifted = (nx - min_x) % (2 * width)
|
||||
if nx_shifted > width:
|
||||
nx_shifted = 2 * width - nx_shifted
|
||||
nx = min_x + nx_shifted
|
||||
|
||||
if height > 0:
|
||||
ny_shifted = (ny - min_y) % (2 * height)
|
||||
if ny_shifted > height:
|
||||
ny_shifted = 2 * height - ny_shifted
|
||||
ny = min_y + ny_shifted
|
||||
|
||||
x, y = nx, ny
|
||||
waypoints.append((x, y))
|
||||
|
||||
return waypoints
|
||||
28
src/navigation/patterns/spiral.py
Normal file
28
src/navigation/patterns/spiral.py
Normal file
@@ -0,0 +1,28 @@
|
||||
def plan_spiral(start, initial_leg, leg_increment, max_legs):
|
||||
waypoints = [start]
|
||||
x, y = start
|
||||
|
||||
directions = [
|
||||
(0, 1),
|
||||
(1, 0),
|
||||
(0, -1),
|
||||
(-1, 0),
|
||||
]
|
||||
|
||||
distance = initial_leg
|
||||
dir_idx = 0
|
||||
legs_at_this_dist = 0
|
||||
|
||||
for _ in range(max_legs):
|
||||
dx_dir, dy_dir = directions[dir_idx % 4]
|
||||
x += dx_dir * distance
|
||||
y += dy_dir * distance
|
||||
waypoints.append((x, y))
|
||||
|
||||
dir_idx += 1
|
||||
legs_at_this_dist += 1
|
||||
if legs_at_this_dist >= 2:
|
||||
legs_at_this_dist = 0
|
||||
distance += leg_increment
|
||||
|
||||
return waypoints
|
||||
@@ -10,7 +10,10 @@ from enum import Enum
|
||||
from control.uav_controller import Controller
|
||||
from vision.object_detector import ObjectDetector
|
||||
from utils.helpers import distance_2d
|
||||
|
||||
from safety.geofence import clip_to_geofence
|
||||
from navigation.patterns.spiral import plan_spiral
|
||||
from navigation.patterns.lawnmower import plan_lawnmower
|
||||
from navigation.patterns.levy import plan_levy
|
||||
|
||||
class SearchMode(Enum):
|
||||
SPIRAL = "spiral"
|
||||
@@ -41,12 +44,13 @@ class Search:
|
||||
self.on_target_found = None
|
||||
self._dispatched_targets = set()
|
||||
|
||||
hfov = 1.3962634 # 80 deg horizontal FOV
|
||||
hfov = 1.3962634
|
||||
self.cam_fov_meters = 2.0 * self.altitude * math.tan(hfov / 2.0)
|
||||
|
||||
self.spiral_max_legs = 40
|
||||
self.spiral_initial_leg = self.cam_fov_meters
|
||||
self.spiral_leg_increment = self.cam_fov_meters * 0.8
|
||||
overlap_spacing = self.cam_fov_meters * 0.8
|
||||
self.spiral_initial_leg = overlap_spacing
|
||||
self.spiral_leg_increment = overlap_spacing
|
||||
|
||||
self.lawn_width = 30.0
|
||||
self.lawn_height = 30.0
|
||||
@@ -67,17 +71,38 @@ class Search:
|
||||
setattr(self, key, value)
|
||||
|
||||
def plan(self, start_pos=(0.0, 0.0), geofence_points=None):
|
||||
if geofence_points:
|
||||
warn_dist = getattr(self, 'geofence_warn_dist', 3.0)
|
||||
xs = [p[0] for p in geofence_points]
|
||||
ys = [p[1] for p in geofence_points]
|
||||
safe_w = (max(xs) - min(xs)) - 2 * warn_dist
|
||||
safe_h = (max(ys) - min(ys)) - 2 * warn_dist
|
||||
if safe_w > 0 and safe_h > 0:
|
||||
self.lawn_width = safe_w
|
||||
self.lawn_height = safe_h
|
||||
self.levy_field_size = min(safe_w, safe_h)
|
||||
|
||||
hfov = 1.3962634
|
||||
self.cam_fov_meters = 2.0 * self.altitude * math.tan(hfov / 2.0)
|
||||
overlap_spacing = self.cam_fov_meters * 0.8
|
||||
self.spiral_initial_leg = overlap_spacing
|
||||
self.spiral_leg_increment = overlap_spacing
|
||||
self.lawn_lane_spacing = overlap_spacing
|
||||
self.lawn_lanes = int(self.lawn_width / max(self.lawn_lane_spacing, 0.1)) + 1
|
||||
|
||||
if self.mode == SearchMode.SPIRAL:
|
||||
waypoints = self._plan_spiral(start_pos)
|
||||
waypoints = plan_spiral(start_pos, self.spiral_initial_leg, self.spiral_leg_increment, self.spiral_max_legs)
|
||||
elif self.mode == SearchMode.LAWNMOWER:
|
||||
waypoints = self._plan_lawnmower(start_pos)
|
||||
waypoints = plan_lawnmower(start_pos, self.lawn_width, self.lawn_height, self.lawn_lane_spacing)
|
||||
elif self.mode == SearchMode.LEVY:
|
||||
waypoints = self._plan_levy(start_pos)
|
||||
waypoints = plan_levy(start_pos, self.levy_max_steps, self.levy_field_size, overlap_spacing, self.levy_angle_dist)
|
||||
else:
|
||||
waypoints = [start_pos]
|
||||
|
||||
if geofence_points:
|
||||
waypoints = self._clip_to_geofence(waypoints, geofence_points)
|
||||
warn_dist = getattr(self, 'geofence_warn_dist', 3.0)
|
||||
stop_on_leave = (self.mode == SearchMode.SPIRAL)
|
||||
waypoints = clip_to_geofence(waypoints, geofence_points, warn_dist, stop_on_leave)
|
||||
|
||||
self.waypoints = waypoints
|
||||
self.current_wp = 0
|
||||
@@ -91,136 +116,6 @@ class Search:
|
||||
|
||||
return waypoints
|
||||
|
||||
def _plan_spiral(self, start):
|
||||
waypoints = [start]
|
||||
x, y = start
|
||||
|
||||
directions = [
|
||||
(0, 1), # East (+Y)
|
||||
(1, 0), # North (+X)
|
||||
(0, -1), # West (-Y)
|
||||
(-1, 0), # South (-X)
|
||||
]
|
||||
|
||||
distance = self.spiral_initial_leg
|
||||
dir_idx = 0
|
||||
legs_at_this_dist = 0
|
||||
|
||||
for _ in range(self.spiral_max_legs):
|
||||
dx_dir, dy_dir = directions[dir_idx % 4]
|
||||
x += dx_dir * distance
|
||||
y += dy_dir * distance
|
||||
waypoints.append((x, y))
|
||||
|
||||
dir_idx += 1
|
||||
legs_at_this_dist += 1
|
||||
if legs_at_this_dist >= 2:
|
||||
legs_at_this_dist = 0
|
||||
distance += self.spiral_leg_increment
|
||||
|
||||
return waypoints
|
||||
|
||||
def _plan_lawnmower(self, start):
|
||||
waypoints = [start]
|
||||
sx, sy = start
|
||||
lane_spacing = self.lawn_lane_spacing
|
||||
height = self.lawn_height
|
||||
num_lanes = max(1, int(self.lawn_width / lane_spacing))
|
||||
|
||||
for lane in range(num_lanes):
|
||||
lane_x = sx + lane * lane_spacing
|
||||
if lane % 2 == 0:
|
||||
target_y = sy + height
|
||||
else:
|
||||
target_y = sy
|
||||
|
||||
waypoints.append((lane_x, target_y))
|
||||
|
||||
if lane < num_lanes - 1:
|
||||
next_x = sx + (lane + 1) * lane_spacing
|
||||
waypoints.append((next_x, target_y))
|
||||
|
||||
return waypoints
|
||||
|
||||
def _plan_levy(self, start):
|
||||
waypoints = [start]
|
||||
x, y = start
|
||||
|
||||
for _ in range(self.levy_max_steps):
|
||||
angle_deg = self.levy_angle_dist.rvs()
|
||||
angle_rad = math.radians(angle_deg)
|
||||
raw_distance = levy.rvs(loc=1, scale=1)
|
||||
distance = min(max(raw_distance, 1.0), self.levy_field_size)
|
||||
|
||||
x += distance * math.cos(angle_rad)
|
||||
y += distance * math.sin(angle_rad)
|
||||
waypoints.append((x, y))
|
||||
|
||||
return waypoints
|
||||
|
||||
def _clip_to_geofence(self, waypoints, polygon):
|
||||
warn_dist = getattr(self, 'geofence_warn_dist', 3.0)
|
||||
safe_polygon = []
|
||||
if warn_dist > 0:
|
||||
cx = sum(p[0] for p in polygon) / len(polygon)
|
||||
cy = sum(p[1] for p in polygon) / len(polygon)
|
||||
for x, y in polygon:
|
||||
dx = x - cx
|
||||
dy = y - cy
|
||||
length = math.sqrt(dx*dx + dy*dy)
|
||||
if length > 0:
|
||||
shrink = warn_dist / length
|
||||
safe_polygon.append((x - dx * shrink, y - dy * shrink))
|
||||
else:
|
||||
safe_polygon.append((x, y))
|
||||
else:
|
||||
safe_polygon = polygon
|
||||
|
||||
clipped = []
|
||||
for wx, wy in waypoints:
|
||||
if self._point_in_polygon(wx, wy, safe_polygon):
|
||||
clipped.append((wx, wy))
|
||||
else:
|
||||
nearest = self._nearest_point_on_polygon(wx, wy, safe_polygon)
|
||||
if not clipped or distance_2d(clipped[-1], nearest) > 0.5:
|
||||
clipped.append(nearest)
|
||||
return clipped
|
||||
|
||||
@staticmethod
|
||||
def _point_in_polygon(px, py, polygon):
|
||||
n = len(polygon)
|
||||
inside = False
|
||||
j = n - 1
|
||||
for i in range(n):
|
||||
xi, yi = polygon[i]
|
||||
xj, yj = polygon[j]
|
||||
if ((yi > py) != (yj > py)) and \
|
||||
(px < (xj - xi) * (py - yi) / (yj - yi) + xi):
|
||||
inside = not inside
|
||||
j = i
|
||||
return inside
|
||||
|
||||
@staticmethod
|
||||
def _nearest_point_on_polygon(px, py, polygon):
|
||||
min_dist = float('inf')
|
||||
nearest = (px, py)
|
||||
n = len(polygon)
|
||||
for i in range(n):
|
||||
x1, y1 = polygon[i]
|
||||
x2, y2 = polygon[(i + 1) % n]
|
||||
dx, dy = x2 - x1, y2 - y1
|
||||
length_sq = dx * dx + dy * dy
|
||||
if length_sq == 0:
|
||||
proj = (x1, y1)
|
||||
else:
|
||||
t = max(0, min(1, ((px - x1) * dx + (py - y1) * dy) / length_sq))
|
||||
proj = (x1 + t * dx, y1 + t * dy)
|
||||
dist = math.sqrt((px - proj[0])**2 + (py - proj[1])**2)
|
||||
if dist < min_dist:
|
||||
min_dist = dist
|
||||
nearest = proj
|
||||
return nearest
|
||||
|
||||
def run(self):
|
||||
if not self.waypoints:
|
||||
self.plan()
|
||||
@@ -305,9 +200,9 @@ class Search:
|
||||
if marker_id in self.target_ids and marker_id not in self._dispatched_targets:
|
||||
self._dispatched_targets.add(marker_id)
|
||||
self.running = False
|
||||
|
||||
|
||||
final_pos = self.center_over_target(marker_id)
|
||||
|
||||
|
||||
print(f"\n[SEARCH] Target ID:{marker_id} centered! "
|
||||
f"Dispatching UGV to ({final_pos['x']:.1f}, {final_pos['y']:.1f})")
|
||||
if self.on_target_found:
|
||||
@@ -339,7 +234,7 @@ class Search:
|
||||
|
||||
IMG_W, IMG_H = 640, 480
|
||||
IMG_CX, IMG_CY = IMG_W / 2, IMG_H / 2
|
||||
HFOV = 1.3962634 # 80° horizontal FOV
|
||||
HFOV = 1.3962634
|
||||
|
||||
self.ctrl.update_state()
|
||||
current_alt = self.ctrl.altitude
|
||||
@@ -391,7 +286,7 @@ class Search:
|
||||
|
||||
IMG_W, IMG_H = 640, 480
|
||||
IMG_CX, IMG_CY = IMG_W / 2, IMG_H / 2
|
||||
HFOV = 1.3962634 # 80° horizontal FOV
|
||||
HFOV = 1.3962634
|
||||
|
||||
CENTER_PX = 30
|
||||
MAX_CORRECTIONS = 200
|
||||
@@ -418,7 +313,7 @@ class Search:
|
||||
hfov, center_px, max_corrections, max_lost, gain, allowed_ids=None):
|
||||
if allowed_ids is None:
|
||||
allowed_ids = self.land_ids
|
||||
|
||||
|
||||
lost_count = 0
|
||||
centered = False
|
||||
|
||||
@@ -466,15 +361,13 @@ class Search:
|
||||
alt = max(self.ctrl.altitude, 0.5)
|
||||
ground_w = 2.0 * alt * math.tan(hfov / 2.0)
|
||||
m_per_px = ground_w / img_w
|
||||
|
||||
# Calculate movement in the UAV's body frame
|
||||
|
||||
correction_forward = -err_y * m_per_px * gain
|
||||
correction_right = err_x * m_per_px * gain
|
||||
|
||||
self.ctrl.update_state()
|
||||
yaw = self.ctrl.current_yaw
|
||||
|
||||
# Rotate body correction (Forward, Right) into world NED (North, East)
|
||||
|
||||
correction_n = correction_forward * math.cos(yaw) - correction_right * math.sin(yaw)
|
||||
correction_e = correction_forward * math.sin(yaw) + correction_right * math.cos(yaw)
|
||||
|
||||
@@ -494,9 +387,9 @@ class Search:
|
||||
self.ctrl.update_state()
|
||||
pos = self.ctrl.get_local_position()
|
||||
dist_total = distance_2d((pos['x'], pos['y']), (target_x, target_y))
|
||||
|
||||
|
||||
if timeout is None:
|
||||
# Dynamic timeout: 30s base time + 3 seconds per meter
|
||||
|
||||
timeout = 30.0 + (dist_total * 3.0)
|
||||
|
||||
t0 = time.time()
|
||||
@@ -521,23 +414,6 @@ class Search:
|
||||
print()
|
||||
return False
|
||||
|
||||
def _wait_arrival(self, target_x, target_y, timeout=15.0):
|
||||
t0 = time.time()
|
||||
while time.time() - t0 < timeout and self.running:
|
||||
self.ctrl.update_state()
|
||||
pos = self.ctrl.get_local_position()
|
||||
dist = distance_2d(
|
||||
(pos['x'], pos['y']),
|
||||
(target_x, target_y)
|
||||
)
|
||||
elapsed = int(time.time() - t0)
|
||||
print(f"\r[SEARCH] Approaching marker: {dist:.1f}m ({elapsed}s) ",
|
||||
end='', flush=True)
|
||||
if dist < self.POSITION_TOLERANCE:
|
||||
print()
|
||||
return True
|
||||
sleep(self.CHECK_INTERVAL)
|
||||
print()
|
||||
return False
|
||||
|
||||
def move_to_local(self, x, y):
|
||||
@@ -1,183 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Waypoint Follower - Follows relative waypoints without GPS."""
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from geometry_msgs.msg import PoseStamped, Twist
|
||||
from nav_msgs.msg import Odometry, Path
|
||||
from std_msgs.msg import Int32, String, Bool
|
||||
import numpy as np
|
||||
from enum import Enum
|
||||
|
||||
|
||||
class WaypointState(Enum):
|
||||
IDLE = 0
|
||||
NAVIGATING = 1
|
||||
REACHED = 2
|
||||
PAUSED = 3
|
||||
|
||||
|
||||
class WaypointFollower(Node):
|
||||
|
||||
def __init__(self):
|
||||
super().__init__('waypoint_follower')
|
||||
|
||||
self.declare_parameter('waypoint_radius', 0.5)
|
||||
self.declare_parameter('max_velocity', 2.0)
|
||||
self.declare_parameter('kp_linear', 0.8)
|
||||
self.declare_parameter('kp_angular', 1.0)
|
||||
|
||||
self.waypoint_radius = self.get_parameter('waypoint_radius').value
|
||||
self.max_velocity = self.get_parameter('max_velocity').value
|
||||
self.kp_linear = self.get_parameter('kp_linear').value
|
||||
self.kp_angular = self.get_parameter('kp_angular').value
|
||||
|
||||
self.waypoints = []
|
||||
self.current_idx = 0
|
||||
self.state = WaypointState.IDLE
|
||||
self.current_pose = None
|
||||
|
||||
self.odom_sub = self.create_subscription(
|
||||
Odometry, '/uav/local_position/odom', self.odom_callback, 10)
|
||||
|
||||
self.waypoint_sub = self.create_subscription(
|
||||
PoseStamped, '/waypoint_follower/add_waypoint', self.add_waypoint_callback, 10)
|
||||
|
||||
self.path_sub = self.create_subscription(
|
||||
Path, '/waypoint_follower/set_path', self.set_path_callback, 10)
|
||||
|
||||
self.command_sub = self.create_subscription(
|
||||
String, '/waypoint_follower/command', self.command_callback, 10)
|
||||
|
||||
self.cmd_vel_pub = self.create_publisher(Twist, '/uav/cmd_vel', 10)
|
||||
self.current_waypoint_pub = self.create_publisher(Int32, '/waypoint_follower/current_index', 10)
|
||||
self.status_pub = self.create_publisher(String, '/waypoint_follower/status', 10)
|
||||
self.reached_pub = self.create_publisher(Bool, '/waypoint_follower/waypoint_reached', 10)
|
||||
|
||||
self.timer = self.create_timer(0.05, self.control_loop)
|
||||
|
||||
self.get_logger().info('Waypoint Follower Started - Using LOCAL coordinates only')
|
||||
|
||||
def odom_callback(self, msg):
|
||||
self.current_pose = msg.pose.pose
|
||||
|
||||
def add_waypoint_callback(self, msg):
|
||||
waypoint = np.array([
|
||||
msg.pose.position.x,
|
||||
msg.pose.position.y,
|
||||
msg.pose.position.z
|
||||
])
|
||||
self.waypoints.append(waypoint)
|
||||
self.get_logger().info(f'Added waypoint {len(self.waypoints)}: {waypoint}')
|
||||
|
||||
def set_path_callback(self, msg):
|
||||
self.waypoints = []
|
||||
for pose_stamped in msg.poses:
|
||||
waypoint = np.array([
|
||||
pose_stamped.pose.position.x,
|
||||
pose_stamped.pose.position.y,
|
||||
pose_stamped.pose.position.z
|
||||
])
|
||||
self.waypoints.append(waypoint)
|
||||
self.current_idx = 0
|
||||
self.get_logger().info(f'Set path with {len(self.waypoints)} waypoints')
|
||||
|
||||
def command_callback(self, msg):
|
||||
cmd = msg.data.lower()
|
||||
|
||||
if cmd == 'start':
|
||||
if len(self.waypoints) > 0:
|
||||
self.state = WaypointState.NAVIGATING
|
||||
self.get_logger().info('Starting waypoint navigation')
|
||||
elif cmd == 'pause':
|
||||
self.state = WaypointState.PAUSED
|
||||
self.stop()
|
||||
elif cmd == 'resume':
|
||||
self.state = WaypointState.NAVIGATING
|
||||
elif cmd == 'stop':
|
||||
self.state = WaypointState.IDLE
|
||||
self.stop()
|
||||
elif cmd == 'clear':
|
||||
self.waypoints = []
|
||||
self.current_idx = 0
|
||||
self.state = WaypointState.IDLE
|
||||
elif cmd == 'next':
|
||||
if self.current_idx < len(self.waypoints) - 1:
|
||||
self.current_idx += 1
|
||||
elif cmd == 'prev':
|
||||
if self.current_idx > 0:
|
||||
self.current_idx -= 1
|
||||
|
||||
def control_loop(self):
|
||||
self.publish_status()
|
||||
|
||||
if self.state != WaypointState.NAVIGATING:
|
||||
return
|
||||
|
||||
if self.current_pose is None or len(self.waypoints) == 0:
|
||||
return
|
||||
|
||||
if self.current_idx >= len(self.waypoints):
|
||||
self.state = WaypointState.IDLE
|
||||
self.stop()
|
||||
self.get_logger().info('All waypoints reached!')
|
||||
return
|
||||
|
||||
current_pos = np.array([
|
||||
self.current_pose.position.x,
|
||||
self.current_pose.position.y,
|
||||
self.current_pose.position.z
|
||||
])
|
||||
|
||||
target = self.waypoints[self.current_idx]
|
||||
error = target - current_pos
|
||||
distance = np.linalg.norm(error)
|
||||
|
||||
if distance < self.waypoint_radius:
|
||||
reached_msg = Bool()
|
||||
reached_msg.data = True
|
||||
self.reached_pub.publish(reached_msg)
|
||||
|
||||
self.get_logger().info(f'Reached waypoint {self.current_idx + 1}/{len(self.waypoints)}')
|
||||
self.current_idx += 1
|
||||
return
|
||||
|
||||
direction = error / distance
|
||||
speed = min(self.kp_linear * distance, self.max_velocity)
|
||||
velocity = direction * speed
|
||||
|
||||
cmd = Twist()
|
||||
cmd.linear.x = float(velocity[0])
|
||||
cmd.linear.y = float(velocity[1])
|
||||
cmd.linear.z = float(velocity[2])
|
||||
|
||||
self.cmd_vel_pub.publish(cmd)
|
||||
|
||||
def stop(self):
|
||||
cmd = Twist()
|
||||
self.cmd_vel_pub.publish(cmd)
|
||||
|
||||
def publish_status(self):
|
||||
idx_msg = Int32()
|
||||
idx_msg.data = self.current_idx
|
||||
self.current_waypoint_pub.publish(idx_msg)
|
||||
|
||||
status_msg = String()
|
||||
status_msg.data = f'{self.state.name}|{self.current_idx}/{len(self.waypoints)}'
|
||||
self.status_pub.publish(status_msg)
|
||||
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
node = WaypointFollower()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -1 +1 @@
|
||||
"""Safety module for geofencing and failsafe handling."""
|
||||
|
||||
|
||||
@@ -1,154 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Failsafe Handler - Emergency procedures for vision loss and other failures."""
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from geometry_msgs.msg import PoseStamped, TwistStamped
|
||||
from std_msgs.msg import String, Bool
|
||||
from sensor_msgs.msg import BatteryState
|
||||
from enum import Enum
|
||||
|
||||
|
||||
class FailsafeState(Enum):
|
||||
NORMAL = 0
|
||||
VISION_LOSS = 1
|
||||
LOW_BATTERY = 2
|
||||
GEOFENCE_BREACH = 3
|
||||
COMM_LOSS = 4
|
||||
EMERGENCY = 5
|
||||
|
||||
|
||||
class FailsafeHandler(Node):
|
||||
|
||||
def __init__(self):
|
||||
super().__init__('failsafe_handler')
|
||||
|
||||
self.declare_parameter('vision_loss_timeout', 5.0)
|
||||
self.declare_parameter('low_battery_threshold', 20.0)
|
||||
self.declare_parameter('critical_battery_threshold', 10.0)
|
||||
self.declare_parameter('action_on_vision_loss', 'HOLD')
|
||||
self.declare_parameter('action_on_low_battery', 'RTL')
|
||||
self.declare_parameter('action_on_critical_battery', 'LAND')
|
||||
|
||||
self.vision_timeout = self.get_parameter('vision_loss_timeout').value
|
||||
self.low_battery = self.get_parameter('low_battery_threshold').value
|
||||
self.critical_battery = self.get_parameter('critical_battery_threshold').value
|
||||
self.vision_loss_action = self.get_parameter('action_on_vision_loss').value
|
||||
self.low_battery_action = self.get_parameter('action_on_low_battery').value
|
||||
self.critical_battery_action = self.get_parameter('action_on_critical_battery').value
|
||||
|
||||
self.state = FailsafeState.NORMAL
|
||||
self.last_vo_time = None
|
||||
self.battery_percent = 100.0
|
||||
self.geofence_ok = True
|
||||
|
||||
self.vision_triggered = False
|
||||
self.battery_triggered = False
|
||||
|
||||
self.vo_pose_sub = self.create_subscription(
|
||||
PoseStamped, '/uav/visual_odometry/pose', self.vo_callback, 10)
|
||||
|
||||
self.battery_sub = self.create_subscription(
|
||||
BatteryState, '/uav/battery', self.battery_callback, 10)
|
||||
|
||||
self.geofence_sub = self.create_subscription(
|
||||
Bool, '/geofence/status', self.geofence_callback, 10)
|
||||
|
||||
self.geofence_action_sub = self.create_subscription(
|
||||
String, '/geofence/action', self.geofence_action_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.failsafe_status_pub = self.create_publisher(String, '/failsafe/status', 10)
|
||||
self.failsafe_active_pub = self.create_publisher(Bool, '/failsafe/active', 10)
|
||||
|
||||
self.check_timer = self.create_timer(0.5, self.check_failsafes)
|
||||
|
||||
self.get_logger().info('Failsafe Handler Started')
|
||||
|
||||
def vo_callback(self, msg):
|
||||
self.last_vo_time = self.get_clock().now()
|
||||
|
||||
if self.state == FailsafeState.VISION_LOSS:
|
||||
self.state = FailsafeState.NORMAL
|
||||
self.vision_triggered = False
|
||||
self.get_logger().info('Vision restored - returning to normal')
|
||||
|
||||
def battery_callback(self, msg):
|
||||
self.battery_percent = msg.percentage * 100.0
|
||||
|
||||
def geofence_callback(self, msg):
|
||||
self.geofence_ok = msg.data
|
||||
|
||||
def geofence_action_callback(self, msg):
|
||||
self.state = FailsafeState.GEOFENCE_BREACH
|
||||
self.execute_action(msg.data)
|
||||
|
||||
def check_failsafes(self):
|
||||
if self.last_vo_time is not None:
|
||||
elapsed = (self.get_clock().now() - self.last_vo_time).nanoseconds / 1e9
|
||||
if elapsed > self.vision_timeout and not self.vision_triggered:
|
||||
self.state = FailsafeState.VISION_LOSS
|
||||
self.vision_triggered = True
|
||||
self.get_logger().error(f'VISION LOSS - No VO for {elapsed:.1f}s')
|
||||
self.execute_action(self.vision_loss_action)
|
||||
|
||||
if self.battery_percent <= self.critical_battery and not self.battery_triggered:
|
||||
self.state = FailsafeState.LOW_BATTERY
|
||||
self.battery_triggered = True
|
||||
self.get_logger().error(f'CRITICAL BATTERY: {self.battery_percent:.1f}%')
|
||||
self.execute_action(self.critical_battery_action)
|
||||
elif self.battery_percent <= self.low_battery and not self.battery_triggered:
|
||||
self.state = FailsafeState.LOW_BATTERY
|
||||
self.battery_triggered = True
|
||||
self.get_logger().warn(f'LOW BATTERY: {self.battery_percent:.1f}%')
|
||||
self.execute_action(self.low_battery_action)
|
||||
|
||||
self.publish_status()
|
||||
|
||||
def execute_action(self, action):
|
||||
cmd_msg = String()
|
||||
|
||||
if action == 'HOLD':
|
||||
cmd_msg.data = 'hold'
|
||||
elif action == 'RTL':
|
||||
cmd_msg.data = 'rtl'
|
||||
elif action == 'LAND':
|
||||
cmd_msg.data = 'land'
|
||||
elif action == 'STOP':
|
||||
cmd_msg.data = 'stop'
|
||||
else:
|
||||
cmd_msg.data = 'hold'
|
||||
|
||||
self.uav_cmd_pub.publish(cmd_msg)
|
||||
|
||||
ugv_cmd = String()
|
||||
ugv_cmd.data = 'stop'
|
||||
self.ugv_cmd_pub.publish(ugv_cmd)
|
||||
|
||||
self.get_logger().warn(f'Failsafe action executed: {action}')
|
||||
|
||||
def publish_status(self):
|
||||
status_msg = String()
|
||||
status_msg.data = f'{self.state.name}|battery:{self.battery_percent:.1f}|geofence:{self.geofence_ok}'
|
||||
self.failsafe_status_pub.publish(status_msg)
|
||||
|
||||
active_msg = Bool()
|
||||
active_msg.data = self.state != FailsafeState.NORMAL
|
||||
self.failsafe_active_pub.publish(active_msg)
|
||||
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
node = FailsafeHandler()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
86
src/safety/geofence.py
Normal file
86
src/safety/geofence.py
Normal file
@@ -0,0 +1,86 @@
|
||||
#!/usr/bin/env python3
|
||||
|
||||
import math
|
||||
from utils.helpers import distance_2d
|
||||
|
||||
def point_in_polygon(px, py, polygon):
|
||||
n = len(polygon)
|
||||
inside = False
|
||||
j = n - 1
|
||||
for i in range(n):
|
||||
xi, yi = polygon[i]
|
||||
xj, yj = polygon[j]
|
||||
if ((yi > py) != (yj > py)) and (px < (xj - xi) * (py - yi) / (yj - yi) + xi):
|
||||
inside = not inside
|
||||
j = i
|
||||
return inside
|
||||
|
||||
def nearest_point_on_polygon(px, py, polygon):
|
||||
min_dist = float('inf')
|
||||
nearest = (px, py)
|
||||
n = len(polygon)
|
||||
for i in range(n):
|
||||
x1, y1 = polygon[i]
|
||||
x2, y2 = polygon[(i + 1) % n]
|
||||
dx, dy = x2 - x1, y2 - y1
|
||||
length_sq = dx * dx + dy * dy
|
||||
if length_sq == 0:
|
||||
proj = (x1, y1)
|
||||
else:
|
||||
t = max(0, min(1, ((px - x1) * dx + (py - y1) * dy) / length_sq))
|
||||
proj = (x1 + t * dx, y1 + t * dy)
|
||||
dist = math.sqrt((px - proj[0])**2 + (py - proj[1])**2)
|
||||
if dist < min_dist:
|
||||
min_dist = dist
|
||||
nearest = proj
|
||||
return nearest
|
||||
|
||||
def distance_to_polygon_edge(px, py, polygon):
|
||||
min_dist = float('inf')
|
||||
n = len(polygon)
|
||||
for i in range(n):
|
||||
x1, y1 = polygon[i]
|
||||
x2, y2 = polygon[(i + 1) % n]
|
||||
dx, dy = x2 - x1, y2 - y1
|
||||
length_sq = dx * dx + dy * dy
|
||||
if length_sq == 0:
|
||||
dist = math.sqrt((px - x1) ** 2 + (py - y1) ** 2)
|
||||
else:
|
||||
t = max(0, min(1, ((px - x1) * dx + (py - y1) * dy) / length_sq))
|
||||
proj_x = x1 + t * dx
|
||||
proj_y = y1 + t * dy
|
||||
dist = math.sqrt((px - proj_x) ** 2 + (py - proj_y) ** 2)
|
||||
min_dist = min(min_dist, dist)
|
||||
return min_dist
|
||||
|
||||
def clip_to_geofence(waypoints, polygon, warn_dist=3.0, stop_on_leave=False):
|
||||
safe_polygon = []
|
||||
if warn_dist > 0:
|
||||
cx = sum(p[0] for p in polygon) / len(polygon)
|
||||
cy = sum(p[1] for p in polygon) / len(polygon)
|
||||
for x, y in polygon:
|
||||
dx = x - cx
|
||||
dy = y - cy
|
||||
length = math.sqrt(dx*dx + dy*dy)
|
||||
if length > 0:
|
||||
shrink = warn_dist / length
|
||||
safe_polygon.append((x - dx * shrink, y - dy * shrink))
|
||||
else:
|
||||
safe_polygon.append((x, y))
|
||||
else:
|
||||
safe_polygon = polygon
|
||||
|
||||
clipped = []
|
||||
consecutive_outside = 0
|
||||
for wx, wy in waypoints:
|
||||
if point_in_polygon(wx, wy, safe_polygon):
|
||||
clipped.append((wx, wy))
|
||||
consecutive_outside = 0
|
||||
else:
|
||||
consecutive_outside += 1
|
||||
if stop_on_leave and consecutive_outside >= 2:
|
||||
break
|
||||
nearest = nearest_point_on_polygon(wx, wy, safe_polygon)
|
||||
if not clipped or distance_2d(clipped[-1], nearest) > 0.5:
|
||||
clipped.append(nearest)
|
||||
return clipped
|
||||
@@ -1,162 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Geofence Monitor - GPS ONLY used for safety boundaries."""
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from sensor_msgs.msg import NavSatFix
|
||||
from geometry_msgs.msg import Point
|
||||
from std_msgs.msg import Bool, String
|
||||
from visualization_msgs.msg import Marker
|
||||
import numpy as np
|
||||
from shapely.geometry import Point as ShapelyPoint, Polygon
|
||||
|
||||
|
||||
class GeofenceMonitor(Node):
|
||||
|
||||
def __init__(self):
|
||||
super().__init__('geofence_monitor')
|
||||
|
||||
self.declare_parameter('fence_enabled', True)
|
||||
self.declare_parameter('fence_type', 'polygon')
|
||||
self.declare_parameter('warning_distance', 10.0)
|
||||
self.declare_parameter('breach_action', 'RTL')
|
||||
self.declare_parameter('min_altitude', 0.0)
|
||||
self.declare_parameter('max_altitude', 50.0)
|
||||
self.declare_parameter('consecutive_breaches_required', 3)
|
||||
|
||||
self.fence_enabled = self.get_parameter('fence_enabled').value
|
||||
self.fence_type = self.get_parameter('fence_type').value
|
||||
self.warning_distance = self.get_parameter('warning_distance').value
|
||||
self.breach_action = self.get_parameter('breach_action').value
|
||||
self.min_altitude = self.get_parameter('min_altitude').value
|
||||
self.max_altitude = self.get_parameter('max_altitude').value
|
||||
self.breaches_required = self.get_parameter('consecutive_breaches_required').value
|
||||
|
||||
self.fence_points = [
|
||||
(47.397742, 8.545594),
|
||||
(47.398242, 8.545594),
|
||||
(47.398242, 8.546094),
|
||||
(47.397742, 8.546094),
|
||||
]
|
||||
self.fence_polygon = Polygon(self.fence_points)
|
||||
|
||||
self.current_position = None
|
||||
self.inside_fence = True
|
||||
self.breach_count = 0
|
||||
|
||||
self.gps_sub = self.create_subscription(
|
||||
NavSatFix, '/uav/mavros/global_position/global', self.gps_callback, 10)
|
||||
|
||||
self.fence_status_pub = self.create_publisher(Bool, '/geofence/status', 10)
|
||||
self.fence_action_pub = self.create_publisher(String, '/geofence/action', 10)
|
||||
self.fence_viz_pub = self.create_publisher(Marker, '/geofence/visualization', 10)
|
||||
self.warning_pub = self.create_publisher(String, '/geofence/warning', 10)
|
||||
|
||||
self.viz_timer = self.create_timer(1.0, self.publish_visualization)
|
||||
|
||||
self.get_logger().info('Geofence Monitor Started - GPS ONLY for safety boundaries')
|
||||
self.get_logger().info(f'Fence type: {self.fence_type}, Action: {self.breach_action}')
|
||||
|
||||
def gps_callback(self, msg):
|
||||
if not self.fence_enabled:
|
||||
return
|
||||
|
||||
self.current_position = (msg.latitude, msg.longitude, msg.altitude)
|
||||
|
||||
point = ShapelyPoint(msg.latitude, msg.longitude)
|
||||
inside_horizontal = self.fence_polygon.contains(point)
|
||||
inside_vertical = self.min_altitude <= msg.altitude <= self.max_altitude
|
||||
|
||||
self.inside_fence = inside_horizontal and inside_vertical
|
||||
|
||||
status_msg = Bool()
|
||||
status_msg.data = self.inside_fence
|
||||
self.fence_status_pub.publish(status_msg)
|
||||
|
||||
if not self.inside_fence:
|
||||
self.breach_count += 1
|
||||
|
||||
if not inside_horizontal:
|
||||
distance = self.fence_polygon.exterior.distance(point) * 111000
|
||||
self.get_logger().warn(f'GEOFENCE: Outside boundary by {distance:.1f}m')
|
||||
|
||||
if not inside_vertical:
|
||||
if msg.altitude < self.min_altitude:
|
||||
self.get_logger().warn(f'GEOFENCE: Below min altitude ({msg.altitude:.1f}m)')
|
||||
else:
|
||||
self.get_logger().warn(f'GEOFENCE: Above max altitude ({msg.altitude:.1f}m)')
|
||||
|
||||
if self.breach_count >= self.breaches_required:
|
||||
self.trigger_breach_action()
|
||||
else:
|
||||
self.breach_count = 0
|
||||
|
||||
if inside_horizontal:
|
||||
distance_to_edge = self.fence_polygon.exterior.distance(point) * 111000
|
||||
if distance_to_edge < self.warning_distance:
|
||||
warning_msg = String()
|
||||
warning_msg.data = f'Approaching boundary: {distance_to_edge:.1f}m remaining'
|
||||
self.warning_pub.publish(warning_msg)
|
||||
|
||||
def trigger_breach_action(self):
|
||||
action_msg = String()
|
||||
action_msg.data = self.breach_action
|
||||
self.fence_action_pub.publish(action_msg)
|
||||
|
||||
self.get_logger().error(f'GEOFENCE BREACH ACTION: {self.breach_action}')
|
||||
self.breach_count = 0
|
||||
|
||||
def publish_visualization(self):
|
||||
if not self.fence_enabled:
|
||||
return
|
||||
|
||||
marker = Marker()
|
||||
marker.header.stamp = self.get_clock().now().to_msg()
|
||||
marker.header.frame_id = 'map'
|
||||
marker.ns = 'geofence'
|
||||
marker.id = 0
|
||||
marker.type = Marker.LINE_STRIP
|
||||
marker.action = Marker.ADD
|
||||
|
||||
marker.scale.x = 2.0
|
||||
|
||||
if self.inside_fence:
|
||||
marker.color.r = 0.0
|
||||
marker.color.g = 1.0
|
||||
marker.color.b = 0.0
|
||||
else:
|
||||
marker.color.r = 1.0
|
||||
marker.color.g = 0.0
|
||||
marker.color.b = 0.0
|
||||
marker.color.a = 0.8
|
||||
|
||||
for lat, lon in self.fence_points:
|
||||
p = Point()
|
||||
p.x = (lon - self.fence_points[0][1]) * 111000
|
||||
p.y = (lat - self.fence_points[0][0]) * 111000
|
||||
p.z = 0.0
|
||||
marker.points.append(p)
|
||||
|
||||
p = Point()
|
||||
p.x = 0.0
|
||||
p.y = 0.0
|
||||
p.z = 0.0
|
||||
marker.points.append(p)
|
||||
|
||||
self.fence_viz_pub.publish(marker)
|
||||
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
node = GeofenceMonitor()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -0,0 +1 @@
|
||||
|
||||
|
||||
13
src/utils/config.py
Normal file
13
src/utils/config.py
Normal file
@@ -0,0 +1,13 @@
|
||||
#!/usr/bin/env python3
|
||||
|
||||
import yaml
|
||||
from pathlib import Path
|
||||
|
||||
def load_config(name: str, config_dir: Path) -> dict:
|
||||
path = config_dir / name
|
||||
if not path.exists():
|
||||
print(f"[WARN] Config not found: {path}")
|
||||
return {}
|
||||
with open(path, 'r') as f:
|
||||
return yaml.safe_load(f) or {}
|
||||
|
||||
@@ -1,22 +1,13 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
Unit conversions: Customary (US) to Metric.
|
||||
"""
|
||||
|
||||
def feet_to_meters(feet: float) -> float:
|
||||
return feet * 0.3048
|
||||
|
||||
def inches_to_meters(inches: float) -> float:
|
||||
return inches * 0.0254
|
||||
|
||||
def miles_to_meters(miles: float) -> float:
|
||||
return miles * 1609.34
|
||||
|
||||
def pounds_to_kg(pounds: float) -> float:
|
||||
return pounds * 0.453592
|
||||
|
||||
def ounces_to_kg(ounces: float) -> float:
|
||||
return ounces * 0.0283495
|
||||
|
||||
def mph_to_ms(mph: float) -> float:
|
||||
return mph * 0.44704
|
||||
|
||||
def ned_to_gz(ned_x, ned_y):
|
||||
return ned_y, ned_x
|
||||
|
||||
def gz_to_ned(gz_x, gz_y):
|
||||
return gz_y, gz_x
|
||||
|
||||
@@ -1,97 +1,6 @@
|
||||
#!/usr/bin/env python3
|
||||
|
||||
import numpy as np
|
||||
import yaml
|
||||
from pathlib import Path
|
||||
|
||||
|
||||
def load_yaml_config(filepath):
|
||||
with open(filepath, 'r') as f:
|
||||
return yaml.safe_load(f)
|
||||
|
||||
|
||||
def save_yaml_config(data, filepath):
|
||||
with open(filepath, 'w') as f:
|
||||
yaml.dump(data, f, default_flow_style=False)
|
||||
|
||||
|
||||
def clamp(value, min_val, max_val):
|
||||
return max(min_val, min(value, max_val))
|
||||
|
||||
|
||||
def lerp(a, b, t):
|
||||
return a + (b - a) * clamp(t, 0.0, 1.0)
|
||||
|
||||
|
||||
def smooth_step(edge0, edge1, x):
|
||||
t = clamp((x - edge0) / (edge1 - edge0), 0.0, 1.0)
|
||||
return t * t * (3.0 - 2.0 * t)
|
||||
|
||||
|
||||
def distance_2d(p1, p2):
|
||||
return np.sqrt((p2[0] - p1[0])**2 + (p2[1] - p1[1])**2)
|
||||
|
||||
|
||||
def distance_3d(p1, p2):
|
||||
return np.linalg.norm(np.array(p2) - np.array(p1))
|
||||
|
||||
|
||||
def moving_average(values, window_size):
|
||||
if len(values) < window_size:
|
||||
return np.mean(values)
|
||||
return np.mean(values[-window_size:])
|
||||
|
||||
|
||||
class RateLimiter:
|
||||
def __init__(self, max_rate):
|
||||
self.max_rate = max_rate
|
||||
self.last_value = 0.0
|
||||
|
||||
def apply(self, value, dt):
|
||||
max_change = self.max_rate * dt
|
||||
change = clamp(value - self.last_value, -max_change, max_change)
|
||||
self.last_value += change
|
||||
return self.last_value
|
||||
|
||||
|
||||
class LowPassFilter:
|
||||
def __init__(self, alpha=0.1):
|
||||
self.alpha = alpha
|
||||
self.value = None
|
||||
|
||||
def apply(self, new_value):
|
||||
if self.value is None:
|
||||
self.value = new_value
|
||||
else:
|
||||
self.value = self.alpha * new_value + (1 - self.alpha) * self.value
|
||||
return self.value
|
||||
|
||||
def reset(self):
|
||||
self.value = None
|
||||
|
||||
|
||||
class PIDController:
|
||||
def __init__(self, kp, ki, kd, output_limits=None):
|
||||
self.kp = kp
|
||||
self.ki = ki
|
||||
self.kd = kd
|
||||
self.output_limits = output_limits
|
||||
self.integral = 0.0
|
||||
self.last_error = 0.0
|
||||
self.last_time = None
|
||||
|
||||
def compute(self, error, current_time):
|
||||
dt = 0.0 if self.last_time is None else current_time - self.last_time
|
||||
self.integral += error * dt
|
||||
derivative = (error - self.last_error) / dt if dt > 0 else 0.0
|
||||
output = self.kp * error + self.ki * self.integral + self.kd * derivative
|
||||
if self.output_limits:
|
||||
output = clamp(output, self.output_limits[0], self.output_limits[1])
|
||||
self.last_error = error
|
||||
self.last_time = current_time
|
||||
return output
|
||||
|
||||
def reset(self):
|
||||
self.integral = 0.0
|
||||
self.last_error = 0.0
|
||||
self.last_time = None
|
||||
|
||||
@@ -1,8 +1,4 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Run recorder — captures simulation video and logs, and uploads to local REST API backend.
|
||||
|
||||
Each run gets a temporary folder, and all resources are uploaded directly to the backend.
|
||||
"""
|
||||
|
||||
import os
|
||||
import sys
|
||||
@@ -18,6 +14,7 @@ import requests
|
||||
from pathlib import Path
|
||||
from datetime import datetime
|
||||
import yaml
|
||||
import signal
|
||||
|
||||
try:
|
||||
import PIL.Image
|
||||
@@ -36,7 +33,6 @@ class RunRecorder:
|
||||
self.search_start = 0.0
|
||||
self.search_duration = 0.0
|
||||
|
||||
# Gather config data as JSON
|
||||
config_data = {}
|
||||
for cfg in ["search.yaml", "ugv.yaml", "uav.yaml"]:
|
||||
p = project_dir / "config" / cfg
|
||||
@@ -49,10 +45,10 @@ class RunRecorder:
|
||||
config_data[key] = data
|
||||
except Exception as e:
|
||||
print(f"[REC] Failed to load config {cfg}: {e}")
|
||||
|
||||
|
||||
if "ugv" in config_data and "topics" in config_data["ugv"]:
|
||||
del config_data["ugv"]["topics"]
|
||||
|
||||
|
||||
if "uav" in config_data and "connection" in config_data["uav"]:
|
||||
del config_data["uav"]["connection"]
|
||||
|
||||
@@ -63,7 +59,7 @@ class RunRecorder:
|
||||
if resp.status_code == 200:
|
||||
data = resp.json()
|
||||
self.sim_id = data.get("id", 0)
|
||||
self.sim_name = data.get("name", f"simulation_{self.sim_id}")
|
||||
self.sim_name = f"simulation_{self.sim_id}"
|
||||
except Exception as e:
|
||||
print(f"[REC] API create failed: {e}")
|
||||
|
||||
@@ -72,9 +68,9 @@ class RunRecorder:
|
||||
else:
|
||||
self.run_dir = project_dir / "results" / self.sim_name
|
||||
self.run_dir.mkdir(parents=True, exist_ok=True)
|
||||
|
||||
|
||||
self.run_num = self.sim_id
|
||||
|
||||
|
||||
print(f"[REC] Recording local results to {self.run_dir.name} and API ({self.sim_name}, ID: {self.sim_id})")
|
||||
self._log_path = self.run_dir / "log.txt"
|
||||
self._log_file = open(self._log_path, "w")
|
||||
@@ -86,7 +82,7 @@ class RunRecorder:
|
||||
self._tracker_writer = None
|
||||
self._camera_writer = None
|
||||
self._gazebo_writer = None
|
||||
|
||||
|
||||
self._tracker_size = None
|
||||
self._camera_size = None
|
||||
self._gazebo_size = None
|
||||
@@ -105,21 +101,18 @@ class RunRecorder:
|
||||
self._record_thread = None
|
||||
self._lock = threading.Lock()
|
||||
|
||||
# Gazebo window capture via xwd
|
||||
self._gazebo_wid = None
|
||||
self._find_gazebo_window()
|
||||
threading.Thread(target=self._upload_hardware_info, daemon=True).start()
|
||||
|
||||
def _upload_hardware_info(self):
|
||||
if not self.sim_id:
|
||||
return
|
||||
|
||||
|
||||
payload = {
|
||||
"cpu_info": "Unknown CPU",
|
||||
"gpu_info": "Unknown GPU",
|
||||
"ram_info": "Unknown RAM"
|
||||
}
|
||||
|
||||
|
||||
try:
|
||||
import subprocess
|
||||
cpu = "Unknown"
|
||||
@@ -127,19 +120,19 @@ class RunRecorder:
|
||||
cpu = subprocess.check_output("grep -m 1 'model name' /proc/cpuinfo | cut -d ':' -f 2", shell=True, timeout=2).decode('utf-8').strip()
|
||||
except Exception: pass
|
||||
if cpu: payload["cpu_info"] = cpu
|
||||
|
||||
|
||||
try:
|
||||
ram_kb = int(subprocess.check_output("awk '/MemTotal/ {print $2}' /proc/meminfo", shell=True, timeout=2))
|
||||
payload["ram_info"] = f"{round(ram_kb / 1024 / 1024, 1)} GB"
|
||||
except Exception: pass
|
||||
|
||||
|
||||
try:
|
||||
gpu = subprocess.check_output("lspci | grep -i vga | cut -d ':' -f 3", shell=True, timeout=2).decode('utf-8').strip()
|
||||
if gpu: payload["gpu_info"] = gpu
|
||||
except Exception: pass
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
|
||||
try:
|
||||
requests.put(f"{API_URL}/api/simulations/{self.sim_id}/hardware", json=payload, timeout=5)
|
||||
except Exception as e:
|
||||
@@ -151,8 +144,8 @@ class RunRecorder:
|
||||
try:
|
||||
with open(path, 'rb') as f:
|
||||
resp = requests.post(
|
||||
f"{API_URL}/api/simulations/{self.sim_id}/upload",
|
||||
files={"file": (filename, f)},
|
||||
f"{API_URL}/api/simulations/{self.sim_id}/upload",
|
||||
files={"file": (filename, f)},
|
||||
timeout=60
|
||||
)
|
||||
if resp.status_code == 200:
|
||||
@@ -160,23 +153,41 @@ class RunRecorder:
|
||||
except Exception as e:
|
||||
print(f"[REC] Upload failed for {filename}: {e}")
|
||||
|
||||
def _find_gazebo_window(self):
|
||||
def _find_gazebo_window_ffmpeg(self):
|
||||
try:
|
||||
cmd = "xwininfo -root -tree | grep -i 'gazebo\|gz sim'"
|
||||
output = subprocess.check_output(cmd, shell=True).decode('utf-8')
|
||||
lines = output.strip().split('\n')
|
||||
lines = [l for l in output.strip().split('\n') if "1x1" not in l and "Gazebo Sim" in l]
|
||||
if not lines:
|
||||
print("[REC] Gazebo window not found")
|
||||
return
|
||||
return None
|
||||
|
||||
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}")
|
||||
if wid_match:
|
||||
return wid_match.group(1)
|
||||
except Exception:
|
||||
pass
|
||||
return None
|
||||
|
||||
def _get_window_geometry(self, window_id):
|
||||
try:
|
||||
geo = subprocess.run(
|
||||
["xdotool", "getwindowgeometry", window_id],
|
||||
capture_output=True, text=True
|
||||
)
|
||||
x = y = width = height = None
|
||||
for line in geo.stdout.splitlines():
|
||||
if "Position:" in line:
|
||||
pos = line.split("Position:")[1].strip().split()[0]
|
||||
x, y = map(int, pos.split(","))
|
||||
elif "Geometry:" in line:
|
||||
size = line.split("Geometry:")[1].strip()
|
||||
width, height = map(int, size.split("x"))
|
||||
if None in (x, y, width, height):
|
||||
return None
|
||||
return x, y, width, height
|
||||
except FileNotFoundError:
|
||||
return None
|
||||
|
||||
def set_phase(self, phase: str):
|
||||
if phase == "takeoff":
|
||||
@@ -201,9 +212,57 @@ class RunRecorder:
|
||||
self._tracker_ref = tracker
|
||||
self._camera_ref = camera
|
||||
self._recording = True
|
||||
|
||||
if not self._gazebo_wid:
|
||||
self._find_gazebo_window()
|
||||
|
||||
window_id = self._find_gazebo_window_ffmpeg()
|
||||
geo = None
|
||||
if window_id:
|
||||
geo = self._get_window_geometry(window_id)
|
||||
|
||||
if geo:
|
||||
x, y, width, height = geo
|
||||
width += width % 2
|
||||
height += height % 2
|
||||
else:
|
||||
x, y, width, height = 0, 0, 1920, 1080
|
||||
|
||||
self.gazebo_output_file = str(self.run_dir / "gazebo.mp4")
|
||||
display = os.environ.get("DISPLAY", ":0")
|
||||
if not display.endswith(".0"):
|
||||
display += ".0"
|
||||
|
||||
cmd = [
|
||||
"ffmpeg", "-y",
|
||||
"-f", "x11grab"
|
||||
]
|
||||
|
||||
if window_id:
|
||||
cmd.extend([
|
||||
"-window_id", str(window_id),
|
||||
"-r", str(self.fps),
|
||||
"-i", display,
|
||||
])
|
||||
else:
|
||||
cmd.extend([
|
||||
"-r", str(self.fps),
|
||||
"-s", f"{width}x{height}",
|
||||
"-i", f"{display}+{x},{y}",
|
||||
])
|
||||
|
||||
cmd.extend([
|
||||
"-c:v", "libx264",
|
||||
"-preset", "ultrafast",
|
||||
"-crf", "18",
|
||||
"-pix_fmt", "yuv420p",
|
||||
"-vf", "pad=ceil(iw/2)*2:ceil(ih/2)*2",
|
||||
"-movflags", "+faststart",
|
||||
self.gazebo_output_file
|
||||
])
|
||||
try:
|
||||
self._gazebo_ffmpeg_proc = subprocess.Popen(cmd, stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL)
|
||||
print(f"[REC] Started recording Gazebo window via ffmpeg to gazebo.mp4 (ID: {window_id})")
|
||||
except Exception as e:
|
||||
print(f"[REC] Failed to start ffmpeg: {e}")
|
||||
self._gazebo_ffmpeg_proc = None
|
||||
|
||||
self._record_thread = threading.Thread(target=self._record_loop, daemon=True)
|
||||
self._record_thread.start()
|
||||
@@ -231,26 +290,6 @@ class RunRecorder:
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
if self._gazebo_wid and HAS_PIL:
|
||||
try:
|
||||
proc = subprocess.run(
|
||||
['xwd', '-id', self._gazebo_wid, '-out', '/dev/stdout'],
|
||||
stdout=subprocess.PIPE,
|
||||
stderr=subprocess.PIPE
|
||||
)
|
||||
if proc.returncode == 0:
|
||||
img_pil = PIL.Image.open(io.BytesIO(proc.stdout))
|
||||
img_np = np.array(img_pil)
|
||||
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)
|
||||
self._write_gazebo_frame(img_bgr)
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
elapsed = time.time() - t0
|
||||
sleep_time = max(0, interval - elapsed)
|
||||
time.sleep(sleep_time)
|
||||
@@ -275,23 +314,6 @@ class RunRecorder:
|
||||
self._camera_writer.write(frame)
|
||||
self._camera_frames += 1
|
||||
|
||||
def _write_gazebo_frame(self, frame):
|
||||
h, w = frame.shape[:2]
|
||||
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:
|
||||
frame = cv2.resize(frame, self._gazebo_size)
|
||||
|
||||
self._gazebo_writer.write(frame)
|
||||
self._gazebo_frames += 1
|
||||
|
||||
def snapshot_camera(self, label="snapshot"):
|
||||
if self._camera_ref is None:
|
||||
return
|
||||
@@ -322,6 +344,14 @@ class RunRecorder:
|
||||
if self._record_thread:
|
||||
self._record_thread.join(timeout=3.0)
|
||||
|
||||
if hasattr(self, '_gazebo_ffmpeg_proc') and self._gazebo_ffmpeg_proc:
|
||||
try:
|
||||
self._gazebo_ffmpeg_proc.send_signal(signal.SIGINT)
|
||||
self._gazebo_ffmpeg_proc.wait(timeout=5)
|
||||
except Exception:
|
||||
pass
|
||||
self._upload_file(self.gazebo_output_file, "gazebo.mp4")
|
||||
|
||||
if self._last_tracker_frame is not None:
|
||||
filename = "flight_path.png"
|
||||
path = self.run_dir / filename
|
||||
@@ -341,9 +371,6 @@ class RunRecorder:
|
||||
if self._camera_writer:
|
||||
self._camera_writer.release()
|
||||
self._upload_file(self.run_dir / "camera.avi", "camera.avi")
|
||||
if self._gazebo_writer:
|
||||
self._gazebo_writer.release()
|
||||
self._upload_file(self.run_dir / "gazebo.avi", "gazebo.avi")
|
||||
|
||||
self.stop_logging()
|
||||
self._log_file.close()
|
||||
@@ -356,8 +383,8 @@ class RunRecorder:
|
||||
if self.sim_id:
|
||||
try:
|
||||
requests.put(
|
||||
f"{API_URL}/api/simulations/{self.sim_id}/time",
|
||||
json={"search_time": self.search_duration, "total_time": duration},
|
||||
f"{API_URL}/api/simulations/{self.sim_id}/time",
|
||||
json={"search_time": self.search_duration, "total_time": duration},
|
||||
timeout=5
|
||||
)
|
||||
except Exception as e:
|
||||
@@ -369,7 +396,7 @@ class RunRecorder:
|
||||
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] Gazebo: via ffmpeg (.mp4)\n"
|
||||
f"[REC] ═══════════════════════════════════════\n"
|
||||
)
|
||||
|
||||
|
||||
@@ -1,84 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Coordinate transforms for local navigation."""
|
||||
|
||||
import numpy as np
|
||||
from scipy.spatial.transform import Rotation
|
||||
|
||||
|
||||
def quaternion_to_euler(quat):
|
||||
r = Rotation.from_quat([quat[0], quat[1], quat[2], quat[3]])
|
||||
return r.as_euler('xyz')
|
||||
|
||||
|
||||
def euler_to_quaternion(roll, pitch, yaw):
|
||||
r = Rotation.from_euler('xyz', [roll, pitch, yaw])
|
||||
return r.as_quat()
|
||||
|
||||
|
||||
def rotation_matrix_from_quaternion(quat):
|
||||
r = Rotation.from_quat([quat[0], quat[1], quat[2], quat[3]])
|
||||
return r.as_matrix()
|
||||
|
||||
|
||||
def transform_point(point, translation, rotation_quat):
|
||||
R = rotation_matrix_from_quaternion(rotation_quat)
|
||||
return R @ point + translation
|
||||
|
||||
|
||||
def inverse_transform_point(point, translation, rotation_quat):
|
||||
R = rotation_matrix_from_quaternion(rotation_quat)
|
||||
return R.T @ (point - translation)
|
||||
|
||||
|
||||
def body_to_world(body_vel, yaw):
|
||||
cos_yaw = np.cos(yaw)
|
||||
sin_yaw = np.sin(yaw)
|
||||
|
||||
world_vel = np.zeros(3)
|
||||
world_vel[0] = body_vel[0] * cos_yaw - body_vel[1] * sin_yaw
|
||||
world_vel[1] = body_vel[0] * sin_yaw + body_vel[1] * cos_yaw
|
||||
world_vel[2] = body_vel[2]
|
||||
|
||||
return world_vel
|
||||
|
||||
|
||||
def world_to_body(world_vel, yaw):
|
||||
cos_yaw = np.cos(yaw)
|
||||
sin_yaw = np.sin(yaw)
|
||||
|
||||
body_vel = np.zeros(3)
|
||||
body_vel[0] = world_vel[0] * cos_yaw + world_vel[1] * sin_yaw
|
||||
body_vel[1] = -world_vel[0] * sin_yaw + world_vel[1] * cos_yaw
|
||||
body_vel[2] = world_vel[2]
|
||||
|
||||
return body_vel
|
||||
|
||||
|
||||
def normalize_angle(angle):
|
||||
while angle > np.pi:
|
||||
angle -= 2 * np.pi
|
||||
while angle < -np.pi:
|
||||
angle += 2 * np.pi
|
||||
return angle
|
||||
|
||||
|
||||
def angle_difference(angle1, angle2):
|
||||
diff = angle1 - angle2
|
||||
return normalize_angle(diff)
|
||||
|
||||
|
||||
def create_transformation_matrix(translation, rotation_quat):
|
||||
T = np.eye(4)
|
||||
T[:3, :3] = rotation_matrix_from_quaternion(rotation_quat)
|
||||
T[:3, 3] = translation
|
||||
return T
|
||||
|
||||
|
||||
def invert_transformation_matrix(T):
|
||||
R = T[:3, :3]
|
||||
t = T[:3, 3]
|
||||
|
||||
T_inv = np.eye(4)
|
||||
T_inv[:3, :3] = R.T
|
||||
T_inv[:3, 3] = -R.T @ t
|
||||
return T_inv
|
||||
@@ -1 +1 @@
|
||||
"""Vision processing module for GPS-denied navigation."""
|
||||
|
||||
|
||||
@@ -12,7 +12,6 @@ PF_RGB_INT8 = 3
|
||||
PF_BGR_INT8 = 8
|
||||
PF_R_FLOAT32 = 13
|
||||
|
||||
|
||||
class CameraProcessor:
|
||||
def __init__(self, topics=None, show_gui=True):
|
||||
self.node = Node()
|
||||
@@ -90,7 +89,6 @@ class CameraProcessor:
|
||||
def get_frame(self, camera_name):
|
||||
return self.frames.get(camera_name)
|
||||
|
||||
|
||||
def main():
|
||||
cameras = "both"
|
||||
show_gui = True
|
||||
@@ -135,6 +133,5 @@ def main():
|
||||
else:
|
||||
proc.spin_headless()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
|
||||
@@ -30,7 +30,6 @@ ARUCO_DICT = {
|
||||
W_RES = 640
|
||||
H_RES = 480
|
||||
|
||||
|
||||
class ObjectDetector:
|
||||
CAM_DEG_FOV = 110
|
||||
|
||||
@@ -123,8 +122,7 @@ class ObjectDetector:
|
||||
|
||||
def annotate_frame(self, frame, detections):
|
||||
annotated = frame.copy()
|
||||
|
||||
# Draw center crosshair
|
||||
|
||||
h, w = frame.shape[:2]
|
||||
cx, cy = w // 2, h // 2
|
||||
cv2.line(annotated, (cx - 15, cy), (cx + 15, cy), (0, 255, 0), 2)
|
||||
@@ -147,9 +145,3 @@ class ObjectDetector:
|
||||
|
||||
cv2.addWeighted(overlay, 0.4, annotated, 0.6, 0, annotated)
|
||||
return annotated
|
||||
|
||||
def get_found_markers(self):
|
||||
return self.found_markers
|
||||
|
||||
def reset(self):
|
||||
self.found_markers = {}
|
||||
|
||||
@@ -1,85 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
Optical Flow - Velocity estimation from downward camera.
|
||||
Lucas-Kanade sparse optical flow for GPS-denied velocity estimation.
|
||||
"""
|
||||
|
||||
import time
|
||||
import cv2
|
||||
import numpy as np
|
||||
|
||||
|
||||
class OpticalFlowEstimator:
|
||||
def __init__(self, focal_length=500.0, min_altitude=0.3, max_altitude=10.0):
|
||||
self.focal_length = focal_length
|
||||
self.min_altitude = min_altitude
|
||||
self.max_altitude = max_altitude
|
||||
|
||||
self.prev_frame = None
|
||||
self.prev_points = None
|
||||
self.prev_time = None
|
||||
self.current_altitude = 1.0
|
||||
|
||||
self.velocity = np.zeros(2)
|
||||
|
||||
self.lk_params = dict(
|
||||
winSize=(15, 15),
|
||||
maxLevel=3,
|
||||
criteria=(cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 10, 0.03),
|
||||
)
|
||||
|
||||
self.feature_params = dict(
|
||||
maxCorners=100, qualityLevel=0.3, minDistance=7, blockSize=7
|
||||
)
|
||||
|
||||
self.on_velocity = None
|
||||
|
||||
print("[OF] Optical Flow Estimator initialized")
|
||||
|
||||
def set_altitude(self, altitude):
|
||||
if self.min_altitude < altitude < self.max_altitude:
|
||||
self.current_altitude = altitude
|
||||
|
||||
def process_frame(self, camera_name, frame):
|
||||
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
|
||||
current_time = time.time()
|
||||
|
||||
if self.prev_frame is not None and self.prev_points is not None:
|
||||
new_points, status, error = cv2.calcOpticalFlowPyrLK(
|
||||
self.prev_frame, gray, self.prev_points, None, **self.lk_params
|
||||
)
|
||||
|
||||
if new_points is not None:
|
||||
good_new = new_points[status.flatten() == 1]
|
||||
good_old = self.prev_points[status.flatten() == 1]
|
||||
|
||||
if len(good_new) > 10:
|
||||
flow = good_new - good_old
|
||||
avg_flow = np.mean(flow, axis=0)
|
||||
|
||||
if self.prev_time is not None:
|
||||
dt = current_time - self.prev_time
|
||||
if dt > 0:
|
||||
pixel_velocity = avg_flow / dt
|
||||
self.velocity[0] = (
|
||||
pixel_velocity[0]
|
||||
* self.current_altitude
|
||||
/ self.focal_length
|
||||
)
|
||||
self.velocity[1] = (
|
||||
pixel_velocity[1]
|
||||
* self.current_altitude
|
||||
/ self.focal_length
|
||||
)
|
||||
|
||||
if self.on_velocity:
|
||||
self.on_velocity(self.velocity)
|
||||
|
||||
self.prev_points = cv2.goodFeaturesToTrack(gray, **self.feature_params)
|
||||
self.prev_frame = gray
|
||||
self.prev_time = current_time
|
||||
|
||||
return self.velocity
|
||||
|
||||
def get_velocity(self):
|
||||
return self.velocity.copy()
|
||||
@@ -1,119 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
Visual Odometry - Camera-based position estimation without GPS.
|
||||
ORB/SIFT feature matching with essential matrix decomposition.
|
||||
"""
|
||||
|
||||
import time
|
||||
import cv2
|
||||
import numpy as np
|
||||
from scipy.spatial.transform import Rotation
|
||||
|
||||
|
||||
class VisualOdometry:
|
||||
def __init__(self, camera_matrix=None, detector_type="ORB", min_features=100):
|
||||
self.detector_type = detector_type
|
||||
self.min_features = min_features
|
||||
|
||||
if camera_matrix is not None:
|
||||
self.camera_matrix = np.array(camera_matrix).reshape(3, 3)
|
||||
else:
|
||||
self.camera_matrix = np.array(
|
||||
[[500.0, 0.0, 320.0], [0.0, 500.0, 240.0], [0.0, 0.0, 1.0]]
|
||||
)
|
||||
self.dist_coeffs = np.zeros(5)
|
||||
|
||||
if detector_type == "ORB":
|
||||
self.detector = cv2.ORB_create(nfeatures=500)
|
||||
self.matcher = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=False)
|
||||
elif detector_type == "SIFT":
|
||||
self.detector = cv2.SIFT_create(nfeatures=500)
|
||||
self.matcher = cv2.BFMatcher(cv2.NORM_L2, crossCheck=False)
|
||||
else:
|
||||
self.detector = cv2.ORB_create(nfeatures=500)
|
||||
self.matcher = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=False)
|
||||
|
||||
self.prev_frame = None
|
||||
self.prev_keypoints = None
|
||||
self.prev_descriptors = None
|
||||
self.prev_time = None
|
||||
|
||||
self.position = np.zeros(3)
|
||||
self.orientation = np.eye(3)
|
||||
self.velocity = np.zeros(3)
|
||||
|
||||
self.on_pose = None
|
||||
|
||||
print(f"[VO] Visual Odometry initialized ({detector_type})")
|
||||
|
||||
def process_frame(self, camera_name, frame):
|
||||
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
|
||||
keypoints, descriptors = self.detector.detectAndCompute(gray, None)
|
||||
current_time = time.time()
|
||||
|
||||
if (
|
||||
self.prev_frame is not None
|
||||
and self.prev_descriptors is not None
|
||||
and len(keypoints) >= self.min_features
|
||||
):
|
||||
matches = self._match_features(self.prev_descriptors, descriptors)
|
||||
if len(matches) >= self.min_features:
|
||||
self._estimate_motion(
|
||||
self.prev_keypoints, keypoints, matches, current_time
|
||||
)
|
||||
|
||||
self.prev_frame = gray
|
||||
self.prev_keypoints = keypoints
|
||||
self.prev_descriptors = descriptors
|
||||
self.prev_time = current_time
|
||||
|
||||
def _match_features(self, desc1, desc2):
|
||||
if desc1 is None or desc2 is None:
|
||||
return []
|
||||
|
||||
matches = self.matcher.knnMatch(desc1, desc2, k=2)
|
||||
good = []
|
||||
for pair in matches:
|
||||
if len(pair) == 2:
|
||||
m, n = pair
|
||||
if m.distance < 0.7 * n.distance:
|
||||
good.append(m)
|
||||
return good
|
||||
|
||||
def _estimate_motion(self, prev_kp, curr_kp, matches, current_time):
|
||||
if len(matches) < 5:
|
||||
return
|
||||
|
||||
pts1 = np.float32([prev_kp[m.queryIdx].pt for m in matches])
|
||||
pts2 = np.float32([curr_kp[m.trainIdx].pt for m in matches])
|
||||
|
||||
E, mask = cv2.findEssentialMat(
|
||||
pts1, pts2, self.camera_matrix, method=cv2.RANSAC, prob=0.999, threshold=1.0
|
||||
)
|
||||
|
||||
if E is None:
|
||||
return
|
||||
|
||||
_, R, t, mask = cv2.recoverPose(E, pts1, pts2, self.camera_matrix, mask=mask)
|
||||
|
||||
scale = 1.0
|
||||
translation = scale * t.flatten()
|
||||
|
||||
if self.prev_time is not None:
|
||||
dt = current_time - self.prev_time
|
||||
if dt > 0:
|
||||
self.velocity = translation / dt
|
||||
|
||||
self.position += self.orientation @ translation
|
||||
self.orientation = R @ self.orientation
|
||||
|
||||
if self.on_pose:
|
||||
r = Rotation.from_matrix(self.orientation)
|
||||
self.on_pose(self.position.copy(), r.as_quat())
|
||||
|
||||
def get_pose(self):
|
||||
r = Rotation.from_matrix(self.orientation)
|
||||
return self.position.copy(), r.as_quat()
|
||||
|
||||
def get_velocity(self):
|
||||
return self.velocity.copy()
|
||||
@@ -1,87 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
class VisualServoing:
|
||||
def __init__(self, controller, target_marker_id=0, land_altitude=0.5):
|
||||
self.controller = controller
|
||||
self.target_marker_id = target_marker_id
|
||||
self.land_altitude = land_altitude
|
||||
|
||||
self.kp_xy = 0.5
|
||||
self.kp_z = 0.3
|
||||
self.max_velocity = 0.5
|
||||
self.center_tolerance = 30
|
||||
self.land_distance = 1.0
|
||||
|
||||
self.enabled = False
|
||||
self.target_acquired = False
|
||||
self.centered = False
|
||||
self.image_center = (320, 240)
|
||||
self.last_detection = None
|
||||
|
||||
print(f"[VS] Visual Servoing initialized (target marker: {target_marker_id})")
|
||||
|
||||
def enable(self):
|
||||
self.enabled = True
|
||||
print("[VS] Enabled")
|
||||
|
||||
def disable(self):
|
||||
self.enabled = False
|
||||
print("[VS] Disabled")
|
||||
|
||||
def process_detections(self, detections):
|
||||
if not self.enabled:
|
||||
return False
|
||||
|
||||
target = None
|
||||
for d in detections:
|
||||
if d.get("id") == self.target_marker_id:
|
||||
target = d
|
||||
break
|
||||
|
||||
if target is None:
|
||||
self.target_acquired = False
|
||||
return False
|
||||
|
||||
self.target_acquired = True
|
||||
self.last_detection = target
|
||||
return self.compute_control(target)
|
||||
|
||||
def compute_control(self, detection):
|
||||
center_px = detection["center_px"]
|
||||
distance = detection["distance"]
|
||||
|
||||
error_x = self.image_center[0] - center_px[0]
|
||||
error_y = self.image_center[1] - center_px[1]
|
||||
|
||||
self.centered = (abs(error_x) < self.center_tolerance and
|
||||
abs(error_y) < self.center_tolerance)
|
||||
|
||||
if self.centered and distance < self.land_distance:
|
||||
print(f"\n[VS] Centered and close enough ({distance:.2f}m) - landing")
|
||||
self.controller.land()
|
||||
return True
|
||||
|
||||
vx = np.clip(self.kp_xy * error_y / 100.0, -self.max_velocity, self.max_velocity)
|
||||
vy = np.clip(self.kp_xy * error_x / 100.0, -self.max_velocity, self.max_velocity)
|
||||
|
||||
descend_rate = 0.0
|
||||
if self.centered:
|
||||
descend_rate = min(self.kp_z * distance, self.max_velocity)
|
||||
|
||||
self.controller.update_state()
|
||||
target_z = -(self.controller.altitude - descend_rate)
|
||||
self.controller.move_vel_rel_alt(vx, vy, target_z)
|
||||
|
||||
print(
|
||||
f"\r[VS] ID:{detection['id']} "
|
||||
f"d:{distance:.2f}m "
|
||||
f"err:({error_x:.0f},{error_y:.0f}) "
|
||||
f"vel:({vx:.2f},{vy:.2f}) "
|
||||
f"{'CENTERED' if self.centered else 'ALIGNING'}",
|
||||
end="",
|
||||
flush=True,
|
||||
)
|
||||
return False
|
||||
Reference in New Issue
Block a user