Go to the documentation of this file.00001
00002
00003 import roslib;
00004 roslib.load_manifest('costmap_services')
00005 import rospy
00006
00007 from costmap_services.srv import GetCost
00008 from costmap_services.srv import ScoreTraj
00009
00010 import time
00011 import numpy as np, math
00012 from collections import deque
00013 import thread
00014
00015 class CostmapServices():
00016 def __init__( self, accum = 1, ns='' ):
00017 try:
00018 rospy.init_node( 'costmap_services_node', anonymous = True )
00019 except:
00020 pass
00021
00022 self.accum = accum
00023 self.buff = deque()
00024
00025 rospy.logout( 'CostmapServices: Waiting for services (ie. %s)' % (ns + '/costmap_services/cs/costmap_getcost'))
00026 rospy.wait_for_service( ns + '/costmap_services/cs/costmap_getcost' )
00027 rospy.wait_for_service( ns + '/costmap_services/cs/costmap_scoretraj' )
00028
00029 self._getcost = rospy.ServiceProxy( ns + '/costmap_services/cs/costmap_getcost', GetCost )
00030 self._scoretraj = rospy.ServiceProxy( ns + '/costmap_services/cs/costmap_scoretraj', ScoreTraj )
00031 rospy.logout( 'CostmapServices: Ready.' )
00032
00033 def scoreTrajectory( self, vx, vy, vtheta ):
00034
00035 try:
00036 score = self._scoretraj( vx, vy, vtheta )
00037 return score.cost
00038 except rospy.exceptions.ROSInterruptException, rospy.service.ServiceException:
00039 return -1.0
00040
00041 def scoreTraj_PosHyst( self, vx, vy, vtheta ):
00042
00043
00044 nv = self.scoreTrajectory( vx, vy, vtheta )
00045
00046 self.buff.append( nv )
00047 if len( self.buff ) > self.accum:
00048 self.buff.popleft()
00049
00050 filt = filter( lambda x: x != -1.0, self.buff )
00051 if filt:
00052 return np.mean( filt )
00053 else:
00054 return -1.0
00055
00056 def scoreTraj_NegHyst( self, vx, vy, vtheta ):
00057
00058
00059 nv = self.scoreTrajectory( vx, vy, vtheta )
00060
00061 self.buff.append( nv )
00062 if len( self.buff ) > self.accum:
00063 self.buff.popleft()
00064
00065 filt = filter( lambda x: x != -1.0, self.buff )
00066 if len(filt) == len(self.buff):
00067 return np.mean( filt )
00068 else:
00069 return -1.0
00070
00071 def getMapCost( self, x, y ):
00072 cost = self._getcost( x, y )
00073 return cost.cost
00074
00075
00076
00077
00078 if __name__ == '__main__':
00079 import optparse
00080 p = optparse.OptionParser()
00081 p.add_option('--ns', action='store', type='string', dest='ns',
00082 help='Uses the namespace [ns] for services: /ns/approach_table/costmap_services/cs/...',
00083 default = '')
00084 p.add_option('--xvel', action='store', type='float', dest='xvel',
00085 help='Forward x velocity [default=0.1m/s]',
00086 default = 0.1)
00087 opt, args = p.parse_args()
00088
00089 trials = 100
00090 cs = CostmapServices( ns = opt.ns)
00091
00092 t0 = time.time()
00093 res = [ cs.getMapCost( 0.0, 0.0 ) for i in xrange(trials) ]
00094 t1 = time.time()
00095 rospy.logout( '\tTotal time: %2.2f' % (t1 - t0))
00096 rospy.logout( '\tCall rate (hz): %2.2f' % (trials * 1.0 / (t1 - t0) ))
00097
00098 r = rospy.Rate( 10 )
00099 rtime = []
00100 while not rospy.is_shutdown():
00101 t0 = time.time()
00102 score = cs.scoreTrajectory( opt.xvel, 0.0, 0.0 )
00103
00104 t1 = time.time()
00105
00106 rtime.append( t1 - t0 )
00107 if len(rtime) > 5:
00108 rtime = rtime[1:]
00109
00110 rospy.logout( 'ScoreTraj: %3.2f Avg Rate: %2.2f' % ( score, 1.0 / np.mean( rtime ) ))
00111
00112