working

jhavl · jhavl · commit bb62d04a902d · 2020-10-29T08:29:27.000+10:00
diff --git a/roboticstoolbox/backend/Swift/Swift.py b/roboticstoolbox/backend/Swift/Swift.py
@@ -3,22 +3,13 @@
 @author Jesse Haviland
 """
 
-from subprocess import call, Popen
 from roboticstoolbox.backend.Connector import Connector
 import roboticstoolbox as rp
 import numpy as np
 import spatialmath as sm
 import time
-import websockets
-import asyncio
-from threading import Thread
-from queue import Queue, Empty
-import webbrowser as wb
-import json
-import http.server
-import socketserver
-from pathlib import Path
-import os
+from queue import Queue
+from swift import start_servers
 
 
 class Swift(Connector):  # pragma nocover
@@ -71,40 +62,7 @@ def launch(self):
 
         super().launch()
 
-        # Start a http server
-        self.server = Thread(
-            target=Server, args=(self.inq, ), daemon=True)
-        self.server.start()
-        http_port = self.inq.get()
-
-        # Start our websocket server with a new clean port
-        self.socket = Thread(
-            target=Socket, args=(self.outq, self.inq, ), daemon=True)
-        self.socket.start()
-        port = self.inq.get()
-
-        # Launch the simulator
-        wb.open_new_tab('http://localhost:' + str(http_port))
-        # wb.open_new_tab('file:///home/jesse/swift/public/index.html')
-
-        # Let swift know which port to talk on using the common port
-        loop = asyncio.new_event_loop()
-
-        async def send_port(websocket, path):
-            await websocket.send(str(port))
-            await websocket.wait_closed()
-            loop.stop()
-
-        asyncio.set_event_loop(loop)
-        port_ws = websockets.serve(send_port, "localhost", 8997)
-        loop.run_until_complete(port_ws)
-        loop.run_forever()
-
-        try:
-            self.inq.get(timeout=10)
-        except Empty:
-            print('\nCould not connect to the Swift simulator \n')
-            raise
+        start_servers(self.outq, self.inq)
 
     def step(self, dt=50):
         """
@@ -198,9 +156,9 @@ def add(self, ob, show_robot=True, show_collision=False):
               when the ``step()`` method is called.
 
         """
-        # id = add(robot) adds the robot to the external environment. robot must
-        # be of an appropriate class. This adds a robot object to a list of
-        # robots which will act upon the step() method being called.
+        # id = add(robot) adds the robot to the external environment. robot
+        # must be of an appropriate class. This adds a robot object to a
+        # list of robots which will act upon the step() method being called.
 
         # TODO can add more than a robot right?
 
@@ -215,8 +173,6 @@ def add(self, ob, show_robot=True, show_collision=False):
             loaded = 0
             while loaded == 0:
                 loaded = int(self._send_socket('is_loaded', id))
-                print('loafr')
-                print(loaded)
                 time.sleep(0.1)
 
             self.robots.append(ob)
@@ -232,7 +188,8 @@ def remove(self):
         """
         Remove a robot to the graphical scene
 
-        ``env.remove(robot)`` removes the ``robot`` from the graphical environment.
+        ``env.remove(robot)`` removes the ``robot`` from the graphical
+            environment.
         """
 
         # TODO - shouldn't this have an id argument? which robot does it remove
@@ -301,90 +258,3 @@ def _send_socket(self, code, data):
 
         self.outq.put(msg)
         return self.inq.get()
-
-
-class Socket:
-
-    def __init__(self, outq, inq):
-        self.outq = outq
-        self.inq = inq
-        self.USERS = set()
-        loop = asyncio.new_event_loop()
-        asyncio.set_event_loop(loop)
-
-        started = False
-        port = 51478
-
-        while not started and port < 62000:
-            try:
-                port += 1
-                start_server = websockets.serve(self.serve, "localhost", port)
-                loop.run_until_complete(start_server)
-                started = True
-            except OSError:
-                pass
-
-        self.inq.put(port)
-        loop.run_forever()
-
-    async def register(self, websocket):
-        self.USERS.add(websocket)
-
-    async def serve(self, websocket, path):
-
-        # Initial connection handshake
-        await(self.register(websocket))
-        recieved = await websocket.recv()
-        self.inq.put(recieved)
-
-        # Now onto send, recieve cycle
-        while True:
-            message = await self.producer()
-            await websocket.send(json.dumps(message))
-
-            recieved = await websocket.recv()
-            self.inq.put(recieved)
-            print(recieved)
-
-    async def producer(self):
-        data = self.outq.get()
-        return data
-
-
-class Server:
-
-    def __init__(self, inq):
-
-        PORT = 52000
-        self.inq = inq
-
-        root_dir = Path(rp.__file__).parent / 'public'
-        os.chdir(Path.home())
-
-        class MyHttpRequestHandler(http.server.SimpleHTTPRequestHandler):
-            def do_GET(self):
-
-                home = str(Path.home())
-
-                if self.path == '/':
-                    self.path = str(root_dir / 'index.html')
-                elif self.path.endswith('css') or self.path.endswith('js'):
-                    self.path = str(root_dir) + self.path
-
-                if self.path.startswith(home):
-                    self.path = self.path[len(home):]
-
-                return http.server.SimpleHTTPRequestHandler.do_GET(self)
-
-        Handler = MyHttpRequestHandler
-
-        connected = False
-
-        while not connected and PORT < 62000:
-            try:
-                with socketserver.TCPServer(("", PORT), Handler) as httpd:
-                    self.inq.put(PORT)
-                    connected = True
-                    httpd.serve_forever()
-            except OSError:
-                PORT += 1
diff --git a/roboticstoolbox/backend/__init__.py b/roboticstoolbox/backend/__init__.py
@@ -1,10 +1,10 @@
 from roboticstoolbox.backend.PyPlot import *
 from roboticstoolbox.backend.urdf import *
 
-try:
-    from roboticstoolbox.backend.Swift import *
-except ImportError:
-    pass
+# try:
+from roboticstoolbox.backend.Swift import *
+# except ImportError:
+#     pass
 
 try:
     from roboticstoolbox.backend.VPython import *
