rotate_backup_node.py
Go to the documentation of this file.
00001 #! /usr/bin/python
00002 import time
00003 import roslib
00004 roslib.load_manifest('rospy')
00005 roslib.load_manifest('actionlib')
00006 roslib.load_manifest( 'move_base_msgs' )
00007 roslib.load_manifest('tf')
00008 roslib.load_manifest('geometry_msgs')
00009 roslib.load_manifest('std_msgs')
00010 roslib.load_manifest('hrl_rfid')
00011 roslib.load_manifest('robotis')
00012 roslib.load_manifest('rfid_people_following')
00013 import rospy
00014 
00015 import tf
00016 import tf.transformations as tft
00017 import actionlib
00018 from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
00019 from geometry_msgs.msg import Twist
00020 from geometry_msgs.msg import PointStamped
00021 from geometry_msgs.msg import Point
00022 from geometry_msgs.msg import PoseStamped
00023 from geometry_msgs.msg import Quaternion
00024 from geometry_msgs.msg import PoseWithCovarianceStamped
00025 from std_msgs.msg import Float64
00026 
00027 from rfid_people_following.srv import FloatFloat_Int32
00028 from rfid_people_following.srv import FloatFloat_Int32Response
00029 from rfid_people_following.srv import FloatFloatFloatFloat_Int32
00030 
00031 import numpy as np, math
00032 import time
00033 from threading import Thread
00034 from collections import deque
00035 
00036 def standard_rad(t):
00037     if t > 0:
00038         return ((t + np.pi) % (np.pi * 2))  - np.pi
00039     else:
00040         return ((t - np.pi) % (np.pi * -2)) + np.pi
00041 
00042 
00043 class RotateBackup():
00044     def __init__( self, service_name = '/rotate_backup' ):
00045         try:
00046             rospy.init_node( 'rotater' )
00047         except:
00048             pass
00049         rospy.logout( 'rotate_backup: Initializing service: \'%s\'' % service_name )
00050         self.pub = rospy.Publisher( '/move_base_simple/goal', PoseStamped )
00051         self.pub_direct = rospy.Publisher( '/navigation/cmd_vel', Twist )
00052         self.listener = tf.TransformListener()
00053         self.listener.waitForTransform('/odom_combined', '/base_link',
00054                                        rospy.Time(0), timeout = rospy.Duration(100) )
00055         self._service_rb = rospy.Service( service_name, FloatFloat_Int32, self.move)
00056         self._service_rb_navstack = rospy.Service( service_name+'/navstack', FloatFloatFloatFloat_Int32, self.navstack)
00057         rospy.logout( 'rotate_backup: Service ready' )
00058 
00059     def non_nav_rotate( self, r ):
00060         success, pose, orient = self.get_pose() # orient = rx, ry, rz
00061         if not success:
00062             rospy.logout( 'rotate_backup: Rotate transform fail. Exiting.' )
00063             return
00064         
00065         t_rz = standard_rad( orient[-1] + r )
00066         mov = Twist()
00067         mov.linear.x = 0.05
00068         mov.angular.z = 0.6 * np.sign( r )
00069 
00070         rate = rospy.Rate( 10 )
00071         while not np.allclose( [t_rz], [orient[-1]], atol=[0.08] ): # Not within 5deg of target
00072             success, pose, orient = self.get_pose() # orient = rx, ry, rz
00073             orient[-1] = standard_rad( orient[-1] )
00074             if not success:
00075                 rospy.logout( 'rotate_backup: Rotate transform fail. Exiting.' )
00076                 return
00077             self.pub_direct.publish( mov )
00078             rate.sleep()
00079 
00080 
00081     def non_nav_backup( self, d ):
00082         # Hacky way to backup without relying on nav_stack's bizarre circling.
00083         mov = Twist()
00084         mov.linear.x = 0.1 * np.sign( d )
00085         t0 = time.time()
00086         rate = rospy.Rate( 10.0 )
00087         while time.time() - t0 < np.abs(d) / 0.1:
00088             self.pub_direct.publish( mov )
00089             rate.sleep()
00090 
00091     def get_pose( self ):
00092         ps = PoseStamped()
00093         ps.header.stamp = rospy.Time(0)
00094         ps.header.frame_id = '/base_link'
00095         #ps.pose.position.x = d
00096         
00097         try:
00098             ps_odom = self.listener.transformPose( '/odom_combined', ps )
00099         except:
00100             rospy.logout( 'rotate_backup: Failed transform #1.' )
00101             time.sleep( 2.0 )
00102             return False, [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]
00103             
00104         orient = ps_odom.pose.orientation
00105         rx, ry, rz = tft.euler_from_quaternion([ orient.x, orient.y, orient.z, orient.w ])
00106         pose = [ ps_odom.pose.position.x, ps_odom.pose.position.y, ps_odom.pose.position.z ]
00107         return True, pose, [rx, ry, rz]
00108         
00109 # Hacky version not using actionlib
00110 #     def wait_for_stop( self, duration = 0.5 ):
00111 #         rospy.logout( 'rotate_backup: Waiting for movement to stop.' )
00112 #         t0 = time.time()
00113 #         rate = rospy.Rate( 10 )
00114 #         success, pose, orient = self.get_pose()        
00115 #         if not success:
00116 #             rospy.logout( 'rotate_backup: Waiting 2 sec.' )
00117 #             time.sleep( 2.0 )
00118 #             return
00119 
00120 #         sp = np.array([ pose[0], pose[1], pose[2], orient[-1] ])
00121 
00122 #         while time.time() - t0 < duration:
00123 #             success, pose, orient = self.get_pose()
00124 #             if not success:
00125 #                 rospy.logout( 'rotate_backup: Waiting 2 sec.' )
00126 #                 time.sleep( 2.0 )
00127 #                 return
00128 #             qp = np.array([ pose[0], pose[1], pose[2], orient[-1] ])
00129 #             if not np.allclose( sp, qp, atol=[0.01, 0.01, 0.01, 0.005] ):
00130 #                 t0 = time.time()
00131 #                 sp = qp
00132 #             rate.sleep()
00133 #         return
00134             
00135     def move( self, request ):
00136         r = request.rotate
00137         d = request.displace
00138         rospy.logout( 'rotate_backup: Asked to rotate: %3.2f (deg)' % math.degrees(r))
00139         rospy.logout( 'rotate_backup: Asked to translate (forward-backward): %3.2f (m)' % d)
00140 
00141         self.non_nav_backup( d )
00142         self.non_nav_rotate( r )
00143 
00144 #         success, pose, orient = self.get_pose()        
00145 #         if not success:
00146 #             return FloatFloat_Int32Response( int(False) )
00147 
00148 #         new_point = Point( pose[0], pose[1], pose[2] )
00149 #         old_rx, old_ry, old_rz = orient
00150 #         new_orient = tft.quaternion_from_euler( old_rx, old_ry, old_rz + r  )
00151 #         new_quat = Quaternion( *new_orient )
00152 
00153 #         new_ps = PoseStamped()
00154 #         new_ps.header.stamp = rospy.Time(0)
00155 #         new_ps.header.frame_id = '/odom_combined'
00156 #         new_ps.pose.position = new_point
00157 #         new_ps.pose.orientation = new_quat
00158 
00159 #         self.pub.publish( new_ps )
00160 #         self.wait_for_stop()
00161 #         rospy.logout( 'rotate_backup: Done with call.' )
00162         return FloatFloat_Int32Response( int(True) )
00163 
00164 # Hacky version not using actionlib
00165 #     def navstack( self, request ):
00166 #         new_orient = tft.quaternion_from_euler( 0.0, 0.0, request.ang  ) # rx, ry, rz
00167 #         new_quat = Quaternion( *new_orient )
00168 
00169 #         new_ps = PoseStamped()
00170 #         new_ps.header.stamp = rospy.Time(0)
00171 #         new_ps.header.frame_id = '/map'
00172 #         new_ps.pose.position.x = request.x
00173 #         new_ps.pose.position.y = request.y
00174 #         new_ps.pose.orientation = new_quat
00175 
00176 #         rospy.logout( 'rotate_backup: Requesting navstack move to <x,y,ang-deg> %3.3f %3.3f %3.3f.' % (request.x, request.y, math.degrees(request.ang)) )
00177 #         self.pub.publish( new_ps )
00178 
00179 #         rospy.logout( 'rotate_backup: Waiting for base to stop moving.' )
00180 #         self.wait_for_stop( 7.0 )
00181 #         return int( True )
00182 
00183 
00184     def navstack( self, request ):
00185         rospy.logout( 'rotate_backup: Requesting navstack move to <x,y,ang-deg> %3.3f %3.3f %3.3f.' % (request.x, request.y, math.degrees(request.ang)) )
00186 
00187         client = actionlib.SimpleActionClient( 'move_base', MoveBaseAction )
00188         client.wait_for_server()
00189 
00190         ps = PoseStamped()
00191         ps.header.frame_id = '/map'
00192         ps.header.stamp = rospy.Time(0)
00193         ps.pose.position.x = request.x
00194         ps.pose.position.y = request.y
00195         ps.pose.orientation = Quaternion( *tft.quaternion_from_euler( 0.0, 0.0, request.ang ))
00196 
00197         goal = MoveBaseGoal( ps )
00198         client.send_goal( goal )
00199         rospy.logout( 'rotate_backup: Waiting for base to stop moving.' )
00200         client.wait_for_result()
00201         return int( True )
00202         
00203 
00204 class RotateBackupClient():
00205     def __init__( self, service_name = '/rotate_backup' ):
00206         rospy.logout( 'rotate_backup_client: Waiting for service: \'%s\'' % service_name )
00207         rospy.wait_for_service( service_name )
00208         rospy.logout( 'rotate_backup_client: Service ready.' )
00209         
00210         self._rb_service = rospy.ServiceProxy( service_name, FloatFloat_Int32 )
00211 
00212     def rotate_backup( self, rotate, displace ):
00213         return self._rb_service( rotate, displace )
00214 
00215 if __name__ == '__main__':
00216     rb = RotateBackup()
00217     rospy.spin()


rfid_people_following
Author(s): Travis Deyle (Healthcare Robotics Lab, Georgia Tech)
autogenerated on Wed Nov 27 2013 11:38:30