1#! /usr/bin/env python
2"""
3hybrid node
4===========
5
6Proxy between pocolibs and morse. Get real robot pose from picoweb and publish
7it to morse multinode server (socket, not HLA).
8
9usage: python hybrid_node.py mana|minnie [-d]
10"""
11import sys
12import time
13import urllib
14import logging
15from lxml import etree
16from pymorse.stream import StreamJSON, PollThread
17
18# initialize the logger
19logger = logging.getLogger(__name__)
20handler = logging.StreamHandler()
21handler.setFormatter( logging.Formatter('[%(asctime)s][%(name)s][%(levelname)s] %(message)s') )
22logger.addHandler( handler )
23logger.setLevel(logging.INFO)
24
25result = {
26    'x': 0.0,
27    'y': 0.0,
28    'z': 0.0,
29    'yaw':   0.0,
30    'pitch': 0.0,
31    'roll':  0.0,
32}
33
34def get_robot_pose(picoweb):
35    xml = etree.parse(urllib.urlopen("%s/pom?get=Pos"%picoweb))
36    res = xml.xpath("/pom/Pos/data/pomPos/mainToOrigin/euler")[0]
37    return {key: float(res.xpath(key)[0].text) for key in result.keys()}
38
39robot_picoweb = {
40    'mana'  : 'http://140.93.16.55:8080',
41    'minnie': 'http://140.93.16.57:8080',
42    #'mana'  : 'http://mana-superbase:8080',
43    #'minnie': 'http://minnie-base:8080',
44    #'target': 'http://chrome-dreams:8080',
45}
46
47class HybridNode(object):
48    """ Class definition for synchronisation of real robots with the MORSE simulator
49
50    This component allows hybrid simulation, where the position of real robots
51    is reflected in the simulator. Implemented using the socket multinode
52    mechanism.
53    """
54    out_data = {}
55
56    def __init__(self, host='localhost', port=65000, robot='mana'):
57        """ Create the socket that will be used to commmunicate to the server. """
58        self.robot = robot
59        self.node_stream = None
60        logger.debug("Connecting to %s:%d"%(host, port))
61        try:
62            self.node_stream = StreamJSON(host, port)
63            self.poll_thread = PollThread()
64            self.poll_thread.start()
65            if self.node_stream.connected:
66                logger.info("Connected to %s:%s" % (host, port))
67        except Exception as e:
68            logger.info("Multi-node simulation not available!")
69            logger.warning("Unable to connect to %s:%s"%(host, port))
70            logger.info(str(e))
71
72    def _exchange_data(self):
73        """ Send and receive pickled data through a socket """
74        # Use the existing socket connection
75        self.node_stream.publish(['hybrid', self.out_data])
76        # Wait 1ms for incomming data or return the last one received
77        return self.node_stream.get(timeout=.001) or self.node_stream.last()
78
79    def synchronize(self):
80        if not self.node_stream:
81            logger.debug("not self.node_stream")
82            return
83        if not self.node_stream.connected:
84            logger.debug("not self.node_stream.connected")
85            return
86
87        try:
88            pose = get_robot_pose(robot_picoweb[self.robot])
89            pose['z'] = 2 # XXX hack at laas
90            self.out_data[self.robot] = [ (pose['x'], pose['y'], pose['z']),
91                (pose['roll'], pose['pitch'], pose['yaw']) ]
92            logger.info(repr(self.out_data))
93        except Exception as e:
94            logger.error(str(e))
95
96        # Send the encoded dictionary through a socket
97        #  and receive a reply with any changes in the other nodes
98        in_data = self._exchange_data()
99        logger.debug("Received: %s" % repr(in_data))
100
101    def __del__(self):
102        """ Close the communication socket. """
103        if self.node_stream:
104            self.node_stream.close()
105        # asyncore.close_all() # make sure all connection are closed
106        if 'poll_thread' in dir(self):
107            self.poll_thread.syncstop()
108
109def main(argv):
110    if len(argv) < 2:
111        print(__doc__)
112        return 1
113    if '-d' in argv[1:]:
114        logger.setLevel(logging.DEBUG)
115
116    logger.debug("Hybrid node started...")
117    hn = HybridNode(robot=argv[1])
118
119    try:
120        while 1:
121            hn.synchronize()
122            time.sleep(0.5)
123    except KeyboardInterrupt:
124        logger.info("Quit (Ctrl+C)")
125    finally:
126        del hn
127        logger.info("Closing all connections")
128
129    logger.info("Bye!")
130    return 0
131
132if __name__ == '__main__':
133    import sys
134    sys.exit(main(sys.argv))
135