32 from std_msgs.msg
import Bool
33 from dbw_fca_msgs.msg
import ThrottleCmd
34 from dbw_fca_msgs.msg
import GearReport, SteeringReport
38 rospy.init_node(
'throttle_thrash')
53 rospy.loginfo(
'Preparing to thrash the throttle pedal command to try and induce a fault...')
54 rospy.loginfo(
'Validating that vehicle is parked...')
60 self.
pub = rospy.Publisher(
'/vehicle/throttle_cmd', ThrottleCmd, queue_size=10)
61 rospy.Subscriber(
'/vehicle/dbw_enabled', Bool, self.
recv_enabled)
62 rospy.Subscriber(
'/vehicle/gear_report', GearReport, self.
recv_gear)
63 rospy.Subscriber(
'/vehicle/steering_report', SteeringReport, self.
recv_steering)
70 rospy.logerr(
'Speed check failed. No messages on topic \'/vehicle/steering_report\'')
71 elif not self.msg_steering_report.speed == 0.0:
73 rospy.logerr(
'Speed check failed. Vehicle speed is not zero.')
76 rospy.logerr(
'Gear check failed. No messages on topic \'/vehicle/gear_report\'')
77 elif not self.msg_gear_report.state.gear == self.msg_gear_report.state.PARK:
79 rospy.logerr(
'Gear check failed. Vehicle not in park.')
83 rospy.signal_shutdown(
'')
88 rospy.logwarn(
'Drive-by-wire not enabled!')
93 rospy.Timer(rospy.Duration(0.020), self.
timer_cmd)
94 rospy.loginfo(
'Started thrashing the throttle pedal command to try and induce a fault.')
104 msg.pedal_cmd_type = ThrottleCmd.CMD_PEDAL
109 self.pub.publish(msg)
126 if __name__ ==
'__main__':
129 rospy.on_shutdown(node.shutdown_handler)
131 except rospy.ROSInterruptException:
msg_steering_report_ready
def timer_cmd(self, event)
def recv_steering(self, msg)
def shutdown_handler(self)
def timer_process(self, event)
def recv_enabled(self, msg)