def callback(self, message):
rospy.logdebug("joy_translater received axes %s", message.axes)
command = Control()
command.header = message.header
if message.axes[THROTTLE_AXIS] >= 0:
command.throttle = message.axes[THROTTLE_AXIS]
command.brake = 0.0
else:
command.brake = message.axes[THROTTLE_AXIS] * -1
command.throttle = 0.0
if message.buttons[3]:
command.shift_gears = Control.FORWARD
elif message.buttons[1]:
command.shift_gears = Control.NEUTRAL
elif message.buttons[0]:
command.shift_gears = Control.REVERSE
else:
command.shift_gears = Control.NO_COMMAND
command.steer = message.axes[STEERING_AXIS]
self.last_published = message
self.pub.publish(command)
def callback(self, message):
throttle_value = message.linear.x
gears_value = message.linear.z
steering_value = message.angular.z
rospy.logdebug("joy_translater received Throttle value " +
str(throttle_value))
rospy.logdebug("joy_translater received Gears value " +
str(gears_value))
rospy.logdebug("joy_translater received Steering value " +
str(steering_value))
command = Control()
header = Header()
header.frame_id = "world"
header.stamp = rospy.Time.now()
command.header = header
if throttle_value >= 0:
command.throttle = throttle_value
command.brake = 0.0
else:
command.brake = throttle_value * -1
command.throttle = 0.0
if gears_value > 0.0:
command.shift_gears = Control.FORWARD
elif gears_value == 0.0:
command.shift_gears = Control.NEUTRAL
elif gears_value < 0.0:
command.shift_gears = Control.REVERSE
else:
command.shift_gears = Control.NO_COMMAND
command.steer = steering_value
self.last_published = message
self.pub.publish(command)
def joystick_callback(self, message):
command = Control()
# Set message header
command.header = message.header
# Get steering value
command.steer = message.axes[self._STEERING_AXIS]
# Set GUI steering value
self._steering_scrollbar_signal.emit(
int(message.axes[self._STEERING_AXIS] * -100))
# Get cruise control state
if message.buttons[self._CIRCLE] and self._circle_flag:
self._circle_flag = False
self._cruise_control_state = not self._cruise_control_state
self._cruise_radiobutton_signal.emit(self._cruise_control_state)
elif not message.buttons[self._CIRCLE]:
self._circle_flag = True
# Increment cruise control speed
if message.buttons[self._R1] and self._R1_flag:
self._R1_flag = False
if self._cruise_control_speed < self._MAX_SPEED:
self._cruise_control_speed += 1
self._cruise_lcdnumber_signal.emit(self._cruise_control_speed)
elif not message.buttons[self._R1]:
self._R1_flag = True
# Decrement cruise control speed
if message.buttons[self._L1] and self._L1_flag:
self._L1_flag = False
if self._cruise_control_speed > 0:
self._cruise_control_speed -= 1
self._cruise_lcdnumber_signal.emit(self._cruise_control_speed)
elif not message.buttons[self._L1]:
self._L1_flag = True
# If cruise control was on then
if self._cruise_control_state:
command.shift_gears = Control.FORWARD
self._gears_lineedit_signal.emit('D')
# Calculate the safe distance which prevents collision
safe_velocity = np.sqrt(
2 * self._AMAX * self._closest_distance) * 3.6
# Get the minimum of the safe distance or the speed desired by the driver
desired_velocity = min(self._cruise_control_speed, safe_velocity)
# PID loop
t_current = time.time()
dt = t_current - self._t_previous
v_current_error = desired_velocity - self._current_velocity
self._v_total_error += v_current_error * dt
v_error_rate = (v_current_error - self._v_previous_error) / dt
p_throttle = self._KP * v_current_error
i_throttle = self._KI * self._v_total_error
d_throttle = self._KD * v_error_rate
longitudinal_output = p_throttle + i_throttle + d_throttle
if longitudinal_output >= 0:
command.throttle = np.fmax(np.fmin(longitudinal_output, 1.0),
0.0)
command.brake = 0.0
self._throttle_scrollbar_signal.emit(
int(command.throttle * -100))
else:
command.throttle = 0.0
command.brake = np.fmax(np.fmin(-longitudinal_output, 1.0),
0.0)
self._throttle_scrollbar_signal.emit(int(command.brake * 100))
self._v_previous_error = v_current_error
self._t_previous = t_current
else:
# Reset variables
self._v_total_error = 0
self._v_previous_error = 0
self._t_previous = 0
self._closest_distance = float('inf')
# Get throttle/breaking value
if message.axes[self._THROTTLE_AXIS] >= 0:
command.throttle = message.axes[self._THROTTLE_AXIS]
command.brake = 0.0
else:
command.brake = message.axes[self._THROTTLE_AXIS] * -1
command.throttle = 0.0
# Set GUI throttle value
self._throttle_scrollbar_signal.emit(
int(message.axes[self._THROTTLE_AXIS] * -100))
# Get gears value
if message.buttons[self._TRIANGLE]:
command.shift_gears = Control.FORWARD
self._gears_lineedit_signal.emit('D')
elif message.buttons[self._CROSS]:
command.shift_gears = Control.NEUTRAL
self._gears_lineedit_signal.emit('N')
elif message.buttons[self._SQUARE]:
command.shift_gears = Control.REVERSE
self._gears_lineedit_signal.emit('R')
else:
command.shift_gears = Control.NO_COMMAND
self._t_previous = time.time()
# Send control message
try:
self._prius_publisher.publish(command)
rospy.loginfo("Published commands")
except Exceptionrospy.ROSInterruptException as e:
pass
self._last_published = message