Yassine
2 years ago
commit
f2707ee02f
88 changed files with 1620 additions and 1796 deletions
@ -0,0 +1,62 @@ |
|||||||
|
name: docs |
||||||
|
|
||||||
|
on: |
||||||
|
push: |
||||||
|
branches: |
||||||
|
- master |
||||||
|
pull_request: |
||||||
|
|
||||||
|
concurrency: |
||||||
|
group: ${{ github.event_name == 'push' && github.ref == 'refs/heads/master' && github.run_id || github.head_ref || github.ref }}-${{ github.workflow }}-${{ github.event_name }} |
||||||
|
cancel-in-progress: true |
||||||
|
|
||||||
|
env: |
||||||
|
BASE_IMAGE: openpilot-base |
||||||
|
|
||||||
|
BUILD: selfdrive/test/docker_build.sh base |
||||||
|
|
||||||
|
RUN: docker run --shm-size 1G -v $GITHUB_WORKSPACE:/tmp/openpilot -w /tmp/openpilot -e FILEREADER_CACHE=1 -e PYTHONPATH=/tmp/openpilot -e NUM_JOBS -e JOB_ID -e GITHUB_ACTION -e GITHUB_REF -e GITHUB_HEAD_REF -e GITHUB_SHA -e GITHUB_REPOSITORY -e GITHUB_RUN_ID -v $GITHUB_WORKSPACE/.ci_cache/scons_cache:/tmp/scons_cache -v $GITHUB_WORKSPACE/.ci_cache/comma_download_cache:/tmp/comma_download_cache -v $GITHUB_WORKSPACE/.ci_cache/openpilot_cache:/tmp/openpilot_cache $BASE_IMAGE /bin/sh -c |
||||||
|
|
||||||
|
jobs: |
||||||
|
docs: |
||||||
|
name: build docs |
||||||
|
runs-on: ubuntu-20.04 |
||||||
|
timeout-minutes: 45 |
||||||
|
steps: |
||||||
|
- uses: actions/checkout@v4 |
||||||
|
with: |
||||||
|
submodules: true |
||||||
|
- uses: ./.github/workflows/setup-with-retry |
||||||
|
- name: Build openpilot |
||||||
|
run: | |
||||||
|
${{ env.RUN }} "scons -j$(nproc)" |
||||||
|
- name: Build docs |
||||||
|
run: | |
||||||
|
${{ env.RUN }} "apt update && apt install -y doxygen && cd docs && make html" |
||||||
|
|
||||||
|
- uses: actions/checkout@v4 |
||||||
|
if: github.ref == 'refs/heads/master' && github.repository == 'commaai/openpilot' |
||||||
|
with: |
||||||
|
path: openpilot-docs |
||||||
|
ssh-key: ${{ secrets.OPENPILOT_DOCS_KEY }} |
||||||
|
repository: commaai/openpilot-docs |
||||||
|
- name: Push |
||||||
|
if: github.ref == 'refs/heads/master' && github.repository == 'commaai/openpilot' |
||||||
|
run: | |
||||||
|
set -x |
||||||
|
|
||||||
|
source release/identity.sh |
||||||
|
|
||||||
|
cd openpilot-docs |
||||||
|
|
||||||
|
git checkout --orphan tmp |
||||||
|
git rm -rf --cached . |
||||||
|
|
||||||
|
cp -r ../build/docs/html/ docs/ |
||||||
|
touch docs/.nojekyll |
||||||
|
git add -f . |
||||||
|
|
||||||
|
git commit -m "build docs" |
||||||
|
|
||||||
|
# docs live in different repo to not bloat openpilot's full clone size |
||||||
|
git push -f origin gh-pages |
||||||
@ -1 +1 @@ |
|||||||
Subproject commit bcdbfcfdeb4bf59eae484e9b2c726c8b41c87350 |
Subproject commit 5dcf486c57f2e75cd6e50d2adda02bf776d51c8f |
||||||
@ -1,42 +0,0 @@ |
|||||||
FROM ghcr.io/commaai/openpilot-base:latest |
|
||||||
|
|
||||||
ENV PYTHONUNBUFFERED 1 |
|
||||||
|
|
||||||
ENV OPENPILOT_PATH /tmp/openpilot |
|
||||||
ENV PYTHONPATH ${OPENPILOT_PATH}:${PYTHONPATH} |
|
||||||
ENV POETRY_VIRUALENVS_CREATE false |
|
||||||
|
|
||||||
RUN mkdir -p ${OPENPILOT_PATH} |
|
||||||
WORKDIR ${OPENPILOT_PATH} |
|
||||||
|
|
||||||
COPY SConstruct ${OPENPILOT_PATH} |
|
||||||
|
|
||||||
COPY ./openpilot ${OPENPILOT_PATH}/openpilot |
|
||||||
COPY ./body ${OPENPILOT_PATH}/body |
|
||||||
COPY ./third_party ${OPENPILOT_PATH}/third_party |
|
||||||
COPY ./site_scons ${OPENPILOT_PATH}/site_scons |
|
||||||
COPY ./laika_repo ${OPENPILOT_PATH}/laika_repo |
|
||||||
RUN ln -s ${OPENPILOT_PATH}/laika_repo/laika/ ${OPENPILOT_PATH}/laika |
|
||||||
COPY ./rednose ${OPENPILOT_PATH}/rednose |
|
||||||
COPY ./rednose_repo ${OPENPILOT_PATH}/rednose_repo |
|
||||||
COPY ./tools ${OPENPILOT_PATH}/tools |
|
||||||
COPY ./release ${OPENPILOT_PATH}/release |
|
||||||
COPY ./common ${OPENPILOT_PATH}/common |
|
||||||
COPY ./opendbc ${OPENPILOT_PATH}/opendbc |
|
||||||
COPY ./cereal ${OPENPILOT_PATH}/cereal |
|
||||||
COPY ./panda ${OPENPILOT_PATH}/panda |
|
||||||
COPY ./selfdrive ${OPENPILOT_PATH}/selfdrive |
|
||||||
COPY ./system ${OPENPILOT_PATH}/system |
|
||||||
COPY ./*.md ${OPENPILOT_PATH}/ |
|
||||||
|
|
||||||
RUN --mount=type=bind,source=.ci_cache/scons_cache,target=/tmp/scons_cache,rw scons -j$(nproc) --cache-readonly |
|
||||||
|
|
||||||
RUN apt update && apt install doxygen -y |
|
||||||
COPY ./docs ${OPENPILOT_PATH}/docs |
|
||||||
RUN git init . |
|
||||||
WORKDIR ${OPENPILOT_PATH}/docs |
|
||||||
RUN make html |
|
||||||
|
|
||||||
FROM nginx:1.21 |
|
||||||
COPY --from=0 /tmp/openpilot/build/docs/html /usr/share/nginx/html |
|
||||||
COPY ./docs/docker/nginx.conf /etc/nginx/conf.d/default.conf |
|
||||||
@ -1,15 +0,0 @@ |
|||||||
server { |
|
||||||
listen 80; |
|
||||||
listen [::]:80; |
|
||||||
server_name localhost; |
|
||||||
|
|
||||||
gzip on; |
|
||||||
gzip_types text/html text/plain text/css text/xml text/javascript application/javascript application/x-javascript; |
|
||||||
gzip_min_length 1024; |
|
||||||
gzip_vary on; |
|
||||||
|
|
||||||
root /usr/share/nginx/html; |
|
||||||
location / { |
|
||||||
try_files $uri $uri/ /index.html; |
|
||||||
} |
|
||||||
} |
|
||||||
@ -1 +1 @@ |
|||||||
Subproject commit 6bf6ba773ea4174c702ac9c4af4e8a6fd3cef655 |
Subproject commit 549fa32fc7b0354ebbb48bae846bff380eab9446 |
||||||
File diff suppressed because it is too large
Load Diff
@ -0,0 +1,50 @@ |
|||||||
|
import numpy as np |
||||||
|
from typing import List, Optional, Tuple, Any |
||||||
|
|
||||||
|
|
||||||
|
class NPQueue: |
||||||
|
def __init__(self, maxlen: int, rowsize: int) -> None: |
||||||
|
self.maxlen = maxlen |
||||||
|
self.arr = np.empty((0, rowsize)) |
||||||
|
|
||||||
|
def __len__(self) -> int: |
||||||
|
return len(self.arr) |
||||||
|
|
||||||
|
def append(self, pt: List[float]) -> None: |
||||||
|
if len(self.arr) < self.maxlen: |
||||||
|
self.arr = np.append(self.arr, [pt], axis=0) |
||||||
|
else: |
||||||
|
self.arr[:-1] = self.arr[1:] |
||||||
|
self.arr[-1] = pt |
||||||
|
|
||||||
|
|
||||||
|
class PointBuckets: |
||||||
|
def __init__(self, x_bounds: List[Tuple[float, float]], min_points: List[float], min_points_total: int, points_per_bucket: int, rowsize: int) -> None: |
||||||
|
self.x_bounds = x_bounds |
||||||
|
self.buckets = {bounds: NPQueue(maxlen=points_per_bucket, rowsize=rowsize) for bounds in x_bounds} |
||||||
|
self.buckets_min_points = dict(zip(x_bounds, min_points, strict=True)) |
||||||
|
self.min_points_total = min_points_total |
||||||
|
|
||||||
|
def bucket_lengths(self) -> List[int]: |
||||||
|
return [len(v) for v in self.buckets.values()] |
||||||
|
|
||||||
|
def __len__(self) -> int: |
||||||
|
return sum(self.bucket_lengths()) |
||||||
|
|
||||||
|
def is_valid(self) -> bool: |
||||||
|
individual_buckets_valid = all(len(v) >= min_pts for v, min_pts in zip(self.buckets.values(), self.buckets_min_points.values(), strict=True)) |
||||||
|
total_points_valid = self.__len__() >= self.min_points_total |
||||||
|
return individual_buckets_valid and total_points_valid |
||||||
|
|
||||||
|
def add_point(self, x: float, y: float, bucket_val: float) -> None: |
||||||
|
raise NotImplementedError |
||||||
|
|
||||||
|
def get_points(self, num_points: Optional[int] = None) -> Any: |
||||||
|
points = np.vstack([x.arr for x in self.buckets.values()]) |
||||||
|
if num_points is None: |
||||||
|
return points |
||||||
|
return points[np.random.choice(np.arange(len(points)), min(len(points), num_points), replace=False)] |
||||||
|
|
||||||
|
def load_points(self, points: List[List[float]]) -> None: |
||||||
|
for point in points: |
||||||
|
self.add_point(*point) |
||||||
@ -1,7 +1,80 @@ |
|||||||
IDX_N = 33 |
import numpy as np |
||||||
|
|
||||||
def index_function(idx, max_val=192, max_idx=32): |
def index_function(idx, max_val=192, max_idx=32): |
||||||
return (max_val) * ((idx/max_idx)**2) |
return (max_val) * ((idx/max_idx)**2) |
||||||
|
|
||||||
|
class ModelConstants: |
||||||
|
# time and distance indices |
||||||
|
IDX_N = 33 |
||||||
|
T_IDXS = [index_function(idx, max_val=10.0) for idx in range(IDX_N)] |
||||||
|
X_IDXS = [index_function(idx, max_val=192.0) for idx in range(IDX_N)] |
||||||
|
LEAD_T_IDXS = [0., 2., 4., 6., 8., 10.] |
||||||
|
LEAD_T_OFFSETS = [0., 2., 4.] |
||||||
|
META_T_IDXS = [2., 4., 6., 8., 10.] |
||||||
|
|
||||||
T_IDXS = [index_function(idx, max_val=10.0) for idx in range(IDX_N)] |
# model inputs constants |
||||||
|
MODEL_FREQ = 20 |
||||||
|
FEATURE_LEN = 512 |
||||||
|
HISTORY_BUFFER_LEN = 99 |
||||||
|
DESIRE_LEN = 8 |
||||||
|
TRAFFIC_CONVENTION_LEN = 2 |
||||||
|
NAV_FEATURE_LEN = 256 |
||||||
|
NAV_INSTRUCTION_LEN = 150 |
||||||
|
DRIVING_STYLE_LEN = 12 |
||||||
|
LAT_PLANNER_STATE_LEN = 4 |
||||||
|
|
||||||
|
# model outputs constants |
||||||
|
FCW_THRESHOLDS_5MS2 = np.array([.05, .05, .15, .15, .15], dtype=np.float32) |
||||||
|
FCW_THRESHOLDS_3MS2 = np.array([.7, .7], dtype=np.float32) |
||||||
|
|
||||||
|
DISENGAGE_WIDTH = 5 |
||||||
|
POSE_WIDTH = 6 |
||||||
|
WIDE_FROM_DEVICE_WIDTH = 3 |
||||||
|
SIM_POSE_WIDTH = 6 |
||||||
|
LEAD_WIDTH = 4 |
||||||
|
LANE_LINES_WIDTH = 2 |
||||||
|
ROAD_EDGES_WIDTH = 2 |
||||||
|
PLAN_WIDTH = 15 |
||||||
|
DESIRE_PRED_WIDTH = 8 |
||||||
|
LAT_PLANNER_SOLUTION_WIDTH = 4 |
||||||
|
|
||||||
|
NUM_LANE_LINES = 4 |
||||||
|
NUM_ROAD_EDGES = 2 |
||||||
|
|
||||||
|
LEAD_TRAJ_LEN = 6 |
||||||
|
DESIRE_PRED_LEN = 4 |
||||||
|
|
||||||
|
PLAN_MHP_N = 5 |
||||||
|
LEAD_MHP_N = 2 |
||||||
|
PLAN_MHP_SELECTION = 1 |
||||||
|
LEAD_MHP_SELECTION = 3 |
||||||
|
|
||||||
|
FCW_THRESHOLD_5MS2_HIGH = 0.15 |
||||||
|
FCW_THRESHOLD_5MS2_LOW = 0.05 |
||||||
|
FCW_THRESHOLD_3MS2 = 0.7 |
||||||
|
|
||||||
|
CONFIDENCE_BUFFER_LEN = 5 |
||||||
|
RYG_GREEN = 0.01165 |
||||||
|
RYG_YELLOW = 0.06157 |
||||||
|
|
||||||
|
# model outputs slices |
||||||
|
class Plan: |
||||||
|
POSITION = slice(0, 3) |
||||||
|
VELOCITY = slice(3, 6) |
||||||
|
ACCELERATION = slice(6, 9) |
||||||
|
T_FROM_CURRENT_EULER = slice(9, 12) |
||||||
|
ORIENTATION_RATE = slice(12, 15) |
||||||
|
|
||||||
|
class Meta: |
||||||
|
ENGAGED = slice(0, 1) |
||||||
|
# next 2, 4, 6, 8, 10 seconds |
||||||
|
GAS_DISENGAGE = slice(1, 36, 7) |
||||||
|
BRAKE_DISENGAGE = slice(2, 36, 7) |
||||||
|
STEER_OVERRIDE = slice(3, 36, 7) |
||||||
|
HARD_BRAKE_3 = slice(4, 36, 7) |
||||||
|
HARD_BRAKE_4 = slice(5, 36, 7) |
||||||
|
HARD_BRAKE_5 = slice(6, 36, 7) |
||||||
|
GAS_PRESS = slice(7, 36, 7) |
||||||
|
# next 0, 2, 4, 6, 8, 10 seconds |
||||||
|
LEFT_BLINKER = slice(36, 48, 2) |
||||||
|
RIGHT_BLINKER = slice(37, 48, 2) |
||||||
|
|||||||
@ -0,0 +1,186 @@ |
|||||||
|
import capnp |
||||||
|
import numpy as np |
||||||
|
from typing import Dict |
||||||
|
from cereal import log |
||||||
|
from openpilot.selfdrive.modeld.constants import ModelConstants, Plan, Meta |
||||||
|
|
||||||
|
ConfidenceClass = log.ModelDataV2.ConfidenceClass |
||||||
|
|
||||||
|
class PublishState: |
||||||
|
def __init__(self): |
||||||
|
self.disengage_buffer = np.zeros(ModelConstants.CONFIDENCE_BUFFER_LEN*ModelConstants.DISENGAGE_WIDTH, dtype=np.float32) |
||||||
|
self.prev_brake_5ms2_probs = np.zeros(ModelConstants.DISENGAGE_WIDTH, dtype=np.float32) |
||||||
|
self.prev_brake_3ms2_probs = np.zeros(ModelConstants.DISENGAGE_WIDTH, dtype=np.float32) |
||||||
|
|
||||||
|
def fill_xyzt(builder, t, x, y, z, x_std=None, y_std=None, z_std=None): |
||||||
|
builder.t = t |
||||||
|
builder.x = x.tolist() |
||||||
|
builder.y = y.tolist() |
||||||
|
builder.z = z.tolist() |
||||||
|
if x_std is not None: |
||||||
|
builder.xStd = x_std.tolist() |
||||||
|
if y_std is not None: |
||||||
|
builder.yStd = y_std.tolist() |
||||||
|
if z_std is not None: |
||||||
|
builder.zStd = z_std.tolist() |
||||||
|
|
||||||
|
def fill_xyvat(builder, t, x, y, v, a, x_std=None, y_std=None, v_std=None, a_std=None): |
||||||
|
builder.t = t |
||||||
|
builder.x = x.tolist() |
||||||
|
builder.y = y.tolist() |
||||||
|
builder.v = v.tolist() |
||||||
|
builder.a = a.tolist() |
||||||
|
if x_std is not None: |
||||||
|
builder.xStd = x_std.tolist() |
||||||
|
if y_std is not None: |
||||||
|
builder.yStd = y_std.tolist() |
||||||
|
if v_std is not None: |
||||||
|
builder.vStd = v_std.tolist() |
||||||
|
if a_std is not None: |
||||||
|
builder.aStd = a_std.tolist() |
||||||
|
|
||||||
|
def fill_model_msg(msg: capnp._DynamicStructBuilder, net_output_data: Dict[str, np.ndarray], publish_state: PublishState, |
||||||
|
vipc_frame_id: int, vipc_frame_id_extra: int, frame_id: int, frame_drop: float, |
||||||
|
timestamp_eof: int, timestamp_llk: int, model_execution_time: float, |
||||||
|
nav_enabled: bool, valid: bool) -> None: |
||||||
|
frame_age = frame_id - vipc_frame_id if frame_id > vipc_frame_id else 0 |
||||||
|
msg.valid = valid |
||||||
|
|
||||||
|
modelV2 = msg.modelV2 |
||||||
|
modelV2.frameId = vipc_frame_id |
||||||
|
modelV2.frameIdExtra = vipc_frame_id_extra |
||||||
|
modelV2.frameAge = frame_age |
||||||
|
modelV2.frameDropPerc = frame_drop * 100 |
||||||
|
modelV2.timestampEof = timestamp_eof |
||||||
|
modelV2.locationMonoTime = timestamp_llk |
||||||
|
modelV2.modelExecutionTime = model_execution_time |
||||||
|
modelV2.navEnabled = nav_enabled |
||||||
|
|
||||||
|
# plan |
||||||
|
position = modelV2.position |
||||||
|
fill_xyzt(position, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.POSITION].T, *net_output_data['plan_stds'][0,:,Plan.POSITION].T) |
||||||
|
velocity = modelV2.velocity |
||||||
|
fill_xyzt(velocity, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.VELOCITY].T) |
||||||
|
acceleration = modelV2.acceleration |
||||||
|
fill_xyzt(acceleration, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.ACCELERATION].T) |
||||||
|
orientation = modelV2.orientation |
||||||
|
fill_xyzt(orientation, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.T_FROM_CURRENT_EULER].T) |
||||||
|
orientation_rate = modelV2.orientationRate |
||||||
|
fill_xyzt(orientation_rate, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.ORIENTATION_RATE].T) |
||||||
|
|
||||||
|
# lateral planning |
||||||
|
solution = modelV2.lateralPlannerSolution |
||||||
|
solution.x, solution.y, solution.yaw, solution.yawRate = [net_output_data['lat_planner_solution'][0,:,i].tolist() for i in range(4)] |
||||||
|
solution.xStd, solution.yStd, solution.yawStd, solution.yawRateStd = [net_output_data['lat_planner_solution_stds'][0,:,i].tolist() for i in range(4)] |
||||||
|
|
||||||
|
# times at X_IDXS according to model plan |
||||||
|
PLAN_T_IDXS = [np.nan] * ModelConstants.IDX_N |
||||||
|
PLAN_T_IDXS[0] = 0.0 |
||||||
|
plan_x = net_output_data['plan'][0,:,Plan.POSITION][:,0].tolist() |
||||||
|
for xidx in range(1, ModelConstants.IDX_N): |
||||||
|
tidx = 0 |
||||||
|
# increment tidx until we find an element that's further away than the current xidx |
||||||
|
while tidx < ModelConstants.IDX_N - 1 and plan_x[tidx+1] < ModelConstants.X_IDXS[xidx]: |
||||||
|
tidx += 1 |
||||||
|
if tidx == ModelConstants.IDX_N - 1: |
||||||
|
# if the Plan doesn't extend far enough, set plan_t to the max value (10s), then break |
||||||
|
PLAN_T_IDXS[xidx] = ModelConstants.T_IDXS[ModelConstants.IDX_N - 1] |
||||||
|
break |
||||||
|
# interpolate to find `t` for the current xidx |
||||||
|
current_x_val = plan_x[tidx] |
||||||
|
next_x_val = plan_x[tidx+1] |
||||||
|
p = (ModelConstants.X_IDXS[xidx] - current_x_val) / (next_x_val - current_x_val) |
||||||
|
PLAN_T_IDXS[xidx] = p * ModelConstants.T_IDXS[tidx+1] + (1 - p) * ModelConstants.T_IDXS[tidx] |
||||||
|
|
||||||
|
# lane lines |
||||||
|
modelV2.init('laneLines', 4) |
||||||
|
for i in range(4): |
||||||
|
lane_line = modelV2.laneLines[i] |
||||||
|
fill_xyzt(lane_line, PLAN_T_IDXS, np.array(ModelConstants.X_IDXS), net_output_data['lane_lines'][0,i,:,0], net_output_data['lane_lines'][0,i,:,1]) |
||||||
|
modelV2.laneLineStds = net_output_data['lane_lines_stds'][0,:,0,0].tolist() |
||||||
|
modelV2.laneLineProbs = net_output_data['lane_lines_prob'][0,1::2].tolist() |
||||||
|
|
||||||
|
# road edges |
||||||
|
modelV2.init('roadEdges', 2) |
||||||
|
for i in range(2): |
||||||
|
road_edge = modelV2.roadEdges[i] |
||||||
|
fill_xyzt(road_edge, PLAN_T_IDXS, np.array(ModelConstants.X_IDXS), net_output_data['road_edges'][0,i,:,0], net_output_data['road_edges'][0,i,:,1]) |
||||||
|
modelV2.roadEdgeStds = net_output_data['road_edges_stds'][0,:,0,0].tolist() |
||||||
|
|
||||||
|
# leads |
||||||
|
modelV2.init('leadsV3', 3) |
||||||
|
for i in range(3): |
||||||
|
lead = modelV2.leadsV3[i] |
||||||
|
fill_xyvat(lead, ModelConstants.LEAD_T_IDXS, *net_output_data['lead'][0,i].T, *net_output_data['lead_stds'][0,i].T) |
||||||
|
lead.prob = net_output_data['lead_prob'][0,i].tolist() |
||||||
|
lead.probTime = ModelConstants.LEAD_T_OFFSETS[i] |
||||||
|
|
||||||
|
# meta |
||||||
|
meta = modelV2.meta |
||||||
|
meta.desireState = net_output_data['desire_state'][0].reshape(-1).tolist() |
||||||
|
meta.desirePrediction = net_output_data['desire_pred'][0].reshape(-1).tolist() |
||||||
|
meta.engagedProb = net_output_data['meta'][0,Meta.ENGAGED].item() |
||||||
|
meta.init('disengagePredictions') |
||||||
|
disengage_predictions = meta.disengagePredictions |
||||||
|
disengage_predictions.t = ModelConstants.META_T_IDXS |
||||||
|
disengage_predictions.brakeDisengageProbs = net_output_data['meta'][0,Meta.BRAKE_DISENGAGE].tolist() |
||||||
|
disengage_predictions.gasDisengageProbs = net_output_data['meta'][0,Meta.GAS_DISENGAGE].tolist() |
||||||
|
disengage_predictions.steerOverrideProbs = net_output_data['meta'][0,Meta.STEER_OVERRIDE].tolist() |
||||||
|
disengage_predictions.brake3MetersPerSecondSquaredProbs = net_output_data['meta'][0,Meta.HARD_BRAKE_3].tolist() |
||||||
|
disengage_predictions.brake4MetersPerSecondSquaredProbs = net_output_data['meta'][0,Meta.HARD_BRAKE_4].tolist() |
||||||
|
disengage_predictions.brake5MetersPerSecondSquaredProbs = net_output_data['meta'][0,Meta.HARD_BRAKE_5].tolist() |
||||||
|
|
||||||
|
publish_state.prev_brake_5ms2_probs[:-1] = publish_state.prev_brake_5ms2_probs[1:] |
||||||
|
publish_state.prev_brake_5ms2_probs[-1] = net_output_data['meta'][0,Meta.HARD_BRAKE_5][0] |
||||||
|
publish_state.prev_brake_3ms2_probs[:-1] = publish_state.prev_brake_3ms2_probs[1:] |
||||||
|
publish_state.prev_brake_3ms2_probs[-1] = net_output_data['meta'][0,Meta.HARD_BRAKE_3][0] |
||||||
|
hard_brake_predicted = (publish_state.prev_brake_5ms2_probs > ModelConstants.FCW_THRESHOLDS_5MS2).all() and \ |
||||||
|
(publish_state.prev_brake_3ms2_probs > ModelConstants.FCW_THRESHOLDS_3MS2).all() |
||||||
|
meta.hardBrakePredicted = hard_brake_predicted.item() |
||||||
|
|
||||||
|
# temporal pose |
||||||
|
temporal_pose = modelV2.temporalPose |
||||||
|
temporal_pose.trans = net_output_data['sim_pose'][0,:3].tolist() |
||||||
|
temporal_pose.transStd = net_output_data['sim_pose_stds'][0,:3].tolist() |
||||||
|
temporal_pose.rot = net_output_data['sim_pose'][0,3:].tolist() |
||||||
|
temporal_pose.rotStd = net_output_data['sim_pose_stds'][0,3:].tolist() |
||||||
|
|
||||||
|
# confidence |
||||||
|
if vipc_frame_id % (2*ModelConstants.MODEL_FREQ) == 0: |
||||||
|
# any disengage prob |
||||||
|
brake_disengage_probs = net_output_data['meta'][0,Meta.BRAKE_DISENGAGE] |
||||||
|
gas_disengage_probs = net_output_data['meta'][0,Meta.GAS_DISENGAGE] |
||||||
|
steer_override_probs = net_output_data['meta'][0,Meta.STEER_OVERRIDE] |
||||||
|
any_disengage_probs = 1-((1-brake_disengage_probs)*(1-gas_disengage_probs)*(1-steer_override_probs)) |
||||||
|
# independent disengage prob for each 2s slice |
||||||
|
ind_disengage_probs = np.r_[any_disengage_probs[0], np.diff(any_disengage_probs) / (1 - any_disengage_probs[:-1])] |
||||||
|
# rolling buf for 2, 4, 6, 8, 10s |
||||||
|
publish_state.disengage_buffer[:-ModelConstants.DISENGAGE_WIDTH] = publish_state.disengage_buffer[ModelConstants.DISENGAGE_WIDTH:] |
||||||
|
publish_state.disengage_buffer[-ModelConstants.DISENGAGE_WIDTH:] = ind_disengage_probs |
||||||
|
|
||||||
|
score = 0. |
||||||
|
for i in range(ModelConstants.DISENGAGE_WIDTH): |
||||||
|
score += publish_state.disengage_buffer[i*ModelConstants.DISENGAGE_WIDTH+ModelConstants.DISENGAGE_WIDTH-1-i].item() / ModelConstants.DISENGAGE_WIDTH |
||||||
|
if score < ModelConstants.RYG_GREEN: |
||||||
|
modelV2.confidence = ConfidenceClass.green |
||||||
|
elif score < ModelConstants.RYG_YELLOW: |
||||||
|
modelV2.confidence = ConfidenceClass.yellow |
||||||
|
else: |
||||||
|
modelV2.confidence = ConfidenceClass.red |
||||||
|
|
||||||
|
def fill_pose_msg(msg: capnp._DynamicStructBuilder, net_output_data: Dict[str, np.ndarray], |
||||||
|
vipc_frame_id: int, vipc_dropped_frames: int, timestamp_eof: int, live_calib_seen: bool) -> None: |
||||||
|
msg.valid = live_calib_seen & (vipc_dropped_frames < 1) |
||||||
|
cameraOdometry = msg.cameraOdometry |
||||||
|
|
||||||
|
cameraOdometry.frameId = vipc_frame_id |
||||||
|
cameraOdometry.timestampEof = timestamp_eof |
||||||
|
|
||||||
|
cameraOdometry.trans = net_output_data['pose'][0,:3].tolist() |
||||||
|
cameraOdometry.rot = net_output_data['pose'][0,3:].tolist() |
||||||
|
cameraOdometry.wideFromDeviceEuler = net_output_data['wide_from_device_euler'][0,:].tolist() |
||||||
|
cameraOdometry.roadTransformTrans = net_output_data['road_transform'][0,:3].tolist() |
||||||
|
cameraOdometry.transStd = net_output_data['pose_stds'][0,:3].tolist() |
||||||
|
cameraOdometry.rotStd = net_output_data['pose_stds'][0,3:].tolist() |
||||||
|
cameraOdometry.wideFromDeviceEulerStd = net_output_data['wide_from_device_euler_stds'][0,:].tolist() |
||||||
|
cameraOdometry.roadTransformTransStd = net_output_data['road_transform_stds'][0,:3].tolist() |
||||||
@ -0,0 +1,29 @@ |
|||||||
|
#!/usr/bin/env python3 |
||||||
|
import sys |
||||||
|
import pathlib |
||||||
|
import onnx |
||||||
|
import codecs |
||||||
|
import pickle |
||||||
|
from typing import Tuple |
||||||
|
|
||||||
|
def get_name_and_shape(value_info:onnx.ValueInfoProto) -> Tuple[str, Tuple[int,...]]: |
||||||
|
shape = tuple([int(dim.dim_value) for dim in value_info.type.tensor_type.shape.dim]) |
||||||
|
name = value_info.name |
||||||
|
return name, shape |
||||||
|
|
||||||
|
if __name__ == "__main__": |
||||||
|
model_path = pathlib.Path(sys.argv[1]) |
||||||
|
model = onnx.load(str(model_path)) |
||||||
|
i = [x.key for x in model.metadata_props].index('output_slices') |
||||||
|
output_slices = model.metadata_props[i].value |
||||||
|
|
||||||
|
metadata = {} |
||||||
|
metadata['output_slices'] = pickle.loads(codecs.decode(output_slices.encode(), "base64")) |
||||||
|
metadata['input_shapes'] = dict([get_name_and_shape(x) for x in model.graph.input]) |
||||||
|
metadata['output_shapes'] = dict([get_name_and_shape(x) for x in model.graph.output]) |
||||||
|
|
||||||
|
metadata_path = model_path.parent / (model_path.stem + '_metadata.pkl') |
||||||
|
with open(metadata_path, 'wb') as f: |
||||||
|
pickle.dump(metadata, f) |
||||||
|
|
||||||
|
print(f'saved metadata to {metadata_path}') |
||||||
@ -1,358 +0,0 @@ |
|||||||
#include "selfdrive/modeld/models/driving.h" |
|
||||||
|
|
||||||
#include <cstring> |
|
||||||
|
|
||||||
|
|
||||||
void fill_lead(cereal::ModelDataV2::LeadDataV3::Builder lead, const ModelOutputLeads &leads, int t_idx, float prob_t) { |
|
||||||
std::array<float, LEAD_TRAJ_LEN> lead_t = {0.0, 2.0, 4.0, 6.0, 8.0, 10.0}; |
|
||||||
const auto &best_prediction = leads.get_best_prediction(t_idx); |
|
||||||
lead.setProb(sigmoid(leads.prob[t_idx])); |
|
||||||
lead.setProbTime(prob_t); |
|
||||||
std::array<float, LEAD_TRAJ_LEN> lead_x, lead_y, lead_v, lead_a; |
|
||||||
std::array<float, LEAD_TRAJ_LEN> lead_x_std, lead_y_std, lead_v_std, lead_a_std; |
|
||||||
for (int i=0; i<LEAD_TRAJ_LEN; i++) { |
|
||||||
lead_x[i] = best_prediction.mean[i].x; |
|
||||||
lead_y[i] = best_prediction.mean[i].y; |
|
||||||
lead_v[i] = best_prediction.mean[i].velocity; |
|
||||||
lead_a[i] = best_prediction.mean[i].acceleration; |
|
||||||
lead_x_std[i] = exp(best_prediction.std[i].x); |
|
||||||
lead_y_std[i] = exp(best_prediction.std[i].y); |
|
||||||
lead_v_std[i] = exp(best_prediction.std[i].velocity); |
|
||||||
lead_a_std[i] = exp(best_prediction.std[i].acceleration); |
|
||||||
} |
|
||||||
lead.setT(to_kj_array_ptr(lead_t)); |
|
||||||
lead.setX(to_kj_array_ptr(lead_x)); |
|
||||||
lead.setY(to_kj_array_ptr(lead_y)); |
|
||||||
lead.setV(to_kj_array_ptr(lead_v)); |
|
||||||
lead.setA(to_kj_array_ptr(lead_a)); |
|
||||||
lead.setXStd(to_kj_array_ptr(lead_x_std)); |
|
||||||
lead.setYStd(to_kj_array_ptr(lead_y_std)); |
|
||||||
lead.setVStd(to_kj_array_ptr(lead_v_std)); |
|
||||||
lead.setAStd(to_kj_array_ptr(lead_a_std)); |
|
||||||
} |
|
||||||
|
|
||||||
void fill_meta(cereal::ModelDataV2::MetaData::Builder meta, const ModelOutputMeta &meta_data, PublishState &ps) { |
|
||||||
std::array<float, DESIRE_LEN> desire_state_softmax; |
|
||||||
softmax(meta_data.desire_state_prob.array.data(), desire_state_softmax.data(), DESIRE_LEN); |
|
||||||
|
|
||||||
std::array<float, DESIRE_PRED_LEN * DESIRE_LEN> desire_pred_softmax; |
|
||||||
for (int i=0; i<DESIRE_PRED_LEN; i++) { |
|
||||||
softmax(meta_data.desire_pred_prob[i].array.data(), desire_pred_softmax.data() + (i * DESIRE_LEN), DESIRE_LEN); |
|
||||||
} |
|
||||||
|
|
||||||
std::array<float, DISENGAGE_LEN> lat_long_t = {2, 4, 6, 8, 10}; |
|
||||||
std::array<float, DISENGAGE_LEN> gas_disengage_sigmoid, brake_disengage_sigmoid, steer_override_sigmoid, |
|
||||||
brake_3ms2_sigmoid, brake_4ms2_sigmoid, brake_5ms2_sigmoid; |
|
||||||
for (int i=0; i<DISENGAGE_LEN; i++) { |
|
||||||
gas_disengage_sigmoid[i] = sigmoid(meta_data.disengage_prob[i].gas_disengage); |
|
||||||
brake_disengage_sigmoid[i] = sigmoid(meta_data.disengage_prob[i].brake_disengage); |
|
||||||
steer_override_sigmoid[i] = sigmoid(meta_data.disengage_prob[i].steer_override); |
|
||||||
brake_3ms2_sigmoid[i] = sigmoid(meta_data.disengage_prob[i].brake_3ms2); |
|
||||||
brake_4ms2_sigmoid[i] = sigmoid(meta_data.disengage_prob[i].brake_4ms2); |
|
||||||
brake_5ms2_sigmoid[i] = sigmoid(meta_data.disengage_prob[i].brake_5ms2); |
|
||||||
//gas_pressed_sigmoid[i] = sigmoid(meta_data.disengage_prob[i].gas_pressed);
|
|
||||||
} |
|
||||||
|
|
||||||
std::memmove(ps.prev_brake_5ms2_probs.data(), &ps.prev_brake_5ms2_probs[1], 4*sizeof(float)); |
|
||||||
std::memmove(ps.prev_brake_3ms2_probs.data(), &ps.prev_brake_3ms2_probs[1], 2*sizeof(float)); |
|
||||||
ps.prev_brake_5ms2_probs[4] = brake_5ms2_sigmoid[0]; |
|
||||||
ps.prev_brake_3ms2_probs[2] = brake_3ms2_sigmoid[0]; |
|
||||||
|
|
||||||
bool above_fcw_threshold = true; |
|
||||||
for (int i=0; i<ps.prev_brake_5ms2_probs.size(); i++) { |
|
||||||
float threshold = i < 2 ? FCW_THRESHOLD_5MS2_LOW : FCW_THRESHOLD_5MS2_HIGH; |
|
||||||
above_fcw_threshold = above_fcw_threshold && ps.prev_brake_5ms2_probs[i] > threshold; |
|
||||||
} |
|
||||||
for (int i=0; i<ps.prev_brake_3ms2_probs.size(); i++) { |
|
||||||
above_fcw_threshold = above_fcw_threshold && ps.prev_brake_3ms2_probs[i] > FCW_THRESHOLD_3MS2; |
|
||||||
} |
|
||||||
|
|
||||||
auto disengage = meta.initDisengagePredictions(); |
|
||||||
disengage.setT(to_kj_array_ptr(lat_long_t)); |
|
||||||
disengage.setGasDisengageProbs(to_kj_array_ptr(gas_disengage_sigmoid)); |
|
||||||
disengage.setBrakeDisengageProbs(to_kj_array_ptr(brake_disengage_sigmoid)); |
|
||||||
disengage.setSteerOverrideProbs(to_kj_array_ptr(steer_override_sigmoid)); |
|
||||||
disengage.setBrake3MetersPerSecondSquaredProbs(to_kj_array_ptr(brake_3ms2_sigmoid)); |
|
||||||
disengage.setBrake4MetersPerSecondSquaredProbs(to_kj_array_ptr(brake_4ms2_sigmoid)); |
|
||||||
disengage.setBrake5MetersPerSecondSquaredProbs(to_kj_array_ptr(brake_5ms2_sigmoid)); |
|
||||||
|
|
||||||
meta.setEngagedProb(sigmoid(meta_data.engaged_prob)); |
|
||||||
meta.setDesirePrediction(to_kj_array_ptr(desire_pred_softmax)); |
|
||||||
meta.setDesireState(to_kj_array_ptr(desire_state_softmax)); |
|
||||||
meta.setHardBrakePredicted(above_fcw_threshold); |
|
||||||
} |
|
||||||
|
|
||||||
void fill_confidence(cereal::ModelDataV2::Builder &framed, PublishState &ps) { |
|
||||||
if (framed.getFrameId() % (2*MODEL_FREQ) == 0) { |
|
||||||
// update every 2s to match predictions interval
|
|
||||||
auto dbps = framed.getMeta().getDisengagePredictions().getBrakeDisengageProbs(); |
|
||||||
auto dgps = framed.getMeta().getDisengagePredictions().getGasDisengageProbs(); |
|
||||||
auto dsps = framed.getMeta().getDisengagePredictions().getSteerOverrideProbs(); |
|
||||||
|
|
||||||
float any_dp[DISENGAGE_LEN]; |
|
||||||
float dp_ind[DISENGAGE_LEN]; |
|
||||||
|
|
||||||
for (int i = 0; i < DISENGAGE_LEN; i++) { |
|
||||||
any_dp[i] = 1 - ((1-dbps[i])*(1-dgps[i])*(1-dsps[i])); // any disengage prob
|
|
||||||
} |
|
||||||
|
|
||||||
dp_ind[0] = any_dp[0]; |
|
||||||
for (int i = 0; i < DISENGAGE_LEN-1; i++) { |
|
||||||
dp_ind[i+1] = (any_dp[i+1] - any_dp[i]) / (1 - any_dp[i]); // independent disengage prob for each 2s slice
|
|
||||||
} |
|
||||||
|
|
||||||
// rolling buf for 2, 4, 6, 8, 10s
|
|
||||||
std::memmove(&ps.disengage_buffer[0], &ps.disengage_buffer[DISENGAGE_LEN], sizeof(float) * DISENGAGE_LEN * (DISENGAGE_LEN-1)); |
|
||||||
std::memcpy(&ps.disengage_buffer[DISENGAGE_LEN * (DISENGAGE_LEN-1)], &dp_ind[0], sizeof(float) * DISENGAGE_LEN); |
|
||||||
} |
|
||||||
|
|
||||||
float score = 0; |
|
||||||
for (int i = 0; i < DISENGAGE_LEN; i++) { |
|
||||||
score += ps.disengage_buffer[i*DISENGAGE_LEN+DISENGAGE_LEN-1-i] / DISENGAGE_LEN; |
|
||||||
} |
|
||||||
|
|
||||||
if (score < RYG_GREEN) { |
|
||||||
framed.setConfidence(cereal::ModelDataV2::ConfidenceClass::GREEN); |
|
||||||
} else if (score < RYG_YELLOW) { |
|
||||||
framed.setConfidence(cereal::ModelDataV2::ConfidenceClass::YELLOW); |
|
||||||
} else { |
|
||||||
framed.setConfidence(cereal::ModelDataV2::ConfidenceClass::RED); |
|
||||||
} |
|
||||||
} |
|
||||||
|
|
||||||
template<size_t size> |
|
||||||
void fill_xyzt(cereal::XYZTData::Builder xyzt, const std::array<float, size> &t, |
|
||||||
const std::array<float, size> &x, const std::array<float, size> &y, const std::array<float, size> &z) { |
|
||||||
xyzt.setT(to_kj_array_ptr(t)); |
|
||||||
xyzt.setX(to_kj_array_ptr(x)); |
|
||||||
xyzt.setY(to_kj_array_ptr(y)); |
|
||||||
xyzt.setZ(to_kj_array_ptr(z)); |
|
||||||
} |
|
||||||
|
|
||||||
template<size_t size> |
|
||||||
void fill_xyzt(cereal::XYZTData::Builder xyzt, const std::array<float, size> &t, |
|
||||||
const std::array<float, size> &x, const std::array<float, size> &y, const std::array<float, size> &z, |
|
||||||
const std::array<float, size> &x_std, const std::array<float, size> &y_std, const std::array<float, size> &z_std) { |
|
||||||
fill_xyzt(xyzt, t, x, y, z); |
|
||||||
xyzt.setXStd(to_kj_array_ptr(x_std)); |
|
||||||
xyzt.setYStd(to_kj_array_ptr(y_std)); |
|
||||||
xyzt.setZStd(to_kj_array_ptr(z_std)); |
|
||||||
} |
|
||||||
|
|
||||||
void fill_plan(cereal::ModelDataV2::Builder &framed, const ModelOutputPlanPrediction &plan) { |
|
||||||
std::array<float, TRAJECTORY_SIZE> pos_x, pos_y, pos_z; |
|
||||||
std::array<float, TRAJECTORY_SIZE> pos_x_std, pos_y_std, pos_z_std; |
|
||||||
std::array<float, TRAJECTORY_SIZE> vel_x, vel_y, vel_z; |
|
||||||
std::array<float, TRAJECTORY_SIZE> rot_x, rot_y, rot_z; |
|
||||||
std::array<float, TRAJECTORY_SIZE> acc_x, acc_y, acc_z; |
|
||||||
std::array<float, TRAJECTORY_SIZE> rot_rate_x, rot_rate_y, rot_rate_z; |
|
||||||
|
|
||||||
for (int i=0; i<TRAJECTORY_SIZE; i++) { |
|
||||||
pos_x[i] = plan.mean[i].position.x; |
|
||||||
pos_y[i] = plan.mean[i].position.y; |
|
||||||
pos_z[i] = plan.mean[i].position.z; |
|
||||||
pos_x_std[i] = exp(plan.std[i].position.x); |
|
||||||
pos_y_std[i] = exp(plan.std[i].position.y); |
|
||||||
pos_z_std[i] = exp(plan.std[i].position.z); |
|
||||||
vel_x[i] = plan.mean[i].velocity.x; |
|
||||||
vel_y[i] = plan.mean[i].velocity.y; |
|
||||||
vel_z[i] = plan.mean[i].velocity.z; |
|
||||||
acc_x[i] = plan.mean[i].acceleration.x; |
|
||||||
acc_y[i] = plan.mean[i].acceleration.y; |
|
||||||
acc_z[i] = plan.mean[i].acceleration.z; |
|
||||||
rot_x[i] = plan.mean[i].rotation.x; |
|
||||||
rot_y[i] = plan.mean[i].rotation.y; |
|
||||||
rot_z[i] = plan.mean[i].rotation.z; |
|
||||||
rot_rate_x[i] = plan.mean[i].rotation_rate.x; |
|
||||||
rot_rate_y[i] = plan.mean[i].rotation_rate.y; |
|
||||||
rot_rate_z[i] = plan.mean[i].rotation_rate.z; |
|
||||||
} |
|
||||||
|
|
||||||
fill_xyzt(framed.initPosition(), T_IDXS_FLOAT, pos_x, pos_y, pos_z, pos_x_std, pos_y_std, pos_z_std); |
|
||||||
fill_xyzt(framed.initVelocity(), T_IDXS_FLOAT, vel_x, vel_y, vel_z); |
|
||||||
fill_xyzt(framed.initAcceleration(), T_IDXS_FLOAT, acc_x, acc_y, acc_z); |
|
||||||
fill_xyzt(framed.initOrientation(), T_IDXS_FLOAT, rot_x, rot_y, rot_z); |
|
||||||
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; |
|
||||||
std::array<float, TRAJECTORY_SIZE> left_near_y, left_near_z; |
|
||||||
std::array<float, TRAJECTORY_SIZE> right_near_y, right_near_z; |
|
||||||
std::array<float, TRAJECTORY_SIZE> right_far_y, right_far_z; |
|
||||||
for (int j=0; j<TRAJECTORY_SIZE; j++) { |
|
||||||
left_far_y[j] = lanes.mean.left_far[j].y; |
|
||||||
left_far_z[j] = lanes.mean.left_far[j].z; |
|
||||||
left_near_y[j] = lanes.mean.left_near[j].y; |
|
||||||
left_near_z[j] = lanes.mean.left_near[j].z; |
|
||||||
right_near_y[j] = lanes.mean.right_near[j].y; |
|
||||||
right_near_z[j] = lanes.mean.right_near[j].z; |
|
||||||
right_far_y[j] = lanes.mean.right_far[j].y; |
|
||||||
right_far_z[j] = lanes.mean.right_far[j].z; |
|
||||||
} |
|
||||||
|
|
||||||
auto lane_lines = framed.initLaneLines(4); |
|
||||||
fill_xyzt(lane_lines[0], plan_t, X_IDXS_FLOAT, left_far_y, left_far_z); |
|
||||||
fill_xyzt(lane_lines[1], plan_t, X_IDXS_FLOAT, left_near_y, left_near_z); |
|
||||||
fill_xyzt(lane_lines[2], plan_t, X_IDXS_FLOAT, right_near_y, right_near_z); |
|
||||||
fill_xyzt(lane_lines[3], plan_t, X_IDXS_FLOAT, right_far_y, right_far_z); |
|
||||||
|
|
||||||
framed.setLaneLineStds({ |
|
||||||
exp(lanes.std.left_far[0].y), |
|
||||||
exp(lanes.std.left_near[0].y), |
|
||||||
exp(lanes.std.right_near[0].y), |
|
||||||
exp(lanes.std.right_far[0].y), |
|
||||||
}); |
|
||||||
|
|
||||||
framed.setLaneLineProbs({ |
|
||||||
sigmoid(lanes.prob.left_far.val), |
|
||||||
sigmoid(lanes.prob.left_near.val), |
|
||||||
sigmoid(lanes.prob.right_near.val), |
|
||||||
sigmoid(lanes.prob.right_far.val), |
|
||||||
}); |
|
||||||
} |
|
||||||
|
|
||||||
void fill_road_edges(cereal::ModelDataV2::Builder &framed, const std::array<float, TRAJECTORY_SIZE> &plan_t, |
|
||||||
const ModelOutputRoadEdges &edges) { |
|
||||||
std::array<float, TRAJECTORY_SIZE> left_y, left_z; |
|
||||||
std::array<float, TRAJECTORY_SIZE> right_y, right_z; |
|
||||||
for (int j=0; j<TRAJECTORY_SIZE; j++) { |
|
||||||
left_y[j] = edges.mean.left[j].y; |
|
||||||
left_z[j] = edges.mean.left[j].z; |
|
||||||
right_y[j] = edges.mean.right[j].y; |
|
||||||
right_z[j] = edges.mean.right[j].z; |
|
||||||
} |
|
||||||
|
|
||||||
auto road_edges = framed.initRoadEdges(2); |
|
||||||
fill_xyzt(road_edges[0], plan_t, X_IDXS_FLOAT, left_y, left_z); |
|
||||||
fill_xyzt(road_edges[1], plan_t, X_IDXS_FLOAT, right_y, right_z); |
|
||||||
|
|
||||||
framed.setRoadEdgeStds({ |
|
||||||
exp(edges.std.left[0].y), |
|
||||||
exp(edges.std.right[0].y), |
|
||||||
}); |
|
||||||
} |
|
||||||
|
|
||||||
void fill_model(cereal::ModelDataV2::Builder &framed, const ModelOutput &net_outputs, PublishState &ps) { |
|
||||||
const auto &best_plan = net_outputs.plans.get_best_prediction(); |
|
||||||
std::array<float, TRAJECTORY_SIZE> plan_t; |
|
||||||
std::fill_n(plan_t.data(), plan_t.size(), NAN); |
|
||||||
plan_t[0] = 0.0; |
|
||||||
for (int xidx=1, tidx=0; xidx<TRAJECTORY_SIZE; xidx++) { |
|
||||||
// increment tidx until we find an element that's further away than the current xidx
|
|
||||||
for (int next_tid = tidx + 1; next_tid < TRAJECTORY_SIZE && best_plan.mean[next_tid].position.x < X_IDXS[xidx]; next_tid++) { |
|
||||||
tidx++; |
|
||||||
} |
|
||||||
if (tidx == TRAJECTORY_SIZE - 1) { |
|
||||||
// if the Plan doesn't extend far enough, set plan_t to the max value (10s), then break
|
|
||||||
plan_t[xidx] = T_IDXS[TRAJECTORY_SIZE - 1]; |
|
||||||
break; |
|
||||||
} |
|
||||||
|
|
||||||
// interpolate to find `t` for the current xidx
|
|
||||||
float current_x_val = best_plan.mean[tidx].position.x; |
|
||||||
float next_x_val = best_plan.mean[tidx+1].position.x; |
|
||||||
float p = (X_IDXS[xidx] - current_x_val) / (next_x_val - current_x_val); |
|
||||||
plan_t[xidx] = p * T_IDXS[tidx+1] + (1 - p) * T_IDXS[tidx]; |
|
||||||
} |
|
||||||
|
|
||||||
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); |
|
||||||
|
|
||||||
// meta
|
|
||||||
fill_meta(framed.initMeta(), net_outputs.meta, ps); |
|
||||||
|
|
||||||
// confidence
|
|
||||||
fill_confidence(framed, ps); |
|
||||||
|
|
||||||
// leads
|
|
||||||
auto leads = framed.initLeadsV3(LEAD_MHP_SELECTION); |
|
||||||
std::array<float, LEAD_MHP_SELECTION> t_offsets = {0.0, 2.0, 4.0}; |
|
||||||
for (int i=0; i<LEAD_MHP_SELECTION; i++) { |
|
||||||
fill_lead(leads[i], net_outputs.leads, i, t_offsets[i]); |
|
||||||
} |
|
||||||
|
|
||||||
// temporal pose
|
|
||||||
const auto &v_mean = net_outputs.temporal_pose.velocity_mean; |
|
||||||
const auto &r_mean = net_outputs.temporal_pose.rotation_mean; |
|
||||||
const auto &v_std = net_outputs.temporal_pose.velocity_std; |
|
||||||
const auto &r_std = net_outputs.temporal_pose.rotation_std; |
|
||||||
auto temporal_pose = framed.initTemporalPose(); |
|
||||||
temporal_pose.setTrans({v_mean.x, v_mean.y, v_mean.z}); |
|
||||||
temporal_pose.setRot({r_mean.x, r_mean.y, r_mean.z}); |
|
||||||
temporal_pose.setTransStd({exp(v_std.x), exp(v_std.y), exp(v_std.z)}); |
|
||||||
temporal_pose.setRotStd({exp(r_std.x), exp(r_std.y), exp(r_std.z)}); |
|
||||||
} |
|
||||||
|
|
||||||
void fill_model_msg(MessageBuilder &msg, float *net_output_data, PublishState &ps, uint32_t vipc_frame_id, uint32_t vipc_frame_id_extra, uint32_t frame_id, float frame_drop, |
|
||||||
uint64_t timestamp_eof, uint64_t timestamp_llk, float model_execution_time, const bool nav_enabled, const bool valid) { |
|
||||||
const uint32_t frame_age = (frame_id > vipc_frame_id) ? (frame_id - vipc_frame_id) : 0; |
|
||||||
auto framed = msg.initEvent(valid).initModelV2(); |
|
||||||
framed.setFrameId(vipc_frame_id); |
|
||||||
framed.setFrameIdExtra(vipc_frame_id_extra); |
|
||||||
framed.setFrameAge(frame_age); |
|
||||||
framed.setFrameDropPerc(frame_drop * 100); |
|
||||||
framed.setTimestampEof(timestamp_eof); |
|
||||||
framed.setLocationMonoTime(timestamp_llk); |
|
||||||
framed.setModelExecutionTime(model_execution_time); |
|
||||||
framed.setNavEnabled(nav_enabled); |
|
||||||
if (send_raw_pred) { |
|
||||||
framed.setRawPredictions(kj::ArrayPtr<const float>(net_output_data, NET_OUTPUT_SIZE).asBytes()); |
|
||||||
} |
|
||||||
fill_model(framed, *((ModelOutput*) net_output_data), ps); |
|
||||||
} |
|
||||||
|
|
||||||
void fill_pose_msg(MessageBuilder &msg, float *net_output_data, uint32_t vipc_frame_id, uint32_t vipc_dropped_frames, uint64_t timestamp_eof, const bool valid) { |
|
||||||
const ModelOutput &net_outputs = *((ModelOutput*) net_output_data); |
|
||||||
const auto &v_mean = net_outputs.pose.velocity_mean; |
|
||||||
const auto &r_mean = net_outputs.pose.rotation_mean; |
|
||||||
const auto &t_mean = net_outputs.wide_from_device_euler.mean; |
|
||||||
const auto &v_std = net_outputs.pose.velocity_std; |
|
||||||
const auto &r_std = net_outputs.pose.rotation_std; |
|
||||||
const auto &t_std = net_outputs.wide_from_device_euler.std; |
|
||||||
const auto &road_transform_trans_mean = net_outputs.road_transform.position_mean; |
|
||||||
const auto &road_transform_trans_std = net_outputs.road_transform.position_std; |
|
||||||
|
|
||||||
auto posenetd = msg.initEvent(valid && (vipc_dropped_frames < 1)).initCameraOdometry(); |
|
||||||
posenetd.setTrans({v_mean.x, v_mean.y, v_mean.z}); |
|
||||||
posenetd.setRot({r_mean.x, r_mean.y, r_mean.z}); |
|
||||||
posenetd.setWideFromDeviceEuler({t_mean.x, t_mean.y, t_mean.z}); |
|
||||||
posenetd.setRoadTransformTrans({road_transform_trans_mean.x, road_transform_trans_mean.y, road_transform_trans_mean.z}); |
|
||||||
posenetd.setTransStd({exp(v_std.x), exp(v_std.y), exp(v_std.z)}); |
|
||||||
posenetd.setRotStd({exp(r_std.x), exp(r_std.y), exp(r_std.z)}); |
|
||||||
posenetd.setWideFromDeviceEulerStd({exp(t_std.x), exp(t_std.y), exp(t_std.z)}); |
|
||||||
posenetd.setRoadTransformTransStd({exp(road_transform_trans_std.x), exp(road_transform_trans_std.y), exp(road_transform_trans_std.z)}); |
|
||||||
|
|
||||||
posenetd.setTimestampEof(timestamp_eof); |
|
||||||
posenetd.setFrameId(vipc_frame_id); |
|
||||||
} |
|
||||||
@ -1,273 +0,0 @@ |
|||||||
#pragma once |
|
||||||
|
|
||||||
#include <array> |
|
||||||
#include <memory> |
|
||||||
|
|
||||||
#include "cereal/messaging/messaging.h" |
|
||||||
#include "common/modeldata.h" |
|
||||||
#include "common/util.h" |
|
||||||
#include "selfdrive/modeld/models/commonmodel.h" |
|
||||||
#include "selfdrive/modeld/runners/run.h" |
|
||||||
|
|
||||||
constexpr int FEATURE_LEN = 512; |
|
||||||
constexpr int HISTORY_BUFFER_LEN = 99; |
|
||||||
constexpr int DESIRE_LEN = 8; |
|
||||||
constexpr int DESIRE_PRED_LEN = 4; |
|
||||||
constexpr int TRAFFIC_CONVENTION_LEN = 2; |
|
||||||
constexpr int LAT_PLANNER_STATE_LEN = 4; |
|
||||||
constexpr int NAV_FEATURE_LEN = 256; |
|
||||||
constexpr int NAV_INSTRUCTION_LEN = 150; |
|
||||||
constexpr int DRIVING_STYLE_LEN = 12; |
|
||||||
constexpr int MODEL_FREQ = 20; |
|
||||||
|
|
||||||
constexpr int DISENGAGE_LEN = 5; |
|
||||||
constexpr int BLINKER_LEN = 6; |
|
||||||
constexpr int META_STRIDE = 7; |
|
||||||
|
|
||||||
constexpr int PLAN_MHP_N = 5; |
|
||||||
constexpr int LEAD_MHP_N = 2; |
|
||||||
constexpr int LEAD_TRAJ_LEN = 6; |
|
||||||
constexpr int LEAD_MHP_SELECTION = 3; |
|
||||||
// Padding to get output shape as multiple of 4
|
|
||||||
constexpr int PAD_SIZE = 2; |
|
||||||
|
|
||||||
constexpr float FCW_THRESHOLD_5MS2_HIGH = 0.15; |
|
||||||
constexpr float FCW_THRESHOLD_5MS2_LOW = 0.05; |
|
||||||
constexpr float FCW_THRESHOLD_3MS2 = 0.7; |
|
||||||
|
|
||||||
struct ModelOutputXYZ { |
|
||||||
float x; |
|
||||||
float y; |
|
||||||
float z; |
|
||||||
}; |
|
||||||
static_assert(sizeof(ModelOutputXYZ) == sizeof(float)*3); |
|
||||||
|
|
||||||
struct ModelOutputYZ { |
|
||||||
float y; |
|
||||||
float z; |
|
||||||
}; |
|
||||||
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; |
|
||||||
ModelOutputXYZ acceleration; |
|
||||||
ModelOutputXYZ rotation; |
|
||||||
ModelOutputXYZ rotation_rate; |
|
||||||
}; |
|
||||||
static_assert(sizeof(ModelOutputPlanElement) == sizeof(ModelOutputXYZ)*5); |
|
||||||
|
|
||||||
struct ModelOutputPlanPrediction { |
|
||||||
std::array<ModelOutputPlanElement, TRAJECTORY_SIZE> mean; |
|
||||||
std::array<ModelOutputPlanElement, TRAJECTORY_SIZE> std; |
|
||||||
float prob; |
|
||||||
}; |
|
||||||
static_assert(sizeof(ModelOutputPlanPrediction) == (sizeof(ModelOutputPlanElement)*TRAJECTORY_SIZE*2) + sizeof(float)); |
|
||||||
|
|
||||||
struct ModelOutputPlans { |
|
||||||
std::array<ModelOutputPlanPrediction, PLAN_MHP_N> prediction; |
|
||||||
|
|
||||||
constexpr const ModelOutputPlanPrediction &get_best_prediction() const { |
|
||||||
int max_idx = 0; |
|
||||||
for (int i = 1; i < prediction.size(); i++) { |
|
||||||
if (prediction[i].prob > prediction[max_idx].prob) { |
|
||||||
max_idx = i; |
|
||||||
} |
|
||||||
} |
|
||||||
return prediction[max_idx]; |
|
||||||
} |
|
||||||
}; |
|
||||||
static_assert(sizeof(ModelOutputPlans) == sizeof(ModelOutputPlanPrediction)*PLAN_MHP_N); |
|
||||||
|
|
||||||
struct ModelOutputLinesXY { |
|
||||||
std::array<ModelOutputYZ, TRAJECTORY_SIZE> left_far; |
|
||||||
std::array<ModelOutputYZ, TRAJECTORY_SIZE> left_near; |
|
||||||
std::array<ModelOutputYZ, TRAJECTORY_SIZE> right_near; |
|
||||||
std::array<ModelOutputYZ, TRAJECTORY_SIZE> right_far; |
|
||||||
}; |
|
||||||
static_assert(sizeof(ModelOutputLinesXY) == sizeof(ModelOutputYZ)*TRAJECTORY_SIZE*4); |
|
||||||
|
|
||||||
struct ModelOutputLineProbVal { |
|
||||||
float val_deprecated; |
|
||||||
float val; |
|
||||||
}; |
|
||||||
static_assert(sizeof(ModelOutputLineProbVal) == sizeof(float)*2); |
|
||||||
|
|
||||||
struct ModelOutputLinesProb { |
|
||||||
ModelOutputLineProbVal left_far; |
|
||||||
ModelOutputLineProbVal left_near; |
|
||||||
ModelOutputLineProbVal right_near; |
|
||||||
ModelOutputLineProbVal right_far; |
|
||||||
}; |
|
||||||
static_assert(sizeof(ModelOutputLinesProb) == sizeof(ModelOutputLineProbVal)*4); |
|
||||||
|
|
||||||
struct ModelOutputLaneLines { |
|
||||||
ModelOutputLinesXY mean; |
|
||||||
ModelOutputLinesXY std; |
|
||||||
ModelOutputLinesProb prob; |
|
||||||
}; |
|
||||||
static_assert(sizeof(ModelOutputLaneLines) == (sizeof(ModelOutputLinesXY)*2) + sizeof(ModelOutputLinesProb)); |
|
||||||
|
|
||||||
struct ModelOutputEdgessXY { |
|
||||||
std::array<ModelOutputYZ, TRAJECTORY_SIZE> left; |
|
||||||
std::array<ModelOutputYZ, TRAJECTORY_SIZE> right; |
|
||||||
}; |
|
||||||
static_assert(sizeof(ModelOutputEdgessXY) == sizeof(ModelOutputYZ)*TRAJECTORY_SIZE*2); |
|
||||||
|
|
||||||
struct ModelOutputRoadEdges { |
|
||||||
ModelOutputEdgessXY mean; |
|
||||||
ModelOutputEdgessXY std; |
|
||||||
}; |
|
||||||
static_assert(sizeof(ModelOutputRoadEdges) == (sizeof(ModelOutputEdgessXY)*2)); |
|
||||||
|
|
||||||
struct ModelOutputLeadElement { |
|
||||||
float x; |
|
||||||
float y; |
|
||||||
float velocity; |
|
||||||
float acceleration; |
|
||||||
}; |
|
||||||
static_assert(sizeof(ModelOutputLeadElement) == sizeof(float)*4); |
|
||||||
|
|
||||||
struct ModelOutputLeadPrediction { |
|
||||||
std::array<ModelOutputLeadElement, LEAD_TRAJ_LEN> mean; |
|
||||||
std::array<ModelOutputLeadElement, LEAD_TRAJ_LEN> std; |
|
||||||
std::array<float, LEAD_MHP_SELECTION> prob; |
|
||||||
}; |
|
||||||
static_assert(sizeof(ModelOutputLeadPrediction) == (sizeof(ModelOutputLeadElement)*LEAD_TRAJ_LEN*2) + (sizeof(float)*LEAD_MHP_SELECTION)); |
|
||||||
|
|
||||||
struct ModelOutputLeads { |
|
||||||
std::array<ModelOutputLeadPrediction, LEAD_MHP_N> prediction; |
|
||||||
std::array<float, LEAD_MHP_SELECTION> prob; |
|
||||||
|
|
||||||
constexpr const ModelOutputLeadPrediction &get_best_prediction(int t_idx) const { |
|
||||||
int max_idx = 0; |
|
||||||
for (int i = 1; i < prediction.size(); i++) { |
|
||||||
if (prediction[i].prob[t_idx] > prediction[max_idx].prob[t_idx]) { |
|
||||||
max_idx = i; |
|
||||||
} |
|
||||||
} |
|
||||||
return prediction[max_idx]; |
|
||||||
} |
|
||||||
}; |
|
||||||
static_assert(sizeof(ModelOutputLeads) == (sizeof(ModelOutputLeadPrediction)*LEAD_MHP_N) + (sizeof(float)*LEAD_MHP_SELECTION)); |
|
||||||
|
|
||||||
|
|
||||||
struct ModelOutputPose { |
|
||||||
ModelOutputXYZ velocity_mean; |
|
||||||
ModelOutputXYZ rotation_mean; |
|
||||||
ModelOutputXYZ velocity_std; |
|
||||||
ModelOutputXYZ rotation_std; |
|
||||||
}; |
|
||||||
static_assert(sizeof(ModelOutputPose) == sizeof(ModelOutputXYZ)*4); |
|
||||||
|
|
||||||
struct ModelOutputWideFromDeviceEuler { |
|
||||||
ModelOutputXYZ mean; |
|
||||||
ModelOutputXYZ std; |
|
||||||
}; |
|
||||||
static_assert(sizeof(ModelOutputWideFromDeviceEuler) == sizeof(ModelOutputXYZ)*2); |
|
||||||
|
|
||||||
struct ModelOutputTemporalPose { |
|
||||||
ModelOutputXYZ velocity_mean; |
|
||||||
ModelOutputXYZ rotation_mean; |
|
||||||
ModelOutputXYZ velocity_std; |
|
||||||
ModelOutputXYZ rotation_std; |
|
||||||
}; |
|
||||||
static_assert(sizeof(ModelOutputTemporalPose) == sizeof(ModelOutputXYZ)*4); |
|
||||||
|
|
||||||
struct ModelOutputRoadTransform { |
|
||||||
ModelOutputXYZ position_mean; |
|
||||||
ModelOutputXYZ rotation_mean; |
|
||||||
ModelOutputXYZ position_std; |
|
||||||
ModelOutputXYZ rotation_std; |
|
||||||
}; |
|
||||||
static_assert(sizeof(ModelOutputRoadTransform) == sizeof(ModelOutputXYZ)*4); |
|
||||||
|
|
||||||
struct ModelOutputDisengageProb { |
|
||||||
float gas_disengage; |
|
||||||
float brake_disengage; |
|
||||||
float steer_override; |
|
||||||
float brake_3ms2; |
|
||||||
float brake_4ms2; |
|
||||||
float brake_5ms2; |
|
||||||
float gas_pressed; |
|
||||||
}; |
|
||||||
static_assert(sizeof(ModelOutputDisengageProb) == sizeof(float)*7); |
|
||||||
|
|
||||||
struct ModelOutputBlinkerProb { |
|
||||||
float left; |
|
||||||
float right; |
|
||||||
}; |
|
||||||
static_assert(sizeof(ModelOutputBlinkerProb) == sizeof(float)*2); |
|
||||||
|
|
||||||
struct ModelOutputDesireProb { |
|
||||||
union { |
|
||||||
struct { |
|
||||||
float none; |
|
||||||
float turn_left; |
|
||||||
float turn_right; |
|
||||||
float lane_change_left; |
|
||||||
float lane_change_right; |
|
||||||
float keep_left; |
|
||||||
float keep_right; |
|
||||||
float null; |
|
||||||
}; |
|
||||||
struct { |
|
||||||
std::array<float, DESIRE_LEN> array; |
|
||||||
}; |
|
||||||
}; |
|
||||||
}; |
|
||||||
static_assert(sizeof(ModelOutputDesireProb) == sizeof(float)*DESIRE_LEN); |
|
||||||
|
|
||||||
struct ModelOutputMeta { |
|
||||||
ModelOutputDesireProb desire_state_prob; |
|
||||||
float engaged_prob; |
|
||||||
std::array<ModelOutputDisengageProb, DISENGAGE_LEN> disengage_prob; |
|
||||||
std::array<ModelOutputBlinkerProb, BLINKER_LEN> blinker_prob; |
|
||||||
std::array<ModelOutputDesireProb, DESIRE_PRED_LEN> desire_pred_prob; |
|
||||||
}; |
|
||||||
static_assert(sizeof(ModelOutputMeta) == sizeof(ModelOutputDesireProb) + sizeof(float) + (sizeof(ModelOutputDisengageProb)*DISENGAGE_LEN) + (sizeof(ModelOutputBlinkerProb)*BLINKER_LEN) + (sizeof(ModelOutputDesireProb)*DESIRE_PRED_LEN)); |
|
||||||
|
|
||||||
struct ModelOutputFeatures { |
|
||||||
std::array<float, FEATURE_LEN> feature; |
|
||||||
}; |
|
||||||
static_assert(sizeof(ModelOutputFeatures) == (sizeof(float)*FEATURE_LEN)); |
|
||||||
|
|
||||||
struct ModelOutput { |
|
||||||
const ModelOutputPlans plans; |
|
||||||
const ModelOutputLaneLines lane_lines; |
|
||||||
const ModelOutputRoadEdges road_edges; |
|
||||||
const ModelOutputLeads leads; |
|
||||||
const ModelOutputMeta meta; |
|
||||||
const ModelOutputPose pose; |
|
||||||
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); |
|
||||||
constexpr int NET_OUTPUT_SIZE = OUTPUT_SIZE + FEATURE_LEN + PAD_SIZE; |
|
||||||
|
|
||||||
struct PublishState { |
|
||||||
std::array<float, DISENGAGE_LEN * DISENGAGE_LEN> disengage_buffer = {}; |
|
||||||
std::array<float, 5> prev_brake_5ms2_probs = {}; |
|
||||||
std::array<float, 3> prev_brake_3ms2_probs = {}; |
|
||||||
}; |
|
||||||
|
|
||||||
void fill_model_msg(MessageBuilder &msg, float *net_output_data, PublishState &ps, uint32_t vipc_frame_id, uint32_t vipc_frame_id_extra, uint32_t frame_id, float frame_drop, |
|
||||||
uint64_t timestamp_eof, uint64_t timestamp_llk, float model_execution_time, const bool nav_enabled, const bool valid); |
|
||||||
void fill_pose_msg(MessageBuilder &msg, float *net_outputs, uint32_t vipc_frame_id, uint32_t vipc_dropped_frames, uint64_t timestamp_eof, const bool valid); |
|
||||||
@ -1,26 +0,0 @@ |
|||||||
# distutils: language = c++ |
|
||||||
|
|
||||||
from libcpp cimport bool |
|
||||||
from libc.stdint cimport uint32_t, uint64_t |
|
||||||
|
|
||||||
cdef extern from "cereal/messaging/messaging.h": |
|
||||||
cdef cppclass MessageBuilder: |
|
||||||
size_t getSerializedSize() |
|
||||||
int serializeToBuffer(unsigned char *, size_t) |
|
||||||
|
|
||||||
cdef extern from "selfdrive/modeld/models/driving.h": |
|
||||||
cdef int FEATURE_LEN |
|
||||||
cdef int HISTORY_BUFFER_LEN |
|
||||||
cdef int DESIRE_LEN |
|
||||||
cdef int TRAFFIC_CONVENTION_LEN |
|
||||||
cdef int LAT_PLANNER_STATE_LEN |
|
||||||
cdef int DRIVING_STYLE_LEN |
|
||||||
cdef int NAV_FEATURE_LEN |
|
||||||
cdef int NAV_INSTRUCTION_LEN |
|
||||||
cdef int OUTPUT_SIZE |
|
||||||
cdef int NET_OUTPUT_SIZE |
|
||||||
cdef int MODEL_FREQ |
|
||||||
cdef struct PublishState: pass |
|
||||||
|
|
||||||
void fill_model_msg(MessageBuilder, float *, PublishState, uint32_t, uint32_t, uint32_t, float, uint64_t, uint64_t, float, bool, bool) |
|
||||||
void fill_pose_msg(MessageBuilder, float *, uint32_t, uint32_t, uint64_t, bool) |
|
||||||
@ -1,54 +0,0 @@ |
|||||||
# distutils: language = c++ |
|
||||||
# cython: c_string_encoding=ascii |
|
||||||
|
|
||||||
import numpy as np |
|
||||||
cimport numpy as cnp |
|
||||||
from libcpp cimport bool |
|
||||||
from libc.string cimport memcpy |
|
||||||
from libc.stdint cimport uint32_t, uint64_t |
|
||||||
|
|
||||||
from .commonmodel cimport mat3 |
|
||||||
from .driving cimport FEATURE_LEN as CPP_FEATURE_LEN, HISTORY_BUFFER_LEN as CPP_HISTORY_BUFFER_LEN, DESIRE_LEN as CPP_DESIRE_LEN, \ |
|
||||||
TRAFFIC_CONVENTION_LEN as CPP_TRAFFIC_CONVENTION_LEN, DRIVING_STYLE_LEN as CPP_DRIVING_STYLE_LEN, \ |
|
||||||
NAV_FEATURE_LEN as CPP_NAV_FEATURE_LEN, NAV_INSTRUCTION_LEN as CPP_NAV_INSTRUCTION_LEN, \ |
|
||||||
LAT_PLANNER_STATE_LEN as CPP_LAT_PLANNER_STATE_LEN, \ |
|
||||||
OUTPUT_SIZE as CPP_OUTPUT_SIZE, NET_OUTPUT_SIZE as CPP_NET_OUTPUT_SIZE, MODEL_FREQ as CPP_MODEL_FREQ |
|
||||||
from .driving cimport MessageBuilder, PublishState as cppPublishState |
|
||||||
from .driving cimport fill_model_msg, fill_pose_msg |
|
||||||
|
|
||||||
FEATURE_LEN = CPP_FEATURE_LEN |
|
||||||
HISTORY_BUFFER_LEN = CPP_HISTORY_BUFFER_LEN |
|
||||||
DESIRE_LEN = CPP_DESIRE_LEN |
|
||||||
TRAFFIC_CONVENTION_LEN = CPP_TRAFFIC_CONVENTION_LEN |
|
||||||
LAT_PLANNER_STATE_LEN = CPP_LAT_PLANNER_STATE_LEN |
|
||||||
DRIVING_STYLE_LEN = CPP_DRIVING_STYLE_LEN |
|
||||||
NAV_FEATURE_LEN = CPP_NAV_FEATURE_LEN |
|
||||||
NAV_INSTRUCTION_LEN = CPP_NAV_INSTRUCTION_LEN |
|
||||||
OUTPUT_SIZE = CPP_OUTPUT_SIZE |
|
||||||
NET_OUTPUT_SIZE = CPP_NET_OUTPUT_SIZE |
|
||||||
MODEL_FREQ = CPP_MODEL_FREQ |
|
||||||
|
|
||||||
cdef class PublishState: |
|
||||||
cdef cppPublishState state |
|
||||||
|
|
||||||
def create_model_msg(float[:] model_outputs, PublishState ps, uint32_t vipc_frame_id, uint32_t vipc_frame_id_extra, uint32_t frame_id, float frame_drop, |
|
||||||
uint64_t timestamp_eof, uint64_t timestamp_llk, float model_execution_time, bool nav_enabled, bool valid): |
|
||||||
cdef MessageBuilder msg |
|
||||||
fill_model_msg(msg, &model_outputs[0], ps.state, vipc_frame_id, vipc_frame_id_extra, frame_id, frame_drop, |
|
||||||
timestamp_eof, timestamp_llk, model_execution_time, nav_enabled, valid) |
|
||||||
|
|
||||||
output_size = msg.getSerializedSize() |
|
||||||
output_data = bytearray(output_size) |
|
||||||
cdef unsigned char * output_ptr = output_data |
|
||||||
assert msg.serializeToBuffer(output_ptr, output_size) > 0, "output buffer is too small to serialize" |
|
||||||
return bytes(output_data) |
|
||||||
|
|
||||||
def create_pose_msg(float[:] model_outputs, uint32_t vipc_frame_id, uint32_t vipc_dropped_frames, uint64_t timestamp_eof, bool valid): |
|
||||||
cdef MessageBuilder msg |
|
||||||
fill_pose_msg(msg, &model_outputs[0], vipc_frame_id, vipc_dropped_frames, timestamp_eof, valid) |
|
||||||
|
|
||||||
output_size = msg.getSerializedSize() |
|
||||||
output_data = bytearray(output_size) |
|
||||||
cdef unsigned char * output_ptr = output_data |
|
||||||
assert msg.serializeToBuffer(output_ptr, output_size) > 0, "output buffer is too small to serialize" |
|
||||||
return bytes(output_data) |
|
||||||
@ -0,0 +1,101 @@ |
|||||||
|
import numpy as np |
||||||
|
from typing import Dict |
||||||
|
from openpilot.selfdrive.modeld.constants import ModelConstants |
||||||
|
|
||||||
|
def sigmoid(x): |
||||||
|
return 1. / (1. + np.exp(-x)) |
||||||
|
|
||||||
|
def softmax(x, axis=-1): |
||||||
|
x -= np.max(x, axis=axis, keepdims=True) |
||||||
|
if x.dtype == np.float32 or x.dtype == np.float64: |
||||||
|
np.exp(x, out=x) |
||||||
|
else: |
||||||
|
x = np.exp(x) |
||||||
|
x /= np.sum(x, axis=axis, keepdims=True) |
||||||
|
return x |
||||||
|
|
||||||
|
class Parser: |
||||||
|
def __init__(self, ignore_missing=False): |
||||||
|
self.ignore_missing = ignore_missing |
||||||
|
|
||||||
|
def check_missing(self, outs, name): |
||||||
|
if name not in outs and not self.ignore_missing: |
||||||
|
raise ValueError(f"Missing output {name}") |
||||||
|
return name not in outs |
||||||
|
|
||||||
|
def parse_categorical_crossentropy(self, name, outs, out_shape=None): |
||||||
|
if self.check_missing(outs, name): |
||||||
|
return |
||||||
|
raw = outs[name] |
||||||
|
if out_shape is not None: |
||||||
|
raw = raw.reshape((raw.shape[0],) + out_shape) |
||||||
|
outs[name] = softmax(raw, axis=-1) |
||||||
|
|
||||||
|
def parse_binary_crossentropy(self, name, outs): |
||||||
|
if self.check_missing(outs, name): |
||||||
|
return |
||||||
|
raw = outs[name] |
||||||
|
outs[name] = sigmoid(raw) |
||||||
|
|
||||||
|
def parse_mdn(self, name, outs, in_N=0, out_N=1, out_shape=None): |
||||||
|
if self.check_missing(outs, name): |
||||||
|
return |
||||||
|
raw = outs[name] |
||||||
|
raw = raw.reshape((raw.shape[0], max(in_N, 1), -1)) |
||||||
|
|
||||||
|
pred_mu = raw[:,:,:(raw.shape[2] - out_N)//2] |
||||||
|
n_values = (raw.shape[2] - out_N)//2 |
||||||
|
pred_mu = raw[:,:,:n_values] |
||||||
|
pred_std = np.exp(raw[:,:,n_values: 2*n_values]) |
||||||
|
|
||||||
|
if in_N > 1: |
||||||
|
weights = np.zeros((raw.shape[0], in_N, out_N), dtype=raw.dtype) |
||||||
|
for i in range(out_N): |
||||||
|
weights[:,:,i - out_N] = softmax(raw[:,:,i - out_N], axis=-1) |
||||||
|
|
||||||
|
if out_N == 1: |
||||||
|
for fidx in range(weights.shape[0]): |
||||||
|
idxs = np.argsort(weights[fidx][:,0])[::-1] |
||||||
|
weights[fidx] = weights[fidx][idxs] |
||||||
|
pred_mu[fidx] = pred_mu[fidx][idxs] |
||||||
|
pred_std[fidx] = pred_std[fidx][idxs] |
||||||
|
full_shape = tuple([raw.shape[0], in_N] + list(out_shape)) |
||||||
|
outs[name + '_weights'] = weights |
||||||
|
outs[name + '_hypotheses'] = pred_mu.reshape(full_shape) |
||||||
|
outs[name + '_stds_hypotheses'] = pred_std.reshape(full_shape) |
||||||
|
|
||||||
|
pred_mu_final = np.zeros((raw.shape[0], out_N, n_values), dtype=raw.dtype) |
||||||
|
pred_std_final = np.zeros((raw.shape[0], out_N, n_values), dtype=raw.dtype) |
||||||
|
for fidx in range(weights.shape[0]): |
||||||
|
for hidx in range(out_N): |
||||||
|
idxs = np.argsort(weights[fidx,:,hidx])[::-1] |
||||||
|
pred_mu_final[fidx, hidx] = pred_mu[fidx, idxs[0]] |
||||||
|
pred_std_final[fidx, hidx] = pred_std[fidx, idxs[0]] |
||||||
|
else: |
||||||
|
pred_mu_final = pred_mu |
||||||
|
pred_std_final = pred_std |
||||||
|
|
||||||
|
if out_N > 1: |
||||||
|
final_shape = tuple([raw.shape[0], out_N] + list(out_shape)) |
||||||
|
else: |
||||||
|
final_shape = tuple([raw.shape[0],] + list(out_shape)) |
||||||
|
outs[name] = pred_mu_final.reshape(final_shape) |
||||||
|
outs[name + '_stds'] = pred_std_final.reshape(final_shape) |
||||||
|
|
||||||
|
def parse_outputs(self, outs: Dict[str, np.ndarray]) -> Dict[str, np.ndarray]: |
||||||
|
self.parse_mdn('plan', outs, in_N=ModelConstants.PLAN_MHP_N, out_N=ModelConstants.PLAN_MHP_SELECTION, |
||||||
|
out_shape=(ModelConstants.IDX_N,ModelConstants.PLAN_WIDTH)) |
||||||
|
self.parse_mdn('lane_lines', outs, in_N=0, out_N=0, out_shape=(ModelConstants.NUM_LANE_LINES,ModelConstants.IDX_N,ModelConstants.LANE_LINES_WIDTH)) |
||||||
|
self.parse_mdn('road_edges', outs, in_N=0, out_N=0, out_shape=(ModelConstants.NUM_ROAD_EDGES,ModelConstants.IDX_N,ModelConstants.LANE_LINES_WIDTH)) |
||||||
|
self.parse_mdn('pose', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,)) |
||||||
|
self.parse_mdn('road_transform', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,)) |
||||||
|
self.parse_mdn('sim_pose', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,)) |
||||||
|
self.parse_mdn('wide_from_device_euler', outs, in_N=0, out_N=0, out_shape=(ModelConstants.WIDE_FROM_DEVICE_WIDTH,)) |
||||||
|
self.parse_mdn('lead', outs, in_N=ModelConstants.LEAD_MHP_N, out_N=ModelConstants.LEAD_MHP_SELECTION, |
||||||
|
out_shape=(ModelConstants.LEAD_TRAJ_LEN,ModelConstants.LEAD_WIDTH)) |
||||||
|
self.parse_mdn('lat_planner_solution', outs, in_N=0, out_N=0, out_shape=(ModelConstants.IDX_N,ModelConstants.LAT_PLANNER_SOLUTION_WIDTH)) |
||||||
|
for k in ['lead_prob', 'lane_lines_prob', 'meta']: |
||||||
|
self.parse_binary_crossentropy(k, outs) |
||||||
|
self.parse_categorical_crossentropy('desire_state', outs, out_shape=(ModelConstants.DESIRE_PRED_WIDTH,)) |
||||||
|
self.parse_categorical_crossentropy('desire_pred', outs, out_shape=(ModelConstants.DESIRE_PRED_LEN,ModelConstants.DESIRE_PRED_WIDTH)) |
||||||
|
return outs |
||||||
@ -1,32 +0,0 @@ |
|||||||
import struct |
|
||||||
import json |
|
||||||
|
|
||||||
def load_thneed(fn): |
|
||||||
with open(fn, "rb") as f: |
|
||||||
json_len = struct.unpack("I", f.read(4))[0] |
|
||||||
jdat = json.loads(f.read(json_len).decode('latin_1')) |
|
||||||
weights = f.read() |
|
||||||
ptr = 0 |
|
||||||
for o in jdat['objects']: |
|
||||||
if o['needs_load']: |
|
||||||
nptr = ptr + o['size'] |
|
||||||
o['data'] = weights[ptr:nptr] |
|
||||||
ptr = nptr |
|
||||||
for o in jdat['binaries']: |
|
||||||
nptr = ptr + o['length'] |
|
||||||
o['data'] = weights[ptr:nptr] |
|
||||||
ptr = nptr |
|
||||||
return jdat |
|
||||||
|
|
||||||
def save_thneed(jdat, fn): |
|
||||||
new_weights = [] |
|
||||||
for o in jdat['objects'] + jdat['binaries']: |
|
||||||
if 'data' in o: |
|
||||||
new_weights.append(o['data']) |
|
||||||
del o['data'] |
|
||||||
new_weights_bytes = b''.join(new_weights) |
|
||||||
with open(fn, "wb") as f: |
|
||||||
j = json.dumps(jdat, ensure_ascii=False).encode('latin_1') |
|
||||||
f.write(struct.pack("I", len(j))) |
|
||||||
f.write(j) |
|
||||||
f.write(new_weights_bytes) |
|
||||||
@ -1 +1 @@ |
|||||||
ed2d58ec217fafb7b6b8f5e27ec622acd9e734f4 |
0e0f55cf3bb2cf79b44adf190e6387a83deb6646 |
||||||
|
|||||||
@ -1,5 +1,4 @@ |
|||||||
#!/bin/sh |
#!/bin/sh |
||||||
cd "$(dirname "$0")" |
cd "$(dirname "$0")" |
||||||
export LD_LIBRARY_PATH="/system/lib64:$LD_LIBRARY_PATH" |
|
||||||
export QT_QPA_PLATFORM="offscreen" |
export QT_QPA_PLATFORM="offscreen" |
||||||
exec ./_soundd |
exec ./_soundd |
||||||
|
|||||||
@ -1,5 +1,4 @@ |
|||||||
#!/bin/sh |
#!/bin/sh |
||||||
cd "$(dirname "$0")" |
cd "$(dirname "$0")" |
||||||
export LD_LIBRARY_PATH="/system/lib64:$LD_LIBRARY_PATH" |
|
||||||
export QT_DBL_CLICK_DIST=150 |
export QT_DBL_CLICK_DIST=150 |
||||||
exec ./_ui |
exec ./_ui |
||||||
|
|||||||
@ -0,0 +1,115 @@ |
|||||||
|
#!/usr/bin/env python3 |
||||||
|
import os |
||||||
|
import math |
||||||
|
import time |
||||||
|
import binascii |
||||||
|
import requests |
||||||
|
import serial |
||||||
|
import subprocess |
||||||
|
|
||||||
|
|
||||||
|
def post(url, payload): |
||||||
|
print() |
||||||
|
print("POST to", url) |
||||||
|
r = requests.post( |
||||||
|
url, |
||||||
|
data=payload, |
||||||
|
verify=False, |
||||||
|
headers={ |
||||||
|
"Content-Type": "application/json", |
||||||
|
"X-Admin-Protocol": "gsma/rsp/v2.2.0", |
||||||
|
"charset": "utf-8", |
||||||
|
"User-Agent": "gsma-rsp-lpad", |
||||||
|
}, |
||||||
|
) |
||||||
|
print("resp", r) |
||||||
|
print("resp text", repr(r.text)) |
||||||
|
print() |
||||||
|
r.raise_for_status() |
||||||
|
|
||||||
|
ret = f"HTTP/1.1 {r.status_code}" |
||||||
|
ret += ''.join(f"{k}: {v}" for k, v in r.headers.items() if k != 'Connection') |
||||||
|
return ret.encode() + r.content |
||||||
|
|
||||||
|
|
||||||
|
class LPA: |
||||||
|
def __init__(self): |
||||||
|
self.dev = serial.Serial('/dev/ttyUSB2', baudrate=57600, timeout=1, bytesize=8) |
||||||
|
self.dev.reset_input_buffer() |
||||||
|
self.dev.reset_output_buffer() |
||||||
|
assert "OK" in self.at("AT") |
||||||
|
|
||||||
|
def at(self, cmd): |
||||||
|
print(f"==> {cmd}") |
||||||
|
self.dev.write(cmd.encode() + b'\r\n') |
||||||
|
|
||||||
|
r = b"" |
||||||
|
cnt = 0 |
||||||
|
while b"OK" not in r and b"ERROR" not in r and cnt < 20: |
||||||
|
r += self.dev.read(8192).strip() |
||||||
|
cnt += 1 |
||||||
|
r = r.decode() |
||||||
|
print(f"<== {repr(r)}") |
||||||
|
return r |
||||||
|
|
||||||
|
def download_ota(self, qr): |
||||||
|
return self.at(f'AT+QESIM="ota","{qr}"') |
||||||
|
|
||||||
|
def download(self, qr): |
||||||
|
smdp = qr.split('$')[1] |
||||||
|
out = self.at(f'AT+QESIM="download","{qr}"') |
||||||
|
for _ in range(5): |
||||||
|
line = out.split("+QESIM: ")[1].split("\r\n\r\nOK")[0] |
||||||
|
|
||||||
|
parts = [x.strip().strip('"') for x in line.split(',', maxsplit=4)] |
||||||
|
print(repr(parts)) |
||||||
|
trans, ret, url, payloadlen, payload = parts |
||||||
|
assert trans == "trans" and ret == "0" |
||||||
|
assert len(payload) == int(payloadlen) |
||||||
|
|
||||||
|
r = post(f"https://{smdp}/{url}", payload) |
||||||
|
to_send = binascii.hexlify(r).decode() |
||||||
|
|
||||||
|
chunk_len = 1400 |
||||||
|
for i in range(math.ceil(len(to_send) / chunk_len)): |
||||||
|
state = 1 if (i+1)*chunk_len < len(to_send) else 0 |
||||||
|
data = to_send[i * chunk_len : (i+1)*chunk_len] |
||||||
|
out = self.at(f'AT+QESIM="trans",{len(to_send)},{state},{i},{len(data)},"{data}"') |
||||||
|
assert "OK" in out |
||||||
|
|
||||||
|
if '+QESIM:"download",1' in out: |
||||||
|
raise Exception("profile install failed") |
||||||
|
elif '+QESIM:"download",0' in out: |
||||||
|
print("done, successfully loaded") |
||||||
|
break |
||||||
|
|
||||||
|
def enable(self, iccid): |
||||||
|
self.at(f'AT+QESIM="enable","{iccid}"') |
||||||
|
|
||||||
|
def disable(self, iccid): |
||||||
|
self.at(f'AT+QESIM="disable","{iccid}"') |
||||||
|
|
||||||
|
def delete(self, iccid): |
||||||
|
self.at(f'AT+QESIM="delete","{iccid}"') |
||||||
|
|
||||||
|
def list_profiles(self): |
||||||
|
out = self.at('AT+QESIM="list"') |
||||||
|
return out.strip().splitlines()[1:] |
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__": |
||||||
|
import sys |
||||||
|
|
||||||
|
if "RESTART" in os.environ: |
||||||
|
subprocess.check_call("sudo systemctl stop ModemManager", shell=True) |
||||||
|
subprocess.check_call("/usr/comma/lte/lte.sh stop_blocking", shell=True) |
||||||
|
subprocess.check_call("/usr/comma/lte/lte.sh start", shell=True) |
||||||
|
while not os.path.exists('/dev/ttyUSB2'): |
||||||
|
time.sleep(1) |
||||||
|
time.sleep(3) |
||||||
|
|
||||||
|
lpa = LPA() |
||||||
|
print(lpa.list_profiles()) |
||||||
|
if len(sys.argv) > 1: |
||||||
|
lpa.download(sys.argv[1]) |
||||||
|
print(lpa.list_profiles()) |
||||||
@ -1 +1 @@ |
|||||||
_sensord |
sensord |
||||||
|
|||||||
@ -1,4 +0,0 @@ |
|||||||
#!/bin/sh |
|
||||||
cd "$(dirname "$0")" |
|
||||||
export LD_LIBRARY_PATH="/system/lib64:$LD_LIBRARY_PATH" |
|
||||||
exec ./_sensord |
|
||||||
@ -1 +1 @@ |
|||||||
Subproject commit d8dda2af3afcef0bb772fff580cfa8b3eabf7f69 |
Subproject commit 5a4a62ecaecb2bfd8bb0f77033aca46df4e668bd |
||||||
Loading…
Reference in new issue