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