Go to the documentation of this file.00001
00002 import roslib
00003 roslib.load_manifest('pr2_approach_table')
00004 import rospy
00005
00006 from geometry_msgs.msg import Twist
00007 import actionlib
00008
00009 import costmap_services.python_client as costmap
00010 from pr2_approach_table.srv import ApproachSrv
00011 from pr2_approach_table.msg import ApproachAction, ApproachResult, ApproachGoal
00012
00013 import numpy as np, math
00014 from collections import deque
00015 import os
00016 from threading import Lock
00017
00018 def ctime():
00019 return rospy.Time.now().to_time()
00020
00021 def retfalse():
00022 return False
00023
00024 class Approach( ):
00025 def __init__( self, costmap_ns = '' ):
00026 rospy.logout('approach_node: Initializing')
00027 try:
00028 rospy.init_node('approach_node')
00029 except:
00030 pass
00031
00032
00033 self.cs = costmap.CostmapServices( accum = 3, ns = costmap_ns )
00034
00035
00036
00037 self._pub = rospy.Publisher( 'approach_cmd_vel', Twist )
00038
00039
00040 self._service = rospy.Service( '/approach_table/move_forward_srv',
00041 ApproachSrv,
00042 self.service_request )
00043 self._as = actionlib.SimpleActionServer( '/approach_table/move_forward_act',
00044 ApproachAction,
00045 execute_cb = self.action_request )
00046 self._as.start()
00047
00048 rospy.logout( 'approach_node: Service ready and waiting' )
00049
00050
00051 def service_request( self, req ):
00052 return self.run( forward_vel = req.forward_vel,
00053 forward_mult = req.forward_mult )
00054
00055 def action_request( self, goal ):
00056 def preempt_func():
00057
00058 check = self._as.is_preempt_requested()
00059 if check:
00060 rospy.logout( 'approach_node: action_request preempted!' )
00061 return check
00062
00063 rv = self.run( preempt_func = preempt_func,
00064 forward_vel = goal.forward_vel,
00065 forward_mult = goal.forward_mult )
00066 rospy.logout( 'approach_node: action_request received result %d' % int(rv) )
00067
00068 if preempt_func():
00069 rospy.logout('approach_node: returning actionlib state preempted.')
00070 self._as.set_preempted()
00071 elif rv == True:
00072 self._as.set_succeeded( ApproachResult( int(rv) ))
00073
00074
00075 def run( self, preempt_func = retfalse, forward_vel = 0.05, forward_mult = 1.0 ):
00076
00077 rospy.logout( 'approach_node: Run called for with values: \'%1.2f, %1.2f\'' % (forward_vel, forward_mult) )
00078
00079 rospy.logout('approach_node: Running')
00080
00081 r = rospy.Rate( 10 )
00082 def check_func():
00083 a = not rospy.is_shutdown()
00084
00085
00086 b = self.cs.scoreTraj_PosHyst( forward_vel * forward_mult, 0.0, 0.0 ) != -1.0
00087 c = not preempt_func()
00088 return a and b and c
00089
00090 while check_func():
00091 move_command = Twist()
00092 move_command.linear.x = forward_vel
00093
00094 self._pub.publish( move_command )
00095
00096
00097 try:
00098 r.sleep()
00099 except rospy.ROSInterruptException:
00100 pass
00101
00102 self._pub.publish( Twist() )
00103
00104 if preempt_func():
00105 rospy.logout( 'approach_node: Preempt was requested. May not have finished.' )
00106 rospy.logout( 'approach_node: Exiting' )
00107 return False
00108 else:
00109 rospy.logout( 'approach_node: Exiting' )
00110 return True
00111
00112
00113
00114
00115 if __name__ == '__main__':
00116 import optparse
00117 p = optparse.OptionParser()
00118 p.add_option('--ns', action='store', type='string', dest='ns',
00119 help='Uses the namespace [ns] for services: /ns/approach_table/costmap_services/cs/...',
00120 default = '')
00121 opt, args = p.parse_args()
00122
00123 ap = Approach( costmap_ns = opt.ns )
00124
00125
00126
00127
00128
00129
00130
00131
00132
00133
00134
00135
00136
00137
00138
00139
00140
00141
00142
00143
00144
00145 rospy.spin()
00146
00147
00148
00149