rotate.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 #       
00003 # License: BSD
00004 #   https://raw.github.com/yujinrobot/kobuki/hydro-devel/kobuki_testsuite/LICENSE
00005 #
00006 ##############################################################################
00007 # Imports
00008 ##############################################################################
00009 
00010 import threading
00011 import rospy
00012 import math
00013 from geometry_msgs.msg import Twist
00014 from std_msgs.msg import String
00015 
00016 ##############################################################################
00017 # Classes
00018 ##############################################################################
00019 
00020 '''
00021     implements a rotating motion.
00022 '''
00023 
00024 class RotateTest(threading.Thread):
00025     def __init__(self,cmd_vel_topic,log_topic):
00026         threading.Thread.__init__(self)
00027         self.pub_cmd = rospy.Publisher(cmd_vel_topic,Twist)
00028         self.pub_log = rospy.Publisher(log_topic,String)
00029 
00030         freq = 5
00031         self.rate = rospy.Rate(freq)
00032         self.yaw_rate = 1.2
00033 
00034         twist = Twist()
00035         twist.linear.x = 0
00036         twist.linear.y = 0
00037         twist.linear.z = 0
00038         twist.angular.x = 0
00039         twist.angular.y = 0
00040         twist.angular.z = 0
00041         self.twist = twist
00042 
00043         self.max_rotate_count = freq*int(3.14/ self.yaw_rate)
00044 
00045         self._stop = False
00046 
00047     def stop(self):
00048         self._stop = True
00049 
00050     def run(self):
00051         self._stop = False
00052 
00053         start = rospy.get_rostime()
00054         rospy.sleep(0.5)
00055         twist = self.twist
00056         max_rotate_count = self.max_rotate_count
00057         rotate_count = max_rotate_count
00058         yaw_rate = self.yaw_rate
00059 
00060         while not rospy.is_shutdown() and not self._stop:
00061             if rotate_count == max_rotate_count:
00062                 if twist.angular.z > 0:
00063                     mod = -1.0
00064                 else:
00065                     mod = 1.0
00066                 update = mod*yaw_rate/10.0
00067 
00068                 while math.fabs(twist.angular.z) <= yaw_rate:
00069                     twist.angular.z = twist.angular.z + update
00070                     self.pub_cmd.publish(twist)
00071                     rospy.sleep(0.04)
00072 
00073                  # Make sure it is exact so the inequality in the while loop doesn't mess up next time around
00074                 twist.angular.z = mod*yaw_rate
00075                 rotate_count = 0
00076             else:
00077                 rotate_count += 1
00078             now = rospy.get_rostime()
00079             msg = "Rotate: " + str(now.secs - start.secs)
00080             self.log(msg)
00081             self.pub_cmd.publish(twist)
00082             self.rate.sleep()
00083 
00084     def log(self,msg):
00085         rospy.loginfo(msg)
00086 
00087         t = String(msg)
00088         self.pub_log.publish(t)
00089 


kobuki_testsuite
Author(s): Jorge Santos Simon
autogenerated on Wed Sep 16 2015 04:35:48