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
00089 m.go_x(.2, .05)