This post will walk through how OpenPilot plans in the longitudinal direction.
We can directly look into how the planner_thread works:
We can see the longitudinal plan is conducted by a Planner object. The key step for the longitudinal planner is: PL.update.
Let’s dive into the planner object:
In the planner object, the key function is update(), which outputs the plan message directly. Notice that the planner with and without model are different. Now let’s break down the update function:
If the planner is using the neural network model as one of the solution sources, the update (every planning step) should look like this:
First, we check out the official implementation from Openpilot:
The idea is to create 2 regular mpcs, and 1 mpc_model which takes the NN model outputs (speed, distance, and acceleration vectors) as the input and solves the optimal next-step speed.
From OP community, we also see different implementations of the e2e brake. Notice that model is only used for braking because the choose_solution is to find the lowest planning speed. This means for the acceleration case, we don't have e2e acceleration yet.
The logic is that, if the dp_slow_on_curve is activated, we use the model speed. (Model is able to output a lower speed on curve)
if self.dp_slow_on_curve:
solutions = {'model': self.v_model, 'cruise': self.v_cruise}Impact of the curve:
a speed smoother on the curve
if self.dp_slow_on_curve:
self.v_model, self.a_model = speed_smoother(self.v_acc_start, self.a_acc_start,model_speed, 2*accel_limits[1], accel_limits[0], 2*jerk_limits[1], jerk_limits[0], LON_MPC_STEP)First, it gets the cruise_speed in kph units.
v_ego = sm['carState'].vEgolong_control_state = sm['controlsState'].longControlState
v_cruise_kph = sm['controlsState'].vCruise
force_slow_decel = sm['controlsState'].forceDecelv_cruise_kph = min(v_cruise_kph, V_CRUISE_MAX)
v_cruise_setpoint = v_cruise_kph * CV.KPH_TO_MS
Then it gets the lead vehicle information from radarState:
lead_1 = sm['radarState'].leadOne
lead_2 = sm['radarState'].leadTwoenabled = (long_control_state == LongCtrlState.pid) or (long_control_state == LongCtrlState.stopping)
following = lead_1.status and lead_1.dRel < 45.0 and lead_1.vLeadK > v_ego and lead_1.aLeadK > 0.0
Note that the following state requires some conditions. Obviously, the conditions are related to the relevant distance and speed from the lead_1 vehicle.
lead_1.dRel < 45.0 and lead_1.vLeadK > v_egoSo, does it mean the vehicle is not following if the relevant distance is larger than 45 meters?
Now we calculate the speed for normal cruise control:
It calculates the limits for accelerations, jerks, and accelerations in turns. Then it applies a speed smoother to get v_cruse and a_cruise.
Below is how the two mpc controller works?
The two mpc objects set their current state using (v_acc_start, a_acc_start) and then update the pm and sm messages.
What are the v_acc_start and a_acc_start? What are their roles for mpc controllers?
self.mpc1.set_cur_state(self.v_acc_start, self.a_acc_start)
self.mpc2.set_cur_state(self.v_acc_start, self.a_acc_start)self.mpc1.update(pm, sm['carState'], lead_1)
self.mpc2.update(pm, sm['carState'], lead_2)self.choose_solution(v_cruise_setpoint, enabled)
Finally we apply the choose_solution function on the results from all the available mpc controllers.
self.choose_solution(v_cruise_setpoint, enabled)Now let’s look into how mpc1 and mpc2 set their current state and update the planning results?
- when initializing the two mpc controllers, the mpc class does the following:
self.setup_mpc()
self.v_mpc = 0.0
self.v_mpc_future = 0.0
self.a_mpc = 0.0
self.v_cruise = 0.0
self.prev_lead_status = False
self.prev_lead_x = 0.0
self.new_lead = Falseself.last_cloudlog_t = 0.0def setup_mpc(self):
ffi, self.libmpc = libmpc_py.get_libmpc(self.mpc_id)
self.libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE,
MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK) self.mpc_solution = ffi.new("log_t *")
self.cur_state = ffi.new("state_t *")
self.cur_state[0].v_ego = 0
self.cur_state[0].a_ego = 0
self.a_lead_tau = _LEAD_ACCEL_TAU
Most importantly, it instantiates a mpc solver written in C++.
2. to set the current state, it only takes in the current ego_v and ego_a:
def set_cur_state(self, v, a):
self.cur_state[0].v_ego = v
self.cur_state[0].a_ego = a3. Then the mpc controller executes the update(pm,CS,lead) step. Here it takes the input of the Pub message, current car state, as well as the lead information.
The pm is to receive the results. The lead information is the most important input.
The mpc controller first set up its current state:
It uses the lead information to get the x_lead, v_lead and a_lead.
We see mpc controller initialize the simulation with:
self.libmpc.init_with_simulation(self.v_mpc, x_lead, v_lead, a_lead, self.a_lead_tau)Then it calculates mpc:
# Calculate mpc
t = sec_since_boot()
n_its = self.libmpc.run_mpc(self.cur_state, self.mpc_solution, self.a_lead_tau, a_lead)The regular mpc only takes the lead vehicle information.
Note that the regular mpc controllers are different from the model_mpc.
The model_mpc takes the input of the predicted pose, v and a
def update(self, v_ego, a_ego, poss, speeds, accels):
if len(poss) == 0:
self.valid = False
return x_poly = list(map(float, np.polyfit(self.ts, poss, 3)))
v_poly = list(map(float, np.polyfit(self.ts, speeds, 3)))
a_poly = list(map(float, np.polyfit(self.ts, accels, 3))) # Calculate mpc
self.libmpc.run_mpc(self.cur_state, self.mpc_solution, x_poly, v_poly, a_poly)
The details of the MPC controller for longitudinal control. We look into the c file longtudinal_mpc.c:
Notice that in the c file, it defines N (Number of intervals in the horizon), and the other two types of struct, state_t, and log_t. The state_t corresponds to the current state of ego and lead vehicle. The log_t includes the ref trajectory of the ego vehicle and the lead vehicle and a cost.
#define N ACADO_N /* Number of intervals in the horizon. */notice N = 20ACADOvariables acadoVariables;
ACADOworkspace acadoWorkspace;typedef struct {
double x_ego, v_ego, a_ego, x_l, v_l, a_l;
} state_t;typedef struct {
double x_ego[N+1];// 66, a list
double v_ego[N+1];//66, should be the ref traj
double a_ego[N+1];
double j_ego[N];
double x_l[N+1];
double v_l[N+1];
double a_l[N+1];
double t[N+1];
double cost;
} log_t;
Notice that the run_mpc takes state_t and log_t struct as the input arguments. It requires that the self.cur_state is a state_t struct, and the self.mpc_solution is a log_t struct.
int run_mpc(state_t * x0, log_t * solution, double l, double a_l_0)# it was called:
n_its = self.libmpc.run_mpc(self.cur_state, self.mpc_solution, self.a_lead_tau, a_lead)
4. We see there are two mpc controllers based on lead1 and lead2. What matters from an mpc controller is the speed, which is self.mpc1.v_mpc
OP actually compares the v_mpc from different controllers, and executes the slowest speed from all controller sources.
def choose_solution(self, v_cruise_setpoint, enabled):
if enabled:
solutions = {'cruise': self.v_cruise}
if self.mpc1.prev_lead_status:
solutions['mpc1'] = self.mpc1.v_mpc
if self.mpc2.prev_lead_status:
solutions['mpc2'] = self.mpc2.v_mpc
## we can add model predicted speed hereslowest = min(solutions, key=solutions.get)self.longitudinalPlanSource = slowest
# Choose lowest of MPC and cruise
if slowest == 'mpc1':
self.v_acc = self.mpc1.v_mpc
self.a_acc = self.mpc1.a_mpc
elif slowest == 'mpc2':
self.v_acc = self.mpc2.v_mpc
self.a_acc = self.mpc2.a_mpc
elif slowest == 'cruise':
self.v_acc = self.v_cruise
self.a_acc = self.a_cruiseself.v_acc_future = min([self.mpc1.v_mpc_future, self.mpc2.v_mpc_future, v_cruise_setpoint])
The planner that incorporates model:
