diff --git a/.gitignore b/.gitignore index 91649bd..58f4e2d 100644 --- a/.gitignore +++ b/.gitignore @@ -1,4 +1,8 @@ pytemi/__pycache__/ pytemi.egg-info/ venv/ -test.py \ No newline at end of file +_test/ +test.py +*.pyc +*.csv +*.env diff --git a/README.md b/README.md index 0b45030..41bf3e0 100644 --- a/README.md +++ b/README.md @@ -1,14 +1,5 @@ # pytemi -Control temi using Python scripts over MQTT. - - -## Prerequisites -* [Python 3](https://www.python.org/downloads/) -* [Connect app](https://github.com/hapi-robo/connect/releases) installed on temi -* MQTT broker. Free brokers for testing: - * [Eclipse](http://test.mosquitto.org/) - * [Mosquitto](http://mqtt.eclipse.org) - * [HiveMQ](http://broker.hivemq.com) +A Python package for controlling temi over MQTT. To be used with [temi-mqtt-client](https://github.com/hapi-robo/temi-mqtt-client/). For prototyping/development only. ## Setup @@ -24,15 +15,10 @@ For Linux users, there's a script that will create a Python virtual environment ## Usage -Make sure the robot is connected to an MQTT broker via the [Connect app](https://github.com/hapi-robo/connect/releases). - -Edit the `sample.py` script and adjust the `parameters` appropriately, then run: -``` -python sample.py -``` +Make sure the robot is connected to an MQTT broker via the [temi-mqtt-client](https://github.com/hapi-robo/temi-mqtt-client/) app. -## Sample Script +### Sample Script ``` import pytemi as temi @@ -53,4 +39,4 @@ robot.tts("Going to the Entrance") robot.goto("entrance") ``` -See `sample.py` for more details. +See `./sample.py` for more details. Other examples can be found in the `scripts/` folder. diff --git a/pytemi/README.md b/pytemi/README.md new file mode 100644 index 0000000..ef8b88f --- /dev/null +++ b/pytemi/README.md @@ -0,0 +1,24 @@ +# MQTT Topics +The following is a summary of MQTT topics used by this package. + +## Publish +``` +temi/{id}/command/move/turn_by +temi/{id}/command/move/joystick +temi/{id}/command/move/tilt +temi/{id}/command/move/tilt_by +temi/{id}/command/move/stop +temi/{id}/command/follow/unconstrained +temi/{id}/command/waypoint/goto +temi/{id}/command/tts +temi/{id}/command/media/video +temi/{id}/command/media/webview +``` + +## Subscribe +``` +temi/{id}/status/info +temi/{id}/status/utils/battery +temi/{id}/event/waypoint/goto +temi/{id}/event/user/detection +``` diff --git a/pytemi/connect.py b/pytemi/connect.py index b8ae123..d98e1f8 100644 --- a/pytemi/connect.py +++ b/pytemi/connect.py @@ -12,20 +12,20 @@ def _on_connect(client, userdata, flags, rc): - """Connect to MQTT broker and subscribe to topics - - """ + """Connect to MQTT broker and subscribe to topics""" print( "[STATUS] Connected to: {} (rc:{})".format( client._client_id.decode("ascii"), str(rc) ) ) + # subscribing in on_connect() means that if we lose the connection and + # reconnect, then subscriptions will be renewed + client.subscribe("temi/#") -def _on_disconnect(client, userdata, rc): - """Disconnect from MQTT broker - """ +def _on_disconnect(client, userdata, rc): + """Disconnect from MQTT broker""" print( "[STATUS] Disconnected from: {} (rc:{})".format( client._client_id.decode("ascii"), str(rc) @@ -34,13 +34,19 @@ def _on_disconnect(client, userdata, rc): client.loop_stop() +def _on_message(client, userdata, msg): + """Print out any topics that have no callbacks""" + print("[{}][SUB] {} {}".format(now(), msg.topic, str(msg.payload))) + + def connect(host, port, username=None, password=None): + """Connect to MQTT broker""" client_id = socket.gethostname() + "-" + datetime.now().strftime("%Y%m%d%H%M%S") # create a new MQTT client instance client = mqtt.Client(client_id=client_id) - # attach callbacks + # attach general callbacks client.on_connect = _on_connect client.on_disconnect = _on_disconnect diff --git a/pytemi/robot.py b/pytemi/robot.py index 77eb2ef..362968b 100644 --- a/pytemi/robot.py +++ b/pytemi/robot.py @@ -7,52 +7,90 @@ import math import json +from datetime import datetime + + +def now(): + """Return time in string format""" + return datetime.now().strftime("%H:%M:%S") + + +def _on_status(client, userdata, msg): + d = json.loads(msg.payload) + userdata["locations"] = d["waypoint_list"] + userdata["battery"]["percentage"] = d["battery_percentage"] + + +def _on_battery(client, userdata, msg): + print("[{}] [SUB] [BATTERY] {}".format(now(), str(msg.payload))) + d = json.loads(msg.payload) + userdata["battery"]["percentage"] = d["percentage"] + userdata["battery"]["is_charging"] = d["is_charging"] + + +def _on_goto(client, userdata, msg): + d = json.loads(msg.payload) + userdata["goto"]["location"] = d["location"] + userdata["goto"]["status"] = d["status"] -class Robot: - """Robot Class - """ +def _on_user(client, userdata, msg): + print("[{}] [SUB] [USER] {}".format(now(), str(msg.payload))) + userdata["user"] = json.loads(msg.payload) - def __init__(self, mqtt_client, temi_serial): - """Constructor - """ +class Robot: + """Robot Class""" + + def __init__(self, mqtt_client, temi_serial, silent=True): + """Constructor""" self.client = mqtt_client self.id = temi_serial + self.silent = silent + + # set user data + self.state = {"locations": [], "battery": {}, "goto": {}, "user": {}} + self.client.user_data_set(self.state) + + # attach subscription callbacks + self.client.message_callback_add( + "temi/{}/status/info".format(temi_serial), _on_status + ) + self.client.message_callback_add( + "temi/{}/status/utils/battery".format(temi_serial), _on_battery + ) + self.client.message_callback_add( + "temi/{}/event/waypoint/goto".format(temi_serial), _on_goto + ) + self.client.message_callback_add( + "temi/{}/event/user/detection".format(temi_serial), _on_user + ) def rotate(self, angle): - """Rotate + """Rotate""" + if not self.silent: + print("[CMD] Rotate: {} [deg]".format(angle)) - """ - print("[CMD] Rotate: {} [deg]".format(angle)) - - if (angle != 0): + if angle != 0: topic = "temi/" + self.id + "/command/move/turn_by" payload = json.dumps({"angle": angle}) self.client.publish(topic, payload, qos=0) def translate(self, value): - """Translate - - """ - print("[CMD] Translate: {} [unitless]".format(value)) + """Translate""" + if not self.silent: + print("[CMD] Translate: {} [unitless]".format(value)) - if math.copysign(1, value) > 0: - topic = "temi/" + self.id + "/command/move/forward" - self.client.publish(topic, "{}", qos=0) - elif math.copysign(1, value) < 0: - topic = "temi/" + self.id + "/command/move/backward" - self.client.publish(topic, "{}", qos=0) - else: - pass # do nothing + topic = "temi/" + self.id + "/command/move/joystick" + payload = json.dumps({"x": value, "y": 0}) + self.client.publish(topic, payload, qos=0) def tilt(self, angle): - """Tilt head (absolute angle) - - """ - print("[CMD] Tilt: {} [deg]".format(angle)) + """Tilt head (absolute angle)""" + if not self.silent: + print("[CMD] Tilt: {} [deg]".format(angle)) topic = "temi/" + self.id + "/command/move/tilt" payload = json.dumps({"angle": angle}) @@ -60,10 +98,9 @@ def tilt(self, angle): self.client.publish(topic, payload, qos=0) def tilt_by(self, angle): - """Tilt head (relative angle) - - """ - print("[CMD] Tilt By: {} [deg]".format(angle)) + """Tilt head (relative angle)""" + if not self.silent: + print("[CMD] Tilt By: {} [deg]".format(angle)) topic = "temi/" + self.id + "/command/move/tilt_by" payload = json.dumps({"angle": angle}) @@ -71,30 +108,27 @@ def tilt_by(self, angle): self.client.publish(topic, payload, qos=0) def stop(self): - """Stop - - """ - print("[CMD] Stop") + """Stop""" + if not self.silent: + print("[CMD] Stop") topic = "temi/" + self.id + "/command/move/stop" self.client.publish(topic, "{}", qos=1) def follow(self): - """Follow - - """ - print("[CMD] Follow") + """Follow""" + if not self.silent: + print("[CMD] Follow") topic = "temi/" + self.id + "/command/follow/unconstrained" self.client.publish(topic, "{}", qos=1) def goto(self, location_name): - """Go to a saved location - - """ - print("[CMD] Go-To: {}".format(location_name)) + """Go to a saved location""" + if not self.silent: + print("[CMD] Go-To: {}".format(location_name)) topic = "temi/" + self.id + "/command/waypoint/goto" payload = json.dumps({"location": location_name}) @@ -102,10 +136,9 @@ def goto(self, location_name): self.client.publish(topic, payload, qos=1) def tts(self, text): - """Text-to-speech - - """ - print("[CMD] TTS: {}".format(text)) + """Text-to-speech""" + if not self.silent: + print("[CMD] TTS: {}".format(text)) topic = "temi/" + self.id + "/command/tts" payload = json.dumps({"utterance": text}) @@ -124,66 +157,76 @@ def tts(self, text): # self.client.publish(topic, payload, qos=1) def video(self, url): - """Play video - - """ - print("[CMD] Play Video: {}".format(url)) + """Play video""" + if not self.silent: + print("[CMD] Play Video: {}".format(url)) topic = "temi/" + self.id + "/command/media/video" payload = json.dumps({"url": url}) self.client.publish(topic, payload, qos=1) - def youtube(self, video_id): - """Play YouTube + # def youtube(self, video_id): + # """Play YouTube - """ - print("[CMD] Play YouTube: {}".format(video_id)) + # """ + # if not self.silent: + # print("[CMD] Play YouTube: {}".format(video_id)) - topic = "temi/" + self.id + "/command/media/youtube" - payload = json.dumps({"video_id": video_id}) + # topic = "temi/" + self.id + "/command/media/youtube" + # payload = json.dumps({"video_id": video_id}) - self.client.publish(topic, payload, qos=1) + # self.client.publish(topic, payload, qos=1) def webview(self, url): - """Show webview - - """ - print("[CMD] Show Webview: {}".format(url)) + """Show webview""" + if not self.silent: + print("[CMD] Show Webview: {}".format(url)) topic = "temi/" + self.id + "/command/media/webview" payload = json.dumps({"url": url}) self.client.publish(topic, payload, qos=1) - def app(self, package_name): - """Start Android app - - """ - print("[CMD] Start App: {}".format(package_name)) - - topic = "temi/" + self.id + "/command/app" - payload = json.dumps({"package_name": package_name}) - - self.client.publish(topic, payload, qos=1) + @property + def locations(self): + """Return a list of locations""" + if "locations" in self.state: + return self.state["locations"] + else: + return [] - def call(self, room_name): - """Start a call + @property + def goto_status(self): + if "status" in self.state["goto"]: + return self.state["goto"]["status"] + else: + return None - """ - print("[CMD] Call: {}".format(room_name)) + @property + def battery(self): + return self.state["battery"]["percentage"] - topic = "temi/" + self.id + "/command/call/start" - payload = json.dumps({"room_name": room_name}) + @property + def GOTO_START(self): + return "start" - self.client.publish(topic, payload, qos=1) + @property + def GOTO_ABORT(self): + return "abort" - def hangup(self): - """End a call + @property + def GOTO_GOING(self): + return "going" - """ - print("[CMD] Hangup") + @property + def GOTO_COMPLETE(self): + return "complete" - topic = "temi/" + self.id + "/command/call/end" + @property + def GOTO_CALCULATING(self): + return "calculating" - self.client.publish(topic, "{}", qos=1) + @property + def GOTO_OBSTACLE(self): + return "obstacle detected" diff --git a/requirements.txt b/requirements.txt index 8579e8b..75d47e1 100644 --- a/requirements.txt +++ b/requirements.txt @@ -1 +1,3 @@ +python-dotenv paho-mqtt +inputs \ No newline at end of file diff --git a/sample.py b/sample.py index 671d92b..18708be 100644 --- a/sample.py +++ b/sample.py @@ -22,54 +22,47 @@ # -------------------------------------------------------------- # TEXT-TO-SPEECH COMMANDS # -------------------------------------------------------------- -robot.tts("Going to the Entrance") # command the robot to speak -time.sleep(1) # wait some time for action to complete +robot.tts("Hello World!") # command the robot to speak +time.sleep(1) # wait some time for action to complete # -------------------------------------------------------------- # WAYPOINT COMMANDS # -------------------------------------------------------------- -robot.goto("entrance") # command the robot to go to a saved location -time.sleep(1) # wait some time for action to complete +robot.goto("entrance") # command the robot to go to a saved location +time.sleep(3) # wait some time for action to complete # -------------------------------------------------------------- # MOVE COMMANDS # -------------------------------------------------------------- -robot.tilt(+45) # tilt the robot's head (absolute angle) -time.sleep(3) # wait some time for action to complete +robot.tilt(+45) # tilt the robot's head (absolute angle) +time.sleep(3) # wait some time for action to complete -robot.tilt(-15) # tilt the robot's head (absolute angle) -time.sleep(3) # wait some time for action to complete +robot.tilt(-15) # tilt the robot's head (absolute angle) +time.sleep(3) # wait some time for action to complete -robot.tilt_by(+30) # tilt the robot's head (relative angle) -time.sleep(3) # wait some time for action to complete +robot.tilt_by(+30) # tilt the robot's head (relative angle) +time.sleep(3) # wait some time for action to complete -robot.tilt_by(-10) # tilt the robot's head (relative angle) -time.sleep(3) # wait some time for action to complete +robot.tilt_by(-10) # tilt the robot's head (relative angle) +time.sleep(3) # wait some time for action to complete -robot.rotate(90) # rotate the robot (relative angle) -time.sleep(5) # wait some time for action to complete +robot.rotate(90) # rotate the robot (relative angle) +time.sleep(5) # wait some time for action to complete -robot.rotate(-30) # rotate the robot (relative angle) -time.sleep(5) # wait some time for action to complete +robot.rotate(-30) # rotate the robot (relative angle) +time.sleep(5) # wait some time for action to complete # -------------------------------------------------------------- # MEDIA COMMANDS # -------------------------------------------------------------- -robot.youtube("ZsEano0qwcg") # play YouTube video by passing in a YouTube video ID -time.sleep(30) # wait some time for action to complete - robot.video( "https://roboteam-assets.s3.eu-central-1.amazonaws.com/ui/skills/tutorials/videos/intorducing+temi.mp4" -) # play online video by passing a URL -time.sleep(30) # wait some time for action to complete +) # play online video by passing a URL; make sure video is optimized for the web (this sample video is not) +time.sleep(30) # wait some time for action to complete # show webview by passing a URL -robot.webview("https://www.robotemi.com/") -time.sleep(5) -robot.webview("https://hapi-robo.com/") -time.sleep(5) -robot.webview("https://www.his.co.jp/en/") +robot.webview("https://www.google.com/") time.sleep(5) diff --git a/scripts/README.md b/scripts/README.md new file mode 100644 index 0000000..fc8d929 --- /dev/null +++ b/scripts/README.md @@ -0,0 +1,9 @@ +# pytemi/scripts +Add a `.env` file to this folder with the MQTT server parameters: +``` +# MQTT server parameters +MQTT_HOST="" +MQTT_PORT="" +MQTT_USERNAME="" +MQTT_PASSWORD="" +``` diff --git a/scripts/goto_random.py b/scripts/goto_random.py new file mode 100755 index 0000000..991a2f7 --- /dev/null +++ b/scripts/goto_random.py @@ -0,0 +1,154 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +"""Commands temi to randomly go to saved locations + +""" +import pytemi as temi +import random +import csv +import os + +from time import sleep +from datetime import datetime +from dotenv import load_dotenv + +load_dotenv() + + +# robot parameters +TEMI_SERIAL = "00119260058" + +BATTERY_THRESHOLD_LOW = 20 # [%] +BATTERY_THRESHOLD_CHARGED = 90 # [%] + +# MQTT server parameters +MQTT_HOST = os.getenv("MQTT_HOST") +MQTT_PORT = int(os.getenv("MQTT_PORT")) +MQTT_USERNAME = os.getenv("MQTT_USERNAME") +MQTT_PASSWORD = os.getenv("MQTT_PASSWORD") + + +class bcolors: + """ANSI escape sequences for colours""" + + HEADER = "\033[95m" + OKBLUE = "\033[94m" + OKGREEN = "\033[92m" + WARNING = "\033[93m" + FAIL = "\033[91m" + ENDC = "\033[0m" + BOLD = "\033[1m" + UNDERLINE = "\033[4m" + + +def goto(robot, location): + """Send robot to a location (blocking call)""" + robot.tts("Going to {}".format(location)) + robot.goto(location) + sleep(2) + + # wait until goto-location is reached or is aborted + while robot.goto_status != robot.GOTO_COMPLETE: + print( + "[{}][{}][GOTO] {} [{}%]".format( + datetime.now().strftime("%Y-%m-%d"), + datetime.now().strftime("%H:%M:%S"), + robot.goto_status, + robot.battery, + ) + ) + sleep(1) + + if robot.goto_status == robot.GOTO_ABORT: + robot.tts("Go-To Aborted") + sleep(1) + print( + "{}{}[{}][{}][GOTO] Aborted ({}){}{}".format( + bcolors.BOLD, + bcolors.FAIL, + datetime.now().strftime("%Y-%m-%d"), + datetime.now().strftime("%H:%M:%S"), + location, + bcolors.ENDC, + bcolors.ENDC, + ) + ) + sleep(1) + return -1 + + # success + return 0 + + +def charge(robot): + """Charge robot (blocking call)""" + robot.tts("Charging battery") + while robot.battery < BATTERY_THRESHOLD_CHARGED: + print( + "[{}][{}][CHARGING] Battery: {}%".format( + datetime.now().strftime("%Y-%m-%d"), + datetime.now().strftime("%H:%M:%S"), + robot.battery, + ) + ) + sleep(5 * 60) + + +if __name__ == "__main__": + csv_filename = datetime.now().strftime("%Y%m%d_%H%M%S.csv") + location_old = None + + # connect to the MQTT server + mqtt_client = temi.connect(MQTT_HOST, MQTT_PORT, MQTT_USERNAME, MQTT_PASSWORD) + + # create robot object + robot = temi.Robot(mqtt_client, TEMI_SERIAL) + + # collect locations + while not robot.locations: + print(robot.locations) + sleep(1) + locations = robot.locations + locations.pop(0) # remove "home base" from the list + + # start random patrol + while True: + # return robot to home base when battery is low + if robot.battery < BATTERY_THRESHOLD_LOW: + goto(robot, "home base") + charge(robot) + + # shuffle locations and go to each one + random.shuffle(locations) # shuffle locations + for location in locations: + result = goto(robot, location) + print( + "[{}][{}] {} → {} | {} [{}%]".format( + datetime.now().strftime("%Y-%m-%d"), + datetime.now().strftime("%H:%M:%S"), + location_old, + location, + result, + robot.battery, + ) + ) + + # collect logs and statistics + with open(csv_filename, mode="a") as f: + fieldnames = ["Date", "Time", "From", "To", "Result", "Battery"] + csv_writer = csv.DictWriter(f, fieldnames=fieldnames) + + csv_writer.writerow( + { + "Date": datetime.now().strftime("%Y-%m-%d"), + "Time": datetime.now().strftime("%H:%M:%S"), + "From": location_old, + "To": location, + "Result": result, + "Battery": robot.battery, + } + ) + + # if robot reached the desired location, update old location + if result == 0: + location_old = location diff --git a/scripts/joystick.py b/scripts/joystick.py new file mode 100755 index 0000000..5fbb939 --- /dev/null +++ b/scripts/joystick.py @@ -0,0 +1,164 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +"""Commands temi by joystick + +""" +import pytemi as temi +import time +import threading +import concurrent.futures +import copy +import inputs +import os + +from dotenv import load_dotenv + +load_dotenv() + + +# robot parameters +TEMI_SERIAL = "00119260058" + +# MQTT server parameters +MQTT_HOST = os.getenv("MQTT_HOST") +MQTT_PORT = int(os.getenv("MQTT_PORT")) +MQTT_USERNAME = os.getenv("MQTT_USERNAME") +MQTT_PASSWORD = os.getenv("MQTT_PASSWORD") + +# control +PUBLISH_INTERVAL = 0.1 # [sec] +ANGULAR_VELOCITY = 30 # [deg/s] + +# robot limits +TILT_ANGLE_MAX_POS = 55 # [deg] +TILT_ANGLE_MAX_NEG = -20 # [deg] +ANGULAR_VELOCITY_MIN = 0.3 # [normalized] +LINEAR_VELOCITY_MIN = 0.5 # [normalized] + + +class Pipeline: + """Single element pipeline between producer and consumer""" + + def __init__(self): + self._message = {} + self._lock = threading.Lock() + + def get_message(self): + if self._lock.acquire(blocking=False): + message = copy.deepcopy(self._message) + self._lock.release() + + return message + + def set_message(self, message): + if self._lock.acquire(blocking=False): + self._message = copy.deepcopy(message) + self._lock.release() + + +def publisher(pipeline): + """Robot publisher thread""" + tilt_angle = 0 + linear_velocity = 0.0 + angular_velocity = 0.0 + + while True: + cmd = pipeline.get_message() + + # translate + if "abs_y" in cmd: + # reverse axis + linear_velocity = -cmd["abs_y"] + + # check for minimum linear velocity + if abs(linear_velocity) > LINEAR_VELOCITY_MIN: + robot.translate(linear_velocity) + + # rotate + if "abs_x" in cmd: + # reverse axis + angular_velocity = -cmd["abs_x"] + + # check for minimum angular velocity + if abs(angular_velocity) > ANGULAR_VELOCITY_MIN: + if angular_velocity > 0: + robot.rotate(+ANGULAR_VELOCITY) + elif angular_velocity < 0: + robot.rotate(-ANGULAR_VELOCITY) + else: + pass + + # tilt + if "abs_ry" in cmd: + # scale tilt-angle + if cmd["abs_ry"] > 0: + tilt_angle = +int(TILT_ANGLE_MAX_POS * cmd["abs_ry"]) + elif cmd["abs_ry"] < 0: + tilt_angle = -int(TILT_ANGLE_MAX_NEG * cmd["abs_ry"]) + else: + tilt_angle = 0 + + if abs(tilt_angle) > 0: + robot.tilt_by(tilt_angle) + + # print to console + print("--------------------") + print("Timestamp: {:.2f}".format(time.time())) + print("Translate: {:+.2f}".format(linear_velocity)) + print("Rotate: {:+.2f}".format(angular_velocity)) + print("Tilt: {:+}".format(tilt_angle)) + + # wait + time.sleep(PUBLISH_INTERVAL) + + +def subscriber(pipeline): + """Joystick subscriber thread""" + cmd = {} + + while True: + # clear command + cmd.clear() + + # collect gamepad events (blocking) + events = inputs.get_gamepad() + + # parse all events + for event in events: + if event.ev_type is not "Sync": + # print("{} {} {}".format(event.ev_type, event.code, event.state)) + + val_norm = event.state / 32768.0 + + if event.code is "ABS_Y": + cmd["abs_y"] = val_norm + + if event.code is "ABS_X": + cmd["abs_x"] = val_norm + + if event.code is "ABS_RY": + cmd["abs_ry"] = val_norm + + if cmd: + # print(cmd) + pipeline.set_message(cmd) + + +if __name__ == "__main__": + # connect to the MQTT server + mqtt_client = temi.connect(MQTT_HOST, MQTT_PORT, MQTT_USERNAME, MQTT_PASSWORD) + + # create robot object + robot = temi.Robot(mqtt_client, TEMI_SERIAL) + + # verify joystick connection + if len(inputs.devices.gamepads) == 0: + raise Exception("Could not find any gamepads") + + # construct pipeline + pipeline = Pipeline() + + # start threads + with concurrent.futures.ThreadPoolExecutor(max_workers=2) as executor: + executor.submit(subscriber, pipeline) + executor.submit(publisher, pipeline) diff --git a/setup.py b/setup.py index 82342fc..56a3f10 100644 --- a/setup.py +++ b/setup.py @@ -5,10 +5,12 @@ def readme(): with open("README.md", "r") as f: return f.read() + def requirements(): with open("requirements.txt", "r") as f: return f.read().splitlines() + setuptools.setup( name="pytemi", version="1.0.0",