-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathrb1_ros2_base.urdf.xacro
208 lines (164 loc) · 7.5 KB
/
rb1_ros2_base.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
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
<?xml version="1.0"?>
<robot name="rb1_base"
xmlns:xacro="http://www.ros.org/wiki/xacro">
<!-- ***************** -->
<!-- Imported elements -->
<!-- ***************** -->
<!-- First we import all posible elements defined in the urdf.xacro files. All these elements are defined as macro:xacros -->
<!-- Import RB1 base elements -->
<xacro:include filename="$(find rb1_ros2_description)/urdf/bases/rb1_base_v3.urdf.xacro" />
<!-- v3 name from engineering department -->
<!-- Import RB1 wheels -->
<xacro:include filename="$(find rb1_ros2_description)/urdf/wheels/rubber_wheel.urdf.xacro" />
<xacro:include filename="$(find rb1_ros2_description)/urdf/wheels/omni_wheel.urdf.xacro" />
<!-- Import all available sensors -->
<xacro:include filename="$(find rb1_ros2_description)/urdf/all_sensors.urdf.xacro" />
<!-- Import Gazebo Stuff -->
<xacro:include filename="$(find rb1_ros2_description)/urdf/common.gazebo.xacro" />
<!-- Import Elevator -->
<xacro:include filename="$(find rb1_ros2_description)/urdf/elevator/elevator.urdf.xacro"/>
<!-- ***************** -->
<!-- Global parameters -->
<!-- ***************** -->
<xacro:property name="PI" value="3.1415926535897931"/>
<!-- Wheel parameters -->
<xacro:property name="wheel_offset_x" value="0.0" />
<!-- x,y,z in translation from base_link to the center of the wheel -->
<xacro:property name="wheel_offset_y" value="0.218" />
<xacro:property name="wheel_offset_z" value="0.0512" />
<!-- Castor paramters -->
<xacro:property name="omni_back_x" value="0.195" />
<xacro:property name="omni_back_y" value="0.0" />
<xacro:property name="omni_back_z" value="0.0270" />
<xacro:property name="omni_front_x" value="0.132" />
<xacro:property name="omni_front_y" value="0.1535" />
<xacro:property name="omni_front_z" value="0.0270" />
<!-- ******************* -->
<!-- ELEVATOR PARAMETERS -->
<!-- ******************* -->
<xacro:property name="elevator_x" value="0.0" />
<xacro:property name="elevator_y" value="0.0" />
<xacro:property name="elevator_z" value="0.213" />
<!-- Flag to select the high or low quality model -->
<xacro:property name="hq" value="true" />
<xacro:arg name="prefix" default="robot_"/>
<xacro:arg name="ros_planar_move_plugin" default="false"/>
<xacro:arg name="gpu" default="false"/>
<!-- *************** -->
<!-- Robots Elements -->
<!-- *************** -->
<!-- Here we create the robot elements using the xacro:macros imported at the beggining of this file -->
<!-- RB-1 base -->
<xacro:rb1_base_v3 prefix="$(arg prefix)" publish_bf="true" hq="${hq}" />
<xacro:if value="$(arg ros_planar_move_plugin)">
<xacro:skid_steering prefix="$(arg prefix)" publish_tf="true"/>
</xacro:if>
<xacro:gazebo_colors prefix="$(arg prefix)"/>
<!-- Robot Evelator -->
<xacro:elevator_platform prefix="$(arg prefix)evelator" parent="$(arg prefix)base_link" hq="${hq}">
<origin xyz="${elevator_x} -${elevator_y} ${elevator_z}" rpy="0 0 0"/>
</xacro:elevator_platform>
<!-- WHEELS -->
<xacro:rubber_wheel prefix="$(arg prefix)right" parent="$(arg prefix)base_link" reflect="false" hq="${hq}">
<origin xyz="${wheel_offset_x} -${wheel_offset_y} ${wheel_offset_z}" rpy="0 0 0"/>
</xacro:rubber_wheel>
<xacro:rubber_wheel prefix="$(arg prefix)left" parent="$(arg prefix)base_link" reflect="true" hq="${hq}">
<origin xyz="${wheel_offset_x} ${wheel_offset_y} ${wheel_offset_z}" rpy="0 0 0"/>
</xacro:rubber_wheel>
<!-- OMNI DIRECTIONAL WHEELS -->
<xacro:omni_wheel prefix="$(arg prefix)omni_back" parent="$(arg prefix)base_link" hq="${hq}">
<origin xyz="-${omni_back_x} ${omni_back_y} ${omni_back_z}" rpy="0 0 0"/>
</xacro:omni_wheel>
<xacro:omni_wheel prefix="$(arg prefix)omni_front_left" parent="$(arg prefix)base_link" hq="${hq}">
<origin xyz="${omni_front_x} ${omni_front_y} ${omni_front_z}" rpy="0 0 0"/>
</xacro:omni_wheel>
<xacro:omni_wheel prefix="$(arg prefix)omni_front_right" parent="$(arg prefix)base_link" hq="${hq}">
<origin xyz="${omni_front_x} -${omni_front_y} ${omni_front_z}" rpy="0 0 0"/>
</xacro:omni_wheel>
<!-- SENSORS -->
<xacro:sensor_hokuyo_ust20lx prefix="$(arg prefix)front_laser" parent="$(arg prefix)base_link" prefix_topic="front_laser" gpu="$(arg gpu)">
<origin xyz="0.210 0.0 0.21" rpy="0 3.1416 3.1416"/>
</xacro:sensor_hokuyo_ust20lx>
<!-- <xacro:sensor_imu_hector prefix="$(arg prefix)imu" parent="$(arg prefix)base_link">
<origin xyz="0.002 0.053 0.1485" rpy="0 0 0"/>
</xacro:sensor_imu_hector> -->
<!-- <xacro:sensor_orbbec_astra prefix="$(arg prefix)front_rgbd_camera" parent="$(arg prefix)base_link" prefix_topic="front_rgbd_camera">
<origin xyz="0.234 0.0 0.092" rpy="0 0 0"/>
</xacro:sensor_orbbec_astra> -->
<gazebo reference="$(arg prefix)front_laser_base_link">
<sensor name="lidar" type="ray">
<always_on>true</always_on>
<visualize>false</visualize>
<update_rate>20</update_rate>
<ray>
<scan>
<horizontal>
<samples>1081</samples>
<resolution>1.000000</resolution>
<min_angle>-2.3562</min_angle>
<max_angle>2.3562</max_angle>
</horizontal>
</scan>
<range>
<min>0.06</min>
<max>20.0</max>
<resolution>0.004359297</resolution>
</range>
<noise>
<type>gaussian</type>
<mean>0.0</mean>
<stddev>0.01</stddev>
</noise>
</ray>
<plugin name="scan" filename="libgazebo_ros_ray_sensor.so">
<ros>
<!-- <namespace>${robot_name}</namespace> -->
<remapping>~/out:=scan</remapping>
</ros>
<output_type>sensor_msgs/LaserScan</output_type>
<frame_name>robot_front_laser_base_link</frame_name>
</plugin>
</sensor>
</gazebo>
<!-- Gazebo ros2 control plugins -->
<xacro:ros2_control_gazebo/>
<!-- ROS2 control - diff drive -->
<ros2_control name="diff_drive" type="system">
<hardware>
<plugin>gazebo_ros2_control/GazeboSystem</plugin>
</hardware>
<joint name="robot_left_wheel_joint">
<command_interface name="velocity">
<param name="min">-1</param>
<param name="max">1</param>
</command_interface>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="robot_right_wheel_joint">
<command_interface name="velocity">
<param name="min">-1</param>
<param name="max">1</param>
</command_interface>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
</ros2_control>
<!-- ROS2 control - position -->
<ros2_control name="joint_position" type="system">
<hardware>
<plugin>gazebo_ros2_control/GazeboSystem</plugin>
</hardware>
<joint name="robot_evelator_platform_joint">
<command_interface name="position">
<param name="min">0.0</param>
<param name="max">0.3</param>
</command_interface>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
</ros2_control>
</robot>