-
Notifications
You must be signed in to change notification settings - Fork 1
/
Copy pathposix_last_sitl.launch
115 lines (84 loc) · 5.4 KB
/
posix_last_sitl.launch
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
<launch>
<!-- Posix SITL environment launch script -->
<arg name="x" default="0"/>
<arg name="y" default="0"/>
<arg name="z" default="0"/>
<arg name="R" default="0"/>
<arg name="P" default="0"/>
<arg name="Y" default="0"/>
<arg name="x1" default="0.7"/>
<arg name="y1" default="0"/>
<arg name="z1" default="0"/>
<arg name="R1" default="0"/>
<arg name="P1" default="0"/>
<arg name="Y1" default="0"/>
<arg name="est" default="lpe"/>
<arg name="vehicle_1" default="iris"/>
<arg name="vehicle_2" default="iRobot"/>
<arg name="world" default="$(find mavlink_sitl_gazebo)/worlds/empty.world"/>
<arg name="iris_urdf" default="$(find mavlink_sitl_gazebo)/models/rotors_description/urdf/iris_camera.urdf"/>
<arg name="iRobot_urdf" default="$(find mavlink_sitl_gazebo)/models/rotors_description/urdf/iRobot_tag.urdf"/>
<arg name="rcS" default="$(find px4)/posix-configs/SITL/init/$(arg est)/$(arg vehicle_1)"/>
<arg name="headless" default="false"/>
<arg name="gui" default="true"/>
<arg name="ns" default="/"/>
<arg name="debug" default="false"/>
<arg name="verbose" default="false"/>
<arg name="paused" default="false"/>
<arg name="marker_size" default="4.4" />
<arg name="max_new_marker_error" default="0.08" />
<arg name="max_track_error" default="0.2" />
<arg name="cam_image_topic" default="/camera/depth/points" />
<arg name="cam_info_topic" default="/camera/rgb/camera_info" />
<arg name="output_frame" default="/base_link" />
<node name="sitl" pkg="px4" type="px4" output="screen"
args="$(find px4) $(arg rcS)">
</node>
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="headless" value="$(arg headless)"/>
<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>
<!-- <param name ="robot_description" textfile = "$(arg iris_urdf)"/> -->
<!-- <node name="$(anon vehicle_1_spawn)" output="screen" pkg="gazebo_ros" type="spawn_model"
args="-urdf -file $(arg iris_urdf) -model $(arg vehicle_1) -x $(arg x) -y $(arg y) -z $(arg z) -R $(arg R) -P $(arg P) -Y $(arg Y)"/>
<param name="tf_prefix" type="string" value="$(arg vehicle_1)"/>
<node name="robot_state_publisher_1" pkg="robot_state_publisher" type="robot_state_publisher" />
<node name="joint_state_publisher_1" pkg="joint_state_publisher" type="joint_state_publisher" /> -->
<!-- <param name ="robot_description_2" textfile = "$(arg iRobot_urdf)"/>
<node name="$(anon vehicle_2_spawn)" output="screen" pkg="gazebo_ros" type="spawn_model"
args="-urdf -file $(arg iRobot_urdf) -model $(arg vehicle_2) -x $(arg x1) -y $(arg y1) -z $(arg z1) -R $(arg R1) -P $(arg P1) -Y $(arg Y1)"/>
<param name="tf_prefix" type="string" value="$(arg model_name)" />
<param name="tf_prefix" type="string" value="$(arg vehicle_2)" />
<node name="robot_state_publisher_2" pkg="robot_state_publisher" type="robot_state_publisher" />
<node name="joint_state_publisher_2" pkg="joint_state_publisher" type="joint_state_publisher" /> -->
<!-- inserisco il nodo dell'ar_track_alvar -->
<node name="ar_track_alvar" pkg="ar_track_alvar" type="individualMarkers" respawn="false" output="screen">
<param name="marker_size" type="double" value="$(arg marker_size)" />
<param name="max_new_marker_error" type="double" value="$(arg max_new_marker_error)" />
<param name="max_track_error" type="double" value="$(arg max_track_error)" />
<param name="output_frame" type="string" value="$(arg output_frame)" />
<remap from="camera_image" to="$(arg cam_image_topic)" />
<remap from="camera_info" to="$(arg cam_info_topic)" />
</node>
<!-- <arg name="vehicle_1" /> -->
<!-- <arg name="init_pose"/> -->
<group ns="$(arg vehicle_1)">
<param name="tf_prefix" value="$(arg vehicle_1)" />
<node name="robot_state_publisher_1" pkg="robot_state_publisher" type="robot_state_publisher" />
<node name="joint_state_publisher_1" pkg="joint_state_publisher" type="joint_state_publisher" />
<!--<param name="robot_description" command="$(find xacro)/xacro.py '$(find px4)/Tools/sitl_gazebo/models/rotors_description/urdf/iris_camera.xacro'" /> -->
<param name ="robot_description" textfile = "$(arg iris_urdf)"/>
<node name="$(anon vehicle_1_spawn)" pkg="gazebo_ros" type="spawn_model" output="screen" args=" -urdf -file $(arg iris_urdf) -model $(arg vehicle_1) -x $(arg x) -y $(arg y) -z $(arg z) -R $(arg R) -P $(arg P) -Y $(arg Y)" respawn="false"/>
</group>
<group ns="$(arg vehicle_2)">
<param name="tf_prefix" value="$(arg vehicle_2)" />
<node pkg="tf" type="static_transform_publisher" name="link1_broadcaster" args="$(arg x1) $(arg y1) $(arg z1) $(arg R1) $(arg P1) $(arg Y1) ENU $(arg vehicle_2)/odom 30" />
<param name ="robot_description_2" textfile = "$(arg iRobot_urdf)"/>
<node name="$(anon vehicle_2_spawn)" pkg="gazebo_ros" type="spawn_model" output="screen" args="-urdf -file $(arg iRobot_urdf) -model $(arg vehicle_2) -x $(arg x1) -y $(arg y1) -z $(arg z1) -R $(arg R1) -P $(arg P1) -Y $(arg Y1)" respawn="false"/>
</group>
</launch>
<!-- vim: set et ft=xml fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : -->