-
Notifications
You must be signed in to change notification settings - Fork 195
/
wamv_gazebo.urdf.xacro
180 lines (150 loc) · 6.98 KB
/
wamv_gazebo.urdf.xacro
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
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
<?xml version="1.0"?>
<!-- Basic WAM-V with gazebo plugins for dynamics -->
<robot xmlns:xacro="http://ros.org/wiki/xacro"
name="WAM-V">
<xacro:arg name="locked" default="false" />
<xacro:arg name="thruster_config" default="H" />
<xacro:arg name="camera_enabled" default="false" />
<xacro:arg name="gps_enabled" default="false" />
<xacro:arg name="imu_enabled" default="false" />
<xacro:arg name="lidar_enabled" default="false" />
<xacro:arg name="ground_truth_enabled" default="false" />
<xacro:arg name="vrx_sensors_enabled" default="false" />
<xacro:arg name="pinger_enabled" default="false" />
<xacro:arg name="ball_shooter_enabled" default="false" />
<xacro:arg name="thruster_namespace" default="thrusters/"/>
<xacro:arg name="camera_namespace" default="cameras/"/>
<xacro:arg name="sensor_namespace" default="sensors/"/>
<xacro:arg name="pinger_namespace" default="pingers/"/>
<xacro:arg name="shooter_namespace" default="shooters/"/>
<!-- Note: this is only used for some sensors that do not correctly use the
robotNamespace parameter -->
<xacro:arg name="namespace" default="wamv"/>
<xacro:property name="thruster_namespace" value="$(arg thruster_namespace)" scope="global" />
<xacro:property name="camera_namespace" value="$(arg camera_namespace)" scope="global" />
<xacro:property name="sensor_namespace" value="$(arg sensor_namespace)" scope="global" />
<xacro:property name="pinger_namespace" value="$(arg pinger_namespace)" scope="global" />
<xacro:property name="shooter_namespace" value="$(arg shooter_namespace)" scope="global" />
<xacro:property name="namespace" value="$(arg namespace)" scope="global" />
<!-- Sensor yaml file -->
<xacro:arg name="yaml_component_generation" default="false"/>
<xacro:arg name="component_xacro_file" default = ""/>
<!-- Thruster yaml file -->
<xacro:arg name="yaml_thruster_generation" default="false"/>
<xacro:arg name="thruster_xacro_file" default = ""/>
<!-- === The WAM-V platform === -->
<xacro:include filename="$(find wamv_gazebo)/urdf/wamv_gazebo.xacro"/>
<!-- === Batteries === -->
<xacro:include filename="$(find wamv_description)/urdf/battery.xacro"/>
<xacro:battery prefix="left" position="0 1 0.45" orientation="0 0 0"/>
<xacro:battery prefix="right" position="0 -1 0.45" orientation="0 0 0"/>
<!-- === Thrusters === -->
<!-- Use thruster yaml file if given -->
<xacro:if value="$(arg yaml_thruster_generation)">
<xacro:wamv_gazebo thruster_layout="$(arg thruster_xacro_file)"/>
</xacro:if>
<!-- Otherwise, add thrusters based on thruster_config variable -->
<xacro:unless value="$(arg yaml_thruster_generation)">
<xacro:property name="thruster_conf" value="$(arg thruster_config)"/>
<!-- Default WAM-V with two aft thrusters -->
<xacro:if value="${thruster_conf == 'H'}">
<xacro:wamv_gazebo thruster_layout="$(find wamv_gazebo)/urdf/thruster_layouts/wamv_aft_thrusters.xacro"/>
</xacro:if>
<!-- WAMV with "T" thruster configuration -->
<xacro:if value="${thruster_conf == 'T'}">
<xacro:wamv_gazebo thruster_layout="$(find wamv_gazebo)/urdf/thruster_layouts/wamv_t_thrusters.xacro"/>
</xacro:if>
<!-- WAMV with "X" thruster configuration -->
<xacro:if value="${thruster_conf == 'X'}">
<xacro:wamv_gazebo thruster_layout="$(find wamv_gazebo)/urdf/thruster_layouts/wamv_x_thrusters.xacro"/>
</xacro:if>
</xacro:unless>
<!-- === Decide if we lock the robot to the world === -->
<xacro:if value="$(arg locked)">
<gazebo>
<link name="wamv_external_link"/>
<joint name="wamv_external_pivot_joint" type="universal">
<parent>wamv::wamv/base_link</parent>
<child>wamv_external_link</child>
<axis>
<xyz>1 0 0</xyz>
</axis>
<axis2>
<xyz>0 1 0</xyz>
</axis2>
</joint>
<joint name="wamv_external_riser" type="prismatic">
<parent>world</parent>
<child>wamv_external_link</child>
<axis>
<limit>
<lower>-3</lower>
<upper>3</upper>
</limit>
<xyz>0 0 1</xyz>
</axis>
</joint>
</gazebo>
</xacro:if>
<!-- === Sensors === -->
<!-- Use sensor yaml file if given -->
<xacro:if value="$(arg yaml_component_generation)">
<xacro:include filename="$(arg component_xacro_file)"/>
<xacro:yaml_components />
<!-- Add CPU Cases -->
<xacro:include filename="$(find wamv_description)/urdf/cpu_cases.xacro" />
<xacro:cpu_cases position="-0.15 0 1.53" orientation="0 0 0"/>
</xacro:if>
<!-- Otherwise, add sensors based on enable variables -->
<xacro:unless value="$(arg yaml_component_generation)">
<!-- Add a front camera -->
<xacro:if value="$(arg camera_enabled)">
<xacro:wamv_camera name="front_camera" y="0.3" x="0.75" P="${radians(15)}" />
</xacro:if>
<!-- Add simulated GPS -->
<xacro:if value="$(arg gps_enabled)">
<xacro:wamv_gps name="gps_wamv" x="-0.85" />
</xacro:if>
<!-- Add Simulated IMU -->
<xacro:if value="$(arg imu_enabled)">
<xacro:wamv_imu name="imu_wamv" y="-0.2" />
</xacro:if>
<!-- Add 3D LIDAR -->
<xacro:if value="$(arg lidar_enabled)">
<xacro:lidar name="lidar_wamv" y="-0.3" type="16_beam"/>
</xacro:if>
<!-- Add P3D ground truth -->
<xacro:if value="$(arg ground_truth_enabled)">
<xacro:wamv_p3d name="p3d_wamv"/>
</xacro:if>
<!-- Add pinger -->
<xacro:if value="$(arg pinger_enabled)">
<xacro:wamv_pinger name="pinger" position="1.0 0 -1.0" />
</xacro:if>
<!-- Add ball shooter (default pitch angle: ~-60 deg) -->
<xacro:if value="$(arg ball_shooter_enabled)">
<xacro:wamv_ball_shooter name="ball_shooter" x="0.54" y="0.30" z="1.296" pitch="-1.04"/>
</xacro:if>
<!-- ==== VRX sensor configuration ==== -->
<xacro:if value="$(arg vrx_sensors_enabled)">
<!-- Add CPU Cases -->
<xacro:include filename="$(find wamv_description)/urdf/cpu_cases.xacro" />
<xacro:cpu_cases position="-0.15 0 1.53" orientation="0 0 0"/>
<!-- Add a stereo camera pair -->
<xacro:wamv_camera name="front_left_camera" y="0.1" x="0.75" P="${radians(15)}" />
<xacro:wamv_camera name="front_right_camera" y="-0.1" x="0.75" P="${radians(15)}" />
<!-- Add a camera facing right -->
<xacro:wamv_camera name="middle_right_camera" y="-0.45" P="${radians(15)}" Y="${radians(-90)}" post_Y="${radians(-90)}" />
<!-- Add simulated GPS -->
<xacro:wamv_gps name="gps_wamv" x="-0.85" />
<!-- Add Simulated IMU -->
<xacro:wamv_imu name="imu_wamv" y="-0.2" />
<!-- Add 3D LIDAR -->
<xacro:lidar name="lidar_wamv" type="16_beam"/>
<!-- Add pinger -->
<xacro:wamv_pinger name="pinger" position="1.0 0 -1.0" />
<!-- Add ball shooter (default pitch angle: ~-60 deg) -->
<xacro:wamv_ball_shooter name="ball_shooter" x="0.54" y="0.30" z="1.296" pitch="-1.04"/>
</xacro:if>
</xacro:unless>
</robot>