def update_custom_object(self, sdk_obj):
if not sdk_obj.pose.is_comparable(self.robot.pose):
print('Should never get here:', sdk_obj.pose, self.robot.pose)
return
id = 'CustomMarkerObj-' + str(sdk_obj.object_type.name[-2:])
if not sdk_obj.is_unique:
id += '-' + str(sdk_obj.object_id)
if id in self.objects:
wmobject = self.objects[id]
wmobject.sdk_obj = sdk_obj # In case created marker before seeing it
else:
type = sdk_obj.object_type
if type in custom_objs.custom_marker_types:
wmobject = CustomMarkerObj(sdk_obj, id)
elif type in custom_objs.custom_cube_types:
wmobject = CustomCubeObj(sdk_obj, id)
else: # if we don't know what else to do with it, treat as a custom marker
wmobject = CustomMarkerObj(sdk_obj, id)
self.objects[id] = wmobject
wmobject.pose_confidence = +1
self.update_coords_from_sdk(wmobject, sdk_obj)
if isinstance(wmobject, CustomMarkerObj):
wmobject.orientation, wmobject.rotation, _, _ = \
get_orientation_state(sdk_obj.pose.rotation.q0_q1_q2_q3, isPlanar=True)
elif isinstance(wmobject, CustomCubeObj):
wmobject.orientation, _, _, wmobject.rotation = \
get_orientation_state(sdk_obj.pose.rotation.q0_q1_q2_q3, isPlanar=False)
def __init__(self, sdk_obj, id=None, x=0, y=0, z=0, theta=0, rotation=0):
# 'theta' is orientation relative to North in particle filter reference frame
# 'rotation' is orientation relative to "up" in the camera image
if id is None:
custom_type = sdk_obj.object_type.name[-2:]
id = 'CustomMarkerObj-' + str(custom_type)
super().__init__(id, x, y, z)
self.theta = wrap_angle(theta)
self.sdk_obj = sdk_obj
self.marker_number = int(id[-2:])
self.size = self.custom_marker_size
if self.sdk_obj:
self.orientation, self.rotation, _, _ = \
get_orientation_state(self.sdk_obj.pose.rotation.q0_q1_q2_q3, True)
else:
self.rotation = wrap_angle(rotation)
if abs(self.rotation) < 0.1:
self.orientation = geometry.ORIENTATION_UPRIGHT
elif abs(self.rotation - pi / 2) < 0.1:
self.orientation = geometry.ORIENTATION_LEFT
elif abs(self.rotation + pi / 2) < 0.1:
self.orientation = geometry.ORIENTATION_RIGHT
elif abs(wrap_angle(self.rotation + pi)) < 0.1:
self.orientation = geometry.ORIENTATION_INVERTED
else:
self.orientation = geometry.ORIENTATION_TILTED
def __init__(self, sdk_obj, id=None, x=0, y=0, z=0, theta=0):
if id is None:
id = 'Charger'
super().__init__(id, x, y, z)
self.sdk_obj = sdk_obj
if sdk_obj:
self.sdk_obj.wm_obj = self
self.update_from_sdk = True
self.orientation = geometry.ORIENTATION_UPRIGHT
self.theta = theta
self.size = self.charger_size
if self.sdk_obj and self.sdk_obj.pose:
self.orientation, _, _, self.theta = get_orientation_state(
self.sdk_obj.pose.rotation.q0_q1_q2_q3)
def update_cube(self, cube):
cube_id = 'Cube-' + str(cube.cube_id)
if cube_id in self.objects:
foreign_id = "LightCubeForeignObj-" + str(cube.cube_id)
if foreign_id in self.objects:
# remove foreign cube when local cube seen
del self.objects[foreign_id]
wmobject = self.objects[cube_id]
wmobject.sdk_obj = cube # In case created before seen
if self.robot.carrying is wmobject:
if cube.is_visible: # we thought we were carrying it, but we're wrong
self.robot.carrying = None
return self.update_cube(cube)
else: # we do appear to be carrying it
self.update_carried_object(wmobject)
elif cube.pose is None: # not in contact with cube
return None
else:
# Cube is not in the worldmap, so add it.
wmobject = LightCubeObj(cube)
self.objects[cube_id] = wmobject
if cube.is_visible:
wmobject.update_from_sdk = True # In case we've just dropped it; now we see it
wmobject.pose_confidence = +1
elif (cube.pose is None):
return wmobject
elif wmobject.update_from_sdk and not cube.pose.is_comparable(
self.robot.pose): # Robot picked up or cube moved
if (self.robot.fetching and self.robot.fetching.sdk_obj is cube) or \
(self.robot.carrying and self.robot.carrying.sdk_obj is cube):
pass
else:
wmobject.pose_confidence = -1
return wmobject
else: # Robot re-localized so cube 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 if we've dropped it and haven't seen it yet
self.update_coords_from_sdk(wmobject, cube)
wmobject.orientation, _, _, wmobject.theta = get_orientation_state(
cube.pose.rotation.q0_q1_q2_q3)
return wmobject
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