Add robot.RobotOrientation and handling

examples/events.py

@@ -63,6 +63,20 @@ def on_robot_wheels_moving(cli, state: bool):
  print("Stopped moving.")
 
 
+def on_robot_orientation_change(cli, orientation: pycozmo.robot.RobotOrientation):
+ del cli
+ if orientation == pycozmo.robot.RobotOrientation.ON_THREADS:
+ print("On threads.")
+ elif orientation == pycozmo.robot.RobotOrientation.ON_BACK:
+ print("On back.")
+ elif orientation == pycozmo.robot.RobotOrientation.ON_FACE:
+ print("On front.")
+ elif orientation == pycozmo.robot.RobotOrientation.ON_LEFT_SIDE:
+ print("On left side.")
+ elif orientation == pycozmo.robot.RobotOrientation.ON_RIGHT_SIDE:
+ print("On right side.")
+
+
 def pycozmo_program(cli: pycozmo.client.Client):
 
  cli.add_handler(pycozmo.protocol_encoder.RobotState, on_robot_state, one_shot=True)
@@ -74,6 +88,7 @@ def pycozmo_program(cli: pycozmo.client.Client):
  cli.add_handler(pycozmo.event.EvtRobotChargingChange, on_robot_charging)
  cli.add_handler(pycozmo.event.EvtCliffDetectedChange, on_cliff_detected)
  cli.add_handler(pycozmo.event.EvtRobotWheelsMovingChange, on_robot_wheels_moving)
+ cli.add_handler(pycozmo.event.EvtRobotOrientationChange, on_robot_orientation_change)
 
  while True:
  try:

pycozmo/anim_controller.py

@@ -87,6 +87,9 @@ def start(self):
  self.cli.add_handler(protocol_encoder.Keyframe, self._on_keyframe)
  self.cli.add_handler(protocol_encoder.AnimationStarted, self._on_animation_started)
  self.cli.add_handler(protocol_encoder.AnimationEnded, self._on_animation_ended)
+ self.cli.add_handler(event.EvtRobotAnimatingChange, self._on_animating_change)
+ self.cli.add_handler(event.EvtRobotAnimBufferFullChange, self._on_anim_buffer_full_change)
+ self.cli.add_handler(event.EvtRobotAnimatingIdleChange, self._on_amimating_idle_change)
 
  def stop(self):
  self.stop_flag = True
@@ -106,6 +109,15 @@ def _on_animation_ended(self, cli, pkt: protocol_encoder.AnimationEnded):
  self._clear_last_image_pkt()
  self.cli.conn.post_event(event.EvtAnimationCompleted, self.cli)
 
+ def _on_animating_change(self):
+ pass
+
+ def _on_anim_buffer_full_change(self):
+ pass
+
+ def _on_amimating_idle_change(self):
+ pass
+
  def _run(self):
  logger.debug("Animation controller started...")
 

pycozmo/brain.py

@@ -15,6 +15,7 @@
 from . import activity
 from . import anim
 from . import util
