def handle_incoming_car(msg):
control = Control()
control.shift_gears = 2
print('Receiving command', msg)
args = msg.split(',')
speed_level = int(args[7])
x = float(args[5])
y = float(args[6])
deg = xy2deg(x, y)
delta = 15
if (45 + delta) <= deg <= (135 - delta): # forward
control.throttle = 0.2 * speed_level
elif (225 + delta) <= deg <= (315 - delta): #backward
control.brake = 1.0
elif (135 - delta) <= deg <= (225 + delta): #left
control.throttle = 0.2 * speed_level
control.steer = 0.2 * speed_level
if deg >= 180:
control.shift_gears = 3
else: #right
control.throttle = 0.2 * speed_level
control.steer = -0.2 * speed_level
if 270 <= deg <= 360:
control.shift_gears = 3
vel_pub.publish(control)
def cmd_callback(data):
global wheelbase
global ackermann_cmd_topic
global frame_id
global pub
global correct_v
global correct_yaw
global v
global yaw
global last_recorded_vel
global last_recorded_ang
global kp
global ki
global kd
global dt
global integral
global last_error
prius_vel = Control()
target_vel = data.linear.x
print("Command Velocity: "),
if (abs(target_vel) < 0.2):
target_vel = 0
print("0.0")
if (target_vel >= 0.2):
prius_vel.shift_gears = 2
print("+"),
print(target_vel)
if (target_vel <= -0.2):
prius_vel.shift_gears = 3
print(target_vel)
integral = integral + ki * (target_vel - last_recorded_vel)
integral = min(3, max(-3, integral))
v = kp * (target_vel - last_recorded_vel) + integral + kd * (
target_vel - last_recorded_vel - last_error)
v = min(1, max(-1, v))
last_error = (target_vel - last_recorded_vel)
#v = data.linear.x + correct_v
yaw = data.angular.z + correct_yaw
steering = convert_trans_rot_vel_to_steering_angle(v, yaw, wheelbase)
if (target_vel <= -0.2):
v = v * -1.0
# msg = AckermannDriveStamped()
# msg.header.stamp = rospy.Time.now()
# msg.header.frame_id = frame_id
# msg.drive.steering_angle = steering
# msg.drive.speed = v
prius_vel.throttle = v
prius_vel.brake = 0
prius_vel.steer = steering / 3.14
if (prius_vel.throttle < 0):
prius_vel.brake = 1
#pub.publish(msg)
pub.publish(prius_vel)
def forward(self, throttle, steer_angle):
"""forward generates a forward-moving Control msg with throttle and steering angle
:param throttle: throttle in range [-1.0, 1.0]
:param steer_angle: steering angle in range [-1.0, 1.0]
if throttle < 0.0 then apply brakes
"""
assert steer_angle >= -1.0 and steer_angle <= 1.0, "Require steer angle to be in range [-1.0, 1.0]: {0}".format(
steer_angle)
brake = 0.0
if throttle < 0.0:
brake = abs(throttle)
throttle = 0.0
assert brake >= 0.0 and brake <= 1.0, "Brake must be in range [0.0, 1.0]: {0}".format(
brake)
assert throttle >= 0.0 and throttle <= 1.0, "Throttle must be in range [0.0, 1.0]: {0}".format(
throttle)
msg = Control()
self.populate_header(msg)
msg.throttle = throttle # throttle as given
msg.steer = steer_angle # steering angle as given
msg.brake = brake # braking amount
msg.shift_gears = 2 # forward gearing
return msg
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 sendCtrlCmdsToGazebo(self):
"""
Send control commands to Gazebo, which updates the robot state according to its inertial model.
"""
command_msg = Control()
command_msg.header.stamp = rospy.get_rostime()
command_msg.throttle = self.throttle
command_msg.brake = 0.0
command_msg.shift_gears = Control.FORWARD
steer = self.steering
command_msg.steer = steer
self.control_pub.publish(command_msg)
position_msg = PointStamped()
position_msg.header.stamp = rospy.get_rostime()
position_msg.point.x = self.lin_position
self.linear_position_pub.publish(position_msg)
return
def control_test():
pub = rospy.Publisher("/prius", Control, queue_size=1)
rospy.init_node('sim_control', anonymous=True)
rate = rospy.Rate(10) # 10hz
command = Control()
command.header.stamp = rospy.Time.now()
command.throttle = 0.2
command.brake = 0
command.steer = 0.0
command.shift_gears = Control.NO_COMMAND
# while not rospy.is_shutdown():
for i in range(10):
pub.publish(command)
rate.sleep()
for i in range(10):
command.brake = 0.5
pub.publish(command)
rate.sleep()
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
self.pub = rospy.Publisher('car_1/prius', Control, queue_size=1)
stdscr.refresh()
key = ''
while key != ord('q'):
key = stdscr.getch()
stdscr.refresh()
command = Control()
# fill in the conditions to increment/decrement throttle/steer
if key == curses.KEY_UP:
command.throttle = message.axes[THROTTLE_AXIS]
command.brake = 0.0
command.shift_gears = Control.FORWARD
elif key == curses.KEY_DOWN:
forward = forward - 1
elif key == curses.KEY_LEFT:
left = left + 1
elif key == curses.KEY_RIGHT:
left = left - 1
elif key == ord(' '):
# this key will center the steer and throttle
left = 0
forward = 0
pub.publish(command)
curses.endwin()
speed = rospy.get_param("~speed", 0.5)
turn = rospy.get_param("~turn", 1.0)
status = 0
command = Control()
try:
print(msg)
print(vels(speed, turn))
while (1):
#command = Control()
key = getKey()
# if key == 'i': throttle +, steering 0, gear FORWARD, braking 0
if key == 'i':
command.throttle = speed
command.steer = 0.0
command.shift_gears = Control.FORWARD
command.brake = 0.0
# if key == 'u': throttle +, steering +, gear FORWARD, braking 0
elif key == 'u':
command.throttle = speed
command.steer = turn
command.shift_gears = Control.FORWARD
command.brake = 0.0
# if key == 'o': throttle +, steering -, gear FORWARD, braking 0
elif key == 'o':
command.throttle = speed
command.steer = -turn
command.shift_gears = Control.FORWARD
command.brake = 0.0
# if key == 'k': throttle 0, steering 0, gear NEUTRAL, braking +
elif key == 'k':