def on_pose2d(self, timestamp, data):
x, y, heading = data
pose = (x / 1000.0, y / 1000.0, math.radians(heading / 100.0))
if self.last_position is not None:
self.is_moving = (self.last_position != pose)
dist = math.hypot(pose[0] - self.last_position[0], pose[1] - self.last_position[1])
direction = ((pose[0] - self.last_position[0]) * math.cos(self.last_position[2]) +
(pose[1] - self.last_position[1]) * math.sin(self.last_position[2]))
if direction < 0:
dist = -dist
else:
dist = 0.0
self.last_position = pose
if self.start_pose is None:
self.start_pose = pose
self.traveled_dist += dist
x, y, z = self.xyz
x += math.cos(self.pitch) * math.cos(self.yaw) * dist
y += math.cos(self.pitch) * math.sin(self.yaw) * dist
z += math.sin(self.pitch) * dist
x0, y0, z0 = self.offset
self.last_send_time = self.bus.publish('pose2d', [round((x + x0) * 1000), round((y + y0) * 1000),
round(math.degrees(self.yaw) * 100)])
if self.virtual_bumper is not None:
if self.is_virtual:
self.virtual_bumper.update_pose(timedelta(seconds=self.sim_time_sec), pose)
else:
self.virtual_bumper.update_pose(self.time, pose)
self.xyz = x, y, z
self.trace.update_trace(self.xyz)
# pose3d
dist3d = quaternion.rotate_vector([dist, 0, 0], self.orientation)
self.xyz_quat = [a + b for a, b in zip(self.xyz_quat, dist3d)]
xyz_quat = [p + o for p, o in zip(self.xyz_quat, self.offset)]
self.bus.publish('pose3d', [xyz_quat, self.orientation])
def on_pose2d(self):
x, y, heading = self.pose2d
pose = (x / 1000.0, y / 1000.0, math.radians(heading / 100.0))
if self.last_position is not None:
dist = math.hypot(pose[0] - self.last_position[0],
pose[1] - self.last_position[1])
direction = ((pose[0] - self.last_position[0]) *
math.cos(self.last_position[2]) +
(pose[1] - self.last_position[1]) *
math.sin(self.last_position[2]))
if direction < 0:
dist = -dist
else:
dist = 0.0
self.last_position = pose
x, y, z = self.xyz
x += math.cos(self.pitch) * math.cos(self.yaw) * dist
y += math.cos(self.pitch) * math.sin(self.yaw) * dist
z += math.sin(self.pitch) * dist
x0, y0, z0 = self.offset
self.last_send_time = self.publish('pose2d', [
round((x + x0) * 1000),
round((y + y0) * 1000),
round(math.degrees(self.yaw) * 100)
])
self.xyz = x, y, z
# pose3d
if self.orientation is not None:
dist3d = quaternion.rotate_vector([dist, 0, 0], self.orientation)
self.xyz_quat = [a + b for a, b in zip(self.xyz_quat, dist3d)]
self.publish('pose3d', [self.xyz_quat, self.orientation])
def on_desired_speed(self, dt, channel, data):
forward = data[0]/1000. # from millimeters per second to meters per second
angular = math.radians(data[1]/100.) # from centidegrees per second to radians per second
dtime = 0.1 # simulation step time in seconds
max_acc_forward = 1 # meters per sec**2
max_acc_angular = 1 # radians per sec**2
acc_forward = np.clip(-max_acc_forward, (forward - self.speed_forward)/dtime, max_acc_forward)
acc_angular = np.clip(-max_acc_angular, (angular - self.speed_angular)/dtime, max_acc_angular)
self.speed_forward += acc_forward * dtime
self.speed_angular += acc_angular * dtime
log("desired", f"forward {forward:.2f} m/s", f"angular {math.degrees(angular):.2f} deg/s")
log("current", f"forward {self.speed_forward:.2f} m/s", f"angular {math.degrees(self.speed_angular):.2f} deg/s")
dist = dtime * self.speed_forward
dist3d = quaternion.rotate_vector([dist, 0, 0], self.orientation)
self.xyz = [a + b for a, b in zip(self.xyz, dist3d)]
turn = quaternion.from_axis_angle((0,0,1), dtime * self.speed_angular)
self.orientation = quaternion.multiply(self.orientation, turn)
self.time += datetime.timedelta(seconds=dtime)
heading_centidegrees = round(math.degrees(quaternion.heading(self.orientation))*100)
x_mm, y_mm, z_mm = [round(c*1000) for c in self.xyz]
self.bus.publish('orientation', self.orientation)
self.bus.publish('rot', [heading_centidegrees, 0, 0])
self.bus.publish('pose2d', [x_mm, y_mm, heading_centidegrees])
self.bus.publish('pose3d', [[a - b for a, b, in zip(self.xyz, self.origin)], self.orientation])
self.bus.publish('sim_time_sec', self.time.total_seconds())
self.bus.publish('scan', self.scan())
def pose_callback(self, frame):
try:
pose_frame = frame.as_pose_frame()
profile = pose_frame.get_profile()
with self.origins_lock:
try:
name = self.origins[profile.unique_id()]
except KeyError:
print(
'Pose callback call before registering the stream origin.'
)
return
pose = pose_frame.get_pose_data()
n = frame.get_frame_number()
if n % self.pose_subsample != 0:
return
timestamp = frame.get_timestamp()
except Exception as e:
print(e)
self.finished.set()
return
orientation = quaternion_multiply(
self.tracking_sensor_pitch_quat_inv[name],
t265_to_osgar_orientation(pose.rotation))
self.publish(f'{name}_orientation', orientation)
x, y, z = rotate_vector(t265_to_osgar_position(pose.translation),
self.tracking_sensor_yaw_quat[name])
yaw = quaternion.heading(orientation)
self.publish(
f'{name}_pose2d',
[int(x * 1000),
int(y * 1000),
int(math.degrees(yaw) * 100)])
self.publish(f'{name}_pose3d', [[x, y, z], orientation])
self.publish(f'{name}_pose_raw', [
n,
timestamp,
[pose.translation.x, pose.translation.y, pose.translation.z],
[
pose.rotation.x, pose.rotation.y, pose.rotation.z,
pose.rotation.w
],
[pose.velocity.x, pose.velocity.y, pose.velocity.z],
[
pose.angular_velocity.x, pose.angular_velocity.y,
pose.angular_velocity.z
],
[pose.acceleration.x, pose.acceleration.y, pose.acceleration.z],
[
pose.angular_acceleration.x, pose.angular_acceleration.y,
pose.angular_acceleration.z
],
[pose.mapper_confidence, pose.tracker_confidence],
]) # raw RealSense2 Pose data
def pose_callback(self, frame):
try:
pose = frame.as_pose_frame().get_pose_data()
n = frame.get_frame_number()
if n % self.pose_subsample != 0:
return
timestamp = frame.get_timestamp()
except Exception as e:
print(e)
self.finished.set()
return
orientation = quaternion_multiply(
self.tracking_sensor_pitch_quat_inv,
t265_to_osgar_orientation(pose.rotation))
self.publish('orientation', orientation)
x, y, z = rotate_vector(t265_to_osgar_position(pose.translation),
self.tracking_sensor_yaw_quat)
yaw = quaternion.heading(orientation)
self.publish(
'pose2d',
[int(x * 1000),
int(y * 1000),
int(math.degrees(yaw) * 100)])
self.publish('pose3d', [[x, y, z], orientation])
self.publish('pose_raw', [
n,
timestamp,
[pose.translation.x, pose.translation.y, pose.translation.z],
[
pose.rotation.x, pose.rotation.y, pose.rotation.z,
pose.rotation.w
],
[pose.velocity.x, pose.velocity.y, pose.velocity.z],
[
pose.angular_velocity.x, pose.angular_velocity.y,
pose.angular_velocity.z
],
[pose.acceleration.x, pose.acceleration.y, pose.acceleration.z],
[
pose.angular_acceleration.x, pose.angular_acceleration.y,
pose.angular_acceleration.z
],
[pose.mapper_confidence, pose.tracker_confidence],
]) # raw RealSense2 Pose data
def on_odom(self, pose2d):
x, y, heading = pose2d
if self.orientation is None:
return
odom = Pose2d(x / 1000.0, y / 1000.0, math.radians(heading / 100.0))
if self.last_odom is not None:
dist = math.hypot(odom.x - self.last_odom.x, odom.y - self.last_odom.y)
direction = ((odom.x - self.last_odom.x) * math.cos(self.last_odom.heading) +
(odom.y - self.last_odom.y) * math.sin(self.last_odom.heading))
if direction < 0:
dist = -dist
else:
dist = 0.0
self.last_odom = odom
dist3d = quaternion.rotate_vector([dist, 0, 0], self.orientation)
self.xyz = [a + b for a, b in zip(self.xyz, dist3d)]
self.bus.publish('pose3d', [self.xyz, self.orientation])
def update(self):
packet = self.bus.listen()
if packet is not None:
# print('SubT', packet)
timestamp, channel, data = packet
if self.time is None or int(self.time.seconds) // 60 != int(
timestamp.seconds) // 60:
print(timestamp, '(%.1f %.1f %.1f)' % self.xyz,
sorted(self.stat.items()))
print(timestamp,
list(('%.1f' % (v / 100)) for v in self.voltage))
self.stat.clear()
self.time = timestamp
if not VIRTUAL_WORLD:
self.sim_time_sec = self.time.total_seconds()
self.stat[channel] += 1
if channel == 'pose2d':
x, y, heading = data
pose = (x / 1000.0, y / 1000.0, math.radians(heading / 100.0))
if self.last_position is not None:
self.is_moving = (self.last_position != pose)
dist = math.hypot(pose[0] - self.last_position[0],
pose[1] - self.last_position[1])
direction = ((pose[0] - self.last_position[0]) *
math.cos(self.last_position[2]) +
(pose[1] - self.last_position[1]) *
math.sin(self.last_position[2]))
if direction < 0:
dist = -dist
else:
dist = 0.0
self.last_position = pose
if self.start_pose is None:
self.start_pose = pose
self.traveled_dist += dist
x, y, z = self.xyz
x += math.cos(self.pitch) * math.cos(self.yaw) * dist
y += math.cos(self.pitch) * math.sin(self.yaw) * dist
z += math.sin(self.pitch) * dist
self.bus.publish('pose2d', [
round(x * 1000),
round(y * 1000),
round(math.degrees(self.yaw) * 100)
])
self.xyz = x, y, z
self.trace.update_trace(self.xyz)
# pose3d
dist3d = quaternion.rotate_vector([dist, 0, 0],
self.orientation)
self.xyz_quat = [a + b for a, b in zip(self.xyz_quat, dist3d)]
self.bus.publish('pose3d', [self.xyz_quat, self.orientation])
elif channel == 'scan':
self.scan = data
if self.local_planner is not None:
self.local_planner.update(data)
elif channel == 'rot':
self.yaw, self.pitch, self.roll = [
math.radians(x / 100) for x in data
]
elif channel == 'orientation':
self.orientation = data
elif channel == 'sim_time_sec':
self.sim_time_sec = data
elif channel == 'acc':
acc = [x / 1000.0 for x in data]
gacc = np.matrix([[0., 0.,
9.80]]) # Gravitational acceleration.
cos_pitch = math.cos(self.pitch)
sin_pitch = math.sin(self.pitch)
# TODO: Once roll is correct, incorporate it here too.
egacc = np.matrix(
[ # Expected gravitational acceleration given known pitch.
[cos_pitch, 0., sin_pitch], [0., 1., 0.],
[-sin_pitch, 0., cos_pitch]
]) * gacc.T
cacc = np.asarray(
acc
) - egacc.T # Corrected acceleration (without gravitational acceleration).
magnitude = math.hypot(cacc[0, 0], cacc[0, 1])
if magnitude > 12.0:
print(self.time, 'Collision!', acc, 'reported:',
self.collision_detector_enabled)
if self.collision_detector_enabled:
self.collision_detector_enabled = False
raise Collision()
elif channel == 'artf':
artifact_data, deg_100th, dist_mm = data
x, y, z = self.xyz
angle, dist = self.yaw + math.radians(
deg_100th / 100.0), dist_mm / 1000.0
ax = x + math.cos(angle) * dist
ay = y + math.sin(angle) * dist
az = z
self.maybe_remember_artifact(artifact_data, (ax, ay, az))
elif channel == 'voltage':
self.voltage = data
return channel
def update(self):
packet = self.bus.listen()
if packet is not None:
# print('SubT', packet)
timestamp, channel, data = packet
if self.time is None or int(self.time.seconds)//60 != int(timestamp.seconds)//60:
print(timestamp, '(%.1f %.1f %.1f)' % self.xyz, sorted(self.stat.items()))
print(timestamp, list(('%.1f' % (v/100)) for v in self.voltage))
self.stat.clear()
self.time = timestamp
if not VIRTUAL_WORLD:
self.sim_time_sec = self.time.total_seconds()
self.stat[channel] += 1
if channel == 'pose2d':
x, y, heading = data
pose = (x/1000.0, y/1000.0, math.radians(heading/100.0))
if self.last_position is not None:
self.is_moving = (self.last_position != pose)
dist = math.hypot(pose[0] - self.last_position[0], pose[1] - self.last_position[1])
direction = ((pose[0] - self.last_position[0]) * math.cos(self.last_position[2]) +
(pose[1] - self.last_position[1]) * math.sin(self.last_position[2]))
if direction < 0:
dist = -dist
else:
dist = 0.0
self.last_position = pose
if self.start_pose is None:
self.start_pose = pose
self.traveled_dist += dist
x, y, z = self.xyz
x += math.cos(self.pitch) * math.cos(self.yaw) * dist
y += math.cos(self.pitch) * math.sin(self.yaw) * dist
z += math.sin(self.pitch) * dist
self.bus.publish('pose2d', [round(x*1000), round(y*1000),
round(math.degrees(self.yaw)*100)])
self.xyz = x, y, z
self.trace.update_trace(self.xyz)
# pose3d
dist3d = quaternion.rotate_vector([dist, 0, 0], self.orientation)
self.xyz_quat = [a+b for a, b in zip(self.xyz_quat, dist3d)]
self.bus.publish('pose3d', [self.xyz_quat, self.orientation])
elif channel == 'scan':
self.scan = data
if self.local_planner is not None:
self.local_planner.update(data)
elif channel == 'rot':
self.yaw, self.pitch, self.roll = [math.radians(x/100) for x in data]
elif channel == 'orientation':
self.orientation = data
elif channel == 'sim_time_sec':
self.sim_time_sec = data
elif channel == 'acc':
acc = [x/1000.0 for x in data]
gacc = np.matrix([[0., 0., 9.80]]) # Gravitational acceleration.
cos_pitch = math.cos(self.pitch)
sin_pitch = math.sin(self.pitch)
# TODO: Once roll is correct, incorporate it here too.
egacc = np.matrix([ # Expected gravitational acceleration given known pitch.
[ cos_pitch, 0., sin_pitch],
[ 0., 1., 0.],
[-sin_pitch, 0., cos_pitch]
]) * gacc.T
cacc = np.asarray(acc) - egacc.T # Corrected acceleration (without gravitational acceleration).
magnitude = math.hypot(cacc[0, 0], cacc[0, 1])
if magnitude > 12.0:
print(self.time, 'Collision!', acc, 'reported:', self.collision_detector_enabled)
if self.collision_detector_enabled:
self.collision_detector_enabled = False
raise Collision()
elif channel == 'artf':
artifact_data, deg_100th, dist_mm = data
x, y, z = self.xyz
angle, dist = self.yaw + math.radians(deg_100th/100.0), dist_mm/1000.0
ax = x + math.cos(angle) * dist
ay = y + math.sin(angle) * dist
az = z
self.maybe_remember_artifact(artifact_data, (ax, ay, az))
elif channel == 'voltage':
self.voltage = data
return channel