140 lines
6.1 KiB
XML
140 lines
6.1 KiB
XML
<launch>
|
|
|
|
<!-- MAVROS posix SITL environment launch script -->
|
|
<!-- launches Gazebo environment and 2x: MAVROS, PX4 SITL, and spawns vehicle -->
|
|
<!-- vehicle model and world -->
|
|
<arg name="est" default="ekf2"/>
|
|
<arg name="world" default="$(find mavlink_sitl_gazebo)/worlds/outdoor2.world"/>
|
|
<!-- gazebo configs -->
|
|
<arg name="gui" default="true"/>
|
|
<arg name="debug" default="false"/>
|
|
<arg name="verbose" default="false"/>
|
|
<arg name="paused" default="false"/>
|
|
<!-- Gazebo sim -->
|
|
<include file="$(find gazebo_ros)/launch/empty_world.launch">
|
|
<arg name="gui" value="$(arg gui)"/>
|
|
<arg name="world_name" value="$(arg world)"/>
|
|
<arg name="debug" value="$(arg debug)"/>
|
|
<arg name="verbose" value="$(arg verbose)"/>
|
|
<arg name="paused" value="$(arg paused)"/>
|
|
</include>
|
|
|
|
<arg name="obstaclestopper" default="false"/>
|
|
<group ns="ugv_0">
|
|
<param name="robot_description"
|
|
command="$(find xacro)/xacro.py '$(find catvehicle)/urdf/catvehicle1-3.xacro' roboname:='ugv_0'" />
|
|
<include file="$(find catvehicle)/launch/catvehicle.launch">
|
|
<arg name="robot_name" value="ugv_0"/>
|
|
<arg name="init_pose" value="-x 0 -y 0 -z 0 -R 0 -P 0 -Y 0"/>
|
|
<arg name="config_file" value="catvehicle_control.yaml"/>
|
|
<arg name="obstaclestopper" value="$(arg obstaclestopper)"/>
|
|
</include>
|
|
|
|
<!-- Uncomment this to get immediate motion from the car
|
|
<node name="openLoopCircle" pkg="safeopenloopcircle" type="safeopenloopcircle_node"/>
|
|
-->
|
|
</group>
|
|
|
|
<!-- 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="-3"/>
|
|
<arg name="y" value="0"/>
|
|
<arg name="z" value="0"/>
|
|
<arg name="R" value="0"/>
|
|
<arg name="P" value="0"/>
|
|
<arg name="Y" value="0"/>
|
|
<arg name="vehicle" value="iris"/>
|
|
<arg name="sdf" value="iris_downward_camera"/>
|
|
<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"/>
|
|
<arg name="config_yaml" value="$(find mavros)/launch/px4_config_0.yaml"/>
|
|
</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="-4.5"/>
|
|
<arg name="y" value="0"/>
|
|
<arg name="z" value="0"/>
|
|
<arg name="R" value="0"/>
|
|
<arg name="P" value="0"/>
|
|
<arg name="Y" value="0"/>
|
|
<arg name="vehicle" value="iris"/>
|
|
<arg name="sdf" value="iris_downward_camera"/>
|
|
<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"/>
|
|
<arg name="config_yaml" value="$(find mavros)/launch/px4_config_1.yaml"/>
|
|
</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="-6"/>
|
|
<arg name="y" value="0"/>
|
|
<arg name="z" value="0"/>
|
|
<arg name="R" value="0"/>
|
|
<arg name="P" value="0"/>
|
|
<arg name="Y" value="0"/>
|
|
<arg name="vehicle" value="iris"/>
|
|
<arg name="sdf" value="iris_downward_camera"/>
|
|
<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"/>
|
|
<arg name="config_yaml" value="$(find mavros)/launch/px4_config_2.yaml"/>
|
|
</include>
|
|
</group>
|
|
|
|
<!-- <node pkg="tf" type="static_transform_publisher" name="0_base_link_to_base_link_frd"
|
|
args="0 0 -0.05 3.1415927 0 0 /base_link_0 /base_link_frd_0 100" />
|
|
<node pkg="tf" type="static_transform_publisher" name="1_base_link_to_base_link_frd"
|
|
args="0 0 -0.05 3.1415927 0 0 /base_link_1 /base_link_frd_1 100" />
|
|
<node pkg="tf" type="static_transform_publisher" name="2_base_link_to_base_link_frd"
|
|
args="0 0 -0.05 3.1415927 0 0 /base_link_2 /base_link_frd_2 100" /> -->
|
|
|
|
|
|
</launch>
|