From ee0ff57484afaabdefa10e4636407efb6e0c0dba Mon Sep 17 00:00:00 2001
From: Ryan Nemiroff <ryguyn@gmail.com>
Date: Sat, 25 May 2019 05:00:20 -0700
Subject: [PATCH] Add ugv_to_ground.py

---
 tools/requirements.txt |  2 ++
 tools/ugv.py           |  5 +++-
 tools/ugv_cmd.py       |  7 +++---
 tools/ugv_to_ground.py | 54 ++++++++++++++++++++++++++++++++++++++++++
 4 files changed, 64 insertions(+), 4 deletions(-)
 create mode 100755 tools/ugv_to_ground.py

diff --git a/tools/requirements.txt b/tools/requirements.txt
index e69de29..8dbbc4a 100644
--- a/tools/requirements.txt
+++ b/tools/requirements.txt
@@ -0,0 +1,2 @@
+python-socketio==4.0.1
+PyYAML==5.1
diff --git a/tools/ugv.py b/tools/ugv.py
index 7832ef1..a00512e 100755
--- a/tools/ugv.py
+++ b/tools/ugv.py
@@ -19,8 +19,9 @@ class UGVComms:
     MAX_WRITE_RETRY = 5
     RETRY_TIME = 3.0
 
-    def __init__(self, serial_port: serial.Serial):
+    def __init__(self, serial_port: serial.Serial, on_msg_received=None):
         self.ser = serial_port
+        self.on_msg_received = on_msg_received
         self.msg_acks = []
         self.ack_cv = threading.Condition()
         self.next_command_id = 1
@@ -84,6 +85,8 @@ class UGVComms:
         if msg is None:
             return
         log.debug("received UGV message: %s", msg)
+        if self.on_msg_received:
+            self.on_msg_received(msg)
         if msg.HasField("command_ack"):
             with self.ack_cv:
                 self.msg_acks.append(msg.command_ack)
diff --git a/tools/ugv_cmd.py b/tools/ugv_cmd.py
index ceaa8a3..5a05a78 100755
--- a/tools/ugv_cmd.py
+++ b/tools/ugv_cmd.py
@@ -48,7 +48,8 @@ def cli_cmd(names=None, description=""):
 
 
 class UGV_CLI:
-    def __init__(self):
+    def __init__(self, on_msg_received=None):
+        self.on_msg_received = on_msg_received
         self.is_running = False
         self.last_state = messages.STATE_IDLE
         self.commands = {
@@ -137,7 +138,7 @@ class UGV_CLI:
         self.last_status()
 
     @cli_cmd(names=["ping", "p"], description="Ping the UGV")
-    def get_status(self):
+    def ping(self):
         cmd = messages.GroundCommand()
         cmd.type = messages.CMD_PING
         self.ugv.write_command(cmd)
@@ -172,7 +173,7 @@ class UGV_CLI:
         readline.parse_and_bind("tab: complete")
         readline.set_completer(self.complete_command)
 
-        self.ugv = UGVComms(ser)
+        self.ugv = UGVComms(ser, self.on_msg_received)
         self.ugv.start()
         time.sleep(0.2)
         last_line = None
diff --git a/tools/ugv_to_ground.py b/tools/ugv_to_ground.py
new file mode 100755
index 0000000..12f677c
--- /dev/null
+++ b/tools/ugv_to_ground.py
@@ -0,0 +1,54 @@
+#!/usr/bin/env python3
+import socketio
+import signal
+import logging
+from base64 import b64decode, b64encode
+
+from ugv_cmd import UGV_CLI
+import messages_pb2 as messages
+from google.protobuf.message import Message
+
+def encode_msg(msg: Message):
+    data = msg.SerializeToString()
+    return b64encode(data)
+
+if __name__ == "__main__":
+    server_ip = 'localhost'
+
+    sio = socketio.Client()
+
+    def on_msg_received(msg: messages.UGV_Message):
+       sio.emit('UGV_MESSAGE', encode_msg(msg), namespace='/ugv')
+
+    ugv_cli = UGV_CLI(on_msg_received)
+
+    @sio.on('connect', namespace='/ugv')
+    def on_connect():
+        print("connected to ground server")
+
+    @sio.on('disconnect', namespace='/ugv')
+    def on_disconnect():
+        print("disconnected from ground server!")
+
+    @sio.on('SET_TARGET', namespace='/ugv')
+    def set_target(msg):
+        print("Setting UGV target")
+        ugv_cli.set_target(msg['lat'], msg['lng'])
+
+    @sio.on('DRIVE_TO_TARGET', namespace='/ugv')
+    def drive_to_target():
+        print("Driving to target!")
+        ugv_cli.drive_to_target()
+
+    while True:
+        try:
+            sio.connect('http://'+server_ip+':8081', namespaces=['/ugv'], transports='websocket')
+            break
+        except:
+            print("Can't connect to ground server. Retrying in 2 seconds...")
+            sio.sleep(2)
+
+    logging.basicConfig(format='%(asctime)s [%(name)s] %(levelname)s: %(message)s', datefmt='%Y-%b-%d %H:%M:%S')
+    logging.getLogger().setLevel(logging.INFO)
+
+    ugv_cli.start()