approach_services.py
Go to the documentation of this file.
00001 #! /usr/bin/python
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: # Node probably already initialized elsewhere
00030             pass
00031 
00032         # Create Costmap Services obj
00033         self.cs = costmap.CostmapServices( accum = 3, ns = costmap_ns ) # be really conservative!
00034         # Note: After moving, this will require accum * -1's before stopping.
00035 
00036         # Publish move_base command
00037         self._pub = rospy.Publisher( 'approach_cmd_vel', Twist )
00038 
00039         # Alterative ways to call using ROS services / actionlib
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             # self._as should be in scope and bound @ function def. (I think for python...)
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():  # this is a relatively new addition
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         # preempt_func => returns true when a preempt request is received, else false
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             # forward_mult is a hack to let us get closer at the current forward_vel
00085             #   without reducing the footprint.
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             # Don't do this too fast (avoid unwanted buffering / rate issues)
00097             try:
00098                 r.sleep()
00099             except rospy.ROSInterruptException: # rospy shutdown request received
00100                 pass
00101 
00102         self._pub.publish( Twist() ) # Stop moving!
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     # There are (at least) three ways to call this code.
00126     # 1) Python:
00127     
00128     # ap.run( forward_vel = 0.05, forward_mult = 0.5 )
00129 
00130     # 2) ROS service:
00131 
00132     # rosservice call /approach_table/move_forward_srv 0.05 0.5
00133 
00134     # 3) ROS actionlib:
00135 
00136     # try:
00137     #     client = actionlib.SimpleActionClient( '/approach_table/move_forward_act', ApproachAction )
00138     #     client.wait_for_server()
00139     #     client.send_goal( ActionGoal( 0.05, 0.5 ))
00140     #     client.wait_for_result()
00141     #     print client.get_result()
00142     # except rospy.ROSInterruptException:
00143     #     print 'Program interrupted before completion'
00144 
00145     rospy.spin()
00146 
00147     
00148 
00149 


pr2_approach_table
Author(s): Travis Deyle, Advisor: Prof. Charlie Kemp (Healthcare Robotics Lab at Georgia Tech)
autogenerated on Wed Nov 27 2013 12:06:43