3
3
@author Jesse Haviland
4
4
"""
5
5
6
- import os
7
- from subprocess import call , Popen
8
6
from roboticstoolbox .backend .Connector import Connector
9
- import zerorpc
10
7
import roboticstoolbox as rp
11
8
import numpy as np
12
9
import spatialmath as sm
13
10
import time
11
+ from queue import Queue
12
+ from swift import start_servers
14
13
15
14
16
15
class Swift (Connector ): # pragma nocover
@@ -43,7 +42,10 @@ class Swift(Connector): # pragma nocover
43
42
def __init__ (self ):
44
43
super (Swift , self ).__init__ ()
45
44
46
- # Popen(['npm', 'start', '--prefix', os.environ['SIM_ROOT']])
45
+ self .robots = []
46
+ self .shapes = []
47
+ self .outq = Queue ()
48
+ self .inq = Queue ()
47
49
48
50
#
49
51
# Basic methods to do with the state of the external program
@@ -60,11 +62,7 @@ def launch(self):
60
62
61
63
super ().launch ()
62
64
63
- self .robots = []
64
- self .shapes = []
65
-
66
- self .swift = zerorpc .Client ()
67
- self .swift .connect ("tcp://127.0.0.1:4242" )
65
+ start_servers (self .outq , self .inq )
68
66
69
67
def step (self , dt = 50 ):
70
68
"""
@@ -81,9 +79,9 @@ def step(self, dt=50):
81
79
- Each robot in the scene is updated based on
82
80
their control type (position, velocity, acceleration, or torque).
83
81
- Upon acting, the other three of the four control types will be
84
- updated in the internal state of the robot object.
85
- - The control type is defined by the robot object, and not all robot
86
- objects support all control types.
82
+ updated in the internal state of the robot object.
83
+ - The control type is defined by the robot object, and not all
84
+ robot objects support all control types.
87
85
- Execution is blocked for the specified interval
88
86
89
87
"""
@@ -149,17 +147,18 @@ def add(self, ob, show_robot=True, show_collision=False):
149
147
:return: object id within visualizer
150
148
:rtype: int
151
149
152
- ``id = env.add(robot)`` adds the ``robot`` to the graphical environment.
150
+ ``id = env.add(robot)`` adds the ``robot`` to the graphical
151
+ environment.
153
152
154
153
.. note::
155
154
156
155
- Adds the robot object to a list of robots which will be updated
157
156
when the ``step()`` method is called.
158
157
159
158
"""
160
- # id = add(robot) adds the robot to the external environment. robot must
161
- # be of an appropriate class. This adds a robot object to a list of
162
- # 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.
163
162
164
163
# TODO can add more than a robot right?
165
164
@@ -169,26 +168,28 @@ def add(self, ob, show_robot=True, show_collision=False):
169
168
robot = ob .to_dict ()
170
169
robot ['show_robot' ] = show_robot
171
170
robot ['show_collision' ] = show_collision
172
- id = self .swift . robot ( robot )
171
+ id = self ._send_socket ( 'robot' , robot )
173
172
174
- loaded = False
175
- while not loaded :
176
- loaded = self .swift . is_loaded ( id )
173
+ loaded = 0
174
+ while loaded == 0 :
175
+ loaded = int ( self ._send_socket ( 'is_loaded' , id ) )
177
176
time .sleep (0.1 )
178
177
179
178
self .robots .append (ob )
180
179
return id
181
- elif isinstance (ob , rp .Shape ):
182
- shape = ob .to_dict ()
183
- id = self .swift .shape (shape )
184
- self .shapes .append (ob )
185
- return id
180
+ # elif isinstance(ob, rp.Shape):
181
+ # shape = ob.to_dict()
182
+ # id = self.swift.shape(shape)
183
+ # id = self._send_socket('shape', shape)
184
+ # self.shapes.append(ob)
185
+ # return id
186
186
187
187
def remove (self ):
188
188
"""
189
189
Remove a robot to the graphical scene
190
190
191
- ``env.remove(robot)`` removes the ``robot`` from the graphical environment.
191
+ ``env.remove(robot)`` removes the ``robot`` from the graphical
192
+ environment.
192
193
"""
193
194
194
195
# TODO - shouldn't this have an id argument? which robot does it remove
@@ -239,13 +240,21 @@ def _draw_all(self):
239
240
240
241
for i in range (len (self .robots )):
241
242
self .robots [i ].fkine_all ()
242
- self .swift .robot_poses ([i , self .robots [i ].fk_dict ()])
243
+ # self.swift.robot_poses([i, self.robots[i].fk_dict()])
244
+ self ._send_socket ('robot_poses' , [i , self .robots [i ].fk_dict ()])
243
245
244
246
for i in range (len (self .shapes )):
245
- self .swift .shape_poses ([i , self .shapes [i ].fk_dict ()])
247
+ # self.swift.shape_poses([i, self.shapes[i].fk_dict()])
248
+ self ._send_socket ('shape_poses' , [i , self .shapes [i ].fk_dict ()])
249
+
250
+ # def record_start(self, file):
251
+ # self.swift.record_start(file)
252
+
253
+ # def record_stop(self):
254
+ # self.swift.record_stop(1)
246
255
247
- def record_start (self , file ):
248
- self . swift . record_start ( file )
256
+ def _send_socket (self , code , data ):
257
+ msg = [ code , data ]
249
258
250
- def record_stop ( self ):
251
- self .swift . record_stop ( 1 )
259
+ self . outq . put ( msg )
260
+ return self .inq . get ( )
0 commit comments