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'
