rc.py PyCozmo joystick example doesn't work with Client API

[CozmoCyke]

i try to use rc.py example in the inputs branch with the last dev branch of PyCozmo. Before the new Client API , it is working ok. But now with the new Client API I have the following error :

Windows 7 Game sir G3w with USB cable.

D:\dev\Cozmo\pycozmo\20190921\examples>python rc-inputs.py Microsoft Keyboard Microsoft Mouse Microsoft X-Box 360 pad 2019-09-22 02:22:35.918 root INFO Initializing... 2019-09-22 02:22:35.968 pycozmo.general INFO Firmware version 2381. 2019-09-22 02:22:36.029 pycozmo.general INFO Body S/N 0x45a0330c. Traceback (most recent call last): File "rc-inputs.py", line 269, in main() File "rc-inputs.py", line 262, in main res = app.init() File "rc-inputs.py", line 66, in init self.cli.send(pkt) AttributeError: 'Client' object has no attribute 'send'

[zayfod]

I think that you have installed an older version with pip and you are using the examples from a newer one.

Actually, you are using the rc-inputs.py example from the inputs branche which was created, before the introduction of the new client API.

Wait a few more days for the final integration of the inputs module.

[zayfod]

The raceback above cannot be reproduced on dev.

I just fixed this with #e58b208c:

Traceback (most recent call last):
  File "/usr/lib/python3.7/threading.py", line 917, in _bootstrap_inner
    self.run()
  File "/usr/lib/python3.7/threading.py", line 865, in run
    self._target(*self._args, **self._kwargs)
  File "/home/kaloyan/work/pycozmo/examples/rc.py", line 111, in run
    self._handler(event)
  File "/home/kaloyan/work/pycozmo/examples/rc.py", line 318, in _handle_input
    self._drive_wheels(self.speed_left, self.speed_right)
  File "/home/kaloyan/work/pycozmo/examples/rc.py", line 191, in _drive_wheels
    self.cli.drive_wheels(lwheel_speed=lw, rwheel_speed=rw)
  File "/home/kaloyan/work/pycozmo/pycozmo/client.py", line 310, in drive_wheels
    lwheel_accel_mmps2=lwheel_acc, rwheel_accel_mmps2=rwheel_acc)
  File "/home/kaloyan/work/pycozmo/pycozmo/protocol_encoder.py", line 972, in __init__
    self.lwheel_accel_mmps2 = lwheel_accel_mmps2
  File "/home/kaloyan/work/pycozmo/pycozmo/protocol_encoder.py", line 997, in lwheel_accel_mmps2
    self._lwheel_accel_mmps2 = validate_float("lwheel_accel_mmps2", value)
  File "/home/kaloyan/work/pycozmo/pycozmo/protocol_utils.py", line 20, in validate_float
    value = float(value)
TypeError: float() argument must be a string or a number, not 'NoneType'

[CozmoCyke]

I try again with rc-input.py (inputs branch) rc.py (dev branch) works only on Linux (import evdev) but not on Windows ! I unfortunately the same error :-(

D:\dev\Cozmo\pycozmo\20190922\examples>python rc-inputs.py Microsoft Keyboard Microsoft Mouse Microsoft X-Box 360 pad 2019-09-23 00:39:22.461 root INFO Initializing... 2019-09-23 00:39:22.504 pycozmo.general INFO Firmware version 2381. 2019-09-23 00:39:22.533 pycozmo.general INFO Body S/N 0x45a0330c. Traceback (most recent call last): File "rc-inputs.py", line 269, in main() File "rc-inputs.py", line 262, in main res = app.init() File "rc-inputs.py", line 66, in init self.cli.send(pkt) AttributeError: 'Client' object has no attribute 'send'

[CozmoCyke]

Hello ,

I found the errors ! :-) You must use the Client API in the rc.py !

Here's the code who works for me :

#!/usr/bin/env python

import argparse
import logging
import time
import threading

import inputs

import pycozmo

