def test_Duration(self):
Duration = rospy.Duration
# test from_sec
v = Duration(1000)
self.assertEquals(v, Duration.from_sec(v.to_sec()))
self.assertEquals(v, v.from_sec(v.to_sec()))
v = Duration(0, 1000)
self.assertEquals(v, Duration.from_sec(v.to_sec()))
self.assertEquals(v, v.from_sec(v.to_sec()))
# test neg
v = -Duration(1, -1)
self.assertEquals(-1, v.secs)
self.assertEquals(1, v.nsecs)
v = -Duration(-1, -1)
self.assertEquals(1, v.secs)
self.assertEquals(1, v.nsecs)
v = -Duration(-1, 1)
self.assertEquals(0, v.secs)
self.assertEquals(999999999, v.nsecs)
# test addition
failed = False
try:
v = Duration(1, 0) + Time(1, 0)
failed = True
except:
pass
self.failIf(failed, "Duration + Time must fail")
try:
v = Duration(1, 0) + 1
failed = True
except:
pass
self.failIf(failed, "Duration + int must fail")
v = Duration(1, 0) + Duration(1, 0)
self.assertEquals(2, v.secs)
self.assertEquals(0, v.nsecs)
self.assertEquals(Duration(2, 0), v)
v = Duration(-1, -1) + Duration(1, 1)
self.assertEquals(0, v.secs)
self.assertEquals(0, v.nsecs)
self.assertEquals(Duration(), v)
v = Duration(1) + Duration(0, 1000000000)
self.assertEquals(2, v.secs)
self.assertEquals(0, v.nsecs)
self.assertEquals(Duration(2), v)
v = Duration(100, 100) + Duration(300)
self.assertEquals(Duration(400, 100), v)
v = Duration(100, 100) + Duration(300, 300)
self.assertEquals(Duration(400, 400), v)
v = Duration(100, 100) + Duration(300, -101)
self.assertEquals(Duration(399, 999999999), v)
# test subtraction
try:
v = Duration(1, 0) - 1
failed = True
except:
pass
self.failIf(failed, "Duration - non duration must fail")
try:
v = Duration(1, 0) - Time(1, 0)
failed = True
except:
pass
self.failIf(failed, "Duration - Time must fail")
v = Duration(1, 0) - Duration(1, 0)
self.assertEquals(Duration(), v)
v = Duration(-1, -1) - Duration(1, 1)
self.assertEquals(Duration(-3, 999999998), v)
v = Duration(1) - Duration(0, 1000000000)
self.assertEquals(Duration(), v)
v = Duration(2) - Duration(0, 1000000000)
self.assertEquals(Duration(1), v)
v = Duration(100, 100) - Duration(300)
self.assertEquals(Duration(-200, 100), v)
v = Duration(100, 100) - Duration(300, 101)
self.assertEquals(Duration(-201, 999999999), v)
# test abs
self.assertEquals(abs(Duration()), Duration())
self.assertEquals(abs(Duration(1)), Duration(1))
self.assertEquals(abs(Duration(-1)), Duration(1))
self.assertEquals(abs(Duration(0, -1)), Duration(0, 1))
self.assertEquals(abs(Duration(-1, -1)), Duration(1, 1))
def test_Duration(self):
Duration = rospy.Duration
# test from_sec
v = Duration(1000)
self.assertEquals(v, Duration.from_sec(v.to_sec()))
self.assertEquals(v, v.from_sec(v.to_sec()))
v = Duration(0,1000)
self.assertEquals(v, Duration.from_sec(v.to_sec()))
self.assertEquals(v, v.from_sec(v.to_sec()))
# test neg
v = -Duration(1, -1)
self.assertEquals(-1, v.secs)
self.assertEquals(1, v.nsecs)
v = -Duration(-1, -1)
self.assertEquals(1, v.secs)
self.assertEquals(1, v.nsecs)
v = -Duration(-1, 1)
self.assertEquals(0, v.secs)
self.assertEquals(999999999, v.nsecs)
# test addition
failed = False
try:
v = Duration(1,0) + Time(1, 0)
failed = True
except: pass
self.failIf(failed, "Duration + Time must fail")
try:
v = Duration(1,0) + 1
failed = True
except: pass
self.failIf(failed, "Duration + int must fail")
v = Duration(1,0) + Duration(1, 0)
self.assertEquals(2, v.secs)
self.assertEquals(0, v.nsecs)
self.assertEquals(Duration(2, 0), v)
v = Duration(-1,-1) + Duration(1, 1)
self.assertEquals(0, v.secs)
self.assertEquals(0, v.nsecs)
self.assertEquals(Duration(), v)
v = Duration(1) + Duration(0, 1000000000)
self.assertEquals(2, v.secs)
self.assertEquals(0, v.nsecs)
self.assertEquals(Duration(2), v)
v = Duration(100, 100) + Duration(300)
self.assertEquals(Duration(400, 100), v)
v = Duration(100, 100) + Duration(300, 300)
self.assertEquals(Duration(400, 400), v)
v = Duration(100, 100) + Duration(300, -101)
self.assertEquals(Duration(399, 999999999), v)
# test subtraction
try:
v = Duration(1,0) - 1
failed = True
except: pass
self.failIf(failed, "Duration - non duration must fail")
try:
v = Duration(1, 0) - Time(1,0)
failed = True
except: pass
self.failIf(failed, "Duration - Time must fail")
v = Duration(1,0) - Duration(1, 0)
self.assertEquals(Duration(), v)
v = Duration(-1,-1) - Duration(1, 1)
self.assertEquals(Duration(-3, 999999998), v)
v = Duration(1) - Duration(0, 1000000000)
self.assertEquals(Duration(), v)
v = Duration(2) - Duration(0, 1000000000)
self.assertEquals(Duration(1), v)
v = Duration(100, 100) - Duration(300)
self.assertEquals(Duration(-200, 100), v)
v = Duration(100, 100) - Duration(300, 101)
self.assertEquals(Duration(-201, 999999999), v)
# test abs
self.assertEquals(abs(Duration()), Duration())
self.assertEquals(abs(Duration(1)), Duration(1))
self.assertEquals(abs(Duration(-1)), Duration(1))
self.assertEquals(abs(Duration(0,-1)), Duration(0,1))
self.assertEquals(abs(Duration(-1,-1)), Duration(1,1))
class BezierPath:
def __init__(self, *args, **kwargs):
self.bezier_curve = None
self.duration = None
if args:
if len(args) == 1 and type(args[0]) is BezierPathMsg:
self.bezier_curve = BezierCurve(args[0].order,
args[0].control_points)
self.duration = args[0].duration.data
elif len(args) == 2:
if type(args[0]) is BezierCurve:
self.bezier_curve = args[0]
elif type(args[0]) is list:
self.bezier_curve = BezierCurve(args[0])
else:
raise ValueError(f'Unknown argument {args[0]!r}')
if type(args[1]) is Duration:
self.duration = args[1]
elif type(args[1]) is float:
self.duration = Duration(args[1])
else:
raise ValueError(f'Unknown argument {args[1]!r}')
else:
raise ValueError(f'Unknown arguments {args!r}')
if kwargs:
for k, v in kwargs.items():
if k == 'msg':
if type(v) is not BezierPathMsg:
raise ValueError(
f'{k!r} must be {BezierPathMsg}, got {type(v)}')
self.bezier_curve = v.bezier_curve
self.duration = v.duration
elif k == 'bezier_curve':
if type(v) is not BezierCurve:
raise ValueError(
f'{k!r} must be {BezierCurve}, got {type(v)}')
self.bezier_curve = v
elif k == 'duration':
if type(v) is not Duration:
raise ValueError(
f'{k!r} must be {Duration}, got {type(v)}')
self.duration = v
else:
raise ValueError(f'Unknown keyword argument {k!r}')
def __repr__(self):
return f'{self.__class__.__name__}({self.bezier_curve!r}, [{self.duration!r}])'
def __str__(self):
return f'{self.__class__.__name__}[{self.bezier_curve!s}, {self.duration!s}ns]'
def to_param(self, secs):
return (secs.to_sec() if type(secs) is Duration else
float(secs)) / self.duration.to_sec()
def from_param(self, vec):
dt = 1.0 / self.duration.to_sec()
return Vector3(vec.x * dt, vec.y * dt, vec.z * dt)
def at(self, secs):
t = self.to_param(secs)
return self.bezier_curve.at(t)
def vel_at(self, secs):
t = self.to_param(secs)
vec = self.bezier_curve.deriv(t)
return self.from_param(vec)
def speed_at(self, secs):
vel = self.vel_at(secs)
return math.sqrt(vel.x**2 + vel.y**2 + vel.z**2)
def accel_at(self, secs):
t = self.to_param(secs)
dv = self.bezier_curve.hodograph().hodograph().at(t)
dt = self.duration.to_sec()
return Vector3(dv.x / (dt**2), dv.y / (dt**2), dv.z / (dt**2))
def angle_at(self, secs):
vel = self.vel_at(secs)
if vel.x == 0 and vel.y == 0:
vel = self.accel_at(secs)
return math.atan2(vel.y, vel.x)
def angular_vel_at(self, secs):
vel = self.vel_at(secs)
accel = self.accel_at(secs)
return (vel.x * accel.x - vel.y * accel.y) / (vel.x**2 + vel.y**2)
def to_msg(self):
duration_msg = DurationMsg(self.duration)
msg = BezierPathMsg(order=self.bezier_curve.order,
control_points=self.bezier_curve.control_points,
duration=duration_msg)
return msg
def split(self, secs):
t = self.to_param(secs)
curve1, curve2 = self.bezier_curve.de_casteljau(t)
duration1 = Duration(secs)
duration2 = Duration(self.duration.to_sec() - secs)
path1 = BezierPath(bezier_curve=curve1, duration=duration1)
path2 = BezierPath(bezier_curve=curve2, duration=duration2)
return path1, path2
class GeographicTrajectory:
"""
This class represents position of vehicle in time, acquired from GNSS.
It supports two types of message:
- nmea_msgs/Sentence
- sensor_msgs/NavSatFix
Attributes
----------
timestamps : list of rospy.Time
List of discrete timestamp at which pose is recorded
coordinates : np.ndarray
Position of vehicle at each timestamp
duration : rospy.Duration
Length of position recording
frame_id : str
Frame ID of coordinate sensor
"""
supportedMsgTypes = ['sensor_msgs/NavSatFix', 'nmea_msgs/Sentence']
def __init__(self,
randomBag=None,
eastingShift=0.0,
northingShift=0.0,
heightShift=0.0,
startTime=_startTime0,
stopTime=_stopTime_1):
if (randomBag is None):
self.timestamps = []
self.coordinates = []
self.duration = Duration(0)
return
if (randomBag.type() == 'sensor_msgs/NavSatFix'):
self.timestamps, self.coordinates = GeographicTrajectory._parseFromNavSatFix(
randomBag, eastingShift, northingShift, heightShift, startTime,
stopTime)
elif (randomBag.type() == 'nmea_msgs/Sentence'):
self.timestamps, self.coordinates = GeographicTrajectory._parseFromNmea(
randomBag, eastingShift, northingShift, heightShift, startTime,
stopTime)
else:
raise ValueError("Input bag is of unknown type")
self.duration = self.timestamps[-1] - self.timestamps[0]
self.frame_id = randomBag[0].header.frame_id
def __getitem__(self, t):
if isinstance(t, numbers.Integral):
return {
'timestamp': self.timestamps[t],
'pose': self.coordinates[t]
}
if hasattr(t, "to_sec"):
if t < self.timestamps[0] or t > self.timestamps[-1]:
raise ValueError("Requested timestamp is out of range")
return {'timestamp': t, 'pose': self.positionAt(t)}
if isinstance(t, numbers.Real):
return {
'timestamp': self.timestamps[0] + rospy.Duration.from_sec(t),
'pose': self.positionAt(t)
}
def __len__(self):
return len(self.timestamps)
def positionAt(self, time):
"""
Returns position at requested time using interpolation
"""
if (isinstance(time, float)):
if (time < 0 or time > self.duration.to_sec()):
raise ValueError("Offset value is outside bag timestamp range")
time = self.timestamps[0] + Duration.from_sec(time)
elif (isinstance(time, Time)):
if (time < self.timestamps[0] or time > self.timestamps[-1]):
raise ValueError("Timestamp value is outside the bag range")
_t1 = bisect_left(self.timestamps, time)
if _t1 == len(self.timestamps) - 1:
_t1 = len(self.timestamps) - 2
t1 = self.timestamps[_t1]
t2 = self.timestamps[_t1 + 1]
r = ((time - t1).to_sec()) / (t2 - t1).to_sec()
# return self.coordinates[_t1] + (self.coordinates[_t1+1] - self.coordinates[_t1])*r
return GeographicTrajectory.interpolate(self.coordinates[_t1],
self.coordinates[_t1 + 1], r)
def buildFromTimestamps(self, timestampList):
"""
Creates new trajectory based on current one using interpolation of each timestamp
"""
track = GeographicTrajectory()
for t in tqdm(timestampList):
if t <= self.timestamps[0]:
track.coordinates.append(self.coordinates[0])
elif t >= self.timestamps[-1]:
track.coordinates.append(self.coordinates[-1])
else:
pos = self.positionAt(t)
track.coordinates.append(pos)
track.timestamps.append(t)
track.duration = self.timestamps[-1] - self.timestamps[0]
track.coordinates = np.array(track.coordinates)
track.frame_id = self.frame_id
return track
@staticmethod
def interpolate(pq1, pq2, ratio):
position = pq1[0:3] + (pq2[0:3] - pq1[0:3]) * ratio
orientation = tfx.quaternion_slerp(pq1[3:7], pq2[3:7], ratio)
return np.append(position, orientation)
@staticmethod
def parseFromBag(randomBag):
pass
@staticmethod
def _parseFromNmea(randomBag,
eastingShift=0.0,
northingShift=0.0,
heightShift=0.0,
startTime=_startTime0,
stopTime=_stopTime_1):
try:
import pynmea2
except ImportError:
raise RuntimeError(
"NMEA support is not available; install pynmea2")
parsedCoordinates = []
timestamps = []
msgSamples = GeographicTrajectory._createTimeRange(
randomBag, startTime, stopTime)
i = 0
for s in tqdm(msgSamples):
rawmsg = randomBag[s]
i += 1
try:
m = pynmea2.parse(rawmsg.sentence)
coord = utm.fromLatLong(float(m.latitude), float(m.longitude),
float(m.altitude))
gcoord = [
coord.easting, coord.northing, coord.altitude, 0, 0, 0, 0
]
if len(parsedCoordinates) > 1:
quat = GeographicTrajectory.orientationFromPositionOnlyYaw(
parsedCoordinates[-1], parsedCoordinates[-2])
gcoord[3:7] = quat
parsedCoordinates.append(gcoord)
timestamps.append(rawmsg.header.stamp)
except (AttributeError, TypeError, pynmea2.SentenceTypeError):
continue
parsedCoordinates = np.array(parsedCoordinates)
return timestamps, parsedCoordinates
@staticmethod
def _parseFromNavSatFix(randomBag,
eastingShift=0.0,
northingShift=0.0,
heightShift=0.0,
startTime=_startTime0,
stopTime=_stopTime_1):
assert (randomBag.type() == 'sensor_msgs/NavSatFix')
timestamps = []
i = 0
msgSamples = GeographicTrajectory._createTimeRange(
randomBag, startTime, stopTime)
parsedCoordinates = np.zeros((len(msgSamples), 7), dtype=np.float)
for s in tqdm(msgSamples):
rawmsg = randomBag[s]
coord = utm.fromLatLong(rawmsg.latitude, rawmsg.longitude,
rawmsg.altitude)
parsedCoordinates[i, 0:3] = [
coord.easting + eastingShift, coord.northing + northingShift,
coord.altitude + heightShift
]
if i >= 1:
quat = GeographicTrajectory.orientationFromPositionOnlyYaw(
parsedCoordinates[i], parsedCoordinates[i - 1])
parsedCoordinates[i, 3:7] = quat
if i == 1:
parsedCoordinates[0, 3:7] = quat
timestamps.append(rawmsg.header.stamp)
i += 1
return timestamps, parsedCoordinates
@staticmethod
def _createTimeRange(randomBag, startTime, stopTime):
if startTime == _startTime0 and stopTime == _stopTime_1:
msgSamples = range(len(randomBag))
else:
if startTime == _startTime0:
startTime = randomBag.timestamps[0]
if stopTime == _stopTime_1:
stopTime = randomBag.timestamps[-1]
probeTime = startTime - Duration.from_sec(_timeFuzzyOffset)
if probeTime >= randomBag.timestamps[0]:
startTime = probeTime
msgSamples = randomBag.desample(-1, True, startTime, stopTime)
return msgSamples
@staticmethod
def orientationFromPositionOnlyYaw(curPosition, prevPosition):
yaw = math.atan2(curPosition[1] - prevPosition[1],
curPosition[0] - prevPosition[0])
return tfx.quaternion_from_euler(0, 0, yaw)
