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