Skip to content

Commit a1b58f5

Browse files
civerachb-cprciverachb-cpr
authored
Add support for INS sensors + Fixposition XVN (#184)
* Rename swift_antenna -> gnss_spherical, add gnss_helical, start filling in the fixposition URDF. * Add patch antenna STL, move gnss_antenna macro to its own file, fix parameters in the fixposition macro * Add INS description, remove unnecessary parameters from the URDF * Use the new gnss_antenna for the Swiftnav Duro URDF * Add the 3.5cm front offset between the XVN physical mounting point & the sensor origin
1 parent 2f53c0f commit a1b58f5

File tree

8 files changed

+247
-21
lines changed

8 files changed

+247
-21
lines changed

‎clearpath_generator_common/clearpath_generator_common/description/sensors.py

+57-2
Original file line numberDiff line numberDiff line change
@@ -45,6 +45,10 @@
4545
Microstrain,
4646
RedshiftUM7
4747
)
48+
from clearpath_config.sensors.types.ins import (
49+
BaseINS,
50+
Fixposition,
51+
)
4852
from clearpath_config.sensors.types.lidars_2d import BaseLidar2D, HokuyoUST, SickLMS1XX
4953
from clearpath_config.sensors.types.lidars_3d import (
5054
BaseLidar3D,
@@ -136,6 +140,56 @@ def __init__(self, sensor: BaseLidar3D) -> None:
136140
self.UPDATE_RATE: 20 # TODO: link to clearpath_config property
137141
})
138142

143+
class InsDescription(BaseDescription):
144+
NUM_ANTENNAS = 'num_antennas'
145+
146+
GPS_0_TYPE = 'gps_0_type'
147+
GPS_0_XYZ_X = 'gps_0_xyz_x'
148+
GPS_0_XYZ_Y = 'gps_0_xyz_y'
149+
GPS_0_XYZ_Z = 'gps_0_xyz_z'
150+
GPS_0_RPY_R = 'gps_0_rpy_r'
151+
GPS_0_RPY_P = 'gps_0_rpy_p'
152+
GPS_0_RPY_Y = 'gps_0_rpy_y'
153+
GPS_0_PARENT = 'gps_0_parent'
154+
155+
GPS_1_TYPE = 'gps_1_type'
156+
GPS_1_XYZ_X = 'gps_1_xyz_x'
157+
GPS_1_XYZ_Y = 'gps_1_xyz_y'
158+
GPS_1_XYZ_Z = 'gps_1_xyz_z'
159+
GPS_1_RPY_R = 'gps_1_rpy_r'
160+
GPS_1_RPY_P = 'gps_1_rpy_p'
161+
GPS_1_RPY_Y = 'gps_1_rpy_y'
162+
GPS_1_PARENT = 'gps_1_parent'
163+
164+
def __init__(self, sensor: BaseINS) -> None:
165+
super().__init__(sensor)
166+
167+
self.parameters.update({
168+
self.NUM_ANTENNAS: len(sensor.antennas),
169+
170+
self.GPS_0_TYPE: sensor.antennas[0].antenna_type,
171+
self.GPS_0_XYZ_X: sensor.antennas[0].xyz[0],
172+
self.GPS_0_XYZ_Y: sensor.antennas[0].xyz[1],
173+
self.GPS_0_XYZ_Z: sensor.antennas[0].xyz[2],
174+
self.GPS_0_RPY_R: sensor.antennas[0].rpy[0],
175+
self.GPS_0_RPY_P: sensor.antennas[0].rpy[1],
176+
self.GPS_0_RPY_Y: sensor.antennas[0].rpy[2],
177+
self.GPS_0_PARENT: sensor.antennas[0].parent,
178+
179+
# we only have 1 or 2 antennas, so use -1:
180+
# if there's only one antenna this is the same as 0
181+
# but the duplication is safely ignored because
182+
# we set NUM_ANTENNAS above
183+
self.GPS_1_TYPE: sensor.antennas[-1].antenna_type,
184+
self.GPS_1_XYZ_X: sensor.antennas[-1].xyz[0],
185+
self.GPS_1_XYZ_Y: sensor.antennas[-1].xyz[1],
186+
self.GPS_1_XYZ_Z: sensor.antennas[-1].xyz[2],
187+
self.GPS_1_RPY_R: sensor.antennas[-1].rpy[0],
188+
self.GPS_1_RPY_P: sensor.antennas[-1].rpy[1],
189+
self.GPS_1_RPY_Y: sensor.antennas[-1].rpy[2],
190+
self.GPS_1_PARENT: sensor.antennas[-1].parent,
191+
})
192+
139193
class OusterOS1Description(Lidar3dDescription):
140194
SAMPLES_HORIZONTAL = 'samples_h'
141195
SAMPLES_VERTICAL = 'samples_v'
@@ -194,7 +248,7 @@ def __init__(self, sensor: IntelRealsense) -> None:
194248
self.MODEL: sensor.device_type,
195249
})
196250

