From bf3621517c5a3de7ed0266d39739e26a1db08591 Mon Sep 17 00:00:00 2001 From: Reakain Date: Wed, 3 Aug 2022 11:48:10 -0700 Subject: [PATCH 1/2] Un-passed the events related to start and stop charging, as well as for moving on or off the charger. Added event handler functions and property tracking for on/off the charger and on/off charging. --- pycozmo/client.py | 10 ++++++++++ pycozmo/event.py | 4 ++-- 2 files changed, 12 insertions(+), 2 deletions(-) diff --git a/pycozmo/client.py b/pycozmo/client.py index 9b0b7a0..397cad8 100644 --- a/pycozmo/client.py +++ b/pycozmo/client.py @@ -83,6 +83,8 @@ def __init__(self, self.robot_orientation = robot.RobotOrientation.ON_THREADS self.robot_picked_up = False self.robot_moving = False + self.is_on_charger = False + self.is_charging = False # Animation state self.num_anim_bytes_played = 0 self.num_audio_frames_played = 0 @@ -120,6 +122,8 @@ def start(self) -> None: self.add_handler(protocol_encoder.NvStorageOpResult, self._on_nv_storage_op_result) self.add_handler(event.EvtRobotPickedUpChange, self._on_robot_picked_up) self.add_handler(event.EvtRobotWheelsMovingChange, self._on_robot_moving) + self.add_handler(event.EvtRobotOnChargerChange, self._on_charger_change) + self.add_handler(event.EvtRobotChargingChange, self._is_charging_change) self.conn.start() def stop(self) -> None: @@ -330,6 +334,12 @@ def _on_robot_picked_up(self, cli, state): def _on_robot_moving(self, cli, state): self.robot_moving = state + def _on_charger_change(self, cli, state): + self.is_on_charger = state + + def _is_charging_change(self, cli, state): + self.is_charging = state + def _on_animation_state(self, cli, pkt: protocol_encoder.AnimationState): del cli self.num_anim_bytes_played = pkt.num_anim_bytes_played diff --git a/pycozmo/event.py b/pycozmo/event.py index 2d428ea..7dc4254 100644 --- a/pycozmo/event.py +++ b/pycozmo/event.py @@ -126,11 +126,11 @@ class EvtRobotAnimatingIdleChange(Event): class EvtRobotOnChargerChange(Event): - pass + """ Triggered when the robot has moved on or off the charger. """ class EvtRobotChargingChange(Event): - pass + """ Triggered when the robot has started or stopped charging. """ class EvtCliffDetectedChange(Event): From eeec4d079da7f0394d561a66ea233b2cfc2193b0 Mon Sep 17 00:00:00 2001 From: Reakain Date: Wed, 3 Aug 2022 11:56:44 -0700 Subject: [PATCH 2/2] added functions for drive straight, drive off charger contacts, turn in place, and turn in place at speed to match cozmo sdk --- pycozmo/client.py | 29 +++++++++++++++++++++++++++++ 1 file changed, 29 insertions(+) diff --git a/pycozmo/client.py b/pycozmo/client.py index 397cad8..07e40e6 100644 --- a/pycozmo/client.py +++ b/pycozmo/client.py @@ -429,6 +429,35 @@ def drive_wheels(self, lwheel_speed: float, rwheel_speed: float, time.sleep(duration) self.stop_all_motors() + def drive_straight(self, distance: float, speed: float) -> None: + if distance < 0.0: + speed = -speed + duration = np.abs(distance/speed) + self.drive_wheels(speed, speed, duration=duration) + + def drive_off_charger_contacts(self) -> None: + self.conn.send(protocol_encoder.EnableStopOnCliff(False)) + target_pose = util.Pose(100.0, 0.0, 0.0, angle_z=util.Angle(degrees=0.0)) + self.go_to_pose(target_pose, relative_to_robot=True) + self.conn.send(protocol_encoder.EnableStopOnCliff(True)) + + def turn_in_place(self, angle_rad: float, speed: Optional[float] = 40.0, + accel: Optional[float] = 0.0, angle_tolerance: Optional[float] = 0.02, + is_absolute: Optional[bool] = False) -> None: + pkt = protocol_encoder.TurnInPlace(angle_rad=angle_rad, speed_rad_per_sec=speed, + accel_rad_per_sec2=accel, angle_tolerance_rad=angle_tolerance, + is_absolute=is_absolute) + self.conn.send(pkt) + + def turn_in_place_at_speed(self, direction: int, speed: Optional[float] = 40.0, + accel: Optional[float] = 0.0, duration: Optional[float] = None) -> None: + pkt = protocol_encoder.TurnInPlaceAtSpeed(wheel_speed_mmps=speed, wheel_accel_mmps2=accel, + direction=direction) + self.conn.send(pkt) + if duration is not None: + time.sleep(duration) + self.stop_all_motors() + def stop_all_motors(self) -> None: pkt = protocol_encoder.StopAllMotors() self.conn.send(pkt)