sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

6f6e3749-1b7c-42e8-a33b-03929b7fc476/700

Browse Source
pull/30209/head
Yassine 2 years ago
parent 33c51c122b
commit ad43247ef6
6 changed files with 60 additions and 85 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 2
      cereal
  2. 92
      selfdrive/controls/lib/lateral_planner.py
  3. 4
      selfdrive/controls/plannerd.py
  4. 28
      selfdrive/modeld/models/driving.cc
  5. 15
      selfdrive/modeld/models/driving.h
  6. 4
      selfdrive/modeld/models/supercombo.onnx

2
cereal

@ -1 +1 @@
Subproject commit 4b334f6f10877e4a666b23983de2d27934ebf3b1
Subproject commit 6bb94587261692da139ddc861ceaa4a3baff52aa

92
selfdrive/controls/lib/lateral_planner.py
Unescape View File

@ -1,29 +1,10 @@
import time
import numpy as np
from openpilot.common.realtime import DT_MDL
from openpilot.common.numpy_fast import interp
from openpilot.system.swaglog import cloudlog
from openpilot.selfdrive.controls.lib.lateral_mpc_lib.lat_mpc import LateralMpc
from openpilot.selfdrive.controls.lib.lateral_mpc_lib.lat_mpc import N as LAT_MPC_N
from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, MIN_SPEED, get_speed_error
from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper
import cereal.messaging as messaging
from cereal import log
TRAJECTORY_SIZE = 33
CAMERA_OFFSET = 0.04
PATH_COST = 1.0
LATERAL_MOTION_COST = 0.11
LATERAL_ACCEL_COST = 0.0
LATERAL_JERK_COST = 0.04
# Extreme steering rate is unpleasant, even
# when it does not cause bad jerk.
# TODO this cost should be lowered when low
# speed lateral control is stable on all cars
STEERING_RATE_COST = 700.0
class LateralPlanner:
def __init__(self, CP, debug=False):
@ -42,29 +23,21 @@ class LateralPlanner:
self.t_idxs = np.arange(TRAJECTORY_SIZE)
self.y_pts = np.zeros((TRAJECTORY_SIZE,))
self.v_plan = np.zeros((TRAJECTORY_SIZE,))
self.x_sol = np.zeros((TRAJECTORY_SIZE, 4), dtype=np.float32)
self.v_ego = 0.0
self.l_lane_change_prob = 0.0
self.r_lane_change_prob = 0.0
self.debug_mode = debug
self.lat_mpc = LateralMpc()
self.reset_mpc(np.zeros(4))
def reset_mpc(self, x0=None):
if x0 is None:
x0 = np.zeros(4)
self.x0 = x0
self.lat_mpc.reset(x0=self.x0)
def update(self, sm):
# clip speed , lateral planning is not possible at 0 speed
measured_curvature = sm['controlsState'].curvature
# TODO: do something for 0 speed
# TODO: is a small first order filter needed here?
v_ego_car = sm['carState'].vEgo
# Parse model predictions
md = sm['modelV2']
if len(md.position.x) == TRAJECTORY_SIZE and len(md.orientation.x) == TRAJECTORY_SIZE:
if len(md.position.x) == TRAJECTORY_SIZE and len(md.orientation.x) == TRAJECTORY_SIZE and len(md.lateralPlannerSolution.x) == TRAJECTORY_SIZE:
self.path_xyz = np.column_stack([md.position.x, md.position.y, md.position.z])
self.t_idxs = np.array(md.position.t)
self.plan_yaw = np.array(md.orientation.z)
@ -73,6 +46,8 @@ class LateralPlanner:
car_speed = np.linalg.norm(self.velocity_xyz, axis=1) - get_speed_error(md, v_ego_car)
self.v_plan = np.clip(car_speed, MIN_SPEED, np.inf)
self.v_ego = self.v_plan[0]
# YOLO e2e planning
self.x_sol = np.column_stack([md.lateralPlannerSolution.x, md.lateralPlannerSolution.y, md.lateralPlannerSolution.yaw, md.lateralPlannerSolution.yawRate])
# Lane change logic
desire_state = md.meta.desireState
@ -82,66 +57,23 @@ class LateralPlanner:
lane_change_prob = self.l_lane_change_prob + self.r_lane_change_prob
self.DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob)
self.lat_mpc.set_weights(PATH_COST, LATERAL_MOTION_COST,
LATERAL_ACCEL_COST, LATERAL_JERK_COST,
STEERING_RATE_COST)
y_pts = self.path_xyz[:LAT_MPC_N+1, 1]
heading_pts = self.plan_yaw[:LAT_MPC_N+1]
yaw_rate_pts = self.plan_yaw_rate[:LAT_MPC_N+1]
self.y_pts = y_pts
assert len(y_pts) == LAT_MPC_N + 1
assert len(heading_pts) == LAT_MPC_N + 1
assert len(yaw_rate_pts) == LAT_MPC_N + 1
lateral_factor = np.clip(self.factor1 - (self.factor2 * self.v_plan**2), 0.0, np.inf)
p = np.column_stack([self.v_plan, lateral_factor])
self.lat_mpc.run(self.x0,
p,
y_pts,
heading_pts,
yaw_rate_pts)
# init state for next iteration
# mpc.u_sol is the desired second derivative of psi given x0 curv state.
# with x0[3] = measured_yaw_rate, this would be the actual desired yaw rate.
# instead, interpolate x_sol so that x0[3] is the desired yaw rate for lat_control.
self.x0[3] = interp(DT_MDL, self.t_idxs[:LAT_MPC_N + 1], self.lat_mpc.x_sol[:, 3])
# Check for infeasible MPC solution
mpc_nans = np.isnan(self.lat_mpc.x_sol[:, 3]).any()
t = time.monotonic()
if mpc_nans or self.lat_mpc.solution_status != 0:
self.reset_mpc()
self.x0[3] = measured_curvature * self.v_ego
if t > self.last_cloudlog_t + 5.0:
self.last_cloudlog_t = t
cloudlog.warning("Lateral mpc - nan: True")
if self.lat_mpc.cost > 1e6 or mpc_nans:
self.solution_invalid_cnt += 1
else:
self.solution_invalid_cnt = 0
def publish(self, sm, pm):
plan_solution_valid = self.solution_invalid_cnt < 2
plan_send = messaging.new_message('lateralPlan')
plan_send.valid = sm.all_checks(service_list=['carState', 'controlsState', 'modelV2'])
lateralPlan = plan_send.lateralPlan
lateralPlan.modelMonoTime = sm.logMonoTime['modelV2']
lateralPlan.dPathPoints = self.y_pts.tolist()
lateralPlan.psis = self.lat_mpc.x_sol[0:CONTROL_N, 2].tolist()
lateralPlan.psis = self.x_sol[0:CONTROL_N, 2].tolist()
lateralPlan.curvatures = (self.lat_mpc.x_sol[0:CONTROL_N, 3]/self.v_ego).tolist()
lateralPlan.curvatureRates = [float(x.item() / self.v_ego) for x in self.lat_mpc.u_sol[0:CONTROL_N - 1]] + [0.0]
lateralPlan.curvatures = (self.x_sol[0:CONTROL_N, 3]/self.v_ego).tolist()
lateralPlan.curvatureRates = [float(0) for _ in range(CONTROL_N-1)] # TODO: unused
lateralPlan.mpcSolutionValid = bool(plan_solution_valid)
lateralPlan.solverExecutionTime = self.lat_mpc.solve_time
lateralPlan.mpcSolutionValid = bool(1)
lateralPlan.solverExecutionTime = 0.0
if self.debug_mode:
lateralPlan.solverCost = self.lat_mpc.cost
lateralPlan.solverState = log.LateralPlan.SolverState.new_message()
lateralPlan.solverState.x = self.lat_mpc.x_sol.tolist()
lateralPlan.solverState.u = self.lat_mpc.u_sol.flatten().tolist()
lateralPlan.solverState.x = self.x_sol.tolist()
lateralPlan.desire = self.DH.desire
lateralPlan.useLaneLines = False

