auto_drive.py
Go to the documentation of this file.
00001 #! /usr/bin/env python
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         #variables used for calculating trajectory controller
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


husky_navigation
Author(s): Ryan Gariepy
autogenerated on Sun Oct 5 2014 22:54:31