321 lines
11 KiB
XML
321 lines
11 KiB
XML
<?xml version="1.0" ?>
|
|
<!--
|
|
Custom UAV-UGV Search & Land World
|
|
===================================
|
|
- ArduPilot iris quadcopter (with SITL plugin + gimbal camera)
|
|
- UGV target vehicle at (10, 5) for the drone to search for and land on
|
|
- Visual navigation markers (colored squares on the ground)
|
|
- Optimized for WSL: lower physics rate, no shadows
|
|
-->
|
|
<sdf version="1.9">
|
|
<world name="uav_ugv_search">
|
|
|
|
<!-- Physics: 4ms step (250 Hz) for better WSL performance -->
|
|
<physics name="2ms" type="ignore">
|
|
<max_step_size>0.002</max_step_size>
|
|
<real_time_factor>1.0</real_time_factor>
|
|
</physics>
|
|
|
|
<!-- Required Gazebo Harmonic system plugins -->
|
|
<plugin filename="gz-sim-physics-system"
|
|
name="gz::sim::systems::Physics">
|
|
</plugin>
|
|
<plugin filename="gz-sim-sensors-system"
|
|
name="gz::sim::systems::Sensors">
|
|
<render_engine>ogre2</render_engine>
|
|
</plugin>
|
|
<plugin filename="gz-sim-user-commands-system"
|
|
name="gz::sim::systems::UserCommands">
|
|
</plugin>
|
|
<plugin filename="gz-sim-scene-broadcaster-system"
|
|
name="gz::sim::systems::SceneBroadcaster">
|
|
</plugin>
|
|
<plugin filename="gz-sim-imu-system"
|
|
name="gz::sim::systems::Imu">
|
|
</plugin>
|
|
<plugin filename="gz-sim-navsat-system"
|
|
name="gz::sim::systems::NavSat">
|
|
</plugin>
|
|
|
|
<!-- Scene: no shadows for WSL performance -->
|
|
<scene>
|
|
<ambient>1.0 1.0 1.0</ambient>
|
|
<background>0.6 0.75 0.9</background>
|
|
<sky></sky>
|
|
<shadows>false</shadows>
|
|
</scene>
|
|
|
|
<!-- Spherical coordinates (matches ArduPilot SITL default location) -->
|
|
<spherical_coordinates>
|
|
<latitude_deg>-35.363262</latitude_deg>
|
|
<longitude_deg>149.165237</longitude_deg>
|
|
<elevation>584</elevation>
|
|
<heading_deg>0</heading_deg>
|
|
<surface_model>EARTH_WGS84</surface_model>
|
|
</spherical_coordinates>
|
|
|
|
<!-- Sun (no shadows for performance) -->
|
|
<light type="directional" name="sun">
|
|
<cast_shadows>false</cast_shadows>
|
|
<pose>0 0 10 0 0 0</pose>
|
|
<diffuse>0.9 0.9 0.9 1</diffuse>
|
|
<specular>0.5 0.5 0.5 1</specular>
|
|
<attenuation>
|
|
<range>1000</range>
|
|
<constant>0.9</constant>
|
|
<linear>0.01</linear>
|
|
<quadratic>0.001</quadratic>
|
|
</attenuation>
|
|
<direction>-0.5 0.1 -0.9</direction>
|
|
</light>
|
|
|
|
<!-- ===================== GROUND PLANE ===================== -->
|
|
<model name="ground_plane">
|
|
<static>true</static>
|
|
<link name="link">
|
|
<collision name="collision">
|
|
<geometry>
|
|
<plane>
|
|
<normal>0 0 1</normal>
|
|
<size>100 100</size>
|
|
</plane>
|
|
</geometry>
|
|
</collision>
|
|
<visual name="visual">
|
|
<geometry>
|
|
<plane>
|
|
<normal>0 0 1</normal>
|
|
<size>100 100</size>
|
|
</plane>
|
|
</geometry>
|
|
<material>
|
|
<ambient>0.5 0.55 0.45 1</ambient>
|
|
<diffuse>0.5 0.55 0.45 1</diffuse>
|
|
</material>
|
|
</visual>
|
|
</link>
|
|
</model>
|
|
|
|
<!-- ===================== COORDINATE AXES ===================== -->
|
|
<model name="axes">
|
|
<static>1</static>
|
|
<link name="link">
|
|
<!-- X axis (red) = North -->
|
|
<visual name="r">
|
|
<cast_shadows>0</cast_shadows>
|
|
<pose>5 0 0.05 0 0 0</pose>
|
|
<geometry><box><size>10 0.02 0.02</size></box></geometry>
|
|
<material>
|
|
<ambient>1 0 0 0.8</ambient>
|
|
<diffuse>1 0 0 0.8</diffuse>
|
|
<emissive>1 0 0 0.5</emissive>
|
|
</material>
|
|
</visual>
|
|
<!-- Y axis (green) = East -->
|
|
<visual name="g">
|
|
<cast_shadows>0</cast_shadows>
|
|
<pose>0 5 0.05 0 0 0</pose>
|
|
<geometry><box><size>0.02 10 0.02</size></box></geometry>
|
|
<material>
|
|
<ambient>0 1 0 0.8</ambient>
|
|
<diffuse>0 1 0 0.8</diffuse>
|
|
<emissive>0 1 0 0.5</emissive>
|
|
</material>
|
|
</visual>
|
|
<!-- Z axis (blue) = Up -->
|
|
<visual name="b">
|
|
<cast_shadows>0</cast_shadows>
|
|
<pose>0 0 5.05 0 0 0</pose>
|
|
<geometry><box><size>0.02 0.02 10</size></box></geometry>
|
|
<material>
|
|
<ambient>0 0 1 0.8</ambient>
|
|
<diffuse>0 0 1 0.8</diffuse>
|
|
<emissive>0 0 1 0.5</emissive>
|
|
</material>
|
|
</visual>
|
|
<!-- NavSat sensor (needed for GPS/geofence) -->
|
|
<sensor name="navsat_sensor" type="navsat">
|
|
<always_on>1</always_on>
|
|
<update_rate>1</update_rate>
|
|
</sensor>
|
|
</link>
|
|
</model>
|
|
|
|
<!-- ===================== UAV: ArduPilot Iris ===================== -->
|
|
<!-- Uses the ArduPilot iris_with_gimbal model which includes:
|
|
- ArduPilotPlugin (connects to SITL)
|
|
- IMU sensor
|
|
- Gimbal with camera
|
|
- Rotor dynamics / LiftDrag
|
|
-->
|
|
<include>
|
|
<uri>model://iris_with_gimbal</uri>
|
|
<pose degrees="true">0 0 0.195 0 0 90</pose>
|
|
</include>
|
|
|
|
<!-- ===================== UGV: Target Vehicle ===================== -->
|
|
<!-- The drone should search for this vehicle and land on it -->
|
|
<model name="ugv_target">
|
|
<static>true</static>
|
|
<pose>10 5 0 0 0 0</pose>
|
|
|
|
<!-- Main body (blue box) -->
|
|
<link name="base_link">
|
|
<pose>0 0 0.15 0 0 0</pose>
|
|
<visual name="body">
|
|
<geometry><box><size>0.6 0.4 0.2</size></box></geometry>
|
|
<material>
|
|
<ambient>0.2 0.2 0.7 1</ambient>
|
|
<diffuse>0.2 0.2 0.8 1</diffuse>
|
|
</material>
|
|
</visual>
|
|
<collision name="body_collision">
|
|
<geometry><box><size>0.6 0.4 0.2</size></box></geometry>
|
|
</collision>
|
|
</link>
|
|
|
|
<!-- Landing pad on top (bright orange, easy to spot from air) -->
|
|
<link name="landing_pad">
|
|
<pose>0 0 0.26 0 0 0</pose>
|
|
<visual name="pad">
|
|
<geometry><box><size>0.5 0.5 0.01</size></box></geometry>
|
|
<material>
|
|
<ambient>1.0 0.5 0.0 1</ambient>
|
|
<diffuse>1.0 0.5 0.0 1</diffuse>
|
|
<emissive>0.5 0.25 0.0 1</emissive>
|
|
</material>
|
|
</visual>
|
|
</link>
|
|
|
|
<!-- H marker on landing pad (white) -->
|
|
<link name="h_marker_bar1">
|
|
<pose>0 0 0.27 0 0 0</pose>
|
|
<visual name="hbar">
|
|
<geometry><box><size>0.3 0.04 0.005</size></box></geometry>
|
|
<material>
|
|
<ambient>1 1 1 1</ambient>
|
|
<diffuse>1 1 1 1</diffuse>
|
|
<emissive>1 1 1 0.8</emissive>
|
|
</material>
|
|
</visual>
|
|
</link>
|
|
<link name="h_marker_left">
|
|
<pose>-0.12 0 0.27 0 0 0</pose>
|
|
<visual name="hleft">
|
|
<geometry><box><size>0.04 0.25 0.005</size></box></geometry>
|
|
<material>
|
|
<ambient>1 1 1 1</ambient>
|
|
<diffuse>1 1 1 1</diffuse>
|
|
<emissive>1 1 1 0.8</emissive>
|
|
</material>
|
|
</visual>
|
|
</link>
|
|
<link name="h_marker_right">
|
|
<pose>0.12 0 0.27 0 0 0</pose>
|
|
<visual name="hright">
|
|
<geometry><box><size>0.04 0.25 0.005</size></box></geometry>
|
|
<material>
|
|
<ambient>1 1 1 1</ambient>
|
|
<diffuse>1 1 1 1</diffuse>
|
|
<emissive>1 1 1 0.8</emissive>
|
|
</material>
|
|
</visual>
|
|
</link>
|
|
|
|
<!-- Wheels (visual only since static) -->
|
|
<link name="wheel_fl">
|
|
<pose>0.2 0.22 0.06 1.5708 0 0</pose>
|
|
<visual name="v"><geometry><cylinder><radius>0.06</radius><length>0.04</length></cylinder></geometry>
|
|
<material><ambient>0.15 0.15 0.15 1</ambient><diffuse>0.15 0.15 0.15 1</diffuse></material>
|
|
</visual>
|
|
</link>
|
|
<link name="wheel_fr">
|
|
<pose>0.2 -0.22 0.06 1.5708 0 0</pose>
|
|
<visual name="v"><geometry><cylinder><radius>0.06</radius><length>0.04</length></cylinder></geometry>
|
|
<material><ambient>0.15 0.15 0.15 1</ambient><diffuse>0.15 0.15 0.15 1</diffuse></material>
|
|
</visual>
|
|
</link>
|
|
<link name="wheel_rl">
|
|
<pose>-0.2 0.22 0.06 1.5708 0 0</pose>
|
|
<visual name="v"><geometry><cylinder><radius>0.06</radius><length>0.04</length></cylinder></geometry>
|
|
<material><ambient>0.15 0.15 0.15 1</ambient><diffuse>0.15 0.15 0.15 1</diffuse></material>
|
|
</visual>
|
|
</link>
|
|
<link name="wheel_rr">
|
|
<pose>-0.2 -0.22 0.06 1.5708 0 0</pose>
|
|
<visual name="v"><geometry><cylinder><radius>0.06</radius><length>0.04</length></cylinder></geometry>
|
|
<material><ambient>0.15 0.15 0.15 1</ambient><diffuse>0.15 0.15 0.15 1</diffuse></material>
|
|
</visual>
|
|
</link>
|
|
</model>
|
|
|
|
<!-- ===================== VISUAL MARKERS ===================== -->
|
|
<!-- Ground markers for visual navigation reference -->
|
|
|
|
<!-- Origin marker (yellow circle) -->
|
|
<model name="origin_marker">
|
|
<static>true</static>
|
|
<pose>0 0 0.005 0 0 0</pose>
|
|
<link name="link">
|
|
<visual name="v">
|
|
<geometry><cylinder><radius>0.4</radius><length>0.01</length></cylinder></geometry>
|
|
<material>
|
|
<ambient>1 1 0 1</ambient>
|
|
<diffuse>1 1 0 1</diffuse>
|
|
<emissive>0.8 0.8 0 0.6</emissive>
|
|
</material>
|
|
</visual>
|
|
</link>
|
|
</model>
|
|
|
|
<!-- Waypoint marker 1 (red) at 5,0 -->
|
|
<model name="marker_red">
|
|
<static>true</static>
|
|
<pose>5 0 0.005 0 0 0</pose>
|
|
<link name="link">
|
|
<visual name="v">
|
|
<geometry><box><size>0.5 0.5 0.01</size></box></geometry>
|
|
<material>
|
|
<ambient>1 0.1 0.1 1</ambient>
|
|
<diffuse>1 0.1 0.1 1</diffuse>
|
|
<emissive>0.8 0 0 0.5</emissive>
|
|
</material>
|
|
</visual>
|
|
</link>
|
|
</model>
|
|
|
|
<!-- Waypoint marker 2 (green) at 10,0 -->
|
|
<model name="marker_green">
|
|
<static>true</static>
|
|
<pose>10 0 0.005 0 0 0</pose>
|
|
<link name="link">
|
|
<visual name="v">
|
|
<geometry><box><size>0.5 0.5 0.01</size></box></geometry>
|
|
<material>
|
|
<ambient>0.1 1 0.1 1</ambient>
|
|
<diffuse>0.1 1 0.1 1</diffuse>
|
|
<emissive>0 0.8 0 0.5</emissive>
|
|
</material>
|
|
</visual>
|
|
</link>
|
|
</model>
|
|
|
|
<!-- Waypoint marker 3 (blue) at 10,10 -->
|
|
<model name="marker_blue">
|
|
<static>true</static>
|
|
<pose>10 10 0.005 0 0 0</pose>
|
|
<link name="link">
|
|
<visual name="v">
|
|
<geometry><box><size>0.5 0.5 0.01</size></box></geometry>
|
|
<material>
|
|
<ambient>0.1 0.1 1 1</ambient>
|
|
<diffuse>0.1 0.1 1 1</diffuse>
|
|
<emissive>0 0 0.8 0.5</emissive>
|
|
</material>
|
|
</visual>
|
|
</link>
|
|
</model>
|
|
|
|
</world>
|
|
</sdf>
|