throttle_sweep.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 csv
33 from dbw_fca_msgs.msg import ThrottleCmd, ThrottleReport, ThrottleInfoReport
34 from dbw_fca_msgs.msg import GearReport, SteeringReport
35 from math import fabs
36 
38  def __init__(self):
39  rospy.init_node('throttle_sweep')
40 
41  # Variables for logging
42  self.throttle_cmd = 0.0
43  self.msg_throttle_report = ThrottleReport()
45  self.msg_throttle_info_report = ThrottleInfoReport()
47 
48  # Other drive-by-wire variables
49  self.msg_gear_report = GearReport()
50  self.msg_gear_report_ready = False
51  self.msg_steering_report = SteeringReport()
53 
54  # Parameters
55  self.i = -1
56  self.start = 0.07
57  self.end = 0.93
58  self.resolution = 0.002
59  self.duration = rospy.Duration(1.0)
60  rospy.loginfo('Recording throttle pedal data every ' + str(self.duration.to_sec()) + ' seconds from ' + str(self.start) + ' to ' + str(self.end) + ' with ' + str(self.resolution) + ' increments.')
61  rospy.loginfo('This will take ' + str((self.end - self.start) / self.resolution * self.duration.to_sec() / 60.0) + ' minutes.')
62 
63  # Open CSV file
64  self.csv_file = open('throttle_sweep_data.csv', 'w')
65  self.csv_writer = csv.writer(self.csv_file, delimiter=',')
66  self.csv_writer.writerow(['Throttle (%)', 'Measured (%)'])
67 
68  # Publishers and subscribers
69  self.pub = rospy.Publisher('/vehicle/throttle_cmd', ThrottleCmd, queue_size=1)
70  rospy.Subscriber('/vehicle/throttle_report', ThrottleReport, self.recv_throttle)
71  rospy.Subscriber('/vehicle/throttle_info_report', ThrottleInfoReport, self.recv_throttle_info)
72  rospy.Subscriber('/vehicle/gear_report', GearReport, self.recv_gear)
73  rospy.Subscriber('/vehicle/steering_report', SteeringReport, self.recv_steering)
74  rospy.Timer(rospy.Duration(0.02), self.timer_cmd)
75  rospy.Timer(self.duration, self.timer_process)
76 
77  def timer_process(self, event):
78  if self.i < 0:
79  # Check for safe conditions
80  if not self.msg_steering_report_ready:
81  rospy.logerr('Speed check failed. No messages on topic \'/vehicle/steering_report\'')
82  rospy.signal_shutdown('')
83  if self.msg_steering_report.speed > 1.0:
84  rospy.logerr('Speed check failed. Vehicle speed is greater than 1 m/s.')
85  rospy.signal_shutdown('')
86  if not self.msg_gear_report_ready:
87  rospy.logerr('Gear check failed. No messages on topic \'/vehicle/gear_report\'')
88  rospy.signal_shutdown('')
89  if not self.msg_gear_report.state.gear == self.msg_gear_report.state.PARK:
90  rospy.logerr('Gear check failed. Vehicle not in park.')
91  rospy.signal_shutdown('')
92  elif self.i < int((self.end - self.start) / self.resolution + 1):
93  # Check for new messages
94  if not self.msg_throttle_report_ready:
95  rospy.logerr('No new messages on topic \'/vehicle/throttle_report\'')
96  rospy.signal_shutdown('')
98  rospy.logerr('No new messages on topic \'/vehicle/throttle_info_report\'')
99  rospy.signal_shutdown('')
100  if not self.msg_throttle_report.enabled:
101  rospy.logerr('Throttle module not enabled!')
102  rospy.signal_shutdown('')
103 
104  # Make sure values are close to expected
105  diff = self.throttle_cmd - self.msg_throttle_report.pedal_output
106  if (fabs(diff) > 0.01):
107  rospy.logwarn('Not saving data point. Large disparity between pedal request and actual: ' + str(diff))
108  else:
109  # Write data to file
110  rospy.loginfo('Data point: ' + "{:.03f}".format(self.msg_throttle_report.pedal_output) + ', ' +
111  "{:.03f}".format(self.msg_throttle_info_report.throttle_pc))
112  self.csv_writer.writerow(["{:.03f}".format(self.msg_throttle_report.pedal_output),
113  "{:.03f}".format(self.msg_throttle_info_report.throttle_pc)])
114  else:
115  rospy.signal_shutdown('')
116 
117  # Prepare for next iteration
118  self.i += 1
119  self.msg_throttle_report_ready = False
120  self.msg_throttle_info_report_ready = False
121  self.throttle_cmd = self.start + self.i * self.resolution
122 
123  def timer_cmd(self, event):
124  if self.throttle_cmd > 0:
125  msg = ThrottleCmd()
126  msg.enable = True
127  msg.pedal_cmd_type = ThrottleCmd.CMD_PEDAL
128  msg.pedal_cmd = self.throttle_cmd
129  self.pub.publish(msg)
130 
131  def recv_throttle(self, msg):
132  self.msg_throttle_report = msg
133  self.msg_throttle_report_ready = True
134 
135  def recv_throttle_info(self, msg):
136  self.msg_throttle_info_report = msg
138 
139  def recv_gear(self, msg):
140  self.msg_gear_report = msg
141  self.msg_gear_report_ready = True
142 
143  def recv_steering(self, msg):
144  self.msg_steering_report = msg
145  self.msg_steering_report_ready = True
146 
147  def shutdown_handler(self):
148  rospy.loginfo('Saving csv file')
149  self.csv_file.close()
150 
151 if __name__ == '__main__':
152  try:
153  node = ThrottleSweep()
154  rospy.on_shutdown(node.shutdown_handler)
155  rospy.spin()
156  except rospy.ROSInterruptException:
157  pass
def timer_process(self, event)


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