def test_loop_with_small_rotation(self):
forward = euler_to_quaternion(0, 0, 0)
almost_forward = euler_to_quaternion(math.radians(10), 0, 0)
trajectory = [((0, 0, 0), forward),
((2, 0, 0), forward),
((2, 2, 0), forward),
((0, 2, 0), forward),
((0, 0, 0), almost_forward)]
detector = LoopDetector(min_loop_length=5)
detector.add_all(trajectory)
loop = detector.loop()
self.assertEqual(loop, trajectory)
def test_not_a_loop_different_direction(self):
forward = euler_to_quaternion(0, 0, 0)
left = euler_to_quaternion(math.pi/2, 0, 0)
trajectory = [((0, 0, 0), forward),
((2, 0, 0), forward),
((2, 2, 0), forward),
((0, 2, 0), forward),
((0, 0, 0), left)]
detector = LoopDetector(min_loop_length=5)
detector.add_all(trajectory)
loop = detector.loop()
self.assertIsNone(loop)
def __init__(self, config, bus):
super().__init__(config, bus)
self.devices = config.get('devices')
self.depth_subsample = config.get('depth_subsample', 3)
self.depth_rgb = config.get('depth_rgb', False)
self.depth_infra = config.get('depth_infra', False)
self.default_depth_resolution = config.get('depth_resolution',
[640, 360])
self.default_rgb_resolution = config.get('rgb_resolution', [640, 360])
self.depth_fps = config.get('depth_fps', 30)
self.pose_subsample = config.get('pose_subsample', 20)
self.tracking_sensor_pitch_quat_inv = {}
self.tracking_sensor_yaw_quat = {}
streams = []
for device in self.devices:
device_type = device.get('type')
serial_number = device.get('serial_number')
if not serial_number:
g_logger.info(
'Serial number is not set. Use rs-enumerate-devices tool to get camera information.'
)
# https://github.com/IntelRealSense/librealsense/tree/master/tools/enumerate-devices
continue
name = device.get('name', serial_number)
if device_type == 'T200':
for stream in ['pose2d', 'pose3d', 'pose_raw', 'orientation']:
streams.append(f'{name}_{stream}')
self.tracking_sensor_pitch_quat_inv[name] = quaternion_inv(
euler_to_quaternion(0, math.radians(device.get('pitch',
0)), 0))
self.tracking_sensor_yaw_quat[name] = euler_to_quaternion(
math.radians(device.get('yaw', 0)), 0, 0)
elif device_type in ['D400', 'L500']:
for stream in ['depth:gz', 'color', 'infra']:
streams.append(f'{name}_{stream}')
if self.depth_rgb or self.depth_infra:
import cv2
global cv2
else:
g_logger.warning('Device is not specified in the config!')
bus.register(*streams)
self.pipelines = []
self.finished = None
self.origins_lock = threading.Lock()
self.origins = {}
def on_odo_pose(self, data):
x, y, heading = data
pose = (x / 1000.0, y / 1000.0, math.radians(heading / 100.0)
) # TODO: use IMU instead of wheels
if self.last_position is not None:
dist = distance(pose, self.last_position)
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.tf['odo']['latest_xyz']
x += math.cos(self.pitch) * math.cos(pose[2]) * dist
y += math.cos(self.pitch) * math.sin(pose[2]) * dist
z += math.sin(self.pitch) * dist
self.tf['odo']['latest_xyz'] = x, y, z
self.tf['odo']['latest_quat'] = euler_to_quaternion(
self.yaw, self.pitch, self.roll)
self.tf['odo']['timestamp'] = self.sim_time
self.calculate_best_pose()
def test_not_a_loop_too_far_horizontally(self):
forward = euler_to_quaternion(0, 0, 0)
trajectory = [((0, 0, 0), forward),
((2, 0, 0), forward),
((2, 2, 0), forward),
((0, 2, 0), forward)]
detector = LoopDetector(min_loop_length=5)
detector.add_all(trajectory)
loop = detector.loop()
self.assertIsNone(loop)
def test_perfect_loop(self):
forward = euler_to_quaternion(0, 0, 0)
trajectory = [((0, 0, 0), forward),
((2, 0, 0), forward),
((2, 2, 0), forward),
((0, 2, 0), forward),
((0, 0, 0), forward)]
detector = LoopDetector(min_loop_length=5)
detector.add_all(trajectory)
loop = detector.loop()
self.assertEqual(loop, trajectory)
def __init__(self, config, bus):
super().__init__(config, bus)
self.device = config.get('device')
if self.device == 'T200':
bus.register('pose2d', 'pose3d', 'pose_raw', 'orientation')
self.pose_subsample = config.get("pose_subsample", 20)
self.tracking_sensor_pitch_quat_inv = quaternion_inv(
euler_to_quaternion(
0, math.radians(config.get('tracking_sensor_pitch', 0)),
0))
self.tracking_sensor_yaw_quat = euler_to_quaternion(
math.radians(config.get('tracking_sensor_yaw', 0)), 0, 0)
elif self.device in ['D400', 'L500']:
bus.register('depth:gz', 'color', 'infra')
self.depth_subsample = config.get("depth_subsample", 3)
self.depth_rgb = config.get("depth_rgb", False)
self.depth_infra = config.get("depth_infra", False)
self.default_depth_resolution = config.get("depth_resolution",
[640, 360])
self.default_rgb_resolution = config.get("rgb_resolution",
[640, 360])
self.depth_fps = config.get("depth_fps", 30)
self.disable_emitor = config.get("disable_emitor", False)
if self.depth_rgb or self.depth_infra:
import cv2
global cv2
else:
g_logger.warning("Device is not specified in the config!")
self.serial_number = config.get('serial_number')
if not self.serial_number:
g_logger.info(
"Serial number is not set. Use rs-enumerate-devices tool to get camera information."
)
# https://github.com/IntelRealSense/librealsense/tree/master/tools/enumerate-devices
self.verbose = config.get('verbose', False)
self.pose_pipeline = None # not initialized yet
self.depth_pipeline = None
self.finished = None
def test_granularity(self):
forward = euler_to_quaternion(0, 0, 0)
trajectory = [((0, 0, 0), forward),
((1, 0, 0), forward),
((2, 0, 0), forward),
((2, 1, 0), forward),
((2, 2, 0), forward),
((1, 2, 0), forward),
((0, 2, 0), forward),
((0, 1, 0), forward),
((0, 0, 0), forward)]
detector = LoopDetector(min_loop_length=5, granularity=2.0)
detector.add_all(trajectory)
loop = detector.loop()
self.assertEqual(loop, trajectory[::2])
def __init__(self, config, bus):
super().__init__(config, bus)
self.rgb_params = config['rgb']
self.depth_params = config['depth']
self.extrinsic_params = np.asarray(
config.get('extrinsics',
[[1.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0],
[0.0, 0.0, 1.0, 0.0]]))
camera_config = config.get('camera')
serialization_method = config.get('serialization', 'compressed')
assert (serialization_method in ['compressed',
'raw']), serialization_method
self.serialization = compress if serialization_method == 'compressed' else serialize
if camera_config is not None:
self.camera_pose = (
camera_config['xyz'],
euler_to_quaternion(
*[math.radians(x) for x in camera_config['ypr']]))
else:
self.camera_pose = None
self.robot_pose = None
self.img = None
self.depth = None
bus.register('rgbd:null', 'dropped')
# Pixel coordinates relative to the center of the image, with positive
# directions to the right and up.
pxs = np.repeat(np.arange(self.depth_params['w']).reshape(
(1, self.depth_params['w'])),
self.depth_params['h'],
axis=0) - self.depth_params['cx']
pys = self.depth_params['cy'] - np.repeat(np.arange(
self.depth_params['h']).reshape((self.depth_params['h'], 1)),
self.depth_params['w'],
axis=1)
pzs = np.ones((self.depth_params['h'], self.depth_params['w']),
dtype=np.float)
# For each pixel in the image, a vector representing its corresponding
# direction in the depth scene with a unit forward axis.
self.ps = np.dstack([
pxs / self.depth_params['fx'], pys / self.depth_params['fy'], pzs
])
self.ones = np.ones((self.depth_params['h'], self.depth_params['w']))
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 process_packet(self, packet, verbose=False):
msg_id, payload, flags = packet
if msg_id == CAN_ID_BUTTONS:
self.update_buttons(payload)
elif msg_id == CAN_ID_CURRENT:
# expected 24bit integer miliAmps
if len(payload) == 3:
current = struct.unpack('>i', b'\x00' + payload)[0]
# print(current)
else:
self.can_errors += 1
elif msg_id == CAN_ID_JOINT_ANGLE:
if len(payload) == 4:
if self.num_axis == 3:
analog_joint_angle, analog_joint_angle2 = struct.unpack(
'>hh', payload)
# print(self.last_joint_angle,payload)
# print(self.last_joint_angle, self.last_joint_angle2)
self.last_joint_angle, self.last_joint_angle2 = joint_rad(
analog_joint_angle), joint_rad(analog_joint_angle2,
second_ang=True)
self.publish('joint_angle', [
round(math.degrees(self.last_joint_angle) * 100),
round(math.degrees(self.last_joint_angle2) * 100)
])
self.publish(
'joint_angle_pose',
[[[0, 0, 0],
euler_to_quaternion(math.pi - self.last_joint_angle,
0, 0)],
[[0, 0, 0],
euler_to_quaternion(math.pi - self.last_joint_angle2,
0, 0)]])
else:
analog_joint_angle = struct.unpack('>i', payload)[0]
# print(self.last_joint_angle,payload)
# print(self.last_joint_angle)
self.last_joint_angle = joint_rad(analog_joint_angle)
self.publish(
'joint_angle',
[round(math.degrees(self.last_joint_angle) * 100)])
self.publish('joint_angle_pose',
[[0, 0, 0],
euler_to_quaternion(
math.pi - self.last_joint_angle, 0, 0)])
else:
self.can_errors += 1
elif msg_id == CAN_ID_VOLTAGE:
if len(payload) == 4:
self.voltage = struct.unpack_from('>i', payload)[0]
# print(self.voltage)
else:
self.can_errors += 1
elif msg_id == CAN_ID_DOWNDROPS_FRONT:
# assert len(packet) == 2 + 4, len(packet)
if len(payload) == 4:
self.downdrops_front = struct.unpack_from('>HH', payload)
self.publish('downdrops_front', list(self.downdrops_front))
prev = self.bumpers_front
self.bumpers_front = get_downdrop_bumpers(self.downdrops_front)
if prev != self.bumpers_front:
self.bumpers_front_counter = 0 # reset on change
self.bumpers_front_counter += 1
if self.bumpers_front_counter == DOWNDROP_STABLE_COUNT: # report only "slightly stable" values
self.publish('bumpers_front', self.bumpers_front)
# print(self.time, self.downdrops_front)
if self.verbose:
self.debug_downdrops_front.append(
(self.time.total_seconds(), self.downdrops_front[0],
self.downdrops_front[1]))
else:
self.can_errors += 1
elif msg_id == CAN_ID_DOWNDROPS_REAR:
# assert len(packet) == 2 + 4, len(packet)
if len(payload) == 4:
self.downdrops_rear = struct.unpack_from('>HH', payload)
self.publish('downdrops_rear', list(self.downdrops_rear))
prev = self.bumpers_rear
self.bumpers_rear = get_downdrop_bumpers(self.downdrops_rear)
if prev != self.bumpers_rear:
self.bumpers_rear_counter = 0 # reset on change
self.bumpers_rear_counter += 1
if self.bumpers_rear_counter == DOWNDROP_STABLE_COUNT: # report only "slightly stable" values
self.publish('bumpers_rear', self.bumpers_rear)
# print(self.time, self.downdrops_rear)
if self.verbose:
self.debug_downdrops_rear.append(
(self.time.total_seconds(), self.downdrops_rear[0],
self.downdrops_rear[1]))
else:
self.can_errors += 1
elif msg_id in [
CAN_ID_ENCODERS, CAN_ID_VESC_FRONT_L, CAN_ID_VESC_FRONT_R,
CAN_ID_VESC_REAR_L, CAN_ID_VESC_REAR_R, CAN_ID_VESC_REAR_K3_R,
CAN_ID_VESC_REAR_K3_L
]:
if self.update_encoders(msg_id, payload) is not None:
diff = [
e - prev
for e, prev in zip(self.encoders, self.last_pose_encoders)
]
# note, that this craziness is necessary only because the motor indexes
# are assigned first right and then left
if len(diff) == 4:
self.publish('encoders', [
diff[INDEX_FRONT_LEFT], diff[INDEX_FRONT_RIGHT],
diff[INDEX_REAR_LEFT], diff[INDEX_REAR_RIGHT]
])
else:
assert len(diff) == 6, len(diff) # Kloubak K3
self.publish('encoders', [
diff[INDEX_FRONT_LEFT], diff[INDEX_FRONT_RIGHT],
diff[INDEX_REAR_LEFT], diff[INDEX_REAR_RIGHT],
diff[INDEX_REAR_K3_LEFT], diff[INDEX_REAR_K3_RIGHT]
])
if self.update_pose():
self.send_pose()
return True
return False