From bd4f6650faea8b3c42a99087b4f1ce0560b7d652 Mon Sep 17 00:00:00 2001 From: Dean Lee Date: Mon, 30 Nov 2020 19:08:52 +0800 Subject: [PATCH] simplify building capnp messages with arrays (#2617) --- selfdrive/camerad/cameras/camera_qcom.cc | 8 +-- selfdrive/camerad/cameras/camera_qcom2.cc | 2 +- selfdrive/camerad/cameras/camera_webcam.cc | 2 +- selfdrive/locationd/ublox_msg.cc | 9 +-- selfdrive/modeld/models/dmonitoring.cc | 12 ++-- selfdrive/modeld/models/driving.cc | 73 ++++++++-------------- selfdrive/sensord/sensors/bmx055_accel.cc | 4 +- selfdrive/sensord/sensors/bmx055_gyro.cc | 4 +- selfdrive/sensord/sensors/bmx055_magn.cc | 4 +- selfdrive/sensord/sensors/lsm6ds3_accel.cc | 4 +- selfdrive/sensord/sensors/lsm6ds3_gyro.cc | 4 +- selfdrive/sensord/sensors_qcom.cc | 9 +-- 12 files changed, 47 insertions(+), 88 deletions(-) diff --git a/selfdrive/camerad/cameras/camera_qcom.cc b/selfdrive/camerad/cameras/camera_qcom.cc index d758dff480..0c729c9338 100644 --- a/selfdrive/camerad/cameras/camera_qcom.cc +++ b/selfdrive/camerad/cameras/camera_qcom.cc @@ -2141,11 +2141,11 @@ void camera_process_frame(MultiCameraState *s, CameraState *c, int cnt) { if (env_send_rear) { fill_frame_image(framed, (uint8_t*)b->cur_rgb_buf->addr, b->rgb_width, b->rgb_height, b->rgb_stride); } - framed.setFocusVal(kj::ArrayPtr(&s->rear.focus[0], NUM_FOCUS)); - framed.setFocusConf(kj::ArrayPtr(&s->rear.confidence[0], NUM_FOCUS)); - framed.setSharpnessScore(kj::ArrayPtr(&s->lapres[0], ARRAYSIZE(s->lapres))); + framed.setFocusVal(s->rear.focus); + framed.setFocusConf(s->rear.confidence); + framed.setSharpnessScore(s->lapres); framed.setRecoverState(self_recover); - framed.setTransform(kj::ArrayPtr(&b->yuv_transform.v[0], 9)); + framed.setTransform(b->yuv_transform.v); s->pm->send("frame", msg); } diff --git a/selfdrive/camerad/cameras/camera_qcom2.cc b/selfdrive/camerad/cameras/camera_qcom2.cc index 048c06e566..4965340f4a 100644 --- a/selfdrive/camerad/cameras/camera_qcom2.cc +++ b/selfdrive/camerad/cameras/camera_qcom2.cc @@ -1114,7 +1114,7 @@ void camera_process_frame(MultiCameraState *s, CameraState *c, int cnt) { fill_frame_image(framed, (uint8_t*)b->cur_rgb_buf->addr, b->rgb_width, b->rgb_height, b->rgb_stride); } if (c == &s->rear) { - framed.setTransform(kj::ArrayPtr(&b->yuv_transform.v[0], 9)); + framed.setTransform(b->yuv_transform.v); } s->pm->send(c == &s->rear ? "frame" : "wideFrame", msg); diff --git a/selfdrive/camerad/cameras/camera_webcam.cc b/selfdrive/camerad/cameras/camera_webcam.cc index ab871fe013..2e088c2ef4 100644 --- a/selfdrive/camerad/cameras/camera_webcam.cc +++ b/selfdrive/camerad/cameras/camera_webcam.cc @@ -266,7 +266,7 @@ void camera_process_rear(MultiCameraState *s, CameraState *c, int cnt) { auto framed = msg.initEvent().initFrame(); fill_frame_data(framed, b->cur_frame_data, cnt); framed.setImage(kj::arrayPtr((const uint8_t *)b->yuv_ion[b->cur_yuv_idx].addr, b->yuv_buf_size)); - framed.setTransform(kj::ArrayPtr(&b->yuv_transform.v[0], 9)); + framed.setTransform(b->yuv_transform.v); s->pm->send("frame", msg); } diff --git a/selfdrive/locationd/ublox_msg.cc b/selfdrive/locationd/ublox_msg.cc index ce4e4dcac3..74d56ed344 100644 --- a/selfdrive/locationd/ublox_msg.cc +++ b/selfdrive/locationd/ublox_msg.cc @@ -213,8 +213,7 @@ kj::Array UbloxMsgParser::gen_solution() { std::time_t utc_tt = timegm(&timeinfo); gpsLoc.setTimestamp(utc_tt * 1e+03 + msg->nano * 1e-06); float f[] = { msg->velN * 1e-03f, msg->velE * 1e-03f, msg->velD * 1e-03f }; - kj::ArrayPtr ap(&f[0], sizeof(f) / sizeof(f[0])); - gpsLoc.setVNED(ap); + gpsLoc.setVNED(f); gpsLoc.setVerticalAccuracy(msg->vAcc * 1e-03); gpsLoc.setSpeedAccuracy(msg->sAcc * 1e-03); gpsLoc.setBearingAccuracy(msg->headAcc * 1e-05); @@ -318,10 +317,8 @@ kj::Array UbloxMsgParser::gen_nav_data() { eph.setTgd(ephem_data.Tgd); eph.setIonoCoeffsValid(ephem_data.ionoCoeffsValid); if(ephem_data.ionoCoeffsValid) { - kj::ArrayPtr apa(&ephem_data.ionoAlpha[0], sizeof(ephem_data.ionoAlpha) / sizeof(ephem_data.ionoAlpha[0])); - eph.setIonoAlpha(apa); - kj::ArrayPtr apb(&ephem_data.ionoBeta[0], sizeof(ephem_data.ionoBeta) / sizeof(ephem_data.ionoBeta[0])); - eph.setIonoBeta(apb); + eph.setIonoAlpha(ephem_data.ionoAlpha); + eph.setIonoBeta(ephem_data.ionoBeta); } else { eph.setIonoAlpha(kj::ArrayPtr()); eph.setIonoBeta(kj::ArrayPtr()); diff --git a/selfdrive/modeld/models/dmonitoring.cc b/selfdrive/modeld/models/dmonitoring.cc index 3ab5f445c3..0ce6565e54 100644 --- a/selfdrive/modeld/models/dmonitoring.cc +++ b/selfdrive/modeld/models/dmonitoring.cc @@ -188,14 +188,10 @@ void dmonitoring_publish(PubMaster &pm, uint32_t frame_id, const DMonitoringResu framed.setFrameId(frame_id); framed.setModelExecutionTime(execution_time); - kj::ArrayPtr face_orientation(&res.face_orientation[0], ARRAYSIZE(res.face_orientation)); - kj::ArrayPtr face_orientation_std(&res.face_orientation_meta[0], ARRAYSIZE(res.face_orientation_meta)); - kj::ArrayPtr face_position(&res.face_position[0], ARRAYSIZE(res.face_position)); - kj::ArrayPtr face_position_std(&res.face_position_meta[0], ARRAYSIZE(res.face_position_meta)); - framed.setFaceOrientation(face_orientation); - framed.setFaceOrientationStd(face_orientation_std); - framed.setFacePosition(face_position); - framed.setFacePositionStd(face_position_std); + framed.setFaceOrientation(res.face_orientation); + framed.setFaceOrientationStd(res.face_orientation_meta); + framed.setFacePosition(res.face_position); + framed.setFacePositionStd(res.face_position_meta); framed.setFaceProb(res.face_prob); framed.setLeftEyeProb(res.left_eye_prob); framed.setRightEyeProb(res.right_eye_prob); diff --git a/selfdrive/modeld/models/driving.cc b/selfdrive/modeld/models/driving.cc index ebc85f0661..1067d69710 100644 --- a/selfdrive/modeld/models/driving.cc +++ b/selfdrive/modeld/models/driving.cc @@ -171,8 +171,7 @@ void fill_path(cereal::ModelData::PathData::Builder path, const float * data, fl std = stds_arr[0]; poly_fit(points_arr, stds_arr, poly_arr, valid_len_idx); - kj::ArrayPtr poly(&poly_arr[0], ARRAYSIZE(poly_arr)); - path.setPoly(poly); + path.setPoly(poly_arr); path.setProb(1.0); path.setStd(std); path.setValidLen(valid_len); @@ -192,8 +191,7 @@ void fill_lane_line(cereal::ModelData::PathData::Builder path, const float * dat std = stds_arr[0]; poly_fit(points_arr, stds_arr, poly_arr, valid_len_idx); - kj::ArrayPtr poly(&poly_arr[0], ARRAYSIZE(poly_arr)); - path.setPoly(poly); + path.setPoly(poly_arr); path.setProb(prob); path.setStd(std); path.setValidLen(valid_len); @@ -208,10 +206,8 @@ void fill_lead_v2(cereal::ModelDataV2::LeadDataV2::Builder lead, const float * d xyva_arr[i] = data[LEAD_MHP_VALS + i]; xyva_stds_arr[i] = exp(data[LEAD_MHP_VALS + i]); } - kj::ArrayPtr xyva(xyva_arr, LEAD_MHP_VALS); - kj::ArrayPtr xyva_stds(xyva_stds_arr, LEAD_MHP_VALS); - lead.setXyva(xyva); - lead.setXyvaStd(xyva_stds); + lead.setXyva(xyva_arr); + lead.setXyvaStd(xyva_stds_arr); } void fill_lead(cereal::ModelData::LeadData::Builder lead, const float * data, float prob) { @@ -235,39 +231,35 @@ void fill_meta(cereal::ModelData::MetaData::Builder meta, const float * meta_dat softmax(&meta_data[DESIRE_LEN + OTHER_META_SIZE + i*DESIRE_LEN], &desire_pred_softmax[i*DESIRE_LEN], DESIRE_LEN); } - kj::ArrayPtr desire_state(desire_state_softmax, DESIRE_LEN); - meta.setDesireState(desire_state); + meta.setDesireState(desire_state_softmax); meta.setEngagedProb(sigmoid(meta_data[DESIRE_LEN])); meta.setGasDisengageProb(sigmoid(meta_data[DESIRE_LEN + 1])); meta.setBrakeDisengageProb(sigmoid(meta_data[DESIRE_LEN + 2])); meta.setSteerOverrideProb(sigmoid(meta_data[DESIRE_LEN + 3])); - kj::ArrayPtr desire_pred(desire_pred_softmax, DESIRE_PRED_SIZE); - meta.setDesirePrediction(desire_pred); + meta.setDesirePrediction(desire_pred_softmax); } void fill_meta_v2(cereal::ModelDataV2::MetaData::Builder meta, const float * meta_data) { - float desire_state_softmax[DESIRE_LEN]; - float desire_pred_softmax[4*DESIRE_LEN]; + float desire_state_softmax[DESIRE_LEN] = {}; + float desire_pred_softmax[4*DESIRE_LEN] = {}; softmax(&meta_data[0], desire_state_softmax, DESIRE_LEN); for (int i=0; i<4; i++) { softmax(&meta_data[DESIRE_LEN + OTHER_META_SIZE + i*DESIRE_LEN], &desire_pred_softmax[i*DESIRE_LEN], DESIRE_LEN); } - kj::ArrayPtr desire_state(desire_state_softmax, DESIRE_LEN); - meta.setDesireState(desire_state); + meta.setDesireState(desire_state_softmax); meta.setEngagedProb(sigmoid(meta_data[DESIRE_LEN])); meta.setGasDisengageProb(sigmoid(meta_data[DESIRE_LEN + 1])); meta.setBrakeDisengageProb(sigmoid(meta_data[DESIRE_LEN + 2])); meta.setSteerOverrideProb(sigmoid(meta_data[DESIRE_LEN + 3])); - kj::ArrayPtr desire_pred(desire_pred_softmax, DESIRE_PRED_SIZE); - meta.setDesirePrediction(desire_pred); + meta.setDesirePrediction(desire_pred_softmax); } void fill_xyzt(cereal::ModelDataV2::XYZTData::Builder xyzt, const float * data, int columns, int column_offset, float * plan_t_arr) { - float x_arr[TRAJECTORY_SIZE]; - float y_arr[TRAJECTORY_SIZE]; - float z_arr[TRAJECTORY_SIZE]; + float x_arr[TRAJECTORY_SIZE] = {}; + float y_arr[TRAJECTORY_SIZE] = {}; + float z_arr[TRAJECTORY_SIZE] = {}; //float x_std_arr[TRAJECTORY_SIZE]; //float y_std_arr[TRAJECTORY_SIZE]; //float z_std_arr[TRAJECTORY_SIZE]; @@ -288,20 +280,16 @@ void fill_xyzt(cereal::ModelDataV2::XYZTData::Builder xyzt, const float * data, z_arr[i] = data[i*columns + 2 + column_offset]; //z_std_arr[i] = data[columns*(TRAJECTORY_SIZE + i) + 2 + column_offset]; } - kj::ArrayPtr x(x_arr, TRAJECTORY_SIZE); - kj::ArrayPtr y(y_arr, TRAJECTORY_SIZE); - kj::ArrayPtr z(z_arr, TRAJECTORY_SIZE); //kj::ArrayPtr x_std(x_std_arr, TRAJECTORY_SIZE); //kj::ArrayPtr y_std(y_std_arr, TRAJECTORY_SIZE); //kj::ArrayPtr z_std(z_std_arr, TRAJECTORY_SIZE); - kj::ArrayPtr t(t_arr, TRAJECTORY_SIZE); - xyzt.setX(x); - xyzt.setY(y); - xyzt.setZ(z); + xyzt.setX(x_arr); + xyzt.setY(y_arr); + xyzt.setZ(z_arr); //xyzt.setXStd(x_std); //xyzt.setYStd(y_std); //xyzt.setZStd(z_std); - xyzt.setT(t); + xyzt.setT(t_arr); } @@ -334,14 +322,10 @@ void model_publish_v2(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, plan_t_arr[i] = best_plan[i*PLAN_MHP_COLUMNS + 15]; } - auto position = framed.initPosition(); - fill_xyzt(position, best_plan, PLAN_MHP_COLUMNS, 0, plan_t_arr); - auto velocity = framed.initVelocity(); - fill_xyzt(velocity, best_plan, PLAN_MHP_COLUMNS, 3, plan_t_arr); - auto orientation = framed.initOrientation(); - fill_xyzt(orientation, best_plan, PLAN_MHP_COLUMNS, 9, plan_t_arr); - auto orientation_rate = framed.initOrientationRate(); - fill_xyzt(orientation_rate, best_plan, PLAN_MHP_COLUMNS, 12, plan_t_arr); + fill_xyzt(framed.initPosition(), best_plan, PLAN_MHP_COLUMNS, 0, plan_t_arr); + fill_xyzt(framed.initVelocity(), best_plan, PLAN_MHP_COLUMNS, 3, plan_t_arr); + fill_xyzt(framed.initOrientation(), best_plan, PLAN_MHP_COLUMNS, 9, plan_t_arr); + fill_xyzt(framed.initOrientationRate(), best_plan, PLAN_MHP_COLUMNS, 12, plan_t_arr); // lane lines auto lane_lines = framed.initLaneLines(4); @@ -352,8 +336,7 @@ void model_publish_v2(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, lane_line_probs_arr[i] = sigmoid(net_outputs.lane_lines_prob[i]); lane_line_stds_arr[i] = exp(net_outputs.lane_lines[2*TRAJECTORY_SIZE*(4 + i)]); } - kj::ArrayPtr lane_line_probs(lane_line_probs_arr, 4); - framed.setLaneLineProbs(lane_line_probs); + framed.setLaneLineProbs(lane_line_probs_arr); framed.setLaneLineStds(lane_line_stds_arr); // road edges @@ -483,14 +466,10 @@ void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, MessageBuilder msg; auto posenetd = msg.initEvent(vipc_dropped_frames < 1).initCameraOdometry(); - kj::ArrayPtr trans_vs(&trans_arr[0], 3); - posenetd.setTrans(trans_vs); - kj::ArrayPtr rot_vs(&rot_arr[0], 3); - posenetd.setRot(rot_vs); - kj::ArrayPtr trans_std_vs(&trans_std_arr[0], 3); - posenetd.setTransStd(trans_std_vs); - kj::ArrayPtr rot_std_vs(&rot_std_arr[0], 3); - posenetd.setRotStd(rot_std_vs); + posenetd.setTrans(trans_arr); + posenetd.setRot(rot_arr); + posenetd.setTransStd(trans_std_arr); + posenetd.setRotStd(rot_std_arr); posenetd.setTimestampEof(timestamp_eof); posenetd.setFrameId(vipc_frame_id); diff --git a/selfdrive/sensord/sensors/bmx055_accel.cc b/selfdrive/sensord/sensors/bmx055_accel.cc index 5eea0bcdfa..cb78ba0ad4 100644 --- a/selfdrive/sensord/sensors/bmx055_accel.cc +++ b/selfdrive/sensord/sensors/bmx055_accel.cc @@ -62,10 +62,8 @@ void BMX055_Accel::get_event(cereal::SensorEventData::Builder &event){ event.setTimestamp(start_time); float xyz[] = {x, y, z}; - kj::ArrayPtr vs(&xyz[0], 3); - auto svec = event.initAcceleration(); - svec.setV(vs); + svec.setV(xyz); svec.setStatus(true); } diff --git a/selfdrive/sensord/sensors/bmx055_gyro.cc b/selfdrive/sensord/sensors/bmx055_gyro.cc index 61e3d9e405..38a2ff427c 100644 --- a/selfdrive/sensord/sensors/bmx055_gyro.cc +++ b/selfdrive/sensord/sensors/bmx055_gyro.cc @@ -72,10 +72,8 @@ void BMX055_Gyro::get_event(cereal::SensorEventData::Builder &event){ event.setTimestamp(start_time); float xyz[] = {x, y, z}; - kj::ArrayPtr vs(&xyz[0], 3); - auto svec = event.initGyroUncalibrated(); - svec.setV(vs); + svec.setV(xyz); svec.setStatus(true); } diff --git a/selfdrive/sensord/sensors/bmx055_magn.cc b/selfdrive/sensord/sensors/bmx055_magn.cc index bc742e435b..fbe43c206c 100644 --- a/selfdrive/sensord/sensors/bmx055_magn.cc +++ b/selfdrive/sensord/sensors/bmx055_magn.cc @@ -96,10 +96,8 @@ void BMX055_Magn::get_event(cereal::SensorEventData::Builder &event){ event.setTimestamp(start_time); float xyz[] = {x, y, z}; - kj::ArrayPtr vs(&xyz[0], 3); - auto svec = event.initMagneticUncalibrated(); - svec.setV(vs); + svec.setV(xyz); svec.setStatus(true); } diff --git a/selfdrive/sensord/sensors/lsm6ds3_accel.cc b/selfdrive/sensord/sensors/lsm6ds3_accel.cc index a7253b0a62..15a1867259 100644 --- a/selfdrive/sensord/sensors/lsm6ds3_accel.cc +++ b/selfdrive/sensord/sensors/lsm6ds3_accel.cc @@ -53,10 +53,8 @@ void LSM6DS3_Accel::get_event(cereal::SensorEventData::Builder &event){ event.setTimestamp(start_time); float xyz[] = {y, -x, z}; - kj::ArrayPtr vs(&xyz[0], 3); - auto svec = event.initAcceleration(); - svec.setV(vs); + svec.setV(xyz); svec.setStatus(true); } diff --git a/selfdrive/sensord/sensors/lsm6ds3_gyro.cc b/selfdrive/sensord/sensors/lsm6ds3_gyro.cc index 93cb24ea2b..4c983e5448 100644 --- a/selfdrive/sensord/sensors/lsm6ds3_gyro.cc +++ b/selfdrive/sensord/sensors/lsm6ds3_gyro.cc @@ -56,10 +56,8 @@ void LSM6DS3_Gyro::get_event(cereal::SensorEventData::Builder &event){ event.setTimestamp(start_time); float xyz[] = {y, -x, z}; - kj::ArrayPtr vs(&xyz[0], 3); - auto svec = event.initGyroUncalibrated(); - svec.setV(vs); + svec.setV(xyz); svec.setStatus(true); } diff --git a/selfdrive/sensord/sensors_qcom.cc b/selfdrive/sensord/sensors_qcom.cc index 58295679e9..16b7fc98bf 100644 --- a/selfdrive/sensord/sensors_qcom.cc +++ b/selfdrive/sensord/sensors_qcom.cc @@ -150,8 +150,7 @@ void sensor_loop() { switch (data.type) { case SENSOR_TYPE_ACCELEROMETER: { auto svec = log_event.initAcceleration(); - kj::ArrayPtr vs(&data.acceleration.v[0], 3); - svec.setV(vs); + svec.setV(data.acceleration.v); svec.setStatus(data.acceleration.status); break; } @@ -164,8 +163,7 @@ void sensor_loop() { } case SENSOR_TYPE_MAGNETIC_FIELD: { auto svec = log_event.initMagnetic(); - kj::ArrayPtr vs(&data.magnetic.v[0], 3); - svec.setV(vs); + svec.setV(data.magnetic.v); svec.setStatus(data.magnetic.status); break; } @@ -178,8 +176,7 @@ void sensor_loop() { } case SENSOR_TYPE_GYROSCOPE: { auto svec = log_event.initGyro(); - kj::ArrayPtr vs(&data.gyro.v[0], 3); - svec.setV(vs); + svec.setV(data.gyro.v); svec.setStatus(data.gyro.status); break; }