def __start_data_service(self): print 'Starting Data Service' #Create data service if self.proxy is None: raise m3t.M3Exception('M3RtProxy not started') #if self.proxy.IsDataServiceRunning(): # print 'M3RtDataService already running on port',self.data_port # print 'Stopping existing connection...' # self.proxy.RemoveDataService() port = self.proxy.AttachDataService() if port == -1: raise m3t.M3Exception('Unable to attach M3RtDataService') #print '----------------' #print port #print '----------------' self.data_port = port #Create data stream socket try: self.data_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) self.data_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) if self.use_timeout: self.data_socket.setblocking(0) # w a timeout self.data_socket.settimeout(2.0) # Only works for connect except socket.error, msg: self.__stop_data_service() raise m3t.M3Exception('Error: ' + msg[1])
def __check_component(self,component): """Verify that the component type matches the server type""" if self.proxy is None: raise m3t.M3Exception('M3RtProxy not started') idx=self.proxy.GetComponentIdx(component.name) if idx==-1: raise m3t.M3Exception('Component '+component.name+' not available') type=self.proxy.GetComponentType(idx) if type!=component.type: raise m3t.M3Exception('Component type mismatch '+type+' , '+component.type)
def __init__(self, host=None, rpc_port=8000, verbose=True): """M3RtProxy is the client interface to the M3RtServer. It manages the state of the server using XML_RPC methods. It can query the server state, start/stop the run-time system, create a DataService connection, and publish/subscribe desired components to the DataService. The DataService uses a faster TCP/IP socket on port 10000""" self.stopped = False self.host = host self.verbose = verbose if host is None: self.host = m3t.get_config_hostname() if self.host is None: self.host = m3t.get_local_hostname() self.rpc_port = rpc_port self.data_port = 10000 #Currently hardcoded in M3 self.proxy = None self.data_socket = None self.subscribed = {} self.published_param = {} self.published_command = {} self.available_components = [] self.available_component_types = [] self.log_comps = {} self.log_names = [] self.logname = None self.status_raw = mbs.M3StatusAll() self.command_raw = mbs.M3CommandAll() self.ns = 0 self.nsl = 0 self.data_svc = None self.ros_svc = None self.use_timeout = True self.is_server_started = False try: if self.verbose: print 'Starting M3 RPC Client at ', self.host, 'on Port ', self.rpc_port, '...' self.proxy = xmlrpclib.ServerProxy('http://' + self.host + ':' + str(self.rpc_port)) if self.verbose: print 'M3 RPC Client started at ', self.host, 'on Port ', self.rpc_port #Check that connection made try: self.proxy.system.listMethods() except xmlrpclib.Error, v: self.proxy = None raise m3t.M3Exception( 'Error: ' + v + 'Make sure that the M3 RPC Server is running') except socket.error, msg: self.proxy = None raise m3t.M3Exception( 'Check that server is started. Socket Error: ' + str(msg))
def load_log_sample(self, logname, idx): """Load sample idx into the subscribed components""" if self.logname != logname: if not self._load_log(logname): return #Load new page if out of bounds if idx < self.log_start_idx or idx > self.log_end_idx: search_idx = (self.log_file_idx + 1) % len(self.log_info) while True: if self.log_info[search_idx][ 'start_idx'] <= idx and self.log_info[search_idx][ 'end_idx'] >= idx: self.log_file_idx = search_idx self.log_start_idx = self.log_info[search_idx]['start_idx'] self.log_end_idx = self.log_info[search_idx]['end_idx'] break search_idx = (search_idx + 1) % len(self.log_info) if search_idx == self.log_file_idx: raise m3t.M3Exception( 'M3RtProxy invalid log sample idx: ' + str(idx)) filename = self.log_info[self.log_file_idx]['filename'] self.log_page = mbs.M3StatusLogPage() s = self.proxy.get_log_file(filename).data self.log_page.ParseFromString(s) entry_idx = idx - self.log_info[self.log_file_idx]['start_idx'] status_all = self.log_page.entry[entry_idx] for i in range(len(status_all.name)): for name in self.log_names: if name == status_all.name[i]: self.log_comps[name].status.ParseFromString( status_all.datum[i])
def __start_rt_system(self): try: try: self.rtsys_id = self.proxy.AttachRtSystem() print "Rt System attached with id ", self.rtsys_id if self.rtsys_id == -1: raise m3t.M3Exception('M3RtSystem still online') if self.rtsys_id == 0: #failed to start print "Attaching a new rt system failed" self.stop() raise m3t.M3Exception( 'Unable to start M3RtSystem. Try restarting server') except xmlrpclib.ProtocolError, v: raise m3t.M3Exception("xmlrpclib.ProtocolError " + str(v)) except Exception, e: raise m3t.M3Exception("__start_rt_system " + str(e))
def start(self,start_data_svc=True,start_ros_svc=False): """Startup the RtSystem on the server. This will load all available components and begin execution in state SAFEOP. It can also start a DataService""" try: if not self.is_server_started: self.__start_rt_system() except Exception,e: raise m3t.M3Exception('self.__start_rt_system() failed, some components might be faulty ; '+str(e))
def __recv_status(self): if self.data_socket is None: m3t.M3Exception('M3RtProxy data socket not created') nr=array.array('I') rcv=self.__do_receive(4) if (len(rcv)!=4): raise m3t.M3Exception('Incorrect packet recv size from proxy') nr.fromstring(rcv) nr=nr[0] data=self.__do_receive(nr) self.status_raw.ParseFromString(data) for name,v in self.subscribed.items(): for j in range(len(self.status_raw.name)): if name==self.status_raw.name[j]: if len(self.status_raw.datum[j]): #Allow 0 len on serialize errors v['status'].ParseFromString(self.status_raw.datum[j]) v['component'].update_status()
def start_log_service(self,logname, sample_freq_hz=100,samples_per_file=100,logpath=None,verbose=True): """Start logging registered components to directory logname""" if logpath is None: logpath=os.environ['M3_ROBOT'] logpath = logpath.split(':') #Tmp : just get the first one logpath = logpath[-1]+'/robot_log' if not self.proxy.IsRtSystemRunning(): raise m3t.M3Exception('Cannot start log. M3RtSystem is not yet running on the server') return self.proxy.start_log_service(logname,float(sample_freq_hz),self.log_names,int(samples_per_file),logpath,verbose)
def __start_rt_system(self): try: try: self.rtsys_id = self.proxy.AttachRtSystem() if self.rtsys_id == -1: raise m3t.M3Exception('M3RtSystem still online') if self.rtsys_id == 0: #failed to start print "Attaching a new rt system failed" self.stop() raise m3t.M3Exception( 'Unable to start M3RtSystem. Try restarting server') except xmlrpclib.ProtocolError, v: raise m3t.M3Exception(v) #Query available components self.available_components = [] n = self.proxy.GetNumComponents() for i in range(n): name = self.proxy.GetComponentName(i) self.available_components.append(name) #Query component types self.available_component_types = [] for i in range(n): ttype = self.proxy.GetComponentType(i) self.available_component_types.append(ttype)
def __start_rt_system(self): self.rtsys_id=self.proxy.AttachRtSystem() if self.rtsys_id==-1: raise m3t.M3Exception('M3RtSystem still online') if self.rtsys_id==0: #failed to start print "Attaching a new rt system failed" self.stop() raise m3t.M3Exception('Unable to start M3RtSystem. Try restarting server') #Query available components self.available_components=[] n=self.proxy.GetNumComponents() print "Getting all ",n,"components" for i in xrange(n): print "Getting component ",i, name=self.proxy.GetComponentName(i) print " : ",name,self.proxy.GetComponentType(i) self.available_components.append(name) #Query component types self.available_component_types=[] for i in xrange(n): ttype=self.proxy.GetComponentType(i) self.available_component_types.append(ttype)
def __do_receive(self,nr,timeout_total=4.0,timeout_chunk = 2.0): msg = '' chunk='' time_s_total = time.time() if self.use_timeout: while len(msg) < nr and not(time.time()-time_s_total>timeout_total): # A.H: That shouldn't take too long # => Adding a timeout ready = select.select([self.data_socket], [], [], timeout_chunk) if ready[0]: chunk = self.data_socket.recv(nr-len(msg)) msg = msg + chunk #chunk = self.data_socket.recv() if chunk == '': raise m3t.M3Exception('Proxy socket connection broken') else: while len(msg) < nr : chunk = self.data_socket.recv(nr-len(msg)) msg = msg + chunk return msg
def __send_command(self): if self.data_socket is None: m3t.M3Exception('M3RtProxy data socket not created') idx = 0 for name, v in self.published_command.items(): self.command_raw.name_cmd[idx] = name v['component'].load_command() self.command_raw.datum_cmd[idx] = v['command'].SerializeToString() idx = idx + 1 idx = 0 for name, v in self.published_param.items(): self.command_raw.name_param[idx] = name v['component'].load_param() self.command_raw.datum_param[idx] = v['param'].SerializeToString() idx = idx + 1 ## A.H : Sending floats allows to run on 64 bits machines : ## sizeof(int) in python32 : 4 bits ## sizeof(float) in python64 : 8 bits -> server hangs ## WORKAROUND : send floats that weights =4 bits so x86 and x64 speak the same language ## Note : simple_server.cpp has to be modified to receive floats as well ! nh = array.array('f', [9999]).tostring() nc = array.array('f', [self.command_raw.ByteSize()]).tostring() sc = self.command_raw.SerializeToString() self.data_socket.sendall(nh + nc + sc)
def pretty_print_component(self, name): """Display component data on server""" if self.proxy is None: raise m3t.M3Exception('M3RtProxy not started') self.proxy.PrettyPrintComponent(name)
'Unable to start M3RtSystem. Try restarting server') except xmlrpclib.ProtocolError, v: raise m3t.M3Exception(v) #Query available components self.available_components = [] n = self.proxy.GetNumComponents() for i in range(n): name = self.proxy.GetComponentName(i) self.available_components.append(name) #Query component types self.available_component_types = [] for i in range(n): ttype = self.proxy.GetComponentType(i) self.available_component_types.append(ttype) except socket.error, msg: raise m3t.M3Exception( 'Check that server is started. Socket Error: ' + str(msg)) def __stop_data_service(self): try: if self.proxy is not None: self.proxy.RemoveDataService(self.data_port) if self.data_socket is not None: self.data_socket.close() self.data_socket = None except socket.error, msg: pass #Ok because shutting down def __start_data_service(self): print 'Starting Data Service' #Create data service if self.proxy is None:
#Create data stream socket try: self.data_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) self.data_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) if self.use_timeout: self.data_socket.setblocking(0) # w a timeout self.data_socket.settimeout(2.0) # Only works for connect except socket.error, msg: self.__stop_data_service() raise m3t.M3Exception('Error: '+msg[1]) #Connect to data stream try: self.data_socket.connect((self.host,self.data_port)) except socket.error, msg: self.__stop_data_service() raise m3t.M3Exception('Error: '+msg[1]) def _load_log(self,logname): self.logname=logname self.log_info=self.proxy.get_log_info(logname) if len(self.log_info)==0: return False self.log_start_idx=self.log_info[0]['start_idx'] self.log_end_idx=self.log_info[0]['end_idx'] self.log_file_idx=0 filename=self.log_info[0]['filename'] self.log_page=mbs.M3StatusLogPage() s=self.proxy.get_log_file(filename).data self.log_page.ParseFromString(s)
def __start_ros_service(self): #Create ros service if self.proxy is None: raise m3t.M3Exception('M3RtProxy not started') self.proxy.AttachRosService()
raise m3t.M3Exception("xmlrpclib.ProtocolError " + str(v)) except Exception, e: raise m3t.M3Exception("__start_rt_system " + str(e)) #Query available components self.available_components = [] n = self.proxy.GetNumComponents() for i in range(n): name = self.proxy.GetComponentName(i) self.available_components.append(name) #Query component types self.available_component_types = [] for i in range(n): ttype = self.proxy.GetComponentType(i) self.available_component_types.append(ttype) except socket.error, msg: raise m3t.M3Exception( 'Check that server is started. Socket Error: ' + str(msg)) except Exception, e: raise m3t.M3Exception('GetNumComponents failed ' + str(e)) def __stop_data_service(self): try: if self.proxy is not None: self.proxy.RemoveDataService(self.data_port) if self.data_socket is not None: self.data_socket.close() self.data_socket = None except socket.error: pass #Ok because shutting down def __start_data_service(self): print 'Starting Data Service'