00001
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()
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] ):
00072 success, pose, orient = self.get_pose()
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
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
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
00110
00111
00112
00113
00114
00115
00116
00117
00118
00119
00120
00121
00122
00123
00124
00125
00126
00127
00128
00129
00130
00131
00132
00133
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
00145
00146
00147
00148
00149
00150
00151
00152
00153
00154
00155
00156
00157
00158
00159
00160
00161
00162 return FloatFloat_Int32Response( int(True) )
00163
00164
00165
00166
00167
00168
00169
00170
00171
00172
00173
00174
00175
00176
00177
00178
00179
00180
00181
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()