def __init__(self, pt, x=None, u=None, kp=0., ki=0., kd=0., ID='vehicle'):
if x is None:
x = [0., 0., 0., 0.]
self.x = x # Vehicle state.
if u is None:
u = [0., 0.]
self.u = u # Vehicle input.
self.ID = ID
self.frenet = frenetpid.FrenetPID(pt, kp, ki, kd)
self.path_pos = path.PathPosition(pt, [x[0], x[1]])
def __init__(self,
vehicle_id,
position_topic_name,
position_topic_type,
pwm_topic_type,
pwm_topic_name,
v_ref=1.,
k_p=0.,
k_i=0.,
k_d=0.,
delta_t=0.1):
self.v_ref = v_ref # Desired velocity of the vehicle.
self.vehicle_id = vehicle_id # ID of the vehicle.
self.running = False # If controller is running or not.
self.dt = delta_t # Control rate, used by GUI.
self.zero_velocity_pwm = 1500
self.zero_angle_pwm = 1500
self.gear_command = 120 # UNUSED.
self.speed_pwm = self.zero_velocity_pwm
self.angle_pwm = self.zero_angle_pwm
# Setup ROS node.
rospy.init_node('controller', anonymous=True)
# Subscribe to topic that publishes vehicle position.
rospy.Subscriber(position_topic_name, position_topic_type,
self._callback)
# Publish vehicle pwm commands.
self.pub_pwm = rospy.Publisher(pwm_topic_name,
pwm_topic_type,
queue_size=1)
# Create reference path object.
self.pt = path.Path()
# Create frenet controller.
self.frenet = frenetpid.FrenetPID(self.pt, k_p, k_i, k_d)
# Keep track of the most recent vehicle pose.
self.pose = [0., 0., 0., 0.]
print('\nController initialized. Truck {}.\n'.format(self.vehicle_id))
def __init__(self, position_topic_name, control_topic_name, vehicle_id, preceding_id,
is_leader, vehicle_path, Ad, Bd, delta_t, horizon, zeta, Q, R, truck_length,
safety_distance, timegap, delay=0.,
xmin=None, xmax=None, umin=None, umax=None, x0=None, speed_ref=None,
k_p=0., k_i=0., k_d=0., recording_service_name='dmpc/set_measurement',
recording_filename='dmpc'):
self.vehicle_id = vehicle_id # ID of the vehicle.
self.dt = delta_t
self.h = horizon
# Setup ROS node.
rospy.init_node(self.vehicle_id + '_mpc', anonymous=True)
self.is_leader = is_leader # True if the vehicle is the leader.
self.recording = False
self.recorder = helper.RosbagRecorder(recording_filename, '.bag')
self.pose = [0, 0, 0, 0] # Store most recent vehicle pose.
self.timestamp = rospy.get_time() # Store the timestamp for the saved pose.
# Store old states and predicted states. Store enough old states for timegap tracking.
assumed_state_length = horizon + int(2.*timegap/delta_t)
# Store preceding vehicle trajectories.
self.preceding_timestamps = numpy.zeros(assumed_state_length)
self.preceding_velocities = numpy.zeros(assumed_state_length)
self.preceding_positions = numpy.zeros(assumed_state_length)
# Store trajectories of this vehicle.
self.velocities = numpy.zeros(assumed_state_length)
self.positions = numpy.zeros(assumed_state_length)
self.timestamps = numpy.zeros(assumed_state_length)
# Save old assumed states that will be published to simulate communication delay.
self.saved_num = int(math.ceil(delay/self.dt)) + 1
self.delay_counter = 0
self.delayed_velocities = numpy.zeros((self.saved_num, assumed_state_length))
self.delayed_positions = numpy.zeros((self.saved_num, assumed_state_length))
self.delayed_timestamps = numpy.zeros((self.saved_num, assumed_state_length))
self.pt = vehicle_path # Reference path for path tracking.
self.path_position = path.PathPosition(self.pt) # Keep track of longitudinal path position.
self.running = False # Controls whether the controller is running or not.
self.starting_phase_duration = 2.
self.starting_phase = True
self.first_callback = True
self.starting_phase_start_time = 0
# PWM values for the speed, wheel angle, and gears.
self.speed_pwm = 1500
self.angle_pwm = 1500
self.gear_pwm = 120 # Currently unused.
self.gear = 0
self.pwm_max = 1900
self.pwm_min = 1500
self.last_print_time = rospy.get_time()
# Optimal speed profile in space. If none given, optimal speed is 1 m/s everywhere.
if speed_ref is None:
speed_ref = speed_ref.Speed([1], [1])
self.speed_profile = speed_ref
self.original_speed_profile = speed_ref
# Update interval.
self.dt = delta_t
self.rate = rospy.Rate(1. / self.dt)
self.verbose = True # If printing stuff in terminal.
print_interval = 1. # How often to print things in terminal.
self.print_interval_samples = int(print_interval/self.dt)
self.k = 0 # Counter for printing in intervals.
self.control_iteration_time_sum = 0
# Publisher for controlling vehicle.
self.pwm_publisher = rospy.Publisher(control_topic_name, PWM, queue_size=1)
# Subscriber for vehicle positions.
rospy.Subscriber(position_topic_name, MocapState, self._position_callback)
# Publisher for publishing assumed state.
self.assumed_publisher = rospy.Publisher(self.vehicle_id + '/assumed_state', AssumedState,
queue_size=1)
# Subscriber for receiving assumed state of preceding vehicle.
rospy.Subscriber(preceding_id + '/assumed_state', AssumedState,
self._assumed_state_callback)
# Subscriber for starting and stopping the controller.
rospy.Subscriber('global/run', ControllerRun, self._start_stop_callback)
# Service for starting or stopping recording.
rospy.Service(recording_service_name, SetMeasurement, self._set_measurement)
# Service for custom commands.
rospy.Service(self.vehicle_id + '/command', Command, self._command_callback)
# MPC controller for speed control.
self.mpc = solver_distributed_mpc.MPC(Ad, Bd, delta_t, horizon, zeta, Q, R, truck_length,
safety_distance, timegap, xmin=xmin, xmax=xmax,
umin=umin, umax=umax, x0=x0, is_leader=is_leader)
# Frenet controller for path tracking.
self.frenet = frenetpid.FrenetPID(self.pt, k_p, k_i, k_d)
print('Vehicle controller initialized. ID = {}.'.format(self.vehicle_id))
if self.is_leader:
print('Leader vehicle. ')
else:
print('Follower vehicle, following {}.'.format(preceding_id))
def __init__(self, vehicles, vehicle_path, Ad, Bd, delta_t, horizon, zeta, Q, R, truck_length,
safety_distance, timegap, k_p, k_i, k_d, simulation_length, xmin=None, xmax=None,
umin=None, umax=None, speed_ref=None, delay=0., variance=0.):
self.pwm_min = 1500
self.pwm_max = 1990
self.dt = delta_t
self.iterations = int(simulation_length/self.dt)
self.pt = vehicle_path
self.timegap = timegap
self.variance = variance
# Optimal speed profile in space. If none given, optimal speed is 1 m/s everywhere.
if speed_ref is None:
speed_ref = speed_profile.Speed([1], [1])
self.original_speed_profile = speed_ref
self.vehicles = vehicles
self.n = len(vehicles)
self.h = horizon
# Matrices for storing control errors etc.
self.timestamps = numpy.zeros((self.n, self.iterations))
self.xx = numpy.zeros((self.n, self.iterations))
self.yy = numpy.zeros((self.n, self.iterations))
self.yaws = numpy.zeros((self.n, self.iterations))
self.positions = numpy.zeros((self.n, self.iterations))
self.velocities = numpy.zeros((self.n, self.iterations))
self.velocity_references = numpy.zeros((self.n, self.iterations))
self.accelerations = numpy.zeros((self.n, self.iterations))
self.timegaps = numpy.zeros((self.n, self.iterations))
self.path_errors = numpy.zeros((self.n, self.iterations))
self.velocity_errors = numpy.zeros((self.n, self.iterations))
self.speed_inputs = numpy.zeros((self.n, self.iterations))
self.steering_inputs = numpy.zeros((self.n, self.iterations))
assumed_state_length = horizon + int(2.*timegap/delta_t)
self.assumed_timestamps = numpy.zeros((self.n, assumed_state_length))
self.assumed_velocities = numpy.zeros((self.n, assumed_state_length))
self.assumed_positions = numpy.zeros((self.n, assumed_state_length))
# Safe past assumed states that will be used by follower vehicles, simulating a time delay.
self.saved_num = int(math.ceil(delay/self.dt)) + 1
self.current_saved = 0
self.saved_assumed_t = numpy.zeros((self.saved_num, self.n, assumed_state_length))
self.saved_assumed_v = numpy.zeros((self.saved_num, self.n, assumed_state_length))
self.saved_assumed_s = numpy.zeros((self.saved_num, self.n, assumed_state_length))
self.frenets = [] # Path tracking.
self.path_positions = [] # Longitudinal path positions.
self.mpcs = []
self.speed_pwms = [] # Speed control signals.
self.angle_pwms = [] # Wheel angle control signals.
self.speed_profiles = []
# Initialize lists.
for i, vehicle in enumerate(self.vehicles):
x = vehicle.get_x()
self.frenets.append(frenetpid.FrenetPID(vehicle_path, k_p, k_i, k_d))
self.path_positions.append(path.PathPosition(vehicle_path, [x[0], x[1]]))
if i == 0:
is_leader = True
else:
is_leader = False
self.mpcs.append(solver_distributed_mpc.MPC(Ad, Bd, delta_t, horizon, zeta, Q, R,
truck_length, safety_distance, timegap,
xmin=xmin, xmax=xmax, umin=umin, umax=umax,
is_leader=is_leader))
self.speed_pwms.append(1500)
self.angle_pwms.append(1500)
self.speed_profiles.append(speed_ref)
self._order_follower_path_positions()
for i in range(len(self.vehicles)):
self.assumed_positions[i, :] = self.path_positions[i].get_position()
self.saved_assumed_s[:] = self.assumed_positions
self.k = 0
self.mpctime = 0
def __init__(self, position_topic_name, control_topic_name, vehicle_ids, vehicle_path, Ad, Bd,
delta_t, horizon, zeta, Q, R, truck_length, safety_distance, timegap, k_p, k_i,
k_d, xmin=None, xmax=None, umin=None, umax=None, speed_ref=None, delay=0.,
recording_service_name='cmpc/set_measurement', recording_filename='cmpc'):
rospy.init_node('centralized_mpc', anonymous=True)
self.dt = delta_t
self.vehicle_ids = vehicle_ids
self.running = False # If controller is running or not.
self.recording = False
self.bag_filename_prefix = recording_filename
self.recorder = helper.RosbagRecorder(recording_filename, '.bag')
# Variables for starting phase.
self.starting_phase_duration = 2.
self.starting_phase = True
self.starting_phase_start_time = 0
self.pwm_max = 1900
self.pwm_min = 1500
# Variables for printing information in terminal.
self.verbose = True
self.k = 0
print_interval = 1. # How often to print things in terminal.
self.print_interval_samples = int(print_interval/self.dt)
self.control_iteration_time_sum = 0
# Update rate.
self.rate = rospy.Rate(1./self.dt)
# Optimal speed profile in space. If none given, optimal speed is 1 m/s everywhere.
if speed_ref is None:
speed_ref = speed_profile.Speed([1], [1])
self.speed_profile = speed_ref
self.original_speed_profile = speed_ref
# Save old initial positions used by MPC and path tracking to simulate communication delay.
self.saved_num = int(math.ceil(delay/self.dt)) + 1
self.delay_counter = 0
self.delayed_states = numpy.zeros((self.saved_num, len(self.vehicle_ids), 4))
# Centralized MPC solver.
self.mpc = solver_centralized_mpc.MPC(len(self.vehicle_ids), Ad, Bd, self.dt, horizon, zeta,
Q, R, truck_length, safety_distance, timegap,
xmin, xmax, umin, umax)
self.frenets = dict() # Path tracking.
self.path_positions = dict() # Longitudinal path positions.
self.poses = dict() # [x, y, yaw, v]
self.speed_pwms = dict() # Speed control signals.
self.angle_pwms = dict() # Wheel angle control signals.
self.gears = dict()
self.first_callbacks = dict() # If having received first position data in initialization.
# Initialize dictionaries.
for vehicle_id in self.vehicle_ids:
self.frenets[vehicle_id] = frenetpid.FrenetPID(vehicle_path, k_p, k_i, k_d)
self.path_positions[vehicle_id] = path.PathPosition(vehicle_path)
self.poses[vehicle_id] = [0, 0, 0, 0]
self.speed_pwms[vehicle_id] = 1500
self.angle_pwms[vehicle_id] = 1500
self.gears[vehicle_id] = 0
self.first_callbacks[vehicle_id] = True
# Publisher for controlling vehicles.
self.pwm_publisher = rospy.Publisher(control_topic_name, PWM, queue_size=1)
# Subscriber for vehicle positions.
rospy.Subscriber(position_topic_name, MocapState, self._position_callback)
# Subscriber for starting and stopping the controller.
rospy.Subscriber('global/run', ControllerRun, self._start_stop_callback)
# Service for starting or stopping recording.
rospy.Service(recording_service_name, SetMeasurement, self.set_measurement)
# Service for custom commands.
rospy.Service('cmpc/command', Command, self._command_callback)
print('\nCentralized MPC initialized. Vehicles: {}.\n'.format(self.vehicle_ids))
def __init__(self,
position_topic_type,
position_topic_name,
control_topic_type,
control_topic_name,
vehicle_ids,
v=1.,
k_p=0.,
k_i=0.,
k_d=0.,
frequency=10,
distance_offset=0.4,
e_ref=0.5,
k_pv=0.,
k_iv=0.,
k_dv=0.,
vmax=40):
self.v = v # Desired velocity of the vehicle.
self.vehicle_ids = vehicle_ids # IDs.
self.running = False # If controller is running or not.
self.rate = frequency
self.distance_offset = distance_offset
self.e_ref = e_ref
self.k_pv = k_pv
self.k_iv = k_iv
self.k_dv = k_dv
self.dt = 1. / frequency
self.vmin = 0
self.vmax = vmax
self.headstart_samples = frequency
self.k = 0
self.do_startup = True
self.gear2_pwm = 120
# Setup ROS node.
rospy.init_node('pid_platoon_controller', anonymous=True)
# Publisher for controlling vehicle.
self.pub = rospy.Publisher(control_topic_name,
control_topic_type,
queue_size=1)
# Subscriber for vehicle positions.
rospy.Subscriber(position_topic_name, position_topic_type,
self._callback)
# Create reference path object.
self.pt = path.Path()
# Create frenet controllers and dict of last positions.
self.frenets = dict()
self.pids = dict()
self.positions = dict()
self.speed_pwms = dict() # Stores speed PWM for each vehicle.
for i, vehicle_id in enumerate(self.vehicle_ids):
self.frenets[vehicle_id] = frenetpid.FrenetPID(
self.pt, k_p, k_i, k_d)
self.positions[vehicle_id] = [0, 0, 0, 0]
if i > 0:
self.pids[vehicle_id] = PID(self.k_pv, self.k_iv, self.k_dv)
self.speed_pwms[vehicle_id] = 1500
print('\nController initialized. Vehicles {}.\n'.format(
self.vehicle_ids))
def __init__(self,
position_topic_type,
position_topic_name,
control_topic_type,
control_topic_name,
vehicle_ids,
vehicle_path,
Ad,
Bd,
delta_t,
horizon,
zeta,
Q,
R,
truck_length,
safety_distance,
timegap,
xmin=None,
xmax=None,
umin=None,
umax=None,
x0=None,
saved_h=2,
k_p=0.,
k_i=0.,
k_d=0.):
self.verbose = True
self.do_startup = False
self.vehicle_ids = vehicle_ids
self.running = False # If controller is running or not.
self.gear2_pwm = 120
self.dt = delta_t
self.headstart_samples = int(1. / self.dt)
self.vopt = speed_profile.Speed()
self.truck_length = truck_length
self.print_interval = round(1. / self.dt)
self.k = 0
self.last_print_time = time.time()
# Setup ROS node.
rospy.init_node('platoon_controller', anonymous=True)
# Publisher for controlling vehicle.
self.pwm_publisher = rospy.Publisher(control_topic_name,
control_topic_type,
queue_size=1)
# Subscriber for vehicle positions.
rospy.Subscriber(position_topic_name, position_topic_type,
self._callback)
# Reference path.
self.pt = vehicle_path
self.frenets = dict(
) # Frenet path tracking controllers for each vehicle.
self.positions = dict() # Stores vehicle positions [x, y, yaw, v].
self.mpcs = dict() # MPC controllers.
self.path_positions = dict(
) # For keeping track of longitudinal path positions.
self.speed_pwms = dict() # Stores speed PWM for each vehicle.
for i, vehicle_id in enumerate(self.vehicle_ids):
self.frenets[vehicle_id] = frenetpid.FrenetPID(
self.pt, k_p, k_i, k_d)
self.positions[vehicle_id] = [0, 0, 0, 0]
# The first vehicle entered is set to be the leader.
if i == 0:
leader = True
else:
leader = False
self.mpcs[vehicle_id] = old_mpc_solver.TruckMPC(
Ad,
Bd,
delta_t,
horizon,
zeta,
Q,
R,
truck_length,
safety_distance,
timegap,
xmin=xmin,
xmax=xmax,
umin=umin,
umax=umax,
x0=x0,
vehicle_id=vehicle_id,
saved_h=saved_h,
is_leader=leader)
self.path_positions[vehicle_id] = path.PathPosition(self.pt)
self.speed_pwms[vehicle_id] = 1500
print('\nController initialized. Vehicles {}.\n'.format(
self.vehicle_ids))
def __init__(self,
vehicles,
vehicle_path,
Ad,
Bd,
delta_t,
horizon,
zeta,
Q,
R,
truck_length,
safety_distance,
timegap,
k_p,
k_i,
k_d,
simulation_length,
xmin=None,
xmax=None,
umin=None,
umax=None,
speed_ref=None,
delay=0.,
variance=0.):
self.dt = delta_t
self.h = horizon
self.iterations = int(simulation_length / self.dt)
self.pt = vehicle_path
self.variance = variance
self.timegap = timegap
# Optimal speed profile in space. If none given, optimal speed is 1 m/s everywhere.
if speed_ref is None:
speed_ref = speed_profile.Speed([1], [1])
self.speed_profile = speed_ref
self.original_speed_profile = speed_ref
self.pwm_max = 1990
self.pwm_min = 1500
self.mpc = solver_centralized_mpc.MPC(len(vehicles), Ad, Bd, delta_t,
horizon, zeta, Q, R,
truck_length, safety_distance,
timegap, xmin, xmax, umin, umax)
self.vehicles = vehicles
self.n = len(vehicles)
# Matrices for storing control errors etc.
self.timestamps = numpy.kron(numpy.ones((self.n, 1)),
self.dt * numpy.arange(self.iterations))
self.xx = numpy.zeros((self.n, self.iterations))
self.yy = numpy.zeros((self.n, self.iterations))
self.yaws = numpy.zeros((self.n, self.iterations))
self.positions = numpy.zeros((self.n, self.iterations))
self.velocities = numpy.zeros((self.n, self.iterations))
self.velocity_references = numpy.zeros((self.n, self.iterations))
self.accelerations = numpy.zeros((self.n, self.iterations))
self.timegaps = numpy.zeros((self.n, self.iterations))
self.path_errors = numpy.zeros((self.n, self.iterations))
self.velocity_errors = numpy.zeros((self.n, self.iterations))
self.speed_inputs = numpy.zeros((self.n, self.iterations))
self.steering_inputs = numpy.zeros((self.n, self.iterations))
# Save old initial positions used by MPC and path tracking to simulate communication delay.
self.saved_num = int(math.ceil(delay / self.dt)) + 1
self.delay_counter = 0
self.delayed_states = numpy.zeros(
(self.saved_num, len(self.vehicles), 4))
self.frenets = [] # Path tracking.
self.path_positions = [] # Longitudinal path positions.
self.speed_pwms = [] # Speed control signals.
self.angle_pwms = [] # Wheel angle control signals.
# Initialize lists.
for i, vehicle in enumerate(self.vehicles):
x = vehicle.get_x()
self.frenets.append(
frenetpid.FrenetPID(vehicle_path, k_p, k_i, k_d))
self.path_positions.append(
path.PathPosition(vehicle_path, [x[0], x[1]]))
self.speed_pwms.append(1500)
self.angle_pwms.append(1500)
self.delayed_states[:, i, :] = x
self._order_follower_path_positions()
self.k = 0
self.mpctime = 0
