def get_bounding_box(self):
sx, sy, sz = self.size
pts = np.array([[0, 0, sx, sx], [-sy / 2, sy / 2, sy / 2, -sy / 2],
[0, 0, 0, 0], [1, 1, 1, 1]])
pts = geometry.aboutZ(self.theta).dot(pts)
pts = geometry.translate(self.x, self.y).dot(pts)
mins = pts.min(1)
maxs = pts.max(1)
xmin = mins[0]
ymin = mins[1]
xmax = maxs[0]
ymax = maxs[1]
return ((xmin, ymin), (xmax, ymax))
def update_walls(self):
for key, value in self.robot.world.particle_filter.sensor_model.landmarks.items(
):
if key.startswith('Wall-'):
if key in self.objects:
wall = self.objects[key]
if not wall.is_fixed and not wall.is_foreign:
wall.update(self,
x=value[0][0][0],
y=value[0][1][0],
theta=value[1])
else:
print('Creating new wall in worldmap:', key)
wall_spec = wall_marker_dict[key]
wall = WallObj(id=key,
x=value[0][0][0],
y=value[0][1][0],
theta=value[1],
length=wall_spec.length,
height=wall_spec.height,
door_width=wall_spec.door_width,
door_height=wall_spec.door_height,
marker_specs=wall_spec.marker_specs,
doorways=wall_spec.doorways,
door_ids=wall_spec.door_ids,
is_foreign=False,
spec_id=key)
self.objects[key] = wall
wall.pose_confidence = +1
# Make the doorways
wall.make_doorways(self.robot.world.world_map)
# Relocate the aruco markers to their predefined positions
spec = wall_marker_dict.get(wall.id, None)
if spec is None: return
for key, value in spec.marker_specs.items():
if key in self.robot.world.world_map.objects:
aruco_marker = self.robot.world.world_map.objects[key]
dir = value[0] # +1 for front side or -1 for back side
s = 0 if dir == +1 else pi
aruco_marker.theta = wrap_angle(wall.theta + s)
wall_xyz = geometry.point(
-dir * (wall.length / 2 - value[1][0]), 0,
value[1][1])
rel_xyz = geometry.aboutZ(aruco_marker.theta +
pi / 2).dot(wall_xyz)
aruco_marker.x = wall.x + rel_xyz[0][0]
aruco_marker.y = wall.y + rel_xyz[1][0]
aruco_marker.z = rel_xyz[2][0]
aruco_marker.is_fixed = wall.is_fixed
def make_arucos(self, world_map):
"Called by add_fixed_landmark to make fixed aruco markers."
for key, value in self.marker_specs.items():
# Project marker onto the wall; move marker if it already exists
marker = world_map.objects.get(key, None)
if marker is None:
marker_number = int(key[1 + key.rfind('-'):])
marker = ArucoMarkerObj(world_map.robot.world.aruco,
marker_number=marker_number)
world_map.objects[marker.id] = marker
wall_xyz = geometry.point(self.length / 2 - value[1][0], 0,
value[1][1])
s = 0 if value[0] == +1 else pi
rel_xyz = geometry.aboutZ(self.theta + s).dot(wall_xyz)
marker.x = self.x + rel_xyz[1][0]
marker.y = self.y + rel_xyz[0][0]
marker.z = rel_xyz[2][0]
marker.theta = wrap_angle(self.theta + s)
marker.is_fixed = self.is_fixed
def update_charger(self):
charger = self.robot.world.charger
if charger is None: return
charger_id = 'Charger'
wmobject = self.objects.get(charger_id, None)
if wmobject is None:
wmobject = ChargerObj(charger)
self.objects[charger_id] = wmobject
wmobject.sdk_obj = charger # In case we created charger before seeing it
if self.robot.is_on_charger:
wmobject.update_from_sdk = False
theta = wrap_angle(self.robot.world.particle_filter.pose[2] + pi)
charger_offset = np.array([[-30], [0], [0], [1]])
offset = geometry.aboutZ(theta).dot(charger_offset)
wmobject.x = self.robot.world.particle_filter.pose[0] + offset[0,
0]
wmobject.y = self.robot.world.particle_filter.pose[1] + offset[1,
0]
wmobject.theta = theta
wmobject.pose_confidence = +1
elif charger.is_visible:
wmobject.update_from_sdk = True
wmobject.pose_confidence = +1
elif ((charger.pose is None)
or not charger.pose.is_comparable(self.robot.pose)):
wmobject.update_from_sdk = False
wmobject.pose_confidence = -1
else: # Robot re-localized so charger pose came back
pass # skip for now due to SDK bug
# wmobject.update_from_sdk = True
# wmobject.pose_confidence = max(0, wmobject.pose_confidence)
if wmobject.update_from_sdk: # True unless pose isn't comparable
self.update_coords_from_sdk(wmobject, charger)
wmobject.orientation, _, _, wmobject.theta = get_orientation_state(
charger.pose.rotation.q0_q1_q2_q3)
return wmobject
def world_joint(self):
return geometry.translate(self.q[0],
self.q[1]).dot(geometry.aboutZ(self.q[2]))
def revolute(self):
return geometry.aboutZ(-self.q)