def _get_throttle_input(self, vehicle_id, new_vel):
"""Returns the new control input for the vehicle. """
x = self._get_delayed_x(self.vehicle_ids.index(vehicle_id))
# pwm_diff = trxmodel.linear_velocity_to_throttle_input(new_vel) - \
# trxmodel.linear_velocity_to_throttle_input(self.poses[vehicle_id][3])
pwm_diff = trxmodel.linear_velocity_to_throttle_input(new_vel) - \
trxmodel.linear_velocity_to_throttle_input(x[3])
speed_pwm = self.speed_pwms[vehicle_id] + pwm_diff
return speed_pwm
def _get_throttle_input(self):
"""Returns the throttle command. Calculated from MPC and vehicle model. """
if not self.running:
return self.speed_pwm
acceleration = self.mpc.get_instantaneous_acceleration()
new_vel = self.pose[3] + acceleration * self.dt
pwm_diff = trxmodel.linear_velocity_to_throttle_input(new_vel) - \
trxmodel.linear_velocity_to_throttle_input(self.pose[3])
speed_pwm = self.speed_pwm + pwm_diff
return speed_pwm
def _get_throttle_input(self, vehicle_index, new_vel):
"""Returns the new control input for the vehicle. """
pwm_diff = trxmodel.linear_velocity_to_throttle_input(new_vel) - \
trxmodel.linear_velocity_to_throttle_input(
self.vehicles[vehicle_index].get_vel())
speed_pwm = self.speed_pwms[vehicle_index] + pwm_diff
if speed_pwm > self.pwm_max:
speed_pwm = self.pwm_max
if speed_pwm < self.pwm_min:
speed_pwm = self.pwm_min
return speed_pwm
def _startup(self):
end_num = 3 # For last few samples of startup the vehicle will not accelerate.
startup_acc = self.v / ((self.headstart_samples - end_num) * self.dt)
current_vehicle_index = self.k / self.headstart_samples
for i, vehicle_id in enumerate(self.vehicle_ids):
omega = self._get_omega(vehicle_id)
vel = self.positions[vehicle_id][3]
angle_pwm = trxmodel.angular_velocity_to_steering_input(omega, vel)
if i == current_vehicle_index:
print('{:2.0f}/{}: Starting {}'.format(
self.k % self.headstart_samples,
self.headstart_samples - 1, vehicle_id))
if self.k % self.headstart_samples < self.headstart_samples - 3:
acc = startup_acc
else:
acc = 0
else:
acc = 0
new_vel = trxmodel.throttle_input_to_linear_velocity(self.speed_pwms[vehicle_id]) + \
acc * self.dt
self.speed_pwms[
vehicle_id] = trxmodel.linear_velocity_to_throttle_input(
new_vel)
self.pub.publish(vehicle_id, self.speed_pwms[vehicle_id],
angle_pwm, self.gear2_pwm)
self.k += 1
def _get_speed_pwm(self, vehicle_id):
"""Returns the speed control input from the MPC controller. The MPC controller gives
acceleration which is then translated to a new velocity and a control input. """
acceleration = self._get_acceleration(vehicle_id)
new_velocity = trxmodel.throttle_input_to_linear_velocity(self.speed_pwms[vehicle_id]) + \
acceleration * self.dt
self.speed_pwms[
vehicle_id] = trxmodel.linear_velocity_to_throttle_input(
new_velocity)
return self.speed_pwms[vehicle_id]
def _run_pid(self):
for i, vehicle_id in enumerate(self.vehicle_ids):
omega = self._get_omega(vehicle_id)
current_vel = self.positions[vehicle_id][3]
angle_pwm = trxmodel.angular_velocity_to_steering_input(
omega, current_vel)
vel = self._get_vel(vehicle_id)
vel_pwm = trxmodel.linear_velocity_to_throttle_input(vel)
self.pub.publish(vehicle_id, vel_pwm, angle_pwm, self.gear2_pwm)
self.k += 1
def _startup(self):
"""Runs one iteration of the startup. Accelerates one vehicle at a time depending on the
value of k. """
end_num = 3 # For last few samples of startup the vehicle will not accelerate.
startup_acc = self.vopt.get_average() / (
(self.headstart_samples - end_num) * self.dt)
current_vehicle_index = self.k / self.headstart_samples # Current vehicle starting.
for i, vehicle_id in enumerate(self.vehicle_ids):
angle_pwm = self._get_angle_pwm(
vehicle_id) # Steering input for path tracking.
# Accelerate the current vehicle that is started. Others have acceleration zero.
if i == current_vehicle_index:
if self.k % self.headstart_samples == 0:
print('Starting vehicle {}.'.format(vehicle_id))
if self.k % self.headstart_samples < self.headstart_samples - 3:
acc = startup_acc
else:
acc = 0 # End of startup.
else:
acc = 0 # Vehicle already started or not yet started.
# Calculate the new speed control input from the current input and the acceleration.
new_vel = trxmodel.throttle_input_to_linear_velocity(self.speed_pwms[vehicle_id]) + \
acc * self.dt
self.speed_pwms[
vehicle_id] = trxmodel.linear_velocity_to_throttle_input(
new_vel)
# Publish speed and steering inputs.
self.pwm_publisher.publish(vehicle_id, self.speed_pwms[vehicle_id],
angle_pwm, self.gear2_pwm)
# Update the MPC state information.
vel = self.positions[vehicle_id][3]
path_pos = self.path_positions[vehicle_id].get_position()
self.mpcs[vehicle_id].set_new_x0(numpy.array([vel, path_pos]))
self.k += 1
def control(self):
"""Perform control actions from received data. Sends new values to truck. """
if self.running:
# Get control input.
omega = self.frenet.get_omega(self.pose[0], self.pose[1],
self.pose[2], self.pose[3])
speed = self.v_ref
alpha = trxmodel.steering_input_to_wheel_angle(self.angle_pwm)
alpha_prime = self.frenet.get_alpha_prime(self.pose[0],
self.pose[1],
self.pose[2],
self.pose[3], alpha)
new_alpha = alpha + self.dt * alpha_prime
self.angle_pwm = trxmodel.wheel_angle_to_steering_input(new_alpha)
# self.angle_pwm = trxmodel.angular_velocity_to_steering_input(omega, speed)
self.speed_pwm = trxmodel.linear_velocity_to_throttle_input(speed)
# Publish control commands to the topic.
self.publish_values()
