forked from xtdrone/XTDrone
255 lines
11 KiB
XML
255 lines
11 KiB
XML
<?xml version="1.0"?>
|
|
<launch>
|
|
<env name="ROSCONSOLE_CONFIG_FILE" value="$(find vrx_gazebo)/config/custom_rosconsole.conf"/>
|
|
<!-- Gazebo world to load -->
|
|
<arg name="world" default="$(find vrx_gazebo)/worlds/example_course.world" />
|
|
<!-- If true, run gazebo GUI -->
|
|
<arg name="gui" default="true" />
|
|
<!-- If true, run gazebo in verbose mode -->
|
|
<arg name="verbose" default="false"/>
|
|
<!-- If true, start in paused state -->
|
|
<arg name="paused" default="false"/>
|
|
<!-- Set various other gazebo arguments-->
|
|
<arg name="extra_gazebo_args" default=""/>
|
|
<!-- Start in a default namespace -->
|
|
<arg name="namespace" default="wamv"/>
|
|
|
|
<!-- Initial USV location and attitude-->
|
|
<arg name="x" default="158" />
|
|
<arg name="y" default="108" />
|
|
<arg name="z" default="0.1" />
|
|
<arg name="P" default="0" />
|
|
<arg name="R" default="0" />
|
|
<arg name="Y" default="-2.76" />
|
|
|
|
<!-- If true, show non-competition ROS topics (/gazebo/model_states, /vrx/debug/wind/direction, etc.)-->
|
|
<arg name="non_competition_mode" default="true"/>
|
|
<arg name="enable_ros_network" value="$(arg non_competition_mode)"/>
|
|
<env name="VRX_DEBUG" value="$(arg non_competition_mode)"/>
|
|
<env unless="$(arg non_competition_mode)" name="GAZEBO_MODEL_PATH" value="$(find vrx_gazebo)/models:$(find wamv_gazebo)/models:$(find wamv_description)/models:$(optenv GAZEBO_MODEL_PATH)"/>
|
|
|
|
<!-- Allow user specified thruster configurations
|
|
H = stern trusters on each hull
|
|
T = H with a lateral thruster
|
|
X = "holonomic" configuration -->
|
|
<arg name="thrust_config" default="H" />
|
|
|
|
<!-- Do you want to enable sensors? -->
|
|
<arg name="camera_enabled" default="false" />
|
|
<arg name="gps_enabled" default="false" />
|
|
<arg name="imu_enabled" default="false" />
|
|
<arg name="lidar_enabled" default="false" />
|
|
<arg name="ground_truth_enabled" default="false" />
|
|
|
|
<!-- Start Gazebo with the world file -->
|
|
<include file="$(find gazebo_ros)/launch/empty_world.launch">
|
|
<arg name="world_name" value="$(arg world)"/>
|
|
<arg name="verbose" value="$(arg verbose)"/>
|
|
<arg name="paused" value="$(arg paused)"/>
|
|
<arg name="use_sim_time" value="true"/>
|
|
<arg name="gui" value="$(arg gui)" />
|
|
<arg name="enable_ros_network" value="$(arg enable_ros_network)"/>
|
|
<arg name="extra_gazebo_args" value="$(arg extra_gazebo_args)"/>
|
|
</include>
|
|
|
|
<!-- Load robot model -->
|
|
<arg name="urdf" default="$(find wamv_gazebo)/urdf/wamv_gazebo.urdf.xacro"/>
|
|
<param name="$(arg namespace)/robot_description"
|
|
command="$(find xacro)/xacro --inorder '$(arg urdf)'
|
|
thruster_config:=$(arg thrust_config)
|
|
camera_enabled:=$(arg camera_enabled)
|
|
gps_enabled:=$(arg gps_enabled)
|
|
imu_enabled:=$(arg imu_enabled)
|
|
lidar_enabled:=$(arg lidar_enabled)
|
|
ground_truth_enabled:=$(arg ground_truth_enabled)
|
|
namespace:=$(arg namespace) "/>
|
|
|
|
<!-- Spawn model in Gazebo, script depending on non_competition_mode -->
|
|
<node name="spawn_model" pkg="gazebo_ros" type="spawn_model" if="$(arg non_competition_mode)"
|
|
args="-x $(arg x) -y $(arg y) -z $(arg z)
|
|
-R $(arg R) -P $(arg P) -Y $(arg Y)
|
|
-urdf -param $(arg namespace)/robot_description -model wamv"/>
|
|
|
|
<node name="spawn_wamv" pkg="vrx_gazebo" type="spawn_wamv.bash" unless="$(arg non_competition_mode)"
|
|
args="-x $(arg x) -y $(arg y) -z $(arg z)
|
|
-R $(arg R) -P $(arg P) -Y $(arg Y)
|
|
--urdf $(arg urdf) --model wamv"/>
|
|
|
|
<arg name="est" default="ekf2"/>
|
|
<!-- iris_0 -->
|
|
<group ns="iris_0">
|
|
<!-- MAVROS and vehicle configs -->
|
|
<arg name="ID" value="0"/>
|
|
<arg name="ID_in_group" value="0"/>
|
|
<arg name="fcu_url" default="udp://:24540@localhost:34580"/>
|
|
<!-- PX4 SITL and vehicle spawn -->
|
|
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
|
|
<arg name="x" value="170"/>
|
|
<arg name="y" value="111"/>
|
|
<arg name="z" value="2"/>
|
|
<arg name="R" value="0"/>
|
|
<arg name="P" value="0"/>
|
|
<arg name="Y" value="3.14"/>
|
|
<arg name="vehicle" value="iris"/>
|
|
<arg name="sdf" value="iris"/>
|
|
<arg name="mavlink_udp_port" value="18570"/>
|
|
<arg name="mavlink_tcp_port" value="4560"/>
|
|
<arg name="ID" value="$(arg ID)"/>
|
|
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
|
|
</include>
|
|
<!-- MAVROS -->
|
|
<include file="$(find mavros)/launch/px4.launch">
|
|
<arg name="fcu_url" value="$(arg fcu_url)"/>
|
|
<arg name="gcs_url" value=""/>
|
|
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
|
|
<arg name="tgt_component" value="1"/>
|
|
</include>
|
|
</group>
|
|
<!-- iris_1 -->
|
|
<group ns="iris_1">
|
|
<!-- MAVROS and vehicle configs -->
|
|
<arg name="ID" value="1"/>
|
|
<arg name="ID_in_group" value="1"/>
|
|
<arg name="fcu_url" default="udp://:24541@localhost:34581"/>
|
|
<!-- PX4 SITL and vehicle spawn -->
|
|
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
|
|
<arg name="x" value="173"/>
|
|
<arg name="y" value="111"/>
|
|
<arg name="z" value="2"/>
|
|
<arg name="R" value="0"/>
|
|
<arg name="P" value="0"/>
|
|
<arg name="Y" value="3.14"/>
|
|
<arg name="vehicle" value="iris"/>
|
|
<arg name="sdf" value="iris"/>
|
|
<arg name="mavlink_udp_port" value="18571"/>
|
|
<arg name="mavlink_tcp_port" value="4561"/>
|
|
<arg name="ID" value="$(arg ID)"/>
|
|
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
|
|
</include>
|
|
<!-- MAVROS -->
|
|
<include file="$(find mavros)/launch/px4.launch">
|
|
<arg name="fcu_url" value="$(arg fcu_url)"/>
|
|
<arg name="gcs_url" value=""/>
|
|
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
|
|
<arg name="tgt_component" value="1"/>
|
|
</include>
|
|
</group>
|
|
<!-- iris_2 -->
|
|
<group ns="iris_2">
|
|
<!-- MAVROS and vehicle configs -->
|
|
<arg name="ID" value="2"/>
|
|
<arg name="ID_in_group" value="2"/>
|
|
<arg name="fcu_url" default="udp://:24542@localhost:34582"/>
|
|
<!-- PX4 SITL and vehicle spawn -->
|
|
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
|
|
<arg name="x" value="170"/>
|
|
<arg name="y" value="114"/>
|
|
<arg name="z" value="2"/>
|
|
<arg name="R" value="0"/>
|
|
<arg name="P" value="0"/>
|
|
<arg name="Y" value="3.14"/>
|
|
<arg name="vehicle" value="iris"/>
|
|
<arg name="sdf" value="iris"/>
|
|
<arg name="mavlink_udp_port" value="18572"/>
|
|
<arg name="mavlink_tcp_port" value="4562"/>
|
|
<arg name="ID" value="$(arg ID)"/>
|
|
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
|
|
</include>
|
|
<!-- MAVROS -->
|
|
<include file="$(find mavros)/launch/px4.launch">
|
|
<arg name="fcu_url" value="$(arg fcu_url)"/>
|
|
<arg name="gcs_url" value=""/>
|
|
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
|
|
<arg name="tgt_component" value="1"/>
|
|
</include>
|
|
</group>
|
|
<!-- iris_3 -->
|
|
<group ns="iris_3">
|
|
<!-- MAVROS and vehicle configs -->
|
|
<arg name="ID" value="3"/>
|
|
<arg name="ID_in_group" value="3"/>
|
|
<arg name="fcu_url" default="udp://:24543@localhost:34583"/>
|
|
<!-- PX4 SITL and vehicle spawn -->
|
|
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
|
|
<arg name="x" value="173"/>
|
|
<arg name="y" value="114"/>
|
|
<arg name="z" value="2"/>
|
|
<arg name="R" value="0"/>
|
|
<arg name="P" value="0"/>
|
|
<arg name="Y" value="3.14"/>
|
|
<arg name="vehicle" value="iris"/>
|
|
<arg name="sdf" value="iris"/>
|
|
<arg name="mavlink_udp_port" value="18573"/>
|
|
<arg name="mavlink_tcp_port" value="4563"/>
|
|
<arg name="ID" value="$(arg ID)"/>
|
|
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
|
|
</include>
|
|
<!-- MAVROS -->
|
|
<include file="$(find mavros)/launch/px4.launch">
|
|
<arg name="fcu_url" value="$(arg fcu_url)"/>
|
|
<arg name="gcs_url" value=""/>
|
|
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
|
|
<arg name="tgt_component" value="1"/>
|
|
</include>
|
|
</group>
|
|
<!-- iris_4 -->
|
|
<group ns="iris_4">
|
|
<!-- MAVROS and vehicle configs -->
|
|
<arg name="ID" value="4"/>
|
|
<arg name="ID_in_group" value="4"/>
|
|
<arg name="fcu_url" default="udp://:24544@localhost:34584"/>
|
|
<!-- PX4 SITL and vehicle spawn -->
|
|
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
|
|
<arg name="x" value="170"/>
|
|
<arg name="y" value="117"/>
|
|
<arg name="z" value="2"/>
|
|
<arg name="R" value="0"/>
|
|
<arg name="P" value="0"/>
|
|
<arg name="Y" value="3.14"/>
|
|
<arg name="vehicle" value="iris"/>
|
|
<arg name="sdf" value="iris"/>
|
|
<arg name="mavlink_udp_port" value="18574"/>
|
|
<arg name="mavlink_tcp_port" value="4564"/>
|
|
<arg name="ID" value="$(arg ID)"/>
|
|
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
|
|
</include>
|
|
<!-- MAVROS -->
|
|
<include file="$(find mavros)/launch/px4.launch">
|
|
<arg name="fcu_url" value="$(arg fcu_url)"/>
|
|
<arg name="gcs_url" value=""/>
|
|
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
|
|
<arg name="tgt_component" value="1"/>
|
|
</include>
|
|
</group>
|
|
<!-- iris_5 -->
|
|
<group ns="iris_5">
|
|
<!-- MAVROS and vehicle configs -->
|
|
<arg name="ID" value="5"/>
|
|
<arg name="ID_in_group" value="5"/>
|
|
<arg name="fcu_url" default="udp://:24545@localhost:34585"/>
|
|
<!-- PX4 SITL and vehicle spawn -->
|
|
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
|
|
<arg name="x" value="173"/>
|
|
<arg name="y" value="117"/>
|
|
<arg name="z" value="2"/>
|
|
<arg name="R" value="0"/>
|
|
<arg name="P" value="0"/>
|
|
<arg name="Y" value="3.14"/>
|
|
<arg name="vehicle" value="iris"/>
|
|
<arg name="sdf" value="iris"/>
|
|
<arg name="mavlink_udp_port" value="18575"/>
|
|
<arg name="mavlink_tcp_port" value="4565"/>
|
|
<arg name="ID" value="$(arg ID)"/>
|
|
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
|
|
</include>
|
|
<!-- MAVROS -->
|
|
<include file="$(find mavros)/launch/px4.launch">
|
|
<arg name="fcu_url" value="$(arg fcu_url)"/>
|
|
<arg name="gcs_url" value=""/>
|
|
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
|
|
<arg name="tgt_component" value="1"/>
|
|
</include>
|
|
</group>
|
|
</launch>
|
|
|