mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-08-23 09:13:43 +08:00
Code Issues Packages Projects Releases Wiki Activity

Draw model leads (#21864)

Browse Source 
* draw model not radar

* compiles

* wrong way
old-commit-hash: 276b00ccb805b8071db4352b406f39233c2c3867
This commit is contained in:
HaraldSchafer
2021-08-05 20:35:03 -07:00
committed by GitHub
parent fd0839c85d
commit 331eb5d553
2 changed files with 18 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/ui/ui.cc
+11 -14
View File
@@ -63,14 +63,11 @@ static int get_path_length_idx(const cereal::ModelDataV2::XYZTData::Reader &line
return max_idx;
}
static void update_leads(UIState *s, const cereal::RadarState::Reader &radar_state, std::optional<cereal::ModelDataV2::XYZTData::Reader> line) {
for (int i = 0; i < 2; ++i) {
auto lead_data = (i == 0) ? radar_state.getLeadOne() : radar_state.getLeadTwo();
if (lead_data.getStatus()) {
float z = line ? (*line).getZ()[get_path_length_idx(*line, lead_data.getDRel())] : 0.0;
// negative because radarState uses left positive convention
calib_frame_to_full_frame(s, lead_data.getDRel(), -lead_data.getYRel(), z + 1.22, &s->scene.lead_vertices[i]);
}
static void update_leads(UIState *s, const cereal::ModelDataV2::LeadDataV2::Reader &lead_data, std::optional<cereal::ModelDataV2::XYZTData::Reader> line) {
if (lead_data.getProb() > 0.5) {
float z = line ? (*line).getZ()[get_path_length_idx(*line, lead_data.getXyva()[0])] : 0.0;
calib_frame_to_full_frame(s, lead_data.getXyva()[0], lead_data.getXyva()[1], z + 1.22, &s->scene.lead_vertices[0]);
}
}
@@ -112,9 +109,9 @@ static void update_model(UIState *s, const cereal::ModelDataV2::Reader &model) {
}
// update path
auto lead_one = (*s->sm)["radarState"].getRadarState().getLeadOne();
if (lead_one.getStatus()) {
const float lead_d = lead_one.getDRel() * 2.;
auto lead_one = (*s->sm)["modelV2"].getModelV2().getLeads()[0];
if (lead_one.getProb() > 0.5) {
const float lead_d = lead_one.getXyva()[0] * 2.;
max_distance = std::clamp((float)(lead_d - fmin(lead_d * 0.35, 10.)), 0.0f, max_distance);
}
max_idx = get_path_length_idx(model_position, max_distance);
@@ -134,12 +131,12 @@ static void update_state(UIState *s) {
scene.engageable = sm["controlsState"].getControlsState().getEngageable();
scene.dm_active = sm["driverMonitoringState"].getDriverMonitoringState().getIsActiveMode();
}
if (sm.updated("radarState") && s->vg) {
if (sm.updated("modelV2") && s->vg) {
std::optional<cereal::ModelDataV2::XYZTData::Reader> line;
if (sm.rcv_frame("modelV2") > 0) {
line = sm["modelV2"].getModelV2().getPosition();
}
update_leads(s, sm["radarState"].getRadarState(), line);
update_leads(s, sm["modelV2"].getModelV2().getLeads()[0], line);
}
if (sm.updated("liveCalibration")) {
scene.world_objects_visible = true;
@@ -273,7 +270,7 @@ static void update_status(UIState *s) {
QUIState::QUIState(QObject *parent) : QObject(parent) {
ui_state.sm = std::make_unique<SubMaster, const std::initializer_list<const char *>>({
"modelV2", "controlsState", "liveCalibration", "radarState", "deviceState", "roadCameraState",
"modelV2", "controlsState", "liveCalibration", "deviceState", "roadCameraState",
"pandaState", "carParams", "driverMonitoringState", "sensorEvents", "carState", "liveLocationKalman",
});