class InputThread(object):
    """ Thread for reading input. """

    def __init__(self, handler):
        self._stop = False
        self._thread = None
        self._handler = handler

    def start(self):
        self._stop = False
        self._thread = threading.Thread(target=self.run, name="InputThread")
        self._thread.daemon = True
        self._thread.start()

    def stop(self):
        logging.debug("Input thread stopping...")
        self._stop = True
        self._thread.join()
        logging.debug("Input thread joined.")

    def run(self):
        logging.debug("Input thread started.")
        while not self._stop:
            events = inputs.get_gamepad()
            for event in events:
                self._handler(event)
        logging.debug("Input thread stopped.")

class RCApp(object):
    """ Application class. """

    def __init__(self):
        logging.info("Initializing...")
        self._stop = False
        self.input_thread = InputThread(self._handle_input)
        self.cli = pycozmo.Client()
        self.speed = 0.0        # -1.0 - 1.0
        self.steering = 0.0     # -1.0 - 1.0
        self.speed_left = 0.0   # 0 - 1.0
        self.speed_right = 0.0  # 0 - 1.0
        self.lift = True

    def init(self):
        """ Initialize application. """
        self._stop = False
        # Connect to Cozmo
        self.cli.start()
        self.cli.connect()
        self.cli.wait_for_robot()
        # Raise head
        angle = (pycozmo.robot.MAX_HEAD_ANGLE.radians - pycozmo.robot.MIN_HEAD_ANGLE.radians) * 0.1
        #pkt = pycozmo.protocol_encoder.SetHeadAngle(angle_rad=angle)
        self.cli.set_head_angle(angle)
        time.sleep(0.5)
        return True

    def term(self):
        """ Terminate application. """
        logging.info("Terminating...")

        self.cli.stop_all_motors()
        self.cli.disconnect()
        self.cli.stop()

    def run(self):
        """ Main loop. """
        logging.info("Starting...")

        self.input_thread.start()

        while not self._stop:
            try:
                time.sleep(1)
            except KeyboardInterrupt:
                self.stop()

        self.input_thread.stop()

        logging.info("Done.")

    def stop(self):
        logging.debug("Stopping...")
        self._stop = True

    def _drive_lift(self, speed):
        if self.lift:
            self.cli.move_lift(speed)
        else:
            self.cli.move_head(speed)

    def _drive_wheels(self, speed_left, speed_right):
        lw = int(speed_left * pycozmo.MAX_WHEEL_SPEED.mmps)
        rw = int(speed_right * pycozmo.MAX_WHEEL_SPEED.mmps)
        self.cli.drive_wheels(lw, rw)

    @staticmethod
    def _get_motor_thrust(r, theta):
        """
        Convert throttle and steering angle to left and right motor thrust.

        https://robotics.stackexchange.com/questions/2011/how-to-calculate-the-right-and-left-speed-for-a-tank-like-rover

        :param r: throttle percentage [0, 100]
        :param theta: steering angle [-180, 180)
        :return: tuple - left motor and right motor thrust percentage [-100, 100]
        """
        # normalize theta to [-180, 180)
        theta = ((theta + 180.0) % 360.0) - 180.0
        # normalize r to [0, 100]
        r = min(max(0.0, r), 100.0)
        v_a = r * (45.0 - theta % 90.0) / 45.0
        v_b = min(100.0, 2.0 * r + v_a, 2.0 * r - v_a)
        if theta < -90.0:
            return -v_b, -v_a
        elif theta < 0:
            return -v_a, v_b
        elif theta < 90.0:
            return v_b, v_a
        else:
            return v_a, -v_b

    def _handle_input(self, e):
        update = False
        update2 = False

        if e.ev_type == "Key":
            # Button event.
            #   e.state = 1 - press
            #   e.state = 0 - release
            if e.code == "BTN_START":
                if e.state == 1:
                    self.stop()
            elif e.code == "BTN_TRIGGER_HAPPY3":
                # XBox 360 Wireless - Up
                if e.state == 1:
                    self._drive_lift(0.8)
                else:
                    self._drive_lift(0.0)
            elif e.code == "BTN_TRIGGER_HAPPY4":
                # XBox 360 Wireless - Down
                if e.state == 1:
                    self._drive_lift(-0.8)
                else:
                    self._drive_lift(0.0)
            elif e.code == "BTN_TRIGGER_HAPPY1":
                # XBox 360 Wireless - Left
                if e.state == 1:
                    self.lift = False
            elif e.code == "BTN_TRIGGER_HAPPY2":
                # XBox 360 Wireless - Right
                if e.state == 1:
                    self.lift = True
            else:
                # Do nothing.
                pass
        elif e.ev_type == "Absolute":
            # Absolute axis event.
            if e.code == "ABS_RX":
                # e.state = -32768 - full left
                # e.state = 32768 - full right
                self.steering = float(-e.state) / 32768.0
                if -0.15 < self.steering < 0.15:
                    self.steering = 0
                update = True
                logging.debug("Steering: {:.02f}".format(self.steering))
            elif e.code == "ABS_Y":
                # e.state = -32768 - full forward
                # e.state = 32768 - full reverse
                self.speed = float(-e.state) / 32768.0
                if -0.15 < self.speed < 0.15:
                    self.speed = 0
                update = True
                logging.debug("Speed: {:.02f}".format(self.speed))
            elif e.code == "ABS_Z":
                # e.state = 0 - 255
                self.speed_left = float(e.state) / 255.0
                update2 = True
                logging.debug("ML: {:.02f}".format(self.speed_left))
            elif e.code == "ABS_RZ":
                # e.state = 0 - 255
                self.speed_right = float(e.state) / 255.0
                update2 = True
                logging.debug("MR: {:.02f}".format(self.speed_right))
            elif e.code == "ABS_HAT0Y":
                if e.state == -1:
                    # Logitech Gamepad F310 - Up
                    self._drive_lift(0.8)
                elif e.state == 1:
                    # Logitech Gamepad F310 - Down
                    self._drive_lift(-0.8)
                else:
                    self._drive_lift(0.0)
            elif e.code == "ABS_HAT0X":
                if e.state == 1:
                    # Logitech Gamepad F310 - Right
                    self.lift = True
                elif e.state == -1:
                    # Logitech Gamepad F310 - Left
                    self.lift = False
                else:
                    pass
            else:
                # Do nothing.
                pass
        else:
            # Do nothing.
            pass

        if update:
            r = self.speed
            theta = -self.steering * 90.0
            if r < 0:
                r *= -1.0
                theta += 180.0
            v_a, v_b = self._get_motor_thrust(r, theta)
            logging.debug("r: {:.02f}; theta: {:.02f}; v_a: {:.02f}; v_b: {:.02f};".format(
                r, theta, v_a, v_b))
            self._drive_wheels(v_a, v_b)

        if update2:
            self._drive_wheels(self.speed_left, self.speed_right)

def parse_args():
    """ Parse command-line arguments. """
    parser = argparse.ArgumentParser(description=__doc__)
    parser.add_argument('-v', '--verbose', action='store_true', help='verbose')
    args = parser.parse_args()
    return args

def main():
    # Parse command-line.
    args = parse_args()

    # Configure logging.
    level = logging.DEBUG if args.verbose else logging.INFO
    logging.basicConfig(
        format="%(asctime)s.%(msecs)03d %(name)-15s %(levelname)-8s %(message)s",
        datefmt='%Y-%m-%d %H:%M:%S',
        level=level)

    #
    for device in inputs.devices:
        print(device)

    # Create application object.
    app = RCApp()
    res = app.init()
    if res:
        app.run()
        app.term()

if __name__ == '__main__':
    main()

[zayfod]

OK, I am glad you figured it out.

This is rc.py from the inputs branch which is still not integrated in dev and now somewhat obsolete.

I'll take your changes when integrating the inputs branch some time soon.

Thanks.