brake_thrash.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 from std_msgs.msg import Bool
33 from dbw_fca_msgs.msg import BrakeCmd
34 from dbw_fca_msgs.msg import GearReport, SteeringReport
35 
37  def __init__(self):
38  rospy.init_node('brake_thrash')
39 
40  # Shutdown
41  self.shutdown = False
42 
43  # Received messages
44  self.dbw_enabled = False
46  self.msg_gear_report = GearReport()
47  self.msg_gear_report_ready = False
48  self.msg_steering_report = SteeringReport()
49  self.msg_steering_report_ready = False
50 
51  # Parameters
52  self.started = False
53  rospy.loginfo('Preparing to thrash the brake pedal command to try and induce a fault...')
54  rospy.loginfo('Validating that vehicle is parked...')
55 
56  # Publishers and subscribers
57  self.pub = rospy.Publisher('/vehicle/brake_cmd', BrakeCmd, queue_size=10)
58  rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.recv_enabled)
59  rospy.Subscriber('/vehicle/gear_report', GearReport, self.recv_gear)
60  rospy.Subscriber('/vehicle/steering_report', SteeringReport, self.recv_steering)
61  rospy.Timer(rospy.Duration(0.2), self.timer_process)
62 
63  def timer_process(self, event):
64  # Check for safe conditions
65  if not self.msg_steering_report_ready:
66  self.shutdown = True
67  rospy.logerr('Speed check failed. No messages on topic \'/vehicle/steering_report\'')
68  elif not self.msg_steering_report.speed == 0.0:
69  self.shutdown = True
70  rospy.logerr('Speed check failed. Vehicle speed is not zero.')
71  if not self.msg_gear_report_ready:
72  self.shutdown = True
73  rospy.logerr('Gear check failed. No messages on topic \'/vehicle/gear_report\'')
74  elif not self.msg_gear_report.state.gear == self.msg_gear_report.state.PARK:
75  self.shutdown = True
76  rospy.logerr('Gear check failed. Vehicle not in park.')
77 
78  # Check if enabled
79  if self.shutdown:
80  rospy.signal_shutdown('')
81  return
82 
83  # Check if enabled
84  if not self.dbw_enabled:
85  rospy.logwarn('Drive-by-wire not enabled!')
86 
87  # Start command timers
88  if not self.started:
89  self.started = True
90  rospy.Timer(rospy.Duration(0.01000), self.timer_cmd_0)
91  rospy.Timer(rospy.Duration(0.01001), self.timer_cmd_1)
92  rospy.loginfo('Started thrashing the brake pedal command to try and induce a fault.')
93 
94  # Prepare for next iteration
95  self.msg_gear_report_ready = False
96  self.msg_steering_report_ready = False
97 
98  def timer_cmd_0(self, event):
99  if not self.shutdown:
100  msg = BrakeCmd()
101  msg.enable = True
102  msg.pedal_cmd_type = BrakeCmd.CMD_PEDAL
103  msg.pedal_cmd = 0.0
104  self.pub.publish(msg)
105 
106  def timer_cmd_1(self, event):
107  if not self.shutdown:
108  msg = BrakeCmd()
109  msg.enable = True
110  msg.pedal_cmd_type = BrakeCmd.CMD_PEDAL
111  msg.pedal_cmd = 1.0
112  self.pub.publish(msg)
113 
114  def recv_enabled(self, msg):
115  self.dbw_enabled = msg.data
116 
117  def recv_gear(self, msg):
118  self.msg_gear_report = msg
119  self.msg_gear_report_ready = True
120 
121  def recv_steering(self, msg):
122  self.msg_steering_report = msg
123  self.msg_steering_report_ready = True
124 
125  def shutdown_handler(self):
126  pass
127 
128 if __name__ == '__main__':
129  try:
130  node = BrakeThrash()
131  rospy.on_shutdown(node.shutdown_handler)
132  rospy.spin()
133  except rospy.ROSInterruptException:
134  pass
def recv_gear(self, msg)
def timer_cmd_0(self, event)
Definition: brake_thrash.py:98
def recv_enabled(self, msg)
def recv_steering(self, msg)
def timer_cmd_1(self, event)
def timer_process(self, event)
Definition: brake_thrash.py:63


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