def __init__(self, host='localhost', port=65000, robot='mana'):
""" Create the socket that will be used to commmunicate to the server. """
self.robot = robot
self.node_stream = None
logger.debug("Connecting to %s:%d" % (host, port))
try:
self.node_stream = StreamJSON(host, port)
self.poll_thread = PollThread()
self.poll_thread.start()
if self.node_stream.connected:
logger.info("Connected to %s:%s" % (host, port))
except Exception as e:
logger.info("Multi-node simulation not available!")
logger.warning("Unable to connect to %s:%s" % (host, port))
logger.info(str(e))
def initialize(self):
"""
Create the socket that will be used to commmunicate to the server.
"""
self.node_stream = None
self.poll_thread = None
logger.debug("Connecting to %s:%d" % (self.host, self.port) )
try:
self.node_stream = StreamJSON(self.host, self.port)
self.poll_thread = PollThread()
self.poll_thread.start()
if self.node_stream.connected:
logger.info("Connected to %s:%s" % (self.host, self.port) )
except Exception as err:
logger.info("Multi-node simulation not available!")
logger.warning("Unable to connect to %s:%s"%(self.host, self.port) )
logger.warning(str(err))
def __init__(self, host='localhost', port=65000, robot='mana'):
""" Create the socket that will be used to commmunicate to the server. """
self.robot = robot
self.node_stream = None
logger.debug("Connecting to %s:%d"%(host, port))
try:
self.node_stream = StreamJSON(host, port)
self.poll_thread = PollThread()
self.poll_thread.start()
if self.node_stream.connected:
logger.info("Connected to %s:%s" % (host, port))
except Exception as e:
logger.info("Multi-node simulation not available!")
logger.warning("Unable to connect to %s:%s"%(host, port))
logger.info(str(e))
def publish_stream(socketio, frequency=1):
node_stream = StreamJSON("localhost", 65000)
poll_thread = PollThread()
poll_thread.start()
while node_stream.is_up():
node_stream.publish(["morseweb", {}])
try:
data = node_stream.get(timeout=1e-3) or node_stream.last()
except JSONDecodeError:
pass
else:
socketio.emit("simulator.update", data)
time.sleep(frequency)
class HybridNode(object):
""" Class definition for synchronisation of real robots with the MORSE simulator
This component allows hybrid simulation, where the position of real robots
is reflected in the simulator. Implemented using the socket multinode
mechanism.
"""
out_data = {}
def __init__(self, host='localhost', port=65000, robot='mana'):
""" Create the socket that will be used to commmunicate to the server. """
self.robot = robot
self.node_stream = None
logger.debug("Connecting to %s:%d" % (host, port))
try:
self.node_stream = StreamJSON(host, port)
self.poll_thread = PollThread()
self.poll_thread.start()
if self.node_stream.connected:
logger.info("Connected to %s:%s" % (host, port))
except Exception as e:
logger.info("Multi-node simulation not available!")
logger.warning("Unable to connect to %s:%s" % (host, port))
logger.info(str(e))
def _exchange_data(self):
""" Send and receive pickled data through a socket """
# Use the existing socket connection
self.node_stream.publish(['hybrid', self.out_data])
# Wait 1ms for incomming data or return the last one received
return self.node_stream.get(timeout=.001) or self.node_stream.last()
def synchronize(self):
if not self.node_stream:
logger.debug("not self.node_stream")
return
if not self.node_stream.connected:
logger.debug("not self.node_stream.connected")
return
try:
pose = get_robot_pose(robot_picoweb[self.robot])
pose['z'] = 2 # XXX hack at laas
self.out_data[self.robot] = [(pose['x'], pose['y'], pose['z']),
(pose['roll'], pose['pitch'],
pose['yaw'])]
logger.info(repr(self.out_data))
except Exception as e:
logger.error(str(e))
# Send the encoded dictionary through a socket
# and receive a reply with any changes in the other nodes
in_data = self._exchange_data()
logger.debug("Received: %s" % repr(in_data))
def __del__(self):
""" Close the communication socket. """
if self.node_stream:
self.node_stream.close()
# asyncore.close_all() # make sure all connection are closed
if 'poll_thread' in dir(self):
self.poll_thread.syncstop()
class SocketNode(SimulationNodeClass):
"""
Implements multinode simulation using sockets.
"""
out_data = {}
def initialize(self):
"""
Create the socket that will be used to commmunicate to the server.
"""
self.node_stream = None
self.poll_thread = None
self.simulation_time = blenderapi.persistantstorage().time
logger.debug("Connecting to %s:%d" % (self.host, self.port))
try:
self.node_stream = StreamJSON(self.host, self.port)
self.poll_thread = PollThread()
self.poll_thread.start()
if self.node_stream.connected:
logger.info("Connected to %s:%s" % (self.host, self.port))
except Exception as err:
logger.info("Multi-node simulation not available!")
logger.warning("Unable to connect to %s:%s" %
(self.host, self.port))
logger.warning(str(err))
def _exchange_data(self, out_data):
""" Send and receive pickled data through a socket """
# Use the existing socket connection
self.node_stream.publish([self.node_name, out_data])
return self.node_stream.get(timeout=.1) or self.node_stream.last()
def synchronize(self):
if not self.node_stream:
logger.debug("not self.node_stream")
return
if not self.node_stream.connected:
logger.debug("not self.node_stream.connected")
return
# Get the coordinates of local robots
for obj in blenderapi.persistantstorage().robotDict.keys():
#self.out_data[obj.name] = [obj.worldPosition.to_tuple()]
euler_rotation = obj.worldOrientation.to_euler()
self.out_data[obj.name] = [obj.worldPosition.to_tuple(), \
[euler_rotation.x, euler_rotation.y, euler_rotation.z]]
self.out_data['__time'] = [
self.simulation_time.time, 1.0 / blenderapi.getfrequency(),
self.simulation_time.real_time
]
# Send the encoded dictionary through a socket
# and receive a reply with any changes in the other nodes
in_data = self._exchange_data(self.out_data)
logger.debug("Received: %s" % in_data)
if not in_data:
return
try:
self.update_scene(in_data, blenderapi.scene())
except Exception as e:
logger.warning("error while processing incoming data: " + str(e))
def update_scene(self, in_data, scene):
# Update the positions of the external robots
for obj_name, robot_data in in_data.items():
try:
obj = scene.objects[obj_name]
except Exception as e:
logger.debug(
"%s not found in this simulation scenario, but present in another node. Ignoring it!"
% obj_name)
continue
if obj not in blenderapi.persistantstorage().robotDict:
obj.worldPosition = robot_data[0]
obj.worldOrientation = mathutils.Euler(
robot_data[1]).to_matrix()
def finalize(self):
""" Close the communication socket. """
if self.node_stream:
self.node_stream.close()
self.node_stream = None
if self.poll_thread:
self.poll_thread.syncstop(1)
self.poll_thread = None
class SocketNode(SimulationNodeClass):
"""
Implements multinode simulation using sockets.
"""
out_data = {}
def initialize(self):
"""
Create the socket that will be used to commmunicate to the server.
"""
self.node_stream = None
self.poll_thread = None
self.simulation_time = blenderapi.persistantstorage().time
logger.debug("Connecting to %s:%d" % (self.host, self.port) )
try:
self.node_stream = StreamJSON(self.host, self.port)
self.poll_thread = PollThread()
self.poll_thread.start()
if self.node_stream.connected:
logger.info("Connected to %s:%s" % (self.host, self.port) )
except Exception as err:
logger.info("Multi-node simulation not available!")
logger.warning("Unable to connect to %s:%s"%(self.host, self.port) )
logger.warning(str(err))
def _exchange_data(self, out_data):
""" Send and receive pickled data through a socket """
# Use the existing socket connection
self.node_stream.publish([self.node_name, out_data])
return self.node_stream.get(timeout=.1) or self.node_stream.last()
def synchronize(self):
if not self.node_stream:
logger.debug("not self.node_stream")
return
if not self.node_stream.connected:
logger.debug("not self.node_stream.connected")
return
# Get the coordinates of local robots
for obj in blenderapi.persistantstorage().robotDict.keys():
#self.out_data[obj.name] = [obj.worldPosition.to_tuple()]
euler_rotation = obj.worldOrientation.to_euler()
self.out_data[obj.name] = [obj.worldPosition.to_tuple(), \
[euler_rotation.x, euler_rotation.y, euler_rotation.z]]
self.out_data['__time'] = [self.simulation_time.time, 1.0/ blenderapi.getfrequency(), self.simulation_time.real_time]
# Send the encoded dictionary through a socket
# and receive a reply with any changes in the other nodes
in_data = self._exchange_data(self.out_data)
logger.debug("Received: %s" % in_data)
if not in_data:
return
try:
self.update_scene(in_data, blenderapi.scene())
except Exception as e:
logger.warning("error while processing incoming data: " + str(e))
def update_scene(self, in_data, scene):
# Update the positions of the external robots
for obj_name, robot_data in in_data.items():
try:
obj = scene.objects[obj_name]
except Exception as e:
logger.debug("%s not found in this simulation scenario, but present in another node. Ignoring it!" % obj_name)
continue
if obj not in blenderapi.persistantstorage().robotDict:
obj.worldPosition = robot_data[0]
obj.worldOrientation = mathutils.Euler(robot_data[1]).to_matrix()
def finalize(self):
""" Close the communication socket. """
if self.node_stream:
self.node_stream.close()
self.node_stream = None
if self.poll_thread:
self.poll_thread.syncstop(1)
self.poll_thread = None
class HybridNode(object):
""" Class definition for synchronisation of real robots with the MORSE simulator
This component allows hybrid simulation, where the position of real robots
is reflected in the simulator. Implemented using the socket multinode
mechanism.
"""
out_data = {}
def __init__(self, host='localhost', port=65000, robot='mana'):
""" Create the socket that will be used to commmunicate to the server. """
self.robot = robot
self.node_stream = None
logger.debug("Connecting to %s:%d"%(host, port))
try:
self.node_stream = StreamJSON(host, port)
self.poll_thread = PollThread()
self.poll_thread.start()
if self.node_stream.connected:
logger.info("Connected to %s:%s" % (host, port))
except Exception as e:
logger.info("Multi-node simulation not available!")
logger.warning("Unable to connect to %s:%s"%(host, port))
logger.info(str(e))
def _exchange_data(self):
""" Send and receive pickled data through a socket """
# Use the existing socket connection
self.node_stream.publish(['hybrid', self.out_data])
# Wait 1ms for incomming data or return the last one received
return self.node_stream.get(timeout=.001) or self.node_stream.last()
def synchronize(self):
if not self.node_stream:
logger.debug("not self.node_stream")
return
if not self.node_stream.connected:
logger.debug("not self.node_stream.connected")
return
try:
pose = get_robot_pose(robot_picoweb[self.robot])
pose['z'] = 2 # XXX hack at laas
self.out_data[self.robot] = [ (pose['x'], pose['y'], pose['z']),
(pose['roll'], pose['pitch'], pose['yaw']) ]
logger.info(repr(self.out_data))
except Exception as e:
logger.error(str(e))
# Send the encoded dictionary through a socket
# and receive a reply with any changes in the other nodes
in_data = self._exchange_data()
logger.debug("Received: %s" % repr(in_data))
def __del__(self):
""" Close the communication socket. """
if self.node_stream:
self.node_stream.close()
# asyncore.close_all() # make sure all connection are closed
if 'poll_thread' in dir(self):
self.poll_thread.syncstop()
