From c734a9c32c3a7b6b392fe98ddc02d8260d027708 Mon Sep 17 00:00:00 2001
From: Alex Mikhalev <alexmikhalevalex@gmail.com>
Date: Sat, 1 Jun 2019 17:18:37 -0700
Subject: [PATCH] Improve python scripts

---
 tools/ugv_cmd.py       |  7 ++++++
 tools/ugv_to_ground.py | 49 ++++++++++++++++++++++++++++++++----------
 2 files changed, 45 insertions(+), 11 deletions(-)

diff --git a/tools/ugv_cmd.py b/tools/ugv_cmd.py
index 000f08a..4126414 100755
--- a/tools/ugv_cmd.py
+++ b/tools/ugv_cmd.py
@@ -125,6 +125,13 @@ class UGV_CLI:
         self.ugv.write_command(cmd)
         log.info("driving to target")
 
+    @cli_cmd(names=["run_test", "rt"], description="Run test mode")
+    def run_test(self):
+        cmd = messages.GroundCommand()
+        cmd.type = messages.CMD_TEST
+        self.ugv.write_command(cmd)
+        log.info("running test mode")
+
     @cli_cmd(names=["last_status", "ls", "s"], description="Print the last status of the UGV")
     def last_status(self):
         if self.ugv.last_status_time is None:
diff --git a/tools/ugv_to_ground.py b/tools/ugv_to_ground.py
index 21ba7a5..3db8ef9 100755
--- a/tools/ugv_to_ground.py
+++ b/tools/ugv_to_ground.py
@@ -3,46 +3,62 @@ import socketio
 import logging
 from base64 import b64encode
 from threading import Thread
+import time
 
 from ugv_cmd import UGV_CLI
 import messages_pb2 as messages
 from google.protobuf.message import Message
 
+log = logging.getLogger("ugv_to_ground")
+
 def encode_msg(msg: Message):
     data = msg.SerializeToString()
-    return b64encode(data)
+    return b64encode(data).decode('utf-8')
 
-if __name__ == "__main__":
-    logging.basicConfig(format='%(asctime)s [%(name)s] %(levelname)s: %(message)s', datefmt='%Y-%b-%d %H:%M:%S')
-    logging.getLogger().setLevel(logging.INFO)
 
+def ping_thread_entry(ugv_cli):
+    while ugv_cli.is_running and ugv_cli.ugv.ser.is_open:
+        try:
+            ugv_cli.get_status()
+            time.sleep(10.0)
+        except IOError as e:
+            log.error("Error pinging UGV: {}".format(e))
+
+def start_ugv_to_ground():
     server_ip = 'localhost'
 
     sio = socketio.Client()
 
     def on_msg_received(msg: messages.UGV_Message):
-       sio.emit('UGV_MESSAGE', encode_msg(msg), namespace='/ugv')
+        sio.emit('UGV_MESSAGE', encode_msg(msg), namespace='/ugv')
+        pass
 
     ugv_cli = UGV_CLI(on_msg_received)
     ugv_cli.start()
 
     @sio.on('connect', namespace='/ugv')
     def on_connect():
-        print("connected to ground server")
+        log.info("connected to ground server")
 
     @sio.on('disconnect', namespace='/ugv')
     def on_disconnect():
-        print("disconnected from ground server!")
+        log.info("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'])
+        log.info("Setting UGV target")
+        try:
+            ugv_cli.set_target(msg['lat'], msg['lng'])
+        except IOError as e:
+            log.error("Error setting target: {}".format(e))
 
     @sio.on('DRIVE_TO_TARGET', namespace='/ugv')
     def drive_to_target():
-        print("Driving to target!")
-        ugv_cli.drive_to_target()
+        log.info("Driving to target!")
+        try:
+            ugv_cli.drive_to_target()
+        except IOError as e:
+            log.error("Error setting target: {}".format(e))
 
     while True:
         try:
@@ -52,4 +68,15 @@ if __name__ == "__main__":
             print("Can't connect to ground server. Retrying in 2 seconds...")
             sio.sleep(2)
 
+    ping_thread = Thread(target=ping_thread_entry, args=(ugv_cli, ), daemon=True)
+    ping_thread.start()
+
     ugv_cli.run_cli()
+
+
+if __name__ == "__main__":
+    logging.basicConfig(format='%(asctime)s [%(name)s] %(levelname)s: %(message)s', datefmt='%Y-%b-%d %H:%M:%S')
+    logging.getLogger().setLevel(logging.INFO)
+
+    start_ugv_to_ground()
+