throttle_sine.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 # Software License Agreement (BSD License)
4 #
5 # Copyright (c) 2018, Dataspeed Inc.
6 # All rights reserved.
7 #
8 # Redistribution and use in source and binary forms, with or without modification,
9 # are permitted provided that the following conditions are met:
10 #
11 # * Redistributions of source code must retain the above copyright notice,
12 # this list of conditions and the following disclaimer.
13 # * Redistributions in binary form must reproduce the above copyright notice,
14 # this list of conditions and the following disclaimer in the documentation
15 # and/or other materials provided with the distribution.
16 # * Neither the name of Dataspeed Inc. nor the names of its
17 # contributors may be used to endorse or promote products derived from this
18 # software without specific prior written permission.
19 #
20 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
21 # ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
22 # WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
23 # DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
24 # FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
25 # DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
26 # SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
27 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
28 # OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
29 # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
30 
31 import rospy
32 import math
33 
34 from std_msgs.msg import Bool
35 from dbw_fca_msgs.msg import ThrottleCmd
36 
37 
38 class SineTest:
39  def __init__(self):
40  rospy.init_node('sine_test')
41  self.pub = rospy.Publisher('/vehicle/throttle_cmd', ThrottleCmd, queue_size=1)
42  self.sub = rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.recv_enable)
43 
44  self.high_peak = rospy.get_param('~high_peak', 0.92)
45  self.low_peak = rospy.get_param('~low_peak', 0.08)
46  self.period = rospy.get_param('~period', 4)
47 
48  self.enabled = False
49  self.t = 0
50  self.sample_time = 0.02
51 
52  rospy.Timer(rospy.Duration(self.sample_time), self.timer_cb)
53 
54  def timer_cb(self, event):
55  if not self.enabled:
56  self.t = 0
57  return
58 
59  amplitude = 0.5 * (self.high_peak - self.low_peak)
60  offset = 0.5 * (self.high_peak + self.low_peak)
61  cmd = offset - amplitude * math.cos(2 * math.pi / self.period * self.t)
62  self.t += self.sample_time
63 
64  self.pub.publish(ThrottleCmd(enable=True, pedal_cmd_type=ThrottleCmd.CMD_PEDAL, pedal_cmd=cmd))
65 
66  def recv_enable(self, msg):
67  self.enabled = msg.data
68 
69 
70 if __name__ == '__main__':
71  try:
72  node_instance = SineTest()
73  rospy.spin()
74  except rospy.ROSInterruptException:
75  pass
76 
def timer_cb(self, event)
def recv_enable(self, msg)


dbw_fca_can
Author(s): Kevin Hallenbeck
autogenerated on Wed May 12 2021 02:14:05