4
selfdrive/controls/plannerd.py
Unescape View File

@ -21,8 +21,8 @@ def publish_ui_plan(sm, pm, lateral_planner, longitudinal_planner):
ui_send.valid = sm.all_checks(service_list=['carState', 'controlsState', 'modelV2'])
uiPlan = ui_send.uiPlan
uiPlan.frameId = sm['modelV2'].frameId
uiPlan.position.x = np.interp(plan_odo, model_odo, lateral_planner.lat_mpc.x_sol[:,0]).tolist()
uiPlan.position.y = np.interp(plan_odo, model_odo, lateral_planner.lat_mpc.x_sol[:,1]).tolist()
uiPlan.position.x = np.interp(plan_odo, model_odo, lateral_planner.x_sol[:,0]).tolist()
uiPlan.position.y = np.interp(plan_odo, model_odo, lateral_planner.x_sol[:,1]).tolist()
uiPlan.position.z = np.interp(plan_odo, model_odo, lateral_planner.path_xyz[:,2]).tolist()
uiPlan.accel = longitudinal_planner.a_desired_trajectory_full.tolist()
pm.send('uiPlan', ui_send)

28
selfdrive/modeld/models/driving.cc
Unescape View File

@ -175,6 +175,33 @@ void fill_plan(cereal::ModelDataV2::Builder &framed, const ModelOutputPlanPredic
fill_xyzt(framed.initOrientationRate(), T_IDXS_FLOAT, rot_rate_x, rot_rate_y, rot_rate_z);
}
void fill_lateral_planner(cereal::ModelDataV2::Builder &framed, const LateralPlannerOutput &model_lateral_planner_solution) {
std::array<float, TRAJECTORY_SIZE> lateral_plan_solution_x, lateral_plan_solution_y, lateral_plan_solution_yaw, lateral_plan_solution_yaw_rate;
std::array<float, TRAJECTORY_SIZE> lateral_plan_solution_x_std, lateral_plan_solution_y_std, lateral_plan_solution_yaw_std, lateral_plan_solution_yaw_rate_std;
for (int i=0; i<TRAJECTORY_SIZE; i++) {
lateral_plan_solution_x[i] = model_lateral_planner_solution.mean[i].x;
lateral_plan_solution_y[i] = model_lateral_planner_solution.mean[i].y;
lateral_plan_solution_yaw[i] = model_lateral_planner_solution.mean[i].yaw;
lateral_plan_solution_yaw_rate[i] = model_lateral_planner_solution.mean[i].yaw_rate;
lateral_plan_solution_x_std[i] = exp(model_lateral_planner_solution.std[i].x);
lateral_plan_solution_y_std[i] = exp(model_lateral_planner_solution.std[i].y);
lateral_plan_solution_yaw_std[i] = exp(model_lateral_planner_solution.std[i].yaw);
lateral_plan_solution_yaw_rate_std[i] = exp(model_lateral_planner_solution.std[i].yaw_rate);
}
auto lateral_planner_solution = framed.initLateralPlannerSolution();
lateral_planner_solution.setX(to_kj_array_ptr(lateral_plan_solution_x));
lateral_planner_solution.setY(to_kj_array_ptr(lateral_plan_solution_y));
lateral_planner_solution.setYaw(to_kj_array_ptr(lateral_plan_solution_yaw));
lateral_planner_solution.setYawRate(to_kj_array_ptr(lateral_plan_solution_yaw_rate));
lateral_planner_solution.setXStd(to_kj_array_ptr(lateral_plan_solution_x_std));
lateral_planner_solution.setYStd(to_kj_array_ptr(lateral_plan_solution_y_std));
lateral_planner_solution.setYawStd(to_kj_array_ptr(lateral_plan_solution_yaw_std));
lateral_planner_solution.setYawRateStd(to_kj_array_ptr(lateral_plan_solution_yaw_rate_std));
}
void fill_lane_lines(cereal::ModelDataV2::Builder &framed, const std::array<float, TRAJECTORY_SIZE> &plan_t,
const ModelOutputLaneLines &lanes) {
std::array<float, TRAJECTORY_SIZE> left_far_y, left_far_z;
@ -258,6 +285,7 @@ void fill_model(cereal::ModelDataV2::Builder &framed, const ModelOutput &net_out
}
fill_plan(framed, best_plan);
fill_lateral_planner(framed, net_outputs.lateral_planner_solution);
fill_lane_lines(framed, plan_t, net_outputs.lane_lines);
fill_road_edges(framed, plan_t, net_outputs.road_edges);

15
selfdrive/modeld/models/driving.h
Unescape View File

@ -47,6 +47,20 @@ struct ModelOutputYZ {
};
static_assert(sizeof(ModelOutputYZ) == sizeof(float)*2);
struct LateralPlannerOutputElement {
float x;
float y;
float yaw;
float yaw_rate;
};
static_assert(sizeof(LateralPlannerOutputElement) == sizeof(float)*4);
struct LateralPlannerOutput {
std::array<LateralPlannerOutputElement, TRAJECTORY_SIZE> mean;
std::array<LateralPlannerOutputElement, TRAJECTORY_SIZE> std;
};
static_assert(sizeof(LateralPlannerOutput) == (sizeof(LateralPlannerOutputElement)*TRAJECTORY_SIZE*2));
struct ModelOutputPlanElement {
ModelOutputXYZ position;
ModelOutputXYZ velocity;
@ -241,6 +255,7 @@ struct ModelOutput {
const ModelOutputWideFromDeviceEuler wide_from_device_euler;
const ModelOutputTemporalPose temporal_pose;
const ModelOutputRoadTransform road_transform;
const LateralPlannerOutput lateral_planner_solution;
};
constexpr int OUTPUT_SIZE = sizeof(ModelOutput) / sizeof(float);

4
selfdrive/modeld/models/supercombo.onnx
Unescape View File

@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:c63ea3eb6c9b5a20c7420c2dc6d6d0f80a6949a39f6d8b74e574f52734154820
size 47654714
oid sha256:5fc9b6544e8f8f44b746134630d49ee1858497bd216c2197e7344c4732369af2
size 48319232

Write Preview
Loading…
Cancel
Save