from math import pi
import roboticstoolbox as rtb
from spatialgeometry import Mesh
from spatialmath import SO3, SE3
import numpy as np
# TODO
# rotate the rings according to the rotation axis, so that the axles
# point the right way
# Launch the simulator Swift
env = swift.Swift()
env.launch()
path = rtb.path_to_datafile("data")
g1 = Mesh(
filename=str(path / "gimbal-ring1.stl"), color=[34, 143, 201], scale=(1.0 / 3,) * 3
)
g2 = Mesh(
filename=str(path / "gimbal-ring2.stl"), color=[31, 184, 72], scale=(1.1 / 3,) * 3
)
g3 = Mesh(
filename=str(path / "gimbal-ring3.stl"),
color=[240, 103, 103],
scale=(1.1 ** 2 / 3,) * 3,
)
import swift
from math import pi
import roboticstoolbox as rtb
from spatialgeometry import Mesh
from spatialmath import SO3, SE3
import numpy as np
# TODO
# rotate the rings according to the rotation axis, so that the axles
# point the right way
# Launch the simulator Swift
env = swift.Swift()
env.launch()
path = rtb.path_to_datafile('data')
g1 = Mesh(filename=str(path / 'gimbal-ring1.stl'),
color=[34, 143, 201],
scale=(1. / 3, ) * 3)
g2 = Mesh(filename=str(path / 'gimbal-ring2.stl'),
color=[31, 184, 72],
scale=(1.1 / 3, ) * 3)
g3 = Mesh(filename=str(path / 'gimbal-ring3.stl'),
color=[240, 103, 103],
scale=(1.1**2 / 3, ) * 3)
plane = Mesh(filename=str(path / 'spitfire_assy-gear_up.stl'),
scale=(1. / (180 * 3), ) * 3,
def __init__(self, filename, laser=False, verbose=False):
# parse the file data
# we assume g2o format
# VERTEX* vertex_id X Y THETA
# EDGE* startvertex_id endvertex_id X Y THETA IXX IXY IYY IXT IYT ITT
# vertex numbers start at 0
self.laser = laser
self.graph = pgraph.UGraph(verbose=verbose)
path = rtb.path_to_datafile(filename)
if filename.endswith('.zip'):
zf = zipfile.ZipFile(path, 'r')
opener = zf.open
filename = filename[:-4]
else:
opener = open
filename = path
with opener(filename, 'r') as f:
toroformat = False
nlaser = 0
# indices into ROBOTLASER1 record for the 3x3 info matrix in column major
# order
g2o = [0, 1, 2, 1, 3, 4, 2, 4, 5]
toro = [0, 1, 4, 1, 2, 5, 4, 5, 3]
# we keep an array self. = vindex(gi) to map g2o vertex index to PGraph vertex index
vindex = {}
firstlaser = True
for line in f:
# for zip file, we get data as bytes not str
if isinstance(line, bytes):
line = line.decode()
# is it a comment?
if line.startswith('#'):
continue
tokens = line.split(' ')
# g2o format records
if tokens[0] == 'VERTEX_SE2':
v = self.graph.add_vertex([float(x) for x in tokens[2:5]])
id = int(tokens[1])
vindex[id] = v
v.id = id
v.type = 'vertex'
elif tokens[0] == 'VERTEX_XY':
v = self.graph.add_vertex([float(x) for x in tokens[2:4]])
id = int(tokens[1])
vindex[id] = v
v.id = id
v.type = 'landmark'
elif tokens[0] == 'EDGE_SE2':
v1 = vindex[int(tokens[1])]
v2 = vindex[int(tokens[2])]
# create the edge
e = self.graph.add_edge(v1, v2)
# create the edge data as a structure
# X Y T
# 3 4 5
e.mean = np.array([float(x) for x in tokens[3:6]])
# IXX IXY IXT IYY IYT ITT
# 6 7 8 9 10 11
info = np.array([float(x) for x in tokens[6:12]])
e.info = np.reshape(info[g2o], (3, 3))
## TORO format records
elif tokens[0] == 'VERTEX2':
toroformat = True
v = self.graph.add_vertex([float(x) for x in tokens[2:5]])
id = int(tokens[1])
vindex[id] = v
v.id = id
v.type = 'vertex'
elif tokens[0] == 'EDGE2':
toroformat = True
v1 = vindex[int(tokens[1])]
v2 = vindex[int(tokens[2])]
# create the edge
e = self.graph.add_edge(v1, v2)
# create the edge data as a structure
# X Y T
# 3 4 5
e.mean = [float(x) for x in tokens[3:6]]
# IXX IXY IXT IYY IYT ITT
# 6 7 8 9 10 11
info = np.array([float(x) for x in tokens[6:12]])
e.info = np.reshape(info[toro], (3, 3))
elif tokens[0] == 'ROBOTLASER1':
if not laser:
continue
# laser records are associated with the immediately preceding VERTEX record
# not quite sure what all the fields are
# 1 ?
# 2 min scan angle
# 3 scan range
# 4 angular increment
# 5 maximum range possible
# 6 ?
# 7 ?
# 8 N = number of beams
# 9 to 9+N laser range data
# 9+N+1 ?
# 9+N+2 ?
# 9+N+3 ?
# 9+N+4 ?
# 9+N+5 ?
# 9+N+6 ?
# 9+N+7 ?
# 9+N+8 ?
# 9+N+9 ?
# 9+N+10 ?
# 9+N+11 ?
# 9+N+12 timestamp (*nix timestamp)
# 9+N+13 laser type (str)
# 9+N+14 ?
if firstlaser:
nbeams = int(tokens[8])
lasermeta = tokens[2:6]
firstlaser = False
v.theta = np.arange(0, nbeams) * float(tokens[4]) + float(
tokens[2])
v.range = np.array(
[float(x) for x in tokens[9:nbeams + 9]])
v.time = float(tokens[21 + nbeams])
nlaser += 1
else:
raise RuntimeError(
f"Unexpected line {line} in {filename}")
if toroformat:
print(
f"loaded TORO/LAGO format file: {self.graph.n} nodes, {self.graph.ne} edges"
)
else:
print(
f"loaded g2o format file: {self.graph.n} nodes, {self.graph.ne} edges"
)
if nlaser > 0:
lasermeta = [float(x) for x in lasermeta]
self._angmin = lasermeta[0]
self._angmax = sum(lasermeta[0:2])
self._maxrange = lasermeta[3]
fov = np.degrees([self._angmin, self._angmax])
print(
f" {nlaser} laser scans: {nbeams} beams, fov {fov[0]:.1f}° to {fov[1]:.1f}°, max range {self._maxrange}"
)
self.vindex = vindex