oro_ros_server.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 import roslib; roslib.load_manifest('oro_ros')
00003 
00004 import sys, json
00005 
00006 from oro_ros.srv import *
00007 from pyoro import *
00008 import rospy
00009 from rospy.exceptions import ROSException
00010 from rospy.client import *
00011 
00012 MAX_CONNECTION_TRIES = 10
00013 
00014 oro = None
00015 
00016 class ROSHandler:
00017         
00018         def __init__(self, oro):
00019                 
00020                 #add the the Oro class all the methods the server declares
00021                 for m in oro.rpc_methods:
00022                         self.add_methods(m)
00023 
00024         def add_methods(self, m):
00025 
00026                 def innermethod(req):
00027                         rospy.loginfo(">>>> Got request for a %s. Trying to parse the parameters %s as JSON content..."%(m[0], str(req)))
00028                         params = [encode8bits(json.loads(p)) for p in req.params]
00029                         rospy.loginfo(">>>> ok! querying oro-server for %s with parameters %s..."%(m[0], str(params)))
00030                         try:
00031                                 handler = getattr(oro, m[0])
00032                         except AttributeError as ae:
00033                                 raise ROSException("Ooops...the RPC method " + m[0] + " is not provided by ORO.")
00034 
00035                         
00036                         return OroServerQueryResponse(json.dumps(handler(*params)))
00037                                 
00038                 innermethod.__doc__ = "Handler for ROS calls to the oro-server %s method." % m[0]
00039                 innermethod.__name__ = "handle_query_" + m[0]
00040                 setattr(self,innermethod.__name__,innermethod)
00041                 
00042 
00043 
00044 def oro_ros_server():
00045         rospy.init_node('oro_ros_server')
00046         
00047         rosHandler = ROSHandler(oro)
00048         
00049         for srv in oro.rpc_methods:
00050                 try:
00051                         s = rospy.Service("oro/" + srv[0], OroServerQuery, getattr(rosHandler, "handle_query_" + srv[0]))
00052                         rospy.loginfo("Registered ROS service oro/" + srv[0])
00053                 except rospy.service.ServiceException:
00054                         continue
00055 
00056         rospy.loginfo("The ROS-ORO bridge services are running.")
00057         rospy.loginfo("I'm now waiting for some blingbling to eat!")
00058         rospy.spin()
00059 
00060 def usage():
00061     return "Usage:\noro_ros_server.py ORO_HOSTNAME ORO_PORT"
00062 
00063 def connect(tries):
00064         try:
00065                 tries = tries - 1
00066                 return Oro(HOST, PORT)
00067         except:
00068                 if (tries == 0):
00069                         raise OroServerError("Impossible to connect to the server!")
00070                 sleep(2)
00071                 rospy.loginfo("ROS-ORO bridge: Retrying to connect to ORO-server (" + str(tries) + " tries to go)...")
00072                 return connect(tries)
00073 
00074 def encode8bits(o):
00075         if isinstance(o, list):
00076                 return [encode8bits(o2) for o2 in o]
00077         return o.encode('ascii')
00078         
00079 if __name__ == "__main__":
00080         if len(sys.argv) >= 3:
00081                 HOST = sys.argv[1]
00082                 PORT = int(sys.argv[2])
00083         else:
00084                 print usage()
00085                 sys.exit(1)
00086 
00087         rospy.loginfo("Connecting to oro-server on " + HOST + ":" + str(PORT) + "...")
00088 
00089         try:
00090                 oro = connect(MAX_CONNECTION_TRIES)
00091 
00092                 #print "Available methods:\n"
00093 
00094                 #for srv in oro.rpc_methods:
00095                 #       print "\t- %s"%srv[0]
00096 
00097                 oro_ros_server()
00098 
00099         except OroServerError as ose:
00100                 rospy.logerr('Oups! An error occured on ORO server!')
00101                 print(ose)
00102                 raise ROSException("An error occured on ORO server! Check the oro_ros node log!")
00103         finally:
00104                 if (oro):
00105                         oro.close()
00106 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


oro_ros
Author(s): Séverin Lemaignan
autogenerated on Thu May 23 2013 14:57:24