linear_accelerate.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 #
3 # License: BSD
4 # https://raw.github.com/yujinrobot/kobuki/hydro-devel/kobuki_testsuite/LICENSE
5 #
6 ##############################################################################
7 # Imports
8 ##############################################################################
9 
10 import threading
11 import rospy
12 import math
13 from geometry_msgs.msg import Twist
14 from std_msgs.msg import String
15 
16 ##############################################################################
17 # Classes
18 ##############################################################################
19 
20 '''
21  implements a rotating motion.
22 '''
23 
24 class LinearAccelerateTest(threading.Thread):
25  def __init__(self,cmd_vel_topic,log_topic, freq, accl, max_speed):
26  threading.Thread.__init__(self)
27  self.pub_cmd = rospy.Publisher(cmd_vel_topic,Twist)
28  self.pub_log = rospy.Publisher(log_topic,String)
29 
30  twist = Twist()
31  twist.linear.x = 0
32  twist.linear.y = 0
33  twist.linear.z = 0
34  twist.angular.x = 0
35  twist.angular.y = 0
36  twist.angular.z = 0
37  self.twist = twist
38 
39  self.freq = freq
40  self.accl = accl
41  self.max_speed = max_speed
42 
43  self._stop = False
44 
45  def stop(self):
46  self._stop = True
47 
48  def run(self):
49  self._stop = False
50 
51  start = rospy.get_rostime()
52  rospy.sleep(0.5)
53  twist = self.twist
54  freq = self.freq
55  self.rate = rospy.Rate(freq)
56  a = self.accl
57  max_speed = self.max_speed
58 
59  msg = "Time : " + str(rospy.get_rostime().secs) + " Vel : " +str(twist.linear.x)
60  finish = False
61  while not rospy.is_shutdown() and not self._stop:
62  if not finish:
63  twist.linear.x = twist.linear.x + ( 1.0 / freq ) * a
64  if twist.linear.x > max_speed:
65  finish = True
66  twist.linear.x = 0.0
67  msg = "Time : " + str(rospy.get_rostime().secs) + " Vel : " +str(twist.angular.z)
68  self.log(msg)
69  self.pub_cmd.publish(twist)
70  self.rate.sleep()
71  twist.linear.x = 0
72  self.pub_cmd.publish(twist)
73 
74  def log(self,msg):
75  rospy.loginfo(msg)
76  t = String(msg)
77  self.pub_log.publish(t)
def __init__(self, cmd_vel_topic, log_topic, freq, accl, max_speed)


kobuki_testsuite
Author(s): Jorge Santos Simon
autogenerated on Mon Jun 10 2019 13:45:22