motion_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/master/kobuki_testsuite/LICENSE 
00005 #
00006 ##############################################################################
00007 # Imports
00008 ##############################################################################
00009 
00010 import rospy
00011 import math
00012 from geometry_msgs.msg import Twist
00013 from std_msgs.msg import String
00014 
00015 ##############################################################################
00016 # Classes
00017 ##############################################################################
00018 
00019 '''
00020     implements a rotating motion.
00021 '''
00022 
00023 class Rotate():
00024     def __init__(self,cmd_vel_topic):
00025         self.cmd_vel_publisher = rospy.Publisher(cmd_vel_topic,Twist)
00026         self._cmd_vel_frequency = 5
00027         self._rate = rospy.Rate(self._cmd_vel_frequency)
00028         self._yaw_rate = 1.2
00029         
00030         self._max_rotate_count = self._cmd_vel_frequency*int(3.14/ self._yaw_rate)
00031         self._stop = False
00032         self._running = False
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._number_of_turns = -1 # continuous
00044 
00045     def init(self, yaw_rate, number_of_turns=-1):
00046         self._yaw_rate = yaw_rate
00047         self._max_rotate_count = self._cmd_vel_frequency*int(3.14/ self._yaw_rate)
00048 
00049     def shutdown(self):
00050         self.stop()
00051         while self._running:
00052             rospy.sleep(0.5)
00053         self.cmd_vel_publisher.unregister() #This one creates an error for some reason, probably because others already unregister.
00054         
00055     def stop(self):
00056         self._stop = True
00057 
00058     def execute(self):
00059         '''
00060           Drop this into threading.Thread or QThread for execution
00061         '''
00062         if self._running:
00063             rospy.logerr("Kobuki TestSuite: already executing a motion, ignoring the request")
00064             return
00065         self._stop = False
00066         self._running = True
00067         start = rospy.get_rostime()
00068         rospy.sleep(0.5)
00069 
00070         twist = self.twist
00071         max_rotate_count = self._max_rotate_count
00072         rotate_count = max_rotate_count
00073 
00074         while not self._stop and not rospy.is_shutdown():
00075             if rotate_count == max_rotate_count:
00076                 if twist.angular.z > 0:
00077                     mod = -1.0
00078                 else:
00079                     mod = 1.0
00080                 update = mod*self._yaw_rate/10.0
00081 
00082                 while math.fabs(twist.angular.z) <= self._yaw_rate:
00083                     twist.angular.z = twist.angular.z + update
00084                     #print "Command velocity: %s"%twist.angular.z
00085                     self.cmd_vel_publisher.publish(twist)
00086                     rospy.sleep(0.04)
00087 
00088                  # Make sure it is exact so the inequality in the while loop doesn't mess up next time around
00089                 twist.angular.z = mod*self._yaw_rate
00090                 rotate_count = 0
00091             else:
00092                 rotate_count += 1
00093             now = rospy.get_rostime()
00094             msg = "Rotate: " + str(now.secs - start.secs)
00095             self.cmd_vel_publisher.publish(twist)
00096             self._rate.sleep()
00097         if not rospy.is_shutdown():
00098             cmd = Twist()
00099             cmd.angular.z = 0.0
00100             self.cmd_vel_publisher.publish(cmd)
00101         self._running = False
00102 


kobuki_testsuite
Author(s): Jorge Santos Simon
autogenerated on Mon Oct 6 2014 01:33:05