-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathbase.xacro
249 lines (221 loc) · 8.74 KB
/
base.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
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
<?xml version="1.0" ?>
<robot name="zinger" xmlns:xacro="http://www.ros.org/wiki/xacro">
<!-- CONSTANTS -->
<!-- Math -->
<xacro:property name="PI" value="3.1415926535897931"/>
<!-- ARGUMENTS -->
<xacro:arg name="is_simulation" default="true" />
<xacro:arg name="use_fake_hardware" default="true" />
<xacro:arg name="fake_sensor_commands" default="false" />
<xacro:arg name="chassis_version" default="v1" />
<!-- Chassis dimensions -->
<xacro:property name="chassis_mass" value="1.0" /> <!-- arbitrary value for base mass -->
<xacro:property name="chassis_length" value="0.35" />
<xacro:property name="chassis_width" value="0.2" />
<xacro:property name="chassis_height" value="0.06" />
<!-- Suspension dimensions -->
<xacro:property name="wheel_offset" value="0.01" />
<xacro:property name="steering_mass" value="0.2" />
<xacro:property name="steering_radius" value="0.05" />
<xacro:property name="steering_thickness" value="0.01" />
<!-- Wheel dimensions -->
<xacro:property name="wheel_mass" value="0.2" />
<xacro:property name="wheel_radius" value="0.04" />
<xacro:property name="wheel_width" value="0.05" />
<!-- -->
<xacro:property name="front_x_reflect" value="1" />
<xacro:property name="rear_x_reflect" value="-1" />
<xacro:property name="left_y_reflect" value="1" />
<xacro:property name="right_y_reflect" value="-1" />
<!-- INCLUDES -->
<xacro:include filename="$(find zinger_description)/urdf/materials.xacro" />
<xacro:include filename="$(find zinger_description)/urdf/macros.xacro" />
<xacro:if value="$(arg is_simulation)">
<xacro:include filename="$(find zinger_description)/urdf/gazebo.xacro" />
</xacro:if>
<!-- MODEL -->
<!--
An empty base footprint that describes the origin of the robot on the ground
-->
<link name="base_footprint">
</link>
<!--
An empty base link that describes the origin of the robot at the c.o.g of the chassis
-->
<!--
Define the connection between base_link and base_footprint. Relative to base_link the joint
is exactly on the ground plane, because ROS considers the joint frame to be the same
as the child frame.
-->
<joint name="joint_base_to_footprint" type="fixed">
<origin rpy="0 0 0" xyz="0 0 ${wheel_radius + 0.5 * chassis_height}"/>
<parent link="base_footprint"/>
<child link="base_link"/>
</joint>
<link name="base_link">
</link>
<!--
A box that pretends to be the ground. Used for testing that the scuttle model has been assembled correctly.
-->
<!--
<joint name="joint_ground_to_footprint" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0"/>
<parent link="base_footprint"/>
<child link="ground_for_testing"/>
</joint>
<link name="ground_for_testing">
<visual>
<geometry>
<box size="0.5 0.5 0.01"/>
</geometry>
<origin rpy="0 0 0" xyz="0.25 -0.25 -0.005"/>
</visual>
</link>
-->
<!-- The chassis of zinger -->
<!--
Define the connection between base_link and chassis_link. Relative to base_link the joint
is exactly in the c.o.g of the chassis, because ROS considers the joint frame to be the same
as the child frame.
-->
<joint name="chassis_joint" type="fixed">
<origin rpy="0 0 0" xyz="0.0 0 0.0"/>
<parent link="base_link"/>
<child link="chassis_link"/>
</joint>
<!--
The coordinate framework has the X-asis in the direction of forward motion, with the y-axis pointing
to the left. This is equivalent to the coordinate system defined by the kinematics guide.
-->
<link name="chassis_link">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0.0"/>
<mass value="${chassis_mass}"/>
<inertia
ixx="${chassis_mass / 12.0 * (chassis_width * chassis_width + chassis_height * chassis_height)}"
ixy="0"
ixz="0"
iyy="${chassis_mass / 12.0 * (chassis_length * chassis_length + chassis_height * chassis_height)}"
iyz="0"
izz="${chassis_mass / 12.0 * (chassis_length * chassis_length + chassis_width * chassis_width)}"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0.0"/>
<geometry>
<box size="${chassis_length} ${chassis_width} ${chassis_height}" />
</geometry>
<material name="orange"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0.0"/>
<geometry>
<box size="${chassis_length} ${chassis_width} ${chassis_height}" />
</geometry>
</collision>
</link>
<!--
Left front wheel assembly
-->
<xacro:drive_module_assembly
name="left_front"
chassis_length="${chassis_length}"
chassis_width="${chassis_width}"
chassis_height="${chassis_height}"
wheel_mass="${wheel_mass}"
wheel_radius="${wheel_radius}"
wheel_width="${wheel_width}"
steering_mass="${steering_mass}"
steering_radius="${steering_radius}"
steering_thickness="${steering_thickness}"
offset="${wheel_offset}"
x_reflect="${front_x_reflect}"
y_reflect="${left_y_reflect}" />
<!--
Left rear wheel assembly
-->
<xacro:drive_module_assembly
name="left_rear"
chassis_length="${chassis_length}"
chassis_width="${chassis_width}"
chassis_height="${chassis_height}"
wheel_mass="${wheel_mass}"
wheel_radius="${wheel_radius}"
wheel_width="${wheel_width}"
steering_mass="${steering_mass}"
steering_radius="${steering_radius}"
steering_thickness="${steering_thickness}"
offset="${wheel_offset}"
x_reflect="${rear_x_reflect}"
y_reflect="${left_y_reflect}" />
<!--
Right rear wheel assembly
-->
<xacro:drive_module_assembly
name="right_rear"
chassis_length="${chassis_length}"
chassis_width="${chassis_width}"
chassis_height="${chassis_height}"
wheel_mass="${wheel_mass}"
wheel_radius="${wheel_radius}"
wheel_width="${wheel_width}"
steering_mass="${steering_mass}"
steering_radius="${steering_radius}"
steering_thickness="${steering_thickness}"
offset="${wheel_offset}"
x_reflect="${rear_x_reflect}"
y_reflect="${right_y_reflect}" />
<!--
Right front wheel assembly
-->
<xacro:drive_module_assembly
name="right_front"
chassis_length="${chassis_length}"
chassis_width="${chassis_width}"
chassis_height="${chassis_height}"
wheel_mass="${wheel_mass}"
wheel_radius="${wheel_radius}"
wheel_width="${wheel_width}"
steering_mass="${steering_mass}"
steering_radius="${steering_radius}"
steering_thickness="${steering_thickness}"
offset="${wheel_offset}"
x_reflect="${front_x_reflect}"
y_reflect="${right_y_reflect}" />
<ros2_control name="ZingerJointControl" type="system">
<xacro:if value="$(arg is_simulation)">
<hardware>
<plugin>ign_ros2_control/IgnitionSystem</plugin>
</hardware>
</xacro:if>
<xacro:unless value="$(arg is_simulation)">
<xacro:if value="$(arg use_fake_hardware)">
<hardware>
<plugin>mock_components/GenericSystem</plugin>
</hardware>
</xacro:if>
<xacro:unless value="$(arg use_fake_hardware)">
<!-- USE REAL HARDWARE HERE -->
<hardware>
</hardware>
</xacro:unless>
</xacro:unless>
<xacro:steering_module_controller name="left_front" />
<xacro:steering_module_controller name="left_rear" />
<xacro:steering_module_controller name="right_rear" />
<xacro:steering_module_controller name="right_front" />
</ros2_control>
<!-- Sensors -->
<!-- IMU -->
<xacro:sensor_imu name="imu_center" parent_link="chassis_link">
<origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
</xacro:sensor_imu>
<!-- LIDAR -->
<xacro:sensor_lidar
name="rplidar_front"
parent_link="chassis_link"
chassis_top_relative_height="${0.5 * chassis_height}" />
<!-- Bump -->
<!-- Sonar -->
<!-- IR -->
<!-- Cliff sensors -->
</robot>