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