예제 #1
0
 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))
예제 #2
0
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)
예제 #3
0
파일: socket.py 프로젝트: mitya57/morse
 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))
예제 #4
0
 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))
예제 #5
0
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()
예제 #6
0
파일: socket.py 프로젝트: zyh1994/morse
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
예제 #7
0
파일: socket.py 프로젝트: alexfig/morse
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
예제 #8
0
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()