move_base_tutorial1.py
Go to the documentation of this file.
00001 import roslib; roslib.load_manifest('hai_sandbox')
00002 import rospy
00003 import tf.transformations as tr
00004 import hrl_lib.tf_utils as tfu
00005 import hrl_lib.util as ut
00006 import tf
00007 
00008 import geometry_msgs.msg as gm
00009 import trigger_msgs.msg as trm
00010 import sys
00011 import math
00012 import numpy as np
00013 
00014 class MoveBase:
00015     def __init__(self):
00016         rospy.init_node('drive')
00017         self.tw_pub = rospy.Publisher('base_controller/command', gm.Twist)
00018         self.tl = tf.TransformListener()
00019 
00020     def go_ang(self, ang, speed):
00021         dt = math.radians(ang)
00022 
00023         if dt > 0:
00024             sign = -1
00025         elif dt < 0:
00026             sign = 1
00027         else:
00028             sign = 0
00029 
00030         self.tl.waitForTransform('base_footprint', 'odom_combined', rospy.Time(), rospy.Duration(10))
00031         p0_base = tfu.transform('base_footprint', 'odom_combined', self.tl)# \
00032         start_ang = tr.euler_from_matrix(p0_base[0:3, 0:3], 'sxyz')[2]
00033         r = rospy.Rate(100)
00034         dist_so_far = 0.
00035 
00036         last_ang = start_ang
00037         while not rospy.is_shutdown():
00038             pcurrent_base = tfu.transform('base_footprint', 'odom_combined', self.tl) #\
00039             current_ang = tr.euler_from_matrix(pcurrent_base[0:3, 0:3], 'sxyz')[2]
00040             dist_so_far = dist_so_far + (ut.standard_rad(current_ang - last_ang))
00041             if dt > 0 and dist_so_far > dt:
00042                 rospy.loginfo('stopped! %f %f' % (dist_so_far, dt))
00043                 break
00044             elif dt < 0 and dist_so_far < dt:
00045                 rospy.loginfo('stopped! %f %f' % (dist_so_far, dt))
00046                 break  
00047             elif dt == 0:  
00048                 rospy.loginfo('stopped! %f %f' % (dist_so_far, dt))
00049                 break
00050 
00051             tw = gm.Twist()
00052             tw.angular.z = math.radians(speed * sign)
00053 
00054             self.tw_pub.publish(tw)
00055             r.sleep()
00056             last_ang = current_ang
00057 
00058     def go_x(self, x, speed):
00059         print 'go x called!'
00060         vel = speed * np.sign(x)
00061         self.tl.waitForTransform('base_footprint', 'odom_combined', rospy.Time(), rospy.Duration(10))
00062         p0_base = tfu.transform('base_footprint', 'odom_combined', self.tl)
00063         r = rospy.Rate(100)
00064 
00065         while not rospy.is_shutdown():
00066             pcurrent_base = tfu.transform('base_footprint', 'odom_combined', self.tl)
00067             relative_trans = np.linalg.inv(p0_base) * pcurrent_base
00068             dist_moved = np.linalg.norm(relative_trans[0:3,3])
00069             print "%s" % str(dist_moved)
00070             
00071             if dist_moved > np.abs(x):
00072                 rospy.loginfo('stopped! error %f' % (np.abs(dist_moved-np.abs(x))))
00073                 break
00074 
00075             tw = gm.Twist()
00076             tw.linear.x = vel
00077             tw.linear.y = 0
00078             tw.linear.z = 0
00079             tw.angular.x = 0
00080             tw.angular.y = 0
00081             tw.angular.z = 0
00082 
00083             self.tw_pub.publish(tw)
00084             r.sleep()
00085 
00086 if __name__ == '__main__':
00087     m = MoveBase()
00088     #m.go_ang(-390, 100)
00089     m.go_x(.2, .05)


hai_sandbox
Author(s): Hai Nguyen
autogenerated on Wed Nov 27 2013 11:46:56