197-
class LuxonisOAKDDescroption(CameraDescription):
251+
class LuxonisOAKDDescription(CameraDescription):
198252
MODEL = 'model'
199253

200254
def __init__(self, sensor: LuxonisOAKD) -> None:
@@ -227,7 +281,8 @@ def __init__(self, sensor: StereolabsZed) -> None:
227281
CHRoboticsUM6.SENSOR_MODEL: ImuDescription,
228282
RedshiftUM7.SENSOR_MODEL: ImuDescription,
229283
StereolabsZed.SENSOR_MODEL: StereolabsZedDescription,
230-
LuxonisOAKD.SENSOR_MODEL: LuxonisOAKDDescroption,
284+
LuxonisOAKD.SENSOR_MODEL: LuxonisOAKDDescription,
285+
Fixposition.SENSOR_MODEL: InsDescription,
231286
}
232287

233288
def __new__(cls, sensor: BaseSensor) -> BaseDescription:

‎clearpath_sensors_description/meshes/fixposition.stl

11.6 KB
Binary file not shown.

‎clearpath_sensors_description/meshes/gnss_helical.stl

52.6 KB
Binary file not shown.

‎clearpath_sensors_description/meshes/gnss_patch.stl

137 KB
Binary file not shown.

‎clearpath_sensors_description/meshes/swift_antenna.stl renamed to ‎clearpath_sensors_description/meshes/gnss_spherical.stl

File renamed without changes.

‎clearpath_sensors_description/urdf/fixposition.urdf.xacro