+from . import robot
 
 
 __all__ = [
@@ -44,6 +45,10 @@ def __init__(self, cli):
  logger.info("Loaded resources in {:.02f} s.".format(time.perf_counter() - start_time))
 
  self.cli.add_handler(event.EvtCliffDetectedChange, self.on_cliff_detected)
+ self.cli.add_handler(event.EvtRobotOrientationChange, self.on_robot_orientation_change)
+ self.cli.add_handler(event.EvtRobotPickedUpChange, self.on_robot_picked_up_change)
+ self.cli.add_handler(event.EvtRobotFallingChange, self.on_robot_falling_change)
+ self.cli.add_handler(event.EvtRobotOnChargerChange, self.on_robot_on_charger_change)
  # TODO: ...
 
  # Reaction trigger queue
@@ -70,6 +75,28 @@ def on_cliff_detected(self, cli, state: bool) -> None:
  if state:
  self.post_reaction("CliffDetected")
 
+ def on_robot_orientation_change(self, cli, orientation: robot.RobotOrientation) -> None:
+ if orientation == robot.RobotOrientation.ON_THREADS:
+ self.post_reaction("ReturnedToTreads")
+ elif orientation == robot.RobotOrientation.ON_BACK:
+ self.post_reaction("RobotOnBack")
+ elif orientation == robot.RobotOrientation.ON_FACE:
+ self.post_reaction("RobotOnFace")
+ elif orientation == robot.RobotOrientation.ON_LEFT_SIDE or orientation == robot.RobotOrientation.ON_RIGHT_SIDE:
+ self.post_reaction("RobotOnSide")
+
+ def on_robot_picked_up_change(self, cli, state: bool) -> None:
+ if state:
+ self.post_reaction("RobotPickedUp")
+
+ def on_robot_falling_change(self, cli, state: bool):
+ if state:
+ self.post_reaction("RobotFalling")
+
+ def on_robot_on_charger_change(self, cli, state: bool) -> None:
+ if state:
+ self.post_reaction("PlacedOnCharger")
+
  def on_camera_image(self, cli, new_im) -> None:
  """ Process images, coming from the robot camera. """
  # TODO: See cozmo_resources/config/engine/vision_config.json

pycozmo/client.py

@@ -70,6 +70,7 @@ def __init__(self,
  self.accel = util.Vector3(0.0, 0.0, 0.0)
  self.gyro = util.Vector3(0.0, 0.0, 0.0)
  self.robot_status = 0
+ self.robot_orientation = robot.RobotOrientation.ON_THREADS
  # Animation state
  self.num_anim_bytes_played = 0
  self.num_audio_frames_played = 0
@@ -287,6 +288,20 @@ def _on_robot_state(self, cli, pkt: protocol_encoder.RobotState):
  state = (pkt.status & flag) != 0
  logger.debug("%s: %i", robot.RobotStatusFlagNames[flag], state)
  self.dispatch(evt, self, state)
+ # Orientation
+ if pkt.pose_angle_rad < -0.4:
+ robot_orientation = robot.RobotOrientation.ON_LEFT_SIDE
+ elif pkt.pose_angle_rad > 0.4:
+ robot_orientation = robot.RobotOrientation.ON_RIGHT_SIDE
+ elif pkt.pose_pitch_rad < -1.0:
+ robot_orientation = robot.RobotOrientation.ON_FACE
+ elif pkt.pose_pitch_rad > 1.0:
+ robot_orientation = robot.RobotOrientation.ON_BACK
+ else:
+ robot_orientation = robot.RobotOrientation.ON_THREADS
+ if self.robot_orientation != robot_orientation:
+ self.robot_orientation = robot_orientation
+ self.dispatch(event.EvtRobotOrientationChange, self, robot_orientation)
 
  def _on_robot_picked_up(self, cli, state):
  del cli

pycozmo/event.py

@@ -37,6 +37,7 @@
  "EvtRobotWheelsMovingChange",
  "EvtChargerOOSChange",
  "EvtRobotStateUpdated",
+ "EvtRobotOrientationChange",
  "EvtAudioCompleted",
  "EvtAnimationCompleted",
 
@@ -175,6 +176,10 @@ class EvtRobotStateUpdated(Event):
  """ Triggered when a new robot state is received. """
 
 
+class EvtRobotOrientationChange(Event):
+ """ Triggered when the robot orientation changes. """
+
+
 class Dispatcher(object):
  """ Event dispatcher class. """
 

pycozmo/robot.py

@@ -4,6 +4,7 @@
 
 """
 
+import enum
 import math
 
 from . import util
@@ -103,6 +104,15 @@ class RobotStatusFlag(object):
 }
 
 
+class RobotOrientation(enum.Enum):
+ """ Robot orientation enumeration. """
+ ON_THREADS = 0
+ ON_BACK = 1
+ ON_FACE = 2
+ ON_LEFT_SIDE = 3
+ ON_RIGHT_SIDE = 4
+
+
 class LiftPosition(object):
  """
  Represents the position of Cozmo's lift.