controller finished

.gitignore

@@ -52,5 +52,5 @@ qtcreator-*
 CATKIN_IGNORE
 
 # Others
-push_to_vacuum.sh
+.setup.sh
-run_cmd_at_vacuum.sh
+config.json

controller.py

@@ -1,31 +1,59 @@
+import argparse
+
 from libs.env import get_env_var
-from libs.env import set_env_var
+from libs.vacuum_controller import init_controller
-
-from libs.vacuum_controller import VacuumController
 
 
+def help():
+    print('Command Menu')
+    print('help         - this message')
+    print('control      - control the vacuum')
+    print('config       - configuration')
+    print('update       - upload scripts to vacuum')
+    print('quit/exit    - exit controller (Ctrl + D does the same)')
 
-def export_ip_token(ip, token):
-    print("Exporting to environment variables")
-    set_env_var("MIROBO_IP", ip)
-    set_env_var("MIROBO_TOKEN", token)
 
-    print("Exporting to `.setup.sh` for later references")
+def main(args):
-    with open(".setup.sh", "w") as f:
+    c = init_controller(args.ip, args.token)
-        f.write("export MIROBO_IP={}\n".format(ip))
+    while 1:
-        f.write("export MIROBO_TOKEN={}\n".format(token))
+        try:
+            cmd = input(">>> ").split(" ")
+            if cmd[0] == 'quit' or cmd[0] =='exit':
+                print("Exiting..")
+                break
+            elif cmd[0] == 'help':
+                help()
+            elif cmd[0] == 'update':
+                filepath = cmd[1] if len(cmd) > 1 else "init_vacuum.sh"
+                c.update_script(filepath=filepath)
+            elif cmd[0] == 'control':
+                c.manual_control(cmd[1:])
+            elif cmd[0] == 'config':
+                c.configuration(cmd[1:])
+        except KeyboardInterrupt:
+            print("KeyboardInterrupt")
+            continue
+        except EOFError:
+            print("Exiting..")
+            break
+    c.configuration(["save"])
 
 
 if __name__ == '__main__':
-    ip = get_env_var("MIROBO_IP")
+    parser = argparse.ArgumentParser(
-    token = get_env_var("MIROBO_TOKEN")
+        description='Vacuum Controller'
-
+    )
-    c = VacuumController(ip, token)
+    parser.add_argument(
-    if not c.test_connection():
+        '--ip',
-        if ip is None or token is None:
+        dest='ip',
-            c = VacuumController()
+        default=get_env_var("MIROBO_IP"),
-        assert(c.test_connection())
+        help='Specify ip address, default using $MIROBO_IP'
-
+    )
-    export_ip_token(c.ip, c.token)
+    parser.add_argument(
-
+        '--token',
-
+        dest='token',
+        default=get_env_var("MIROBO_TOKEN"),
+        help='Specify token str, default using $MIROBO_TOKEN'
+    )
+    args, __ = parser.parse_known_args()
+    main(args)

libs/vacuum_controller.py

@@ -1,6 +1,66 @@
+import os
+import time
+import json
 import miio
 import codecs
 import socket
+import subprocess
+
+from libs.env import set_env_var
+
+
+def config_help():
+    print('Config Command Menu')
+    print('help             - this message')
+    print('set <key> <val>  - set key value in config')
+    print('get <key>        - get config from key, if `key` not set, print all')
+    print('save/load <file> - save/load configuration from file (default: ./config.json)')
+    print('quit/exit        - exit controller (Ctrl + D does the same)')
+
+
+def control_help():
+    print('Control Command Menu')
+    print('help                     - this message')
+    print('home                     - move vacuum to dock location')
+    print('move auto/pause/stop.    - auto scanning movement (no data parsing)')
+    print('move angle speed time    - move `angle` deg at `speed`m/s for `time`ms')
+    print('goto x_coor y_coor       - move to x,y location on map')
+    print('trace on/off             - manually start/stop collecting trace')
+    print('download trace/map       - download the trace or map on vacuum')
+    print('config <cmds>            - configuration')
+    print('quit/exit                - exit controller (Ctrl + D does the same)')
+
+
+def run_ssh_command(cmd):
+    try:
+        output = subprocess.check_output(
+            "ssh -o ConnectTimeout=10 -t root@${{MIROBO_IP}} '{}'"
+            .format(cmd),
+            shell=True
+        ).decode()
+    except BaseException as e:
+        print("Err: {}".format(e))
+        return None
+    return output
+
+
+def fetch_file_from_vacuum(remote_fp, local_fp="./"):
+    try:
+        subprocess.check_output(
+            "scp -o ConnectTimeout=10 root@${{MIROBO_IP}}:{} {}"
+            .format(remote_fp, local_fp),
+            shell=True
+        )
+        return True
+    except BaseException as e:
+        print("Err: {}".format(e))
+    return False
+
+
+def export_ip_token(ip, token):
+    print("Exporting to environment variables")
+    set_env_var("MIROBO_IP", ip)
+    set_env_var("MIROBO_TOKEN", token)
 
 
 class VacuumController():