+96
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,96 @@
1+
<?xml version="1.0" encoding="UTF-8"?>
2+
<!--
3+
Fixposition mesh originally sourced from
4+
https://docs.fixposition.com/fd/step-model-and-camera-fov-data#STEPmodelandcameraFOVdata-LinktotheSTEPfile
5+
-->
6+
<robot xmlns:xacro="http://wiki.ros.org/xacro">
7+
<xacro:macro name="fixposition" params="
8+
name
9+
parent_link
10+
*origin
11+
model:=xvn
12+
13+
num_antennas:=2
14+
15+
gps_0_type:=spherical
16+
gps_0_xyz_x:=0
17+
gps_0_xyz_y:=0
18+
gps_0_xyz_z:=0
19+
gps_0_rpy_r:=0
20+
gps_0_rpy_p:=0
21+
gps_0_rpy_y:=0
22+
gps_0_parent:=base_link
23+
24+
gps_1_type:=spherical
25+
gps_1_xyz_x:=0
26+
gps_1_xyz_y:=0
27+
gps_1_xyz_z:=0
28+
gps_1_rpy_r:=0
29+
gps_1_rpy_p:=0
30+
gps_1_rpy_y:=0
31+
gps_1_parent:=base_link
32+
">
33+
<xacro:include filename="$(find clearpath_sensors_description)/urdf/gnss_antenna.urdf.xacro" />
34+
35+
<link name="${name}_base_link">
36+
<visual>
37+
<material name="clearpath_white"/>
38+
<geometry>
39+
<mesh filename="package://clearpath_sensors_description/meshes/fixposition.stl" scale="0.001 0.001 0.001" />
40+
</geometry>
41+
<origin xyz="0 0 0" rpy="0 0 0" />
42+
</visual>
43+
<collision>
44+
<geometry>
45+
<!--
46+
Bounding box dimensions from
47+
https://docs.fixposition.com/__attachments/73072655/VRTK2_integration_manual_v2.3.3.pdf
48+
-->
49+
<box size="0.1291 0.1136 0.03" />
50+
</geometry>
51+
<origin xyz="0 0 0.015" rpy="0 0 0" />
52+
</collision>
53+
</link>
54+
<joint name="${name}_base_joint" type="fixed">
55+
<parent link="${parent_link}" />
56+
<child link="${name}_base_link" />
57+
<xacro:insert_block name="origin" />
58+
</joint>
59+
60+
<!--
61+
Sensor origin is on the top of the unit, slightly in front of the mounting point
62+
-->
63+
<link name="${name}_sensor"/>
64+
<joint name="${name}_sensor_joint" type="fixed">
65+
<parent link="${name}_base_link" />
66+
<child link="${name}_sensor" />
67+
<origin xyz="0.035 0 0.03" rpy="0 0 0" />
68+
</joint>
69+
70+
<!--
71+
1 antenna is mandatory, second is optional
72+
-->
73+
<xacro:gnss_antenna
74+
prefix="${name}_gps_0"
75+
parent_link="${gps_0_parent}"
76+
type="${gps_0_type}">
77+
<origin
78+
xyz="${gps_0_xyz_x} ${gps_0_xyz_y} ${gps_0_xyz_z}"
79+
rpy="${gps_0_rpy_r} ${gps_0_rpy_p} ${gps_0_rpy_y}"
80+
/>
81+
</xacro:gnss_antenna>
82+
83+
<xacro:if value="${num_antennas > 1}">
84+
<xacro:gnss_antenna
85+
prefix="${name}_gps_1"
86+
parent_link="${gps_1_parent}"
87+
type="${gps_1_type}">
88+
<origin
89+
xyz="${gps_1_xyz_x} ${gps_1_xyz_y} ${gps_1_xyz_z}"
90+
rpy="${gps_1_rpy_r} ${gps_1_rpy_p} ${gps_1_rpy_y}"
91+
/>
92+
</xacro:gnss_antenna>
93+
</xacro:if>
94+
95+
</xacro:macro>
96+
</robot>

‎clearpath_sensors_description/urdf/gnss_antenna.urdf.xacro

