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 test_min_dist(self):
bus = MagicMock()
logimage = ImageExtractor(bus=bus, config={})
robot_pose = [[0, 0, 0], [0, 0, 0, 1]]
angle = quaternion.euler_to_quaternion(yaw=math.radians(20),
roll=0,
pitch=0)
camera_pose = None
rgb_compressed = b'dummy image'
depth_compressed = None
bus.reset_mock()
for i in range(10):
data = robot_pose, camera_pose, rgb_compressed, depth_compressed
logimage.on_rgbd(data)
robot_pose = [
robot_pose[0],
quaternion.multiply(robot_pose[1], angle)
]
self.assertEqual(len(bus.method_calls), 10)
def parse_packet(self, data):
"""
Parse cortexpilot sensors status message
"""
# expects already validated single sample with 3 bytes length prefix
# and checksum at the end
high, mid, low = data[:3]
assert high == 0, high # fixed packet size 2*256+89 bytes
assert mid == 2, mid
assert low == 89, low
addr, cmd = data[3:5]
assert addr == 1, addr
assert cmd == 0xD, cmd
offset = 5 # payload offset
# 4 byte Flags (unsigned long) 0
# bit 0 -> 1 = BigRedSwitch
# bit 1 -> 1 = MissionButton
# bit 2 -> copy of EnableRun flag (motors enabled)
# bit 3 -> 1 = Payload loaded, 0 = Payload unloaded - payload indicator
# bit 4 -> 1 = LIDAR ScanValid
# bit 5 -> 1 = Manual override
# 4 byte SystemVoltage (float) 4 - battery level for control electronics [V]
# 4 byte PowerVoltage (float) 8 - battery level for motors [V]
self.flags, system_voltage, power_voltage = struct.unpack_from(
'<Iff', data, offset)
self.lidar_valid = (self.flags & 0x10) == 0x10
self.emergency_stop = (self.flags & 0x01) == 0x01
self.voltage = [system_voltage, power_voltage]
self.bus.publish('voltage', [int(v * 100) for v in self.voltage])
# skipped parsing of:
# 4 byte SpeedM1 (float) 12 - normalized motor M1 (R) speed <-1.0 1.0>
# 4 byte SpeedM2 (float) 16 - normalized motor M2 (L) speed <-1.0 1.0>
motors = struct.unpack_from('<ff', data, offset + 12)
# skipped parsing of:
# 4 byte ActualDir (float) 20 - normalized direction for PID controller
# 4 byte EncM1 (signed long) 24 - incremental encoders count for motor M1 (R) since last reset
# 4 byte EncM2 (signed long) 28 - incremental encoders count for motor M2 (L) since last reset
encoders = struct.unpack_from('<II', data, offset + 24)
# skipped parsing of:
# 1 byte GPS_Valid 32 - 1 = valid data from GPS module
# 1 byte GPS_Fix 33 - 0 = no fix, 1 = 2D fix, 2 = 3D fix
# 4 byte GPS_UTCDate (ulong) 34 - GPS date in YYMMDD format
# 4 byte GPS_UTCTime (ulong) 38 - GPS time in HHMMSS format
# 4 byte GPS_Lat (ulong) 42 - format Lat * 1E7
# 4 byte GPS_Lon (ulong) 46 - format Lon * 1E7
# 4 byte GPS_Brg (float) 50 - GPS Bearing <0 .. 359> deg
# 4 byte AHRS_q0 (float) 54 - Orientation Quaternion
# 4 byte AHRS_q1 (float) 58 -
# 4 byte AHRS_q2 (float) 62 -
# 4 byte AHRS_q3 (float) 66 -
qw, qx, qy, qz = struct.unpack_from('<ffff', data, offset + 54)
orientation = qx, qy, qz, qw
# identity quat points to north, we need it to point to east
orientation = quaternion.multiply(orientation,
[0, 0, 0.7071068, 0.7071068])
# correct roll axis by 1.7 degrees
self.orientation = quaternion.multiply(orientation,
[0.0148348, 0, 0, 0.99989])
self.bus.publish('orientation', list(self.orientation))
q1, q2, q3, q0 = self.orientation # quaternion
#print(self.time, "{:.4f} {:.4f} {:.4f} {:.4f}".format(q0, q1, q2, q3))
ax = math.atan2(2 * (q0 * q1 + q2 * q3), 1 - 2 * (q1 * q1 + q2 * q2))
ay = math.asin(2 * (q0 * q2 - q3 * q1))
az = math.atan2(2 * (q0 * q3 + q1 * q2), 1 - 2 * (q2 * q2 + q3 * q3))
# rotation Euler angles are yaw, pitch and roll
#print(self.time, "{:.4f} {:.4f} {:.4f}".format(math.degrees(az), math.degrees(ay), math.degrees(ax)))
self.bus.publish(
'rotation',
[round(math.degrees(angle) * 100) for angle in [az, ay, ax]])
# 4 byte Yaw (float) 70 - Heading (Yaw) - machine orientation to magnetic north <0 .. 359> deg
self.yaw = struct.unpack_from('<f', data, offset + 70)[0]
#print(math.degrees(x), math.degrees(y), math.degrees(z), self.yaw)
# 4 byte AccelX (float) 74
# 4 byte AccelY (float) 78
# 4 byte AccelZ (float) 82
# 4 byte GyroX (float) 86
# 4 byte GyroY (float) 90
# 4 byte GyroZ (float) 94
# 4 byte MagX (float) 98
# 4 byte MagY (float) 102
# 4 byte MagZ (float) 106
# 4 byte SystemTick (ulong) 110 - Uptime in milisecond
uptime = struct.unpack_from('<I', data, offset + 110)[0]
if self.uptime is not None:
uptime_diff = uptime - self.uptime
self.uptime = uptime
if self.last_encoders is not None:
step = [
sint32_diff(x, prev)
for x, prev in zip(encoders, self.last_encoders)
]
self.publish('encoders', step)
dist = ENC_SCALE * sum(step) / len(step)
angle = ENC_SCALE * (step[0] - step[1]) / WHEEL_DISTANCE
x, y, heading = self.pose
# advance robot by given distance and angle
if abs(angle) < 0.0000001: # EPS
# Straight movement - a special case
x += dist * math.cos(heading)
y += dist * math.sin(heading)
#Not needed: heading += angle
else:
# Arc
r = dist / angle
x += -r * math.sin(heading) + r * math.sin(heading + angle)
y += +r * math.cos(heading) - r * math.cos(heading + angle)
heading += angle # not normalized
self.pose = (x, y, heading)
self.send_pose()
self.last_encoders = encoders
# 4 byte LidarTimestamp (ulong) 114 - Value of SystemTick when lidar scan was received
lidar_timestamp = struct.unpack_from('<I', data, offset + 114)[0]
lidar_diff = lidar_timestamp - self.lidar_timestamp
self.lidar_timestamp = lidar_timestamp
if lidar_diff > 150 and self.lidar_valid:
print(self.time, "lidar invalid:", lidar_diff)
self.lidar_valid = False
if lidar_diff != 0 and self.lidar_valid:
# laser
# 480 byte Lidar_Scan (ushort) 118 - 239 two-bytes distances from Lidar <0 .. 65535> in [cm]
# Scan is whole 360 deg with resolution 1.5 deg
scan = struct.unpack_from('<' + 'H' * 239, data,
offset + 118) # TODO should be 240
# restrict scan only to 270 degrees - cut 1/8th on both sides
# scan = scan[30:-30]
# zero_sides = 20
scan = [10 * d for d in reversed(scan)] # scale to millimeters
# scan[:zero_sides] = [0]*zero_sides
# scan[-zero_sides:] = [0]*zero_sides
self.publish('scan', scan)
def parse_packet(self, data):
"""
Parse cortexpilot sensors status message
"""
# expects already validated single sample with 3 bytes length prefix
# and checksum at the end
high, mid, low = data[:3]
assert high == 0, high # fixed packet size 2*256+89 bytes
assert mid == 2, mid
assert low == 89, low
addr, cmd = data[3:5]
assert addr == 1, addr
assert cmd == 0xD, cmd
offset = 5 # payload offset
# 4 byte Flags (unsigned long) 0
# bit 0 -> 1 = BigRedSwitch
# bit 1 -> 1 = MissionButton
# bit 2 -> copy of EnableRun flag (motors enabled)
# bit 3 -> 1 = Payload loaded, 0 = Payload unloaded - payload indicator
# bit 4 -> 1 = LIDAR ScanValid
# bit 5 -> 1 = Manual override
# 4 byte SystemVoltage (float) 4 - battery level for control electronics [V]
# 4 byte PowerVoltage (float) 8 - battery level for motors [V]
self.flags, system_voltage, power_voltage = struct.unpack_from('<Iff', data, offset)
self.lidar_valid = (self.flags & 0x10) == 0x10
self.emergency_stop = (self.flags & 0x01) == 0x01
self.voltage = [system_voltage, power_voltage]
self.bus.publish('voltage', [int(v*100) for v in self.voltage])
# skipped parsing of:
# 4 byte SpeedM1 (float) 12 - normalized motor M1 (R) speed <-1.0 1.0>
# 4 byte SpeedM2 (float) 16 - normalized motor M2 (L) speed <-1.0 1.0>
motors = struct.unpack_from('<ff', data, offset + 12)
# skipped parsing of:
# 4 byte ActualDir (float) 20 - normalized direction for PID controller
# 4 byte EncM1 (signed long) 24 - incremental encoders count for motor M1 (R) since last reset
# 4 byte EncM2 (signed long) 28 - incremental encoders count for motor M2 (L) since last reset
encoders = struct.unpack_from('<II', data, offset + 24)
# skipped parsing of:
# 1 byte GPS_Valid 32 - 1 = valid data from GPS module
# 1 byte GPS_Fix 33 - 0 = no fix, 1 = 2D fix, 2 = 3D fix
# 4 byte GPS_UTCDate (ulong) 34 - GPS date in YYMMDD format
# 4 byte GPS_UTCTime (ulong) 38 - GPS time in HHMMSS format
# 4 byte GPS_Lat (ulong) 42 - format Lat * 1E7
# 4 byte GPS_Lon (ulong) 46 - format Lon * 1E7
# 4 byte GPS_Brg (float) 50 - GPS Bearing <0 .. 359> deg
# 4 byte AHRS_q0 (float) 54 - Orientation Quaternion
# 4 byte AHRS_q1 (float) 58 -
# 4 byte AHRS_q2 (float) 62 -
# 4 byte AHRS_q3 (float) 66 -
orientation = struct.unpack_from('<ffff', data, offset+54)
# identity quat points to north, we need it to point to east
orientation = quaternion.multiply(orientation, [0.7071068, 0, 0, 0.7071068])
# correct roll axis by 1.7 degrees
self.orientation = quaternion.multiply(orientation, [0.99989, 0.0148348, 0, 0, ])
self.bus.publish('orientation', list(self.orientation))
q0, q1, q2, q3 = self.orientation # quaternion
#print(self.time, "{:.4f} {:.4f} {:.4f} {:.4f}".format(q0, q1, q2, q3))
ax = math.atan2(2*(q0*q1+q2*q3), 1-2*(q1*q1+q2*q2))
ay = math.asin(2*(q0*q2-q3*q1))
az = math.atan2(2*(q0*q3+q1*q2), 1-2*(q2*q2+q3*q3))
# rotation Euler angles are yaw, pitch and roll
#print(self.time, "{:.4f} {:.4f} {:.4f}".format(math.degrees(az), math.degrees(ay), math.degrees(ax)))
self.bus.publish('rotation', [round(math.degrees(angle)*100) for angle in [az, ay, ax]])
# 4 byte Yaw (float) 70 - Heading (Yaw) - machine orientation to magnetic north <0 .. 359> deg
self.yaw = struct.unpack_from('<f', data, offset + 70)[0]
#print(math.degrees(x), math.degrees(y), math.degrees(z), self.yaw)
# 4 byte AccelX (float) 74
# 4 byte AccelY (float) 78
# 4 byte AccelZ (float) 82
# 4 byte GyroX (float) 86
# 4 byte GyroY (float) 90
# 4 byte GyroZ (float) 94
# 4 byte MagX (float) 98
# 4 byte MagY (float) 102
# 4 byte MagZ (float) 106
# 4 byte SystemTick (ulong) 110 - Uptime in milisecond
uptime = struct.unpack_from('<I', data, offset + 110)[0]
if self.uptime is not None:
uptime_diff = uptime - self.uptime
self.uptime = uptime
if self.last_encoders is not None:
step = [sint32_diff(x, prev) for x, prev in zip(encoders, self.last_encoders)]
self.publish('encoders', step)
dist = ENC_SCALE * sum(step)/len(step)
angle = ENC_SCALE * (step[0] - step[1])/WHEEL_DISTANCE
x, y, heading = self.pose
# advance robot by given distance and angle
if abs(angle) < 0.0000001: # EPS
# Straight movement - a special case
x += dist * math.cos(heading)
y += dist * math.sin(heading)
#Not needed: heading += angle
else:
# Arc
r = dist / angle
x += -r * math.sin(heading) + r * math.sin(heading + angle)
y += +r * math.cos(heading) - r * math.cos(heading + angle)
heading += angle # not normalized
self.pose = (x, y, heading)
self.send_pose()
self.last_encoders = encoders
# 4 byte LidarTimestamp (ulong) 114 - Value of SystemTick when lidar scan was received
lidar_timestamp = struct.unpack_from('<I', data, offset + 114)[0]
lidar_diff = lidar_timestamp - self.lidar_timestamp
self.lidar_timestamp = lidar_timestamp
if lidar_diff > 150 and self.lidar_valid:
print(self.time, "lidar invalid:", lidar_diff)
self.lidar_valid = False
if lidar_diff != 0 and self.lidar_valid:
# laser
# 480 byte Lidar_Scan (ushort) 118 - 239 two-bytes distances from Lidar <0 .. 65535> in [cm]
# Scan is whole 360 deg with resolution 1.5 deg
scan = struct.unpack_from('<' + 'H'*239, data, offset + 118) # TODO should be 240
# restrict scan only to 270 degrees - cut 1/8th on both sides
scan = scan[30:-30]
zero_sides = 20
scan = [10 * d for d in reversed(scan)] # scale to millimeters
scan[:zero_sides] = [0]*zero_sides
scan[-zero_sides:] = [0]*zero_sides
self.publish('scan', scan)