acc_finder.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 import roslib; roslib.load_manifest('acc_finder')
00003 import rospy
00004 
00005 from geometry_msgs.msg import Twist
00006 from nav_msgs.msg import Odometry
00007 
00008 LIN_MAX = 0.75
00009 ANG_MAX = 1.0   # adjust this value to the rough maximum angular velocity
00010 
00011 state = 'stopped'
00012 start = rospy.Time(0.0)
00013 
00014 def odom_cb(msg):
00015     global state
00016 
00017     twist = msg.twist.twist
00018     t = (rospy.Time.now() - start).to_sec()
00019 
00020     if state == 'wait_for_stop':
00021         if twist.linear.x > -0.05 and twist.linear.x < 0.05 and twist.angular.z > -0.1 and twist.angular.z < 0.1:
00022             state = 'stopped'
00023             rospy.loginfo('state transition --> %s', state)
00024         return
00025 
00026     if state == 'backward' and twist.linear.x < -0.9 * LIN_MAX:
00027         rospy.loginfo('backward from 0 to %f m/s in %f sec', twist.linear.x, t)
00028     elif state == 'forward' and twist.linear.x > 0.9 * LIN_MAX:
00029         rospy.loginfo('forward from 0 to %f m/s in %f sec', twist.linear.x, t)
00030     elif state == 'turning_clockwise' and twist.angular.z < -0.9 * ANG_MAX:
00031         rospy.loginfo('turning_clockwise from 0 to %f rad/s in %f sec', twist.angular.z, t)
00032     elif state == 'turning_counter_clockwise' and twist.angular.z > 0.9 * ANG_MAX:
00033         rospy.loginfo('turning_counter_clockwise from 0 to %f rad/s in %f sec', twist.angular.z, t)
00034     else:
00035         return
00036 
00037     state = 'wait_for_stop'
00038     rospy.loginfo('state transition --> %s', state)
00039 
00040 
00041 def cmd_vel_cb(msg):
00042     global state, start
00043 
00044     if state != 'stopped':
00045         return
00046 
00047     if msg.linear.x < -0.5:
00048         start = rospy.Time.now()
00049         state = 'backward'
00050     elif msg.linear.x > 0.5:
00051         start = rospy.Time.now()
00052         state = 'forward'
00053     elif msg.angular.z < -1.0:
00054         start = rospy.Time.now()
00055         state = 'turning_clockwise'
00056     elif msg.angular.z > 1.0:
00057         start = rospy.Time.now()
00058         state = 'turning_counter_clockwise'
00059     else:
00060         return
00061 
00062     rospy.loginfo('state transition --> %s', state)
00063 
00064 
00065 def listener():
00066     rospy.init_node('acc_finder', anonymous=True)
00067     rospy.Subscriber('odom', Odometry, odom_cb)
00068     rospy.Subscriber('cmd_vel', Twist, cmd_vel_cb)
00069     rospy.loginfo('acc_finder node ready and listening. now use teleop to move your robot to the limits!')
00070     rospy.spin()
00071 
00072 if __name__ == '__main__':
00073     listener()


acc_finder
Author(s): Martin Guenther
autogenerated on Wed Aug 26 2015 16:40:23