+91
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,91 @@
1+
<?xml version="1.0" encoding="UTF-8"?>
2+
<robot xmlns:xacro="http://wiki.ros.org/xacro">
3+
<!--
4+
Macro for adding GNSS (aka: GPS) antennas to robots
5+
6+
3 types are supported:
7+
- "spherical" - dome-shaped antennas commonly used with SwiftNav Duro receivers
8+
- "helical" - cylindrical antennas included by default with Fixposition systems
9+
- "patch" - small, square antennas used with low-cost units like Ublox
10+
11+
The antenna has 1 link (the physical mounting point) called ${prefix}_link and 1
12+
fixed joint called ${prefix}_joint connecting parent_link to ${prefix}_link.
13+
-->
14+
<xacro:macro name="gnss_antenna" params="
15+
prefix
16+
parent_link
17+
*origin
18+
type:=spherical
19+
">
20+
<xacro:if value="${type == 'spherical'}">
21+
<link name="${prefix}_link">
22+
<visual>
23+
<material name="clearpath_white"/>
24+
<geometry>
25+
<mesh filename="package://clearpath_sensors_description/meshes/gnss_spherical.stl" />
26+
</geometry>
27+
</visual>
28+
<collision>
29+
<material name="clearpath_white"/>
30+
<origin xyz="0.0000 -0.0000 0.0324"/>
31+
<geometry>
32+
<sphere radius="0.1122" />
33+
</geometry>
34+
</collision>
35+
</link>
36+
37+
<joint name="${prefix}_joint" type="fixed">
38+
<parent link="${parent_link}" />
39+
<child link="${prefix}_link" />
40+
<xacro:insert_block name="origin" />
41+
</joint>
42+
</xacro:if>
43+
44+
<xacro:if value="${type == 'helical'}">
45+
<link name="${prefix}_link">
46+
<visual>
47+
<material name="clearpath_white"/>
48+
<geometry>
49+
<mesh filename="package://clearpath_sensors_description/meshes/gnss_helical.stl" />
50+
</geometry>
51+
<origin xyz="0 0 0" rpy="0 0 0" />
52+
</visual>
53+
<collision>
54+
<geometry>
55+
<cylinder radius="0.015" length="0.055"/>
56+
</geometry>
57+
<origin xyz="0 0 0.0275" rpy="0 0 0" />
58+
</collision>
59+
</link>
60+
61+
<joint name="${prefix}_joint" type="fixed">
62+
<parent link="${parent_link}" />
63+
<child link="${prefix}_link" />
64+
<xacro:insert_block name="origin" />
65+
</joint>
66+
</xacro:if>
67+
68+
<xacro:if value="${type == 'patch'}">
69+
<link name="${prefix}_link">
70+
<visual>
71+
<material name="clearpath_black" />
72+
<geometry>
73+
<mesh filename="package://clearpath_sensors_description/meshes/gnss_patch.stl" />
74+
</geometry>
75+
</visual>
76+
<collision>
77+
<geometry>
78+
<box size="0.06 0.06 0.02" />
79+
</geometry>
80+
<origin xyz="0 0 0.01" rpy="0 0 0" />
81+
</collision>
82+
</link>
83+
84+
<joint name="${prefix}_joint" type="fixed">
85+
<parent link="${parent_link}" />
86+
<child link="${prefix}_link" />
87+
<xacro:insert_block name="origin" />
88+
</joint>
89+
</xacro:if>
90+
</xacro:macro>
91+
</robot>

‎clearpath_sensors_description/urdf/swiftnav_duro.urdf.xacro

+3-19
Original file line numberDiff line numberDiff line change
@@ -1,26 +1,10 @@
11
<?xml version="1.0"?>
22
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
33
<xacro:macro name="swiftnav_duro" params="name parent_link *origin">
4-
<link name="${name}_link">
5-
<visual>
6-
<material name="clearpath_white"/>
7-
<geometry>
8-
<mesh filename="package://clearpath_sensors_description/meshes/swift_antenna.stl"/>
9-
</geometry>
10-
</visual>
11-
<collision>
12-
<material name="clearpath_white"/>
13-
<origin xyz="0.0000 -0.0000 0.0324"/>
14-
<geometry>
15-
<sphere radius="0.1122" />
16-
</geometry>
17-
</collision>
18-
</link>
19-
<joint name="${name}_joint" type="fixed">
20-
<child link="${name}_link"/>
21-
<parent link="${parent_link}"/>
4+
<xacro:include filename="$(find clearpath_sensors_description)/urdf/gnss_antenna.urdf.xacro" />
5+
<xacro:gnss_antenna prefix="${name}" parent_link="${parent_link}" type="spherical">
226
<xacro:insert_block name="origin"/>
23-
</joint>
7+
</xacro:gnss_antenna>
248

259
<gazebo reference="${name}_link">
2610
<sensor name="${name}" type="navsat">

0 commit comments

Comments
 (0)