@@ -8,17 +68,53 @@ class VacuumController():
     controlling xiaomi vacuum
     '''
 
-    def __init__(self, ip=None, token=None):
+    def __init__(self, ip=None, token=None, forceScan=False):
-        self.ip = ip
+        self.config = {}
-        self.token = token
+        self.tmp = {}
 
-        if self.ip is None:
+        # load old config if exist
+        self.configuration(["load"])
+        if forceScan:
+            print("Ignore prior IP and token, force to scan again!")
+
+        # if forcely specify a new ip and/or token
+        if ip:
+            self.set_ip(ip)
+        if token:
+            self.set_token(token)
+
+        # if we still don't have an IP address, or asked to scan anyways
+        if forceScan or self.get_ip() is None:
             self.finding_ip()
 
-        self.vacuum = miio.Vacuum(ip=self.ip, token=self.token)
+        self.vacuum = miio.Vacuum(
+            ip=self.get_ip(),
+            token=self.get_token()
+        )
 
-        if self.token is None:
+        # now if we still don't have a valid token, or asked to scan anyways
-            self.get_token()
+        if forceScan or self.get_token() is None:
+            self.fetching_token()
+
+        export_ip_token(self.get_ip(), self.get_token())
+
+    def get_ip(self):
+        return self.config.get("ip", None)
+
+    def set_ip(self, ip):
+        self.set_config("ip", ip)
+
+    def get_token(self):
+        return self.config.get("token", None)
+
+    def set_token(self, token):
+        self.set_config("token", token)
+
+    def get_remote_folder(self):
+        return self.config.get("remote_script_folder", "/mnt/data/exp")
+
+    def set_config(self, key, val):
+        self.config[key] = val
 
     def _discover_devices(self, timeout=5):
         ips = []
@@ -51,14 +147,14 @@ class VacuumController():
             print('Err: cannot find any vacuum IP')
             exit(-1)
         if len(ips) == 1:
-            self.ip = ips[0]
+            self.set_ip(ips[0])
             return
         print('Found multiple IPs:')
         for i, ip in enumerate(ips):
             print(' {0}. {1}'.format(i+1, ip))
         try:
             selected = input('Please select one by typing number (1-{}): '.format(len(ips)))
-            self.ip = ips[int(selected)-1]
+            self.set_ip(ips[int(selected)-1])
         except KeyboardInterrupt:
             print('User requested to exit')
             exit(0)
@@ -72,14 +168,14 @@ class VacuumController():
             print('Err: {}'.format(e))
             exit(-1)
 
-    def get_token(self):
+    def fetching_token(self):
         '''
         getting token by handshaking with vacuum
         '''
         print('Sending handshake to get token')
         m = self.vacuum.do_discover()
         self.vacuum.token = m.checksum
-        self.token = codecs.encode(m.checksum, 'hex')
+        self.set_token(codecs.encode(m.checksum, 'hex'))
 
     def test_connection(self):
         '''
@@ -92,3 +188,186 @@ class VacuumController():
         except Exception as e:
             print('Err: {}'.format(e))
         return False
