8000 working · navrobot/robotics-toolbox-python@bb62d04 · GitHub
[go: up one dir, main page]
Skip to content

Commit bb62d04

Browse files
jhavljhavl
committed
working
1 parent 00b5f8f commit bb62d04

File tree

2 files changed

+12
-142
lines changed

2 files changed

+12
-142
lines changed

‎roboticstoolbox/backend/Swift/Swift.py

Lines changed: 8 additions & 138 deletions
Original file line numberDiff line numberDiff line change
@@ -3,22 +3,13 @@
33
@author Jesse Haviland
44
"""
55

6-
from subprocess import call, Popen
76
from roboticstoolbox.backend.Connector import Connector
87
import roboticstoolbox as rp
98
import numpy as np
109
import spatialmath as sm
1110
import time
12-
import websockets
13-
import asyncio
14-
from threading import Thread
15-
from queue import Queue, Empty
16-
import webbrowser as wb
17-
import json
18-
import http.server
19-
import socketserver
20-
from pathlib import Path
21-
import os
11+
from queue import Queue
12+
from swift import start_servers
2213

2314

2415
class Swift(Connector): # pragma nocover
@@ -71,40 +62,7 @@ def launch(self):
7162

7263
super().launch()
7364

74-
# Start a http server
75-
self.server = Thread(
76-
target=Server, args=(self.inq, ), daemon=True)
77-
self.server.start()
78-
http_port = self.inq.get()
79-
80-
# Start our websocket server with a new clean port
81-
self.socket = Thread(
82-
target=Socket, args=(self.outq, self.inq, ), daemon=True)
83-
self.socket.start()
84-
port = self.inq.get()
85-
86-
# Launch the simulator
87-
wb.open_new_tab('http://localhost:' + str(http_port))
88-
# wb.open_new_tab('file:///home/jesse/swift/public/index.html')
89-
90-
# Let swift know which port to talk on using the common port
91-
loop = asyncio.new_event_loop()
92-
93-
async def send_port(websocket, path):
94-
await websocket.send(str(port))
95-
await websocket.wait_closed()
96-
loop.stop()
97-
98-
asyncio.set_event_loop(loop)
99-
port_ws = websockets.serve(send_port, "localhost", 8997)
100-
loop.run_until_complete(port_ws)
101-
loop.run_forever()
102-
103-
try:
104-
self.inq.get(timeout=10)
105-
except Empty:
106-
print('\nCould not connect to the Swift simulator \n')
107-
raise
65+
start_servers(self.outq, self.inq)
10866

10967
def step(self, dt=50):
11068
"""
@@ -198,9 +156,9 @@ def add(self, ob, show_robot=True, show_collision=False):
198156
when the ``step()`` method is called.
199157
200158
"""
201-
# id = add(robot) adds the robot to the external environment. robot must
202-
# be of an appropriate class. This adds a robot object to a list of
203-
# robots which will act upon the step() method being called.
159+
# id = add(robot) adds the robot to the external environment. robot
160+
# must be of an appropriate class. This adds a robot object to a
161+
# list of robots which will act upon the step() method being called.
204162

205163
# TODO can add more than a robot right?
206164

@@ -215,8 +173,6 @@ def add(self, ob, show_robot=True, show_collision=False):
215173
loaded = 0
216174
while loaded == 0:
217175
loaded = int(self._send_socket('is_loaded', id))
218-
print('loafr')
219-
print(loaded)
220176
time.sleep(0.1)
221177

222178
self.robots.append(ob)
@@ -232,7 +188,8 @@ def remove(self):
232188
"""
233189
Remove a robot to the graphical scene
234190
235-
``env.remove(robot)`` removes the ``robot`` from the graphical environment.
191+
``env.remove(robot)`` removes the ``robot`` from the graphical
192+
environment.
236193
"""
237194

238195
# TODO - shouldn't this have an id argument? which robot does it remove
@@ -301,90 +258,3 @@ def _send_socket(self, code, data):
301258

302259
self.outq.put(msg)
303260
return self.inq.get()
304-
305-
306-
class Socket:
307-
308-
def __init__(self, outq, inq):
309-
self.outq = outq
310-
self.inq = inq
311-
self.USERS = set()
312-
loop = asyncio.new_event_loop()
313-
asyncio.set_event_loop(loop)
314-
315-
started = False
316-
port = 51478
317-
318-
while not started and port < 62000:
319-
try:
320-
port += 1
321-
start_server = websockets.serve(self.serve, "localhost", port)
322-
loop.run_until_complete(start_server)
323-
started = True
324-
except OSError:
325-
pass
326-
327-
self.inq.put(port)
328-
loop.run_forever()
329-
330-
async def register(self, websocket):
331-
self.USERS.add(websocket)
332-
333-
async def serve(self, websocket, path):
334-
335-
# Initial connection handshake
336-
await(self.register(websocket))
337-
recieved = await websocket.recv()
338-
self.inq.put(recieved)
339-
340-
# Now onto send, recieve cycle
341-
while True:
342-
message = await self.producer()
343-
await websocket.send(json.dumps(message))
344-
345-
recieved = await websocket.recv()
346-
self.inq.put(recieved)
347-
print(recieved)
348-
349-
async def producer(self):
350-
data = self.outq.get()
351-
return data
352-
353-
354-
class Server:
355-
356-
def __init__(self, inq):
357-
358-
PORT = 52000
359-
self.inq = inq
360-
361-
root_dir = Path(rp.__file__).parent / 'public'
362-
os.chdir(Path.home())
363-
364-
class MyHttpRequestHandler(http.server.SimpleHTTPRequestHandler):
365-
def do_GET(self):
366-
367-
home = str(Path.home())
368-
369-
if self.path == '/':
370-
self.path = str(root_dir / 'index.html')
371-
elif self.path.endswith('css') or self.path.endswith('js'):
372-
self.path = str(root_dir) + self.path
373-
374-
if self.path.startswith(home):
375-
self.path = self.path[len(home):]
376-
377-
return http.server.SimpleHTTPRequestHandler.do_GET(self)
378-
379-
Handler = MyHttpRequestHandler
380-
381-
connected = False
382-
383-
while not connected and PORT < 62000:
384-
try:
385-
with socketserver.TCPServer(("", PORT), Handler) as httpd:
386-
self.inq.put(PORT)
387-
connected = True
388-
httpd.serve_forever()
389-
except OSError:
390-
PORT += 1

‎roboticstoolbox/backend/__init__.py

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -1,10 +1,10 @@
11
from roboticstoolbox.backend.PyPlot import *
22
from roboticstoolbox.backend.urdf import *
33

4-
try:
5-
from roboticstoolbox.backend.Swift import *
6-
except ImportError:
7-
pass
4+
# try:
5+
from roboticstoolbox.backend.Swift import *
6+
# except ImportError:
7+
# pass
88

99
try:
1010
from roboticstoolbox.backend.VPython import *

0 commit comments

Comments
 (0)
0