00001
00002 import roslib; roslib.load_manifest('husky_navigation')
00003
00004 import rospy
00005 import math
00006 import numpy
00007 import geometry_msgs
00008
00009 from nav_msgs.msg import Odometry
00010 from sensor_msgs.msg import Joy
00011 from geometry_msgs.msg import Twist
00012 from geometry_msgs.msg import Pose
00013 from geometry_msgs.msg import PoseWithCovarianceStamped
00014 from geometry_msgs.msg import Quaternion
00015 from tf.transformations import euler_from_quaternion
00016
00017
00018 class AutoDrive(object):
00019 def __init__(self):
00020 rospy.init_node('auto_drive')
00021 self.enable = False
00022 self.watchdog = False
00023 self.curr_odom = PoseWithCovarianceStamped()
00024 self.first_odom = PoseWithCovarianceStamped()
00025 self.received_odom = False
00026 self.received_first_odom = False
00027 self.command_pub = rospy.Publisher('husky/cmd_vel',Twist)
00028
00029 joy_sub = rospy.Subscriber('teleop/joy',Joy,self.joy_callback)
00030 odom_sub = rospy.Subscriber('robot_pose_ekf/odom',PoseWithCovarianceStamped,self.odom_callback)
00031 rospy.Timer(rospy.Duration(0.05), self.output_latest_command)
00032 rospy.Timer(rospy.Duration(0.5), self.odom_watchdog)
00033
00034
00035 self.last_theta_meas = 0
00036 self.curr_theta = 0
00037 self.last_curr_theta = 0
00038 self.curr_wrap_counter = 0
00039 self.r = rospy.get_param('~radius',3)
00040 self.ky = 0.5
00041 self.kc = 0.1
00042 self.reqd_fwd = rospy.get_param('~fwd_vel',0.5)
00043
00044 def joy_callback(self,data):
00045 if data.buttons[3] == 1:
00046 self.enable = True
00047 rospy.loginfo('Autonomous On')
00048 else:
00049 rospy.loginfo('Autonomous Off')
00050 self.enable = False
00051
00052 def odom_callback(self,data):
00053 if self.received_first_odom:
00054 self.received_odom = True
00055 self.curr_odom = data
00056 else:
00057 self.first_odom = data
00058 self.received_first_odom = True
00059
00060 def odom_watchdog(self,event):
00061 if self.received_odom:
00062 self.watchdog = True
00063 self.received_odom = False
00064 else:
00065 watchdog = False
00066
00067
00068 def output_latest_command(self,event):
00069 if self.enable and self.watchdog:
00070 latest_twist = Twist()
00071 x = self.curr_odom.pose.pose.position.x
00072 y = self.curr_odom.pose.pose.position.y
00073 quat = self.curr_odom.pose.pose.orientation
00074 euler = euler_from_quaternion([quat.x,quat.y,quat.z,quat.w])
00075 yaw = euler[2]
00076
00077 rospy.loginfo("X:%f,Y:%f,yaw:%f",x,y,yaw)
00078
00079 self.curr_theta = math.atan2(y,x);
00080
00081 goal_theta = self.curr_theta + (math.pi)/2
00082
00083 e_ct = -self.r + math.fabs(math.sqrt(x*x + y*y))
00084 e_yaw = goal_theta - yaw
00085 e_yaw = math.sin(e_yaw)
00086 e_yaw = math.asin(e_yaw)
00087
00088 reqd_turn = self.ky * (e_yaw) + math.atan2(self.kc*e_ct,self.reqd_fwd)
00089 rospy.loginfo("ct:%f,ey:%f",math.atan2(self.kc*e_ct,self.reqd_fwd),self.ky*e_yaw)
00090 rospy.loginfo("control_output: %f",reqd_turn)
00091 latest_twist.linear.x = self.reqd_fwd;
00092 latest_twist.angular.z = reqd_turn;
00093
00094 self.command_pub.publish(latest_twist)
00095 elif self.enable:
00096 rospy.loginfo("No new position data :(");
00097
00098 if __name__ == '__main__':
00099 obj = AutoDrive()
00100 rospy.loginfo("Autonomous Drive Node On: Waiting for auto 'Y' button")
00101 try:
00102 rospy.spin()
00103 except rospy.ROSInterruptException:
00104 pass