+
+    def update_script(self, filepath="init_vacuum.sh"):
+        subprocess.call("./{}".format(filepath, ), shell=True)
+        return
+
+    def _config(self, cmd):
+        if cmd[0] == 'help':
+            config_help()
+        elif cmd[0] == 'quit' or cmd[0] =='exit':
+            raise EOFError
+        elif cmd[0] == 'set':
+            if len(cmd) < 3:
+                print("Insufficient command")
+                return
+            self.set_config(cmd[1], cmd[2])
+        elif cmd[0] == 'get':
+            if len(cmd) == 1:
+                print(self.config)
+            else:
+                [print(self.config.get(val, None)) for val in cmd[1:]]
+        elif cmd[0] == 'save':
+            filepath = cmd[1] if len(cmd) > 1 else './config.json'
+            json.dump(self.config, open(filepath, 'w'))
+            print("Configs: {}".format(self.config))
+            print("Saved to {}".format(filepath))
+        elif cmd[0] == 'load':
+            filepath = cmd[1] if len(cmd) > 1 else './config.json'
+            if not os.path.isfile(filepath):
+                print("{} does not exit".format(filepath))
+                return
+            self.config = json.load(open(filepath, 'r'))
+            print("Loaded from {}".format(filepath))
+            print("Configs: {}".format(self.config))
+
+    def configuration(self, cmd=None):
+        '''
+        configuration
+        '''
+        if cmd:
+            self._config(cmd)
+            return
+        while 1:
+            try:
+                cmd = input("config >>> ").split(" ")
+                self._config(cmd)
+            except KeyboardInterrupt:
+                print("KeyboardInterrupt")
+                continue
+            except EOFError:
+                print("Exiting config..")
+                break
+            except BaseException as e:
+                print("Err: {}".format(e))
+                raise
+
+    def _control(self, cmd):
+        if cmd[0] == 'help':
+            control_help()
+        elif cmd[0] == 'quit' or cmd[0] =='exit':
+            raise EOFError
+        elif cmd[0] == 'config':
+            self.configuration(cmd[1:])
+        elif cmd[0] == 'trace':
+            if len(cmd) == 1:
+                print("Insufficient command")
+                return False
+            if cmd[1] == 'on' or cmd[1] == 'start' or cmd[1] == 'enable':
+                print("Running script on vacuum..")
+                if run_ssh_command(
+                    "python3 {0}/get_loc_est.py {1} &"
+                    .format(self.get_remote_folder, "tmp.csv")
+                ) is None:
+                    return False
+            elif cmd[1] == 'off' or cmd[1] == 'stop' or cmd[1] == 'disable':
+                print("Stopping python3..")
+                if run_ssh_command("killall python3") is None:
+                    return False
+            else:
+                print("Unknown command: {}".format(cmd))
+                return False
+        elif cmd[0] == 'download':
+            prefix = time.strftime("%Y%m%d_%H%M%S", time.localtime())
+            if len(cmd) == 1:
+                return (
+                    self._control(['download', 'map', prefix]) and
+                    self._control(['download', 'trace', prefix])
+                )
+            if cmd[1] == 'trace':
+                # download the file, after then we delete it 
+                if not fetch_file_from_vacuum(
+                    "{}/tmp_slam.csv"
+                    .format(self.get_remote_folder()),
+                    "./{}_loc.csv"
+                    .format(cmd[2] if len(cmd) > 2 else prefix)
+                ):
+                    return False
+            elif cmd[1] == 'map':
+                # find file first
+                content = run_ssh_command(
+                    "ls /run/shm/*.ppm"
+                )
+                if not content:
+                    return False
+                files = content.rstrip().split('\n')
+                if len(files) == 0:
+                    print("Cannot find map file!")
+                    return False
+                # download only the last ppm file
+                if not fetch_file_from_vacuum(
+                    files[-1],
+                    "./{}_map.ppm".format(cmd[2] if len(cmd) > 2 else prefix)
+                ):
+                    return False
+            else:
+                print("Unknown command: {}".format(cmd))
+                return False
+        elif cmd[0] == 'home':
+            print("Returning Home..")
+            self.vacuum.home()
+        elif cmd[0] == 'status':
+            print(self.vacuum.status())
+        elif cmd[0] == 'move':
+            if len(cmd) == 1:
+                print("Insufficient command")
+                return False
+            if cmd[1] == 'auto':
+                print("Starting..")
+                self.vacuum.start()
+            elif cmd[1] == 'pause':
+                print("Pausing..")
+                self.vacuum.pause()
+            elif cmd[1] == 'stop':
+                print("Stopping..")
+                self.vacuum.stop()
+            elif len(cmd) > 2:
+                try:
+                    duration = int(cmd[3]) if len(cmd) > 3 else 1500
+                    self.vacuum.manual_control(int(cmd[1]), float(cmd[2]), duration)
+                except ValueError:
+                    print("Err: rotation in (-180, 180), speed in (-0.3, 0.3), duration as ms in integer (default 1500)")
+                return False
+        elif cmd[0] == 'goto':
+            if len(cmd) < 3:
+                print("Insufficient command")
+            try:
+                self.vacuum.goto(int(cmd[1]), int(cmd[2]))
+            except ValueError:
+                print("Err: please type into integer")
+                return False
+        return True
+
+
+    def manual_control(self, cmd=None):
+        '''
+        manual control
+        '''
+        if cmd:
+            self._control(cmd)
+            return
+        while 1:
+            try:
+                cmd = input("control >>> ").split(" ")
+                self._control(cmd)
+            except KeyboardInterrupt:
+                print("KeyboardInterrupt")
+                continue
+            except EOFError:
+                print("Exiting control..")
+                break
+            except BaseException as e:
+                print("Err: {}".format(e))
+                raise
+
+
+def init_controller(ip, token):
+    c = VacuumController(ip=ip, token=token, forceScan=False)
+    # if not c.test_connection():
+    #     c = VacuumController(forceScan=True)
+    #     if not c.test_connection():
+    #         print("Cannot connect to vacuum!")
+    #         exit(-1)
+
+    return c