diff --git a/ackermann_cmd_mux/src/throttle_interpolator.py b/ackermann_cmd_mux/src/throttle_interpolator.py index 1714931..9dea879 100755 --- a/ackermann_cmd_mux/src/throttle_interpolator.py +++ b/ackermann_cmd_mux/src/throttle_interpolator.py @@ -39,6 +39,10 @@ def __init__(self): # Create topic subscribers and publishers self.rpm_output = rospy.Publisher(self.rpm_output_topic, Float64,queue_size=1) self.servo_output = rospy.Publisher(self.servo_output_topic, Float64,queue_size=1) + + # Create throttle timeout + self.throttle_timeout = rospy.get_param('throttle_timeout', default=1.0) + self.last_throttle_time = rospy.Time.now() rospy.Subscriber(self.rpm_input_topic, Float64, self._process_throttle_command) rospy.Subscriber(self.servo_input_topic, Float64, self._process_servo_command) @@ -62,13 +66,15 @@ def _publish_throttle_command(self, evt): smoothed_rpm = self.last_rpm + clipped_delta self.last_rpm = smoothed_rpm # print self.desired_rpm, smoothed_rpm - self.rpm_output.publish(Float64(smoothed_rpm)) + if rospy.Time.now() - self.last_throttle_time < self.throttle_timeout: + self.rpm_output.publish(Float64(smoothed_rpm)) def _process_throttle_command(self,msg): input_rpm = msg.data # Do some sanity clipping input_rpm = min(max(input_rpm, self.min_rpm), self.max_rpm) self.desired_rpm = input_rpm + self.last_throttle_time = rospy.Time.now() def _publish_servo_command(self, evt): desired_delta = self.desired_servo_position-self.last_servo