zhudongjer/carrot
1
1
Fork 0
Code Issues Pull Requests Actions 2 Packages Projects Releases Wiki Activity
carrot/selfdrive/ui/carrot.cc

2904 lines
116 KiB
C++
Raw Normal View History

v8-wip4
2024-09-03 16:09:00 +09:00
#include "selfdrive/ui/carrot.h"
#include <cassert>
#include <cmath>
#include <limits>
//#define __TEST
//#define __UI_TEST
#ifdef __APPLE__
#include <OpenGL/gl3.h>
#define NANOVG_GL3_IMPLEMENTATION
#define nvgCreate nvgCreateGL3
#else
#include <GLES3/gl3.h>
#define NANOVG_GLES3_IMPLEMENTATION
#define nvgCreate nvgCreateGLES3
#endif
#define NANOVG_GLES3_IMPLEMENTATION
#include <nanovg_gl.h>
#include <nanovg_gl_utils.h>
#define COLOR_BLACK nvgRGBA(0, 0, 0, 255)
#define COLOR_BLACK_ALPHA(x) nvgRGBA(0, 0, 0, x)
#define COLOR_WHITE nvgRGBA(255, 255, 255, 255)
#define COLOR_WHITE_ALPHA(x) nvgRGBA(255, 255, 255, x)
#define COLOR_RED_ALPHA(x) nvgRGBA(255, 0, 0, x)
#define COLOR_YELLOW nvgRGBA(218, 202, 37, 255)
#define COLOR_YELLOW_ALPHA(x) nvgRGBA(218, 202, 37, x)
//#define COLOR_RED nvgRGBA(201, 34, 49, 255)
#define COLOR_RED nvgRGBA(255, 0, 0, 255)
#define COLOR_OCHRE nvgRGBA(218, 111, 37, 255)
#define COLOR_OCHRE_ALPHA(x) nvgRGBA(218, 111, 37, x)
#define COLOR_GREEN nvgRGBA(0, 203, 0, 255)
#define COLOR_GREEN_ALPHA(x) nvgRGBA(0, 153, 0, x)
#define COLOR_BLUE nvgRGBA(0, 0, 255, 255)
#define COLOR_BLUE_ALPHA(x) nvgRGBA(0, 0, 255, x)
#define COLOR_ORANGE nvgRGBA(255, 175, 3, 255)
#define COLOR_ORANGE_ALPHA(x) nvgRGBA(255, 175, 3, x)
#define COLOR_GREY nvgRGBA(191, 191, 191, 1)
#define COLOR_GREY_ALPHA(x) nvgRGBA(191, 191, 191, x)
#define BOLD "KaiGenGothicKR-Bold"//"Inter-Bold"//"sans-bold"
constexpr float MIN_DRAW_DISTANCE = 10.0;
constexpr float MAX_DRAW_DISTANCE = 100.0;
ModelRenderer* _model = NULL;
extern int get_path_length_idx(const cereal::XYZTData::Reader& line, const float path_height);
int g_fps= 0;
static void ui_draw_text(const UIState* s, float x, float y, const char* string, float size, NVGcolor color, const char* font_name, float borderWidth=3.0, float shadowOffset=0.0, NVGcolor borderColor=COLOR_BLACK, NVGcolor shadowColor=COLOR_BLACK) {
y += 6;
nvgFontFace(s->vg, font_name);
nvgFontSize(s->vg, size);
if (borderWidth > 0.0) {
//NVGcolor borderColor = COLOR_BLACK;
nvgFillColor(s->vg, borderColor);
for (int i = 0; i < 360; i += 45) {
float angle = i * NVG_PI / 180.0f;
float offsetX = borderWidth * cos(angle);
float offsetY = borderWidth * sin(angle);
nvgText(s->vg, x + offsetX, y + offsetY, string, NULL);
}
}
if (shadowOffset != 0.0) {
//NVGcolor shadowColor = COLOR_BLACK;
nvgFillColor(s->vg, shadowColor);
nvgText(s->vg, x + shadowOffset, y + shadowOffset, string, NULL);
}
nvgFillColor(s->vg, color);
nvgText(s->vg, x, y, string, NULL);
}
static void ui_draw_text_vg(NVGcontext* vg, float x, float y, const char* string, float size, NVGcolor color, const char* font_name, float borderWidth = 3.0, float shadowOffset = 0.0, NVGcolor borderColor = COLOR_BLACK, NVGcolor shadowColor = COLOR_BLACK) {
//y += 6;
nvgFontFace(vg, font_name);
nvgFontSize(vg, size);
if (borderWidth > 0.0) {
//NVGcolor borderColor = COLOR_BLACK;
nvgFillColor(vg, borderColor);
for (int i = 0; i < 360; i += 45) {
float angle = i * NVG_PI / 180.0f;
float offsetX = borderWidth * cos(angle);
float offsetY = borderWidth * sin(angle);
nvgText(vg, x + offsetX, y + offsetY, string, NULL);
}
}
if (shadowOffset != 0.0) {
//NVGcolor shadowColor = COLOR_BLACK;
nvgFillColor(vg, shadowColor);
nvgText(vg, x + shadowOffset, y + shadowOffset, string, NULL);
}
nvgFillColor(vg, color);
nvgText(vg, x, y, string, NULL);
}
void ui_draw_line_vg(NVGcontext* vg, const QPolygonF& vd, NVGcolor* color, NVGpaint* paint, float stroke = 0.0, NVGcolor strokeColor = COLOR_WHITE) {
if (vd.size() == 0) return;
nvgBeginPath(vg);
nvgMoveTo(vg, vd.at(0).x(), vd.at(0).y());
for (int i = 1; i < vd.size(); i++) {
nvgLineTo(vg, vd.at(i).x(), vd.at(i).y());
}
nvgClosePath(vg);
if (color) {
nvgFillColor(vg, *color);
}
else if (paint) {
nvgFillPaint(vg, *paint);
}
nvgFill(vg);
if (stroke > 0.0) {
nvgStrokeColor(vg, strokeColor);
nvgStrokeWidth(vg, stroke);
nvgStroke(vg);
}
}
void ui_draw_line2_vg(NVGcontext* vg, float x[], float y[], int size, NVGcolor* color, NVGpaint* paint, float stroke = 0.0, NVGcolor strokeColor = COLOR_WHITE) {
nvgBeginPath(vg);
nvgMoveTo(vg, x[0], y[0]);
for (int i = 1; i < size; i++) {
nvgLineTo(vg, x[i], y[i]);
}
nvgClosePath(vg);
if (color) {
nvgFillColor(vg, *color);
}
else if (paint) {
nvgFillPaint(vg, *paint);
}
nvgFill(vg);
if (stroke > 0.0) {
nvgStrokeColor(vg, strokeColor);
nvgStrokeWidth(vg, stroke);
nvgStroke(vg);
}
}
void ui_draw_image(const UIState* s, const Rect1& r, const char* name, float alpha) {
nvgBeginPath(s->vg);
NVGpaint imgPaint = nvgImagePattern(s->vg, r.x, r.y, r.w, r.h, 0, s->images.at(name), alpha);
nvgRect(s->vg, r.x, r.y, r.w, r.h);
nvgFillPaint(s->vg, imgPaint);
nvgFill(s->vg);
}
void ui_draw_line(const UIState* s, const QPolygonF& vd, NVGcolor* color, NVGpaint* paint, float stroke = 0.0, NVGcolor strokeColor = COLOR_WHITE) {
if (vd.size() == 0) return;
nvgBeginPath(s->vg);
nvgMoveTo(s->vg, vd.at(0).x(), vd.at(0).y());
for (int i = 1; i < vd.size(); i++) {
nvgLineTo(s->vg, vd.at(i).x(), vd.at(i).y());
}
nvgClosePath(s->vg);
if (color) {
nvgFillColor(s->vg, *color);
}
else if (paint) {
nvgFillPaint(s->vg, *paint);
}
nvgFill(s->vg);
if (stroke > 0.0) {
nvgStrokeColor(s->vg, strokeColor);
nvgStrokeWidth(s->vg, stroke);
nvgStroke(s->vg);
}
}
void ui_draw_line2(const UIState* s, float x[], float y[], int size, NVGcolor* color, NVGpaint* paint, float stroke = 0.0, NVGcolor strokeColor = COLOR_WHITE) {
nvgBeginPath(s->vg);
nvgMoveTo(s->vg, x[0], y[0]);
for (int i = 1; i < size; i++) {
nvgLineTo(s->vg, x[i], y[i]);
}
nvgClosePath(s->vg);
if (color) {
nvgFillColor(s->vg, *color);
}
else if (paint) {
nvgFillPaint(s->vg, *paint);
}
nvgFill(s->vg);
if (stroke > 0.0) {
nvgStrokeColor(s->vg, strokeColor);
nvgStrokeWidth(s->vg, stroke);
nvgStroke(s->vg);
}
}
#if 0
void ui_draw_rect(NVGcontext* vg, const Rect1& r, NVGcolor color, int width, float radius) {
nvgBeginPath(vg);
radius > 0 ? nvgRoundedRect(vg, r.x, r.y, r.w, r.h, radius) : nvgRect(vg, r.x, r.y, r.w, r.h);
nvgStrokeColor(vg, color);
nvgStrokeWidth(vg, width);
nvgStroke(vg);
}
#endif
static inline void fill_rect(NVGcontext* vg, const Rect1& r, const NVGcolor* color, const NVGpaint* paint,
float radius, float stroke_width, NVGcolor* stroke_color) {
nvgBeginPath(vg);
if (radius > 0) {
nvgRoundedRect(vg, r.x, r.y, r.w, r.h, radius);
}
else {
nvgRect(vg, r.x, r.y, r.w, r.h);
}
if (color) nvgFillColor(vg, *color);
if (paint) nvgFillPaint(vg, *paint);
nvgFill(vg);
if (stroke_width > 0) {
nvgStrokeWidth(vg, stroke_width);
if (stroke_color) nvgStrokeColor(vg, *stroke_color);
else nvgStrokeColor(vg, nvgRGB(0, 0, 0));
nvgStroke(vg);
}
}
void ui_fill_rect(NVGcontext* vg, const Rect1& r, const NVGcolor& color, float radius, float stroke_width, NVGcolor* stroke_color) {
fill_rect(vg, r, &color, nullptr, radius, stroke_width, stroke_color);
}
void ui_fill_rect(NVGcontext* vg, const Rect1& r, const NVGpaint& paint, float radius, float stroke_width, NVGcolor* stroke_color) {
fill_rect(vg, r, nullptr, &paint, radius, stroke_width, stroke_color);
}
//////////////////// draw_text animated
float a_x = 0.;
float a_y = 0.;
float a_size = 0.;
const int a_max = 100;
char a_font[128] = "";
int a_time = -1;
char a_string[256] = "";
NVGcolor a_color = COLOR_WHITE;
static void ui_draw_text_a2(const UIState* s) {
if (a_time <= 0) return;
nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BOTTOM);
a_time -= 10;
int a_time1 = a_time;
if (a_time1 > 100) a_time1 = 100;
int x = (s->fb_w / 2 * a_time1 + a_x * (a_max - a_time1)) / a_max;
int y = ((s->fb_h - 400) * a_time1 + a_y * (a_max - a_time1)) / a_max;
int size = (350 * a_time1 + a_size * (a_max - a_time1)) / a_max;
if (a_time >= 100) ui_draw_text(s, x, y, a_string, size, a_color, a_font, 9.0, 8.0, COLOR_BLACK, COLOR_BLACK);
else ui_draw_text(s, x, y, a_string, size, a_color, a_font);
}
static void ui_draw_text_a(const UIState* s, float x, float y, const char* string, float size, NVGcolor color, const char* font_name) {
a_x = x;
a_y = y;
strcpy(a_string, string);
a_size = size;
a_color = color;
strcpy(a_font, font_name);
a_time = 130;
}
//////////////////// draw_text animated
//##################### drawPlot
#define PLOT_MAX 400 //500
class DrawPlot : public QObject {
Q_OBJECT
protected:
int plotSize = 0;
int plotIndex = 0;
float plotQueue[3][PLOT_MAX];
float plotMin = 0.;
float plotMax = 0.;
float plotShift = 0.0;
float plotX = 350.0;// 30.0;// 300.0;
float plotWidth = 1000;
float plotY = 40;// 70.0;// 120.0;// 30.0;
float plotHeight = 300.0;
float plotRatio = 1.0;
float plotDx = 2.0;
int show_plot_mode = 0;
int show_plot_mode_prev = -1;
void drawPlotting(const UIState* s, int index, int start, float x, float y[], int size, NVGcolor* color, float stroke = 0.0) {
nvgBeginPath(s->vg);
plotRatio = (plotMax - plotMin) < 1.0 ? plotHeight : plotHeight / (plotMax - plotMin);
float dx = plotDx;
char str[128];
for (int i = 0; i < size; i++) {
float data = y[(start - i + PLOT_MAX) % PLOT_MAX];
float plot_y = plotY + plotHeight - (data - plotMin) * plotRatio;
if (i == 0) {
nvgMoveTo(s->vg, x + (size - i) * dx, plot_y);
sprintf(str, "%.2f", data);
ui_draw_text(s, x + (size - i) * dx + 50, plot_y + ((index > 0) ? 40 : 0), str, 40, *color, BOLD);
}
else nvgLineTo(s->vg, x + (size - i) * dx, plot_y);
}
//nvgClosePath(s->vg);
if (stroke > 0.0) {
nvgStrokeColor(s->vg, *color);
nvgStrokeWidth(s->vg, stroke);
nvgStroke(s->vg);
}
}
V8 wip6 x3 (#154) * update until aaaa2d0, fix.. cruise, default settings.
2025-04-23 07:34:47 +09:00
Params params;
std::deque<float> minDeque[3]; // 최소값을 유지하는 덱
std::deque<float> maxDeque[3]; // 최대값을 유지하는 덱
v8-wip4
2024-09-03 16:09:00 +09:00
void makePlotData(const UIState* s, float data[], char* title) {
SubMaster& sm = *(s->sm);
auto car_state = sm["carState"].getCarState();
auto lp = sm["longitudinalPlan"].getLongitudinalPlan();
auto car_control = sm["carControl"].getCarControl();
auto controls_state = sm["controlsState"].getControlsState();
auto torque_state = controls_state.getLateralControlState().getTorqueState();
float a_ego = car_state.getAEgo();
float v_ego = car_state.getVEgo();
float accel = lp.getAccels()[0];
float speeds_0 = lp.getSpeeds()[0];
float accel_out = car_control.getActuators().getAccel();
const cereal::ModelDataV2::Reader& model = sm["modelV2"].getModelV2();
const auto position = model.getPosition();
const auto velocity = model.getVelocity();
auto lead_radar = sm["radarState"].getRadarState().getLeadOne();
auto live_params = sm["liveParameters"].getLiveParameters();
// 0: yellow, 1: green, 2: orange
switch (show_plot_mode) {
case 0:
case 1:
data[0] = a_ego;
data[1] = accel;
data[2] = accel_out;
sprintf(title, "1.Accel (Y:a_ego, G:a_target, O:a_out)");
break;
case 2:
data[0] = speeds_0;
data[1] = v_ego;
data[2] = a_ego;
sprintf(title, "2.Speed/Accel(Y:speed_0, G:v_ego, O:a_ego)");
break;
case 3:
data[0] = position.getX()[32];
data[1] = velocity.getX()[32];
data[2] = velocity.getX()[0];
sprintf(title, "3.Model(Y:pos_32, G:vel_32, O:vel_0)");
break;
case 4:
data[0] = accel;
data[1] = lead_radar.getALeadK();
data[2] = lead_radar.getVRel();
sprintf(title, "4.Lead(Y:accel, G:a_lead, O:v_rel)");
break;
case 5:
data[0] = a_ego;
data[1] = lead_radar.getALead();
data[2] = lead_radar.getJLead();
sprintf(title, "5.Lead(Y:a_ego, G:a_lead, O:j_lead)");
break;
case 6:
data[0] = torque_state.getActualLateralAccel() * 10.0;
data[1] = torque_state.getDesiredLateralAccel() * 10.0;
data[2] = torque_state.getOutput() * 10.0;
sprintf(title, "6.Steer(Y:actual, G:desire, O:output)");
break;
case 7:
data[0] = car_state.getSteeringAngleDeg();
data[1] = car_control.getActuators().getSteeringAngleDeg();
data[2] = live_params.getAngleOffsetDeg() * 10.0;
sprintf(title, "7.SteerA (Y:Actual, G:Target, O:Offset*10)");
break;
case 8:
data[0] = car_control.getActuators().getCurvature()*10000;
data[1] = car_control.getActuators().getCurvature()*10000;
data[2] = car_control.getActuators().getCurvature()*10000;
sprintf(title, "8.SteerA (Y:Actual, G:Target, O:Offset*10)");
break;
default:
data[0] = data[1] = data[2] = 0;
sprintf(title, "no data");
break;
}
if (show_plot_mode != show_plot_mode_prev) {
plotSize = 0;
plotIndex = 0;
plotMin = 0.;
plotMax = 0.;
V8 wip6 x3 (#154) * update until aaaa2d0, fix.. cruise, default settings.
2025-04-23 07:34:47 +09:00
for (int i = 0; i < 3; i++) {
minDeque[i].clear();
maxDeque[i].clear();
}
v8-wip4
2024-09-03 16:09:00 +09:00
show_plot_mode_prev = show_plot_mode;
}
}
void updatePlotQueue(float plot_data[3]) {
// plotIndex 업데이트
plotIndex = (plotIndex + 1) % PLOT_MAX;
for (int i = 0; i < 3; i++) {
// 오래된 값이 덱의 최솟값/최댓값에 해당하면 제거
if (plotSize == PLOT_MAX) {
if (!minDeque[i].empty() && minDeque[i].front() == plotQueue[i][plotIndex]) {
minDeque[i].pop_front();
}
if (!maxDeque[i].empty() && maxDeque[i].front() == plotQueue[i][plotIndex]) {
maxDeque[i].pop_front();
}
}
// 새 값을 큐에 추가
plotQueue[i][plotIndex] = plot_data[i];
// minDeque 업데이트
while (!minDeque[i].empty() && minDeque[i].back() > plot_data[i]) {
minDeque[i].pop_back();
}
minDeque[i].push_back(plot_data[i]);
// maxDeque 업데이트
while (!maxDeque[i].empty() && maxDeque[i].back() < plot_data[i]) {
maxDeque[i].pop_back();
}
maxDeque[i].push_back(plot_data[i]);
}
// 큐 크기 증가
if (plotSize < PLOT_MAX) {
plotSize++;
}
// 전체 최소값 및 최대값 계산
plotMin = std::numeric_limits<double>::max();
plotMax = std::numeric_limits<double>::lowest();
for (int i = 0; i < 3; i++) {
plotMin = std::min(plotMin, minDeque[i].front());
plotMax = std::max(plotMax, maxDeque[i].front());
}
// plotMin 및 plotMax 범위 제한
if (plotMin > -2.0) plotMin = -2.0;
if (plotMax < 2.0) plotMax = 2.0;
}
public:
void draw(const UIState* s) {
show_plot_mode = params.getInt("ShowPlotMode");
if (show_plot_mode == 0) return;
SubMaster& sm = *(s->sm);
if (sm.alive("carState") && sm.alive("longitudinalPlan"));
else return;
//ui_fill_rect(s->vg, { (int)plotX - 10, (int)plotY - 50, (int)(PLOT_MAX * plotDx) + 150, (int)plotHeight + 100}, COLOR_BLACK_ALPHA(90), 30);
float plot_data[3] = {0., 0., 0. };
char title[128] = "";
makePlotData(s, plot_data, title);
updatePlotQueue(plot_data);
/*
for (int i = 0; i < 3; i++) {
if (plotMin > plot_data[i]) plotMin = plot_data[i];
if (plotMax < plot_data[i]) plotMax = plot_data[i];
}
if (plotMin > -2.) plotMin = -2.0;
if (plotMax < 2.0) plotMax = 2.0;
plotIndex = (plotIndex + 1) % PLOT_MAX;
for(int i=0;i<3;i++) plotQueue[i][plotIndex] = plot_data[i];
if (plotSize < PLOT_MAX - 1) plotSize++;
*/
if (s->fb_w < 1200) return;
NVGcolor color[3] = { COLOR_YELLOW, COLOR_GREEN, COLOR_ORANGE };
for (int i = 0; i < 3; i++) {
drawPlotting(s, i, plotIndex, plotX, plotQueue[i], plotSize, &color[i], 3.0f);
}
ui_draw_text(s, plotX + 400, plotY - 20, title, 25, COLOR_WHITE, BOLD);
}
};
class ModelDrawer {
protected:
template <class T>
float interp(float x, std::initializer_list<T> x_list, std::initializer_list<T> y_list, bool extrapolate)
{
std::vector<T> xData(x_list);
std::vector<T> yData(y_list);
int size = xData.size();
int i = 0;
if (x >= xData[size - 2]) {
i = size - 2;
}
else {
while (x > xData[i + 1]) i++;
}
T xL = xData[i], yL = yData[i], xR = xData[i + 1], yR = yData[i + 1];
if (!extrapolate) {
if (x < xL) yR = yL;
if (x > xR) yL = yR;
}
T dydx = (yR - yL) / (xR - xL);
return yL + dydx * (x - xL);
}
template <class T>
float interp(float x, const T* x_list, const T* y_list, size_t size, bool extrapolate)
{
int i = 0;
if (x >= x_list[size - 2]) {
i = size - 2;
}
else {
while (x > x_list[i + 1]) i++;
}
T xL = x_list[i], yL = y_list[i], xR = x_list[i + 1], yR = y_list[i + 1];
if (!extrapolate) {
if (x < xL) yR = yL;
if (x > xR) yL = yR;
}
T dydx = (yR - yL) / (xR - xL);
return yL + dydx * (x - xL);
}
void update_line_data(const UIState* s, const cereal::XYZTData::Reader& line,
float y_off, float z_off_left, float z_off_right, QPolygonF* pvd, int max_idx, bool allow_invert = true, float y_shift = 0.0, int start_idx = 0) {
const auto line_x = line.getX(), line_y = line.getY(), line_z = line.getZ();
QPolygonF left_points, right_points;
left_points.reserve(max_idx + 1);
right_points.reserve(max_idx + 1);
for (int i = start_idx; i <= max_idx; i++) {
// highly negative x positions are drawn above the frame and cause flickering, clip to zy plane of camera
if (line_x[i] < 0) continue;
QPointF left, right;
bool l = _model->mapToScreen(line_x[i], line_y[i] - y_off + y_shift, line_z[i] + z_off_left, &left);
bool r = _model->mapToScreen(line_x[i], line_y[i] + y_off + y_shift, line_z[i] + z_off_right, &right);
if (l && r) {
// For wider lines the drawn polygon will "invert" when going over a hill and cause artifacts
if (!allow_invert && left_points.size() && left.y() > left_points.back().y()) {
continue;
}
left_points.push_back(left);
right_points.push_front(right);
}
}
*pvd = left_points + right_points;
}
};
class PathEndDrawer : ModelDrawer {
private:
QPointF path_end_left_vertex;
QPointF path_end_right_vertex;
int radarTrackId = -1;
float alpha = 0.85;
float path_fx = 0.0;
float path_fy = 0.0;
float path_fwidth = 0.0;
int path_x = 0;
int path_y = 0;
int path_width = 0;
bool lead_status = false;
float radarDist = 0.0;
float visionDist = 0.0;
int xState = 0;
int trafficState = 0;
float v_ego = 0.0;
bool brakeHoldActive = false;
int softHoldActive = 0;
bool longActive = false;
float t_follow = 0.0;
float tf_distance = 0.0;
QPointF tf_vertex_left;
QPointF tf_vertex_right;
protected:
bool make_data(const UIState* s) {
SubMaster& sm = *(s->sm);
if (!sm.alive("modelV2")) return false;
v_ego = sm["carState"].getCarState().getVEgo();
brakeHoldActive = sm["carState"].getCarState().getBrakeHoldActive();
softHoldActive = sm["carState"].getCarState().getSoftHoldActive();
auto selfdrive_state = sm["selfdriveState"].getSelfdriveState();
longActive = selfdrive_state.getEnabled();
//longActive = sm["carControl"].getCarControl().getLongActive();
//longActive = sm["carControl"].getCarControl().getEnabled();
auto lp = sm["longitudinalPlan"].getLongitudinalPlan();
xState = lp.getXState();
trafficState = lp.getTrafficState();
const cereal::ModelDataV2::Reader& model = sm["modelV2"].getModelV2();
auto line = model.getPosition();
float max_distance = s->max_distance;
int idx = get_path_length_idx(line, max_distance);
float y = line.getY()[idx];
float z = line.getZ()[idx];
auto leadsV3 = model.getLeadsV3()[0];
visionDist = leadsV3.getProb() > .5 ? leadsV3.getX()[0] - 1.52 : 0.0;
auto lead_one = sm["radarState"].getRadarState().getLeadOne();
lead_status = lead_one.getStatus();
if (lead_status) {
z = line.getZ()[get_path_length_idx(line, lead_one.getDRel())];
max_distance = lead_one.getDRel();
y = -lead_one.getYRel();
radarTrackId = lead_one.getRadarTrackId();
radarDist = (lead_one.getRadar()) ? lead_one.getDRel() : 0;
}
else {
radarTrackId = -1;
radarDist = 0;
}
_model->mapToScreen(max_distance, y - 1.2, z + 1.22, &path_end_left_vertex);
_model->mapToScreen(max_distance, y + 1.2, z + 1.22, &path_end_right_vertex);
float lex = path_end_left_vertex.x();
float ley = path_end_left_vertex.y();
float rex = path_end_right_vertex.x();
float rey = path_end_right_vertex.y();
float _path_width = rex - lex;
float _path_x = (lex + rex) / 2;
float _path_y = (ley + rey) / 2;
_path_x = (int)std::clamp(_path_x, 350.f, s->fb_w - 350.f);
_path_y = (int)std::clamp(_path_y, 200.f, s->fb_h - 80.f);
if (isnan(_path_x) || isnan(_path_y) || isnan(_path_width));
else {
path_fx = path_fx * alpha + _path_x * (1. - alpha);
path_fy = path_fy * alpha + _path_y * (1. - alpha);
if (_path_width < 120.) _path_width = 120.;
else if (_path_width > 800.) _path_width = 800.;
path_fwidth = path_fwidth * alpha + _path_width * (1. - alpha);
}
path_x = (int)path_fx;
path_y = (int)path_fy;
path_width = (int)path_fwidth;
t_follow = lp.getTFollow();
tf_distance = lp.getDesiredDistance(); // t_follow* v_ego + 6;
int tf_idx = get_path_length_idx(line, tf_distance);
float tf_y = line.getY()[tf_idx];
float tf_z = line.getZ()[tf_idx];
_model->mapToScreen(tf_distance, tf_y - 1.0, tf_z + 1.22, &tf_vertex_left);
_model->mapToScreen(tf_distance, tf_y + 1.0, tf_z + 1.22, &tf_vertex_right);
return true;
};
bool isLeadSCC() {
return radarTrackId < 2;
}
bool isRadarDetected() {
return radarTrackId >= 0;
}
bool isLeadDetected() {
return lead_status;
}
public:
int getPathX() { return path_x; }
int getPathY() { return path_y; }
void draw(const UIState* s) {
if (!make_data(s)) return;
nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BOTTOM);
int x = path_x;
int y = path_y - 135;
char str[128];
int disp_y = y + 195;// 175;
bool draw_dist = false;
float disp_size = 50;
if (softHoldActive || brakeHoldActive) {
sprintf(str, "%s", (brakeHoldActive) ? "AUTOHOLD" : "SOFTHOLD");
ui_draw_text(s, x, disp_y, str, disp_size, COLOR_WHITE, BOLD);
}
else if (longActive) {
if (xState == 3 || xState == 5) { //XState.e2eStop, XState.e2eStopped
if (v_ego < 1.0) {
sprintf(str, "%s", (trafficState >= 1000) ? "신호오류" : "신호대기");
ui_draw_text(s, x, disp_y, str, disp_size, COLOR_WHITE, BOLD);
}
else {
ui_draw_text(s, x, disp_y, "신호감속중", disp_size, COLOR_WHITE, BOLD);
}
#if 0
else if (getStopDist() > 0.5) {
float dist = getStopDist() * (s->scene.is_metric ? 1 : METER_TO_FOOT);
if (dist < 10.0) sprintf(str, "%.1fM", dist);
else sprintf(str, "%.0fM", dist);
ui_draw_text(s, x, disp_y, str, disp_size, COLOR_WHITE, BOLD);
}
#endif
}
else if (xState == 4) { //XState.e2ePrepare
ui_draw_text(s, x, disp_y, "E2E주행중", disp_size, COLOR_WHITE, BOLD);
}
else if (xState == 0 || xState == 1 || xState == 2) { //XState.lead
draw_dist = true;
}
}
else draw_dist = true;
if (draw_dist) {
//float dist = (getRadarDist() > 0.0) ? getRadarDist() : getVisionDist();
//if (dist < 10.0) sprintf(str, "%.1f", dist);
//else sprintf(str, "%.0f", dist);
//ui_draw_text(s, x, disp_y, str, disp_size, COLOR_WHITE, BOLD);
int wStr = 0, w = 80;
float dist = radarDist * (s->scene.is_metric ? 1 : METER_TO_FOOT);
NVGcolor text_color = (xState==0) ? COLOR_WHITE : (xState==1) ? COLOR_GREY : COLOR_GREEN;
if (dist > 0.0) {
sprintf(str, "%.1f", dist);
wStr = 32 * (strlen(str) + 0);
ui_fill_rect(s->vg, { (int)(x - w - wStr / 2), (int)(disp_y - 35), wStr, 42 }, isLeadSCC() ? COLOR_RED : COLOR_ORANGE, 15);
ui_draw_text(s, x - w, disp_y, str, 40, text_color, BOLD);
}
dist = visionDist * (s->scene.is_metric ? 1 : METER_TO_FOOT);
if (dist > 0.0) {
sprintf(str, "%.1f", dist);
wStr = 32 * (strlen(str) + 0);
ui_fill_rect(s->vg, { (int)(x + w - wStr / 2), (int)(disp_y - 35), wStr, 42 }, COLOR_BLUE, 15);
ui_draw_text(s, x + w, disp_y, str, 40, text_color, BOLD);
}
}
QPolygonF tf_vertext;
tf_vertext.push_back(tf_vertex_left);
tf_vertext.push_back(tf_vertex_right);
ui_draw_line(s, tf_vertext, nullptr, nullptr, 3.0, COLOR_WHITE);
sprintf(str, "%.1f(%.2f)", tf_distance, t_follow);
ui_draw_text(s, tf_vertex_right.x(), tf_vertex_right.y(), str, 25, COLOR_WHITE, BOLD);
float px[7], py[7];
NVGcolor rcolor = isLeadSCC() ? COLOR_RED : COLOR_ORANGE;
NVGcolor pcolor = !isRadarDetected() ? ((trafficState == 1) ? rcolor : COLOR_GREEN) : isRadarDetected() ? rcolor : COLOR_BLUE;
bool show_path_end = true;
if (show_path_end && !isLeadDetected()) {
px[0] = path_x - path_width / 2;
px[1] = path_x + path_width / 2;
px[2] = path_x + path_width / 2;
px[3] = path_x + 20;
px[4] = path_x;
px[5] = path_x - 20;
px[6] = path_x - path_width / 2;
py[0] = path_y;
py[1] = path_y;
py[2] = path_y - 7;
py[3] = path_y - 17;
py[4] = path_y - 0;
py[5] = path_y - 17;
py[6] = path_y - 7;
ui_draw_line2(s, px, py, 7, &pcolor, nullptr, 3.0f);
}
if (isLeadDetected()) {
NVGcolor radar_stroke = isRadarDetected() ? rcolor : COLOR_BLUE;
ui_fill_rect(s->vg, { (int)(path_x - path_width / 2 - 10), (int)(path_y - path_width * 0.8), (int)(path_width + 20), (int)(path_width * 0.8) }, COLOR_BLACK_ALPHA(20), 15, 3, &radar_stroke);
#if 0
px[0] = path_x - path_width / 2 - 10;
px[1] = px[0] + path_width + 20;
px[2] = px[1];
px[3] = px[0];
py[0] = path_y + 20;// 5;
py[1] = py[0];
py[2] = py[1] - path_width * 0.8;
py[3] = py[2];
NVGcolor color2 = COLOR_BLACK_ALPHA(20);
ui_draw_line2(s, px, py, 4, &color2, nullptr, 3.0f, isRadarDetected() ? rcolor : COLOR_BLUE);
#endif
#if 0
auto lead_radar = sm["radarState"].getRadarState().getLeadOne();
float h = path_width * 0.8 / 2.;
float ax[4], ay[4], vx[4], vy[4];
vx[0] = px[0];
vx[1] = vx[0] + 20;
vx[2] = vx[1];
vx[3] = vx[0];
vy[0] = py[0] - h;
vy[1] = vy[0];
float v = lead_radar.getVRel() / 5.0 * h;
if (v < -h) v = -h;
if (v > h) v = h;
vy[2] = vy[1] - v;
vy[3] = vy[2];
NVGcolor vcolor = isLeadSCC() ? COLOR_RED : COLOR_ORANGE;
ui_draw_line2(s, vx, vy, 4, &vcolor, nullptr, 0.0f);
ax[0] = px[1];
ax[1] = px[1] - 20;
ax[2] = ax[1];
ax[3] = ax[0];
ay[0] = py[0] - h;
ay[1] = ay[0];
float a = lead_radar.getALeadK() / 2.0 * h;
if (a < -h) a = -h;
if (a > h) a = h;
ay[2] = ay[1] - a;
ay[3] = ay[2];
NVGcolor acolor = isLeadSCC() ? COLOR_RED : COLOR_ORANGE;
ui_draw_line2(s, ax, ay, 4, &acolor, nullptr, 0.0f);
#endif
}
}
};
class LaneLineDrawer : ModelDrawer {
private:
float lane_line_probs[4];
float road_edge_stds[2];
QPolygonF lane_line_vertices[4];
QPolygonF road_edge_vertices[2];
protected:
bool make_data(const UIState* s) {
SubMaster& sm = *(s->sm);
if (!sm.alive("modelV2")) return false;
const cereal::ModelDataV2::Reader& model = sm["modelV2"].getModelV2();
const auto model_lane_lines = model.getLaneLines();
const auto model_lane_line_probs = model.getLaneLineProbs();
int max_idx = get_path_length_idx(model_lane_lines[0], s->max_distance);
for (int i = 0; i < std::size(lane_line_vertices); i++) {
lane_line_probs[i] = model_lane_line_probs[i];
update_line_data(s, model_lane_lines[i], 0.025 * lane_line_probs[i], 0.0, 0.0, &lane_line_vertices[i], max_idx);
}
// roadedges
const auto model_road_edges = model.getRoadEdges();
const auto model_road_edge_stds = model.getRoadEdgeStds();
int max_idx_road_edge = get_path_length_idx(model_lane_lines[0], 100);
for (int i = 0; i < std::size(road_edge_vertices); i++) {
road_edge_stds[i] = model_road_edge_stds[i];
update_line_data(s, model_road_edges[i], 0.025, 0.0, 0.0, &road_edge_vertices[i], max_idx_road_edge);
}
return true;
}
void drawRoadEdge(const UIState* s) {
NVGcolor color;
for (int i = 0; i < std::size(road_edge_vertices); ++i) {
float temp_f = std::clamp<float>(road_edge_stds[i] / 2.0, 0.0, 1.0);
color = nvgRGBAf(1.0 - temp_f, 0.0, temp_f, 1.0);
ui_draw_line(s, road_edge_vertices[i], &color, nullptr);
}
}
public:
void draw(const UIState* s, int show_lane_info) {
if (show_lane_info < 1) return;
if(!make_data(s)) return;
NVGcolor color;
for (int i = 0; i < std::size(lane_line_vertices); ++i) {
color = nvgRGBAf(1.0, 1.0, 1.0, (lane_line_probs[i] > 0.3) ? 1.0 : 0.0);
ui_draw_line(s, lane_line_vertices[i], &color, nullptr);
}
if(show_lane_info > 1) drawRoadEdge(s);
}
};
class TurnInfoDrawer : ModelDrawer {
private:
int icon_size = 256;
int xSpdLimit = 0;
int xSpdDist = 0;
int xSignType = -1;
int xTurnInfo = -1;
int xDistToTurn = 0;
int nRoadLimitSpeed = 20;
int active_carrot = 0;
int nGoPosDist = 0;
int nGoPosTime = 0;
QString szSdiDescr = "";
QString atc_type;
QString szPosRoadName = "";
QString szTBTMainText = "";
protected:
QPointF navi_turn_point[2];
float navi_turn_point_x[2] = { 0.0, };
float navi_turn_point_y[2] = { 0.0, };
bool navi_turn_point_flag = true;
void drawTurnInfo(const UIState* s) {
nvgTextAlign(s->vg, NVG_ALIGN_LEFT | NVG_ALIGN_BOTTOM);
if (xDistToTurn < 1500 && xDistToTurn > 0) {
SubMaster& sm = *(s->sm);
//const auto carrot_man = sm["carrotMan"].getCarrotMan();
//szTBTMainText = QString::fromStdString(carrot_man.getSzTBTMainText());
const cereal::ModelDataV2::Reader& model = sm["modelV2"].getModelV2();
const auto road_edges = model.getRoadEdges();
int idx = get_path_length_idx(road_edges[0], xDistToTurn);
int m_idx[2] = { 0, 1 };
for (int i = 0; i < 2; i++) {
int m = m_idx[i];
_model->mapToScreen(road_edges[m].getX()[idx], road_edges[m].getY()[idx], road_edges[m].getZ()[idx], &navi_turn_point[i]);
}
float scale = 0.2;
if (xDistToTurn < 200) scale = 1.0 - (0.8 * xDistToTurn / 200.);
scale *= 0.5;
int size_x = 348 * scale;
int size_y = 540 * scale;
int img_x = 0;
int img_y = 0;
float alpha = (navi_turn_point_flag) ? 1.0 : 0.1;
for (int i = 0; i < 2; i++) {
navi_turn_point_x[i] = navi_turn_point_x[i] * (1.0 - alpha) + navi_turn_point[i].x() * alpha;
navi_turn_point_y[i] = navi_turn_point_y[i] * (1.0 - alpha) + navi_turn_point[i].y() * alpha;
}
navi_turn_point_flag = false;
switch (xTurnInfo) {
case 1: case 3: case 5:
img_x = (int)navi_turn_point_x[0] - size_x / 2;
img_y = (int)navi_turn_point_y[0] - size_y;
ui_draw_image(s, { img_x, img_y, size_x, size_y }, "ic_navi_point", 1.0f);
nvgTextAlign(s->vg, NVG_ALIGN_RIGHT | NVG_ALIGN_BOTTOM);
break;
case 2: case 4:
img_x = (int)navi_turn_point_x[1] - size_x / 2;
img_y = (int)navi_turn_point_y[1] - size_y;
ui_draw_image(s, { img_x, img_y, size_x, size_y }, "ic_navi_point", 1.0f);
nvgTextAlign(s->vg, NVG_ALIGN_LEFT | NVG_ALIGN_BOTTOM);
break;
}
char str[128] = "";
sprintf(str, "%s", szTBTMainText.toStdString().c_str());
float text_scale = (scale>0.5)?scale:0.5;
ui_draw_text(s, img_x + size_x / 2, img_y + size_y + 100 * text_scale, str, 100 * text_scale, COLOR_WHITE, BOLD);
}
else navi_turn_point_flag = true;
}
QPointF left_dist_point;
float left_dist_x = 0.0;
float left_dist_y = 0.0;
bool left_dist_flag = true;
void drawSpeedLimit(const UIState* s) {
nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BOTTOM);
if (xSpdLimit <= 0) left_dist_flag = true;
if (xSpdDist > 0) {
SubMaster& sm = *(s->sm);
const cereal::ModelDataV2::Reader& model = sm["modelV2"].getModelV2();
const auto road_edges = model.getRoadEdges();
const auto lane_lines = model.getLaneLines();
char str[128] = "";
if (xSpdDist < 100) {
int idx = get_path_length_idx(lane_lines[2], xSpdDist);
_model->mapToScreen(lane_lines[2].getX()[idx], lane_lines[2].getY()[idx], lane_lines[2].getZ()[idx], &left_dist_point);
}
else {
int idx = get_path_length_idx(road_edges[0], xSpdDist);
_model->mapToScreen(road_edges[1].getX()[idx], road_edges[1].getY()[idx], road_edges[1].getZ()[idx], &left_dist_point);
}
float scale = 0.6;
if (xSpdDist < 200) scale = 1.0 - (0.6 * xSpdDist / 200.);
int bx = left_dist_point.x() + 140 * scale;
int by = left_dist_point.y();
if (left_dist_flag) {
left_dist_x = bx;
left_dist_y = by;
}
else {
left_dist_x = left_dist_x * 0.9 + bx * 0.1;
left_dist_y = left_dist_y * 0.9 + by * 0.1;
}
bx = left_dist_x;
by = left_dist_y;
left_dist_flag = false;
if (xSpdDist > 0) {
if (s->scene.is_metric) {
if (xSpdDist < 1000) sprintf(str, "%d m", xSpdDist);
else sprintf(str, "%.1f km", xSpdDist / 1000.f);
}
else {
// 1 미터 = 3.28084 피트로 계산
if (xSpdDist < 1609) sprintf(str, "%d ft", (int)(xSpdDist * 3.28084)); // 1609m(1마일)보다 작으면 피트로 표시
else sprintf(str, "%.1f mi", xSpdDist / 1609.34f); // 1609m 이상이면 마일로 표시
}
ui_draw_text(s, bx, by + 120 * scale, str, 40 * scale, COLOR_WHITE, BOLD);
}
if (xSignType == 22) {
ui_draw_image(s, { bx - (int)(60 * scale), by - (int)(50 * scale), (int)(120 * scale), (int)(150 * scale) }, "ic_speed_bump", 1.0f);
}
else {
nvgBeginPath(s->vg);
nvgCircle(s->vg, bx, by, 140 / 2 * scale);
nvgFillColor(s->vg, COLOR_WHITE);
nvgFill(s->vg);
nvgBeginPath(s->vg);
nvgCircle(s->vg, bx, by, 130 / 2 * scale);
nvgFillColor(s->vg, COLOR_RED);
nvgFill(s->vg);
nvgBeginPath(s->vg);
nvgCircle(s->vg, bx, by, 110 / 2 * scale);
nvgFillColor(s->vg, COLOR_WHITE);
nvgFill(s->vg);
sprintf(str, "%d", xSpdLimit);
ui_draw_text(s, bx, by + 25 * scale - 6 * (1 - scale), str, 60 * scale, COLOR_BLACK, BOLD, 0.0f, 0.0f);
}
}
}
void drawTurnInfoHud(const UIState* s) {
if (s->fb_w < 1200) return;
#ifdef __UI_TEST
active_carrot = 2;
nGoPosDist = 500000;
nGoPosTime = 4 * 60 * 60;
szSdiDescr = "어린이 보호구역(스쿨존 시작 구간)";
xTurnInfo = 1;
xDistToTurn = 1000;
szPosRoadName = "구문천 1길 17";
#endif
//if (active_carrot <= 1) return;
//printf("nGoPosDist=%d, nGoPosTime=%d\n", nGoPosDist, nGoPosTime);
//if (xDistToTurn <= 0 || nGoPosDist <= 0) return;
char str[128] = "";
int tbt_x = s->fb_w - 800;
int tbt_y = s->fb_h - 250;
NVGcolor stroke_color = COLOR_WHITE;
if (s->scene._current_carrot_display == 3) {
ui_fill_rect(s->vg, { tbt_x, 5, 790, s->fb_h - 15 }, COLOR_BLACK_ALPHA(120), 30, 2, &stroke_color);
}
if (nGoPosDist > 0 && nGoPosTime > 0);
else return;
if (s->scene._current_carrot_display == 3);
else {
ui_fill_rect(s->vg, { tbt_x, tbt_y - 60, 790, 240 + 60 }, COLOR_BLACK_ALPHA(120), 30, 2, &stroke_color);
}
if (szTBTMainText.length() > 0) {
nvgTextAlign(s->vg, NVG_ALIGN_LEFT | NVG_ALIGN_BOTTOM);
ui_draw_text(s, tbt_x + 20, tbt_y - 15, szTBTMainText.toStdString().c_str(), 40, COLOR_WHITE, BOLD);
//ui_draw_text(s, tbt_x + 190, tbt_y - 5, szPosRoadName.toStdString().c_str(), 40, COLOR_WHITE, BOLD);
}
if(xTurnInfo > 0) {
nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BOTTOM);
int bx = tbt_x + 100;
int by = tbt_y + 85;
if (atc_type.length() > 0) {
stroke_color = COLOR_BLACK;
ui_fill_rect(s->vg, { bx - 80, by - 90, 160, 230 }, atc_type.contains("prepare")?COLOR_GREEN_ALPHA(100) : COLOR_GREEN, 15, 1.0f, &stroke_color);
}
switch (xTurnInfo) {
case 1: ui_draw_image(s, { bx - icon_size / 2, by - icon_size / 2, icon_size, icon_size }, "ic_turn_l", 1.0f); break;
case 2: ui_draw_image(s, { bx - icon_size / 2, by - icon_size / 2, icon_size, icon_size }, "ic_turn_r", 1.0f); break;
case 3: ui_draw_image(s, { bx - icon_size / 2, by - icon_size / 2, icon_size, icon_size }, "ic_lane_change_l", 1.0f); break;
case 4: ui_draw_image(s, { bx - icon_size / 2, by - icon_size / 2, icon_size, icon_size }, "ic_lane_change_r", 1.0f); break;
case 7: ui_draw_image(s, { bx - icon_size / 2, by - icon_size / 2, icon_size, icon_size }, "ic_turn_u", 1.0f); break;
case 6: ui_draw_text(s, bx, by + 20, "TG", 35, COLOR_WHITE, BOLD); break;
case 8: ui_draw_text(s, bx, by + 20, "목적지", 35, COLOR_WHITE, BOLD); break;
default:
sprintf(str, "감속:%d", xTurnInfo);
ui_draw_text(s, bx, by + 20, str, 35, COLOR_WHITE, BOLD);
break;
}
if (xDistToTurn < 1000) sprintf(str, "%d m", xDistToTurn);
else sprintf(str, "%.1f km", xDistToTurn / 1000.f);
ui_draw_text(s, bx, by + 120, str, 40, COLOR_WHITE, BOLD);
}
nvgTextAlign(s->vg, NVG_ALIGN_LEFT | NVG_ALIGN_BOTTOM);
if (szSdiDescr.length() > 0) {
float bounds[4]; // [xmin, ymin, xmax, ymax]를 저장하는 배열
nvgFontSize(s->vg, 40);
nvgTextBounds(s->vg, tbt_x + 200, tbt_y + 200, szSdiDescr.toStdString().c_str(), NULL, bounds);
float text_width = bounds[2] - bounds[0];
float text_height = bounds[3] - bounds[1];
ui_fill_rect(s->vg, { (int)bounds[0] - 10, (int)bounds[1] - 2, (int)text_width + 20, (int)text_height + 13 }, COLOR_GREEN, 10);
ui_draw_text(s, tbt_x + 200, tbt_y + 200, szSdiDescr.toStdString().c_str(), 40, COLOR_WHITE, BOLD);
}
else if (szPosRoadName.length() > 0) {
ui_draw_text(s, tbt_x + 200, tbt_y + 200, szPosRoadName.toStdString().c_str(), 40, COLOR_WHITE, BOLD);
//ui_draw_text(s, tbt_x + 190, tbt_y - 5, szPosRoadName.toStdString().c_str(), 40, COLOR_WHITE, BOLD);
}
if (nGoPosDist > 0 && nGoPosTime > 0) {
time_t now = time(NULL); // 현재 시간 얻기
struct tm* local = localtime(&now);
int remaining_minutes = (int)nGoPosTime / 60;
local->tm_min += remaining_minutes;
mktime(local);
sprintf(str, "도착: %.1f분(%02d:%02d)", (float)nGoPosTime / 60., local->tm_hour, local->tm_min);
ui_draw_text(s, tbt_x + 190, tbt_y + 80, str, 50, COLOR_WHITE, BOLD);
sprintf(str, "%.1fkm", nGoPosDist / 1000.);
ui_draw_text(s, tbt_x + 190 + 120, tbt_y + 130, str, 50, COLOR_WHITE, BOLD);
}
}
public:
void draw(const UIState* s) {
nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BOTTOM);
SubMaster& sm = *(s->sm);
if (!sm.alive("modelV2") || !sm.alive("carrotMan") || !sm.alive("carState")) {
active_carrot = -1;
return;
}
const auto carrot_man = sm["carrotMan"].getCarrotMan();
active_carrot = carrot_man.getActiveCarrot();
if (active_carrot > 1) {
xSpdLimit = carrot_man.getXSpdLimit();
xSpdDist = carrot_man.getXSpdDist();
xSignType = carrot_man.getXSpdType();
}
else {
xSpdLimit = 0;
xSpdDist = 0;
xSignType = 0;
}
fix lanemode , roadlimitspeed from car(canfd), (#178) * enable detect redlight during regen decel.. * button mode 2 decel.. * lanemode timeadjust 0.05 -> 0.07 * limit_accel_in_turn: unstable decel during cornering * roadLimitSpeed from Car. * fix roadlimitSpeed display.. * fix lanemode model condition.. * fix.. typo * fix. * test for SK3 * fix.. standstill * Revert "test for SK3" This reverts commit b7ab995c5ef2e482b65704fb18c7bc5a3bcf1171. * fix.. loadLimitSpeed.. * fix roadSpeedLimit(hkg can) * initial "gas" state for bump * fix.. lane mode adjust time.. 5 -> 7 -> 6 * test lanemode (curve -> straight) * fix.. lane * revert * fix. laneline curv -> straight.. * fix.. revert * fix.. lanemode... again.. * canfd 0x1b5
2025-05-29 19:51:47 +09:00
xTurnInfo = carrot_man.getXTurnInfo();
xDistToTurn = carrot_man.getXDistToTurn();
nRoadLimitSpeed = carrot_man.getNRoadLimitSpeed();
v8-wip4
2024-09-03 16:09:00 +09:00
if (active_carrot > 1 || carrot_man.getNGoPosDist() > 0) {
atc_type = QString::fromStdString(carrot_man.getAtcType());
nGoPosDist = carrot_man.getNGoPosDist();
nGoPosTime = carrot_man.getNGoPosTime();
szSdiDescr = QString::fromStdString(carrot_man.getSzSdiDescr());
szPosRoadName = QString::fromStdString(carrot_man.getSzPosRoadName());
szTBTMainText = QString::fromStdString(carrot_man.getSzTBTMainText());
}
else {
fix lanemode , roadlimitspeed from car(canfd), (#178) * enable detect redlight during regen decel.. * button mode 2 decel.. * lanemode timeadjust 0.05 -> 0.07 * limit_accel_in_turn: unstable decel during cornering * roadLimitSpeed from Car. * fix roadlimitSpeed display.. * fix lanemode model condition.. * fix.. typo * fix. * test for SK3 * fix.. standstill * Revert "test for SK3" This reverts commit b7ab995c5ef2e482b65704fb18c7bc5a3bcf1171. * fix.. loadLimitSpeed.. * fix roadSpeedLimit(hkg can) * initial "gas" state for bump * fix.. lane mode adjust time.. 5 -> 7 -> 6 * test lanemode (curve -> straight) * fix.. lane * revert * fix. laneline curv -> straight.. * fix.. revert * fix.. lanemode... again.. * canfd 0x1b5
2025-05-29 19:51:47 +09:00
//xTurnInfo = -1;
//xDistToTurn = 0;
//nRoadLimitSpeed = 20;
v8-wip4
2024-09-03 16:09:00 +09:00
atc_type = "";
nGoPosDist = 0;
nGoPosTime = 0;
szSdiDescr = "";
szPosRoadName = "";
szTBTMainText = "";
}
#ifdef __UI_TEST
active_carrot = 2;
xSpdLimit = 110;
xSpdDist = 12345;
nRoadLimitSpeed = 110;
#endif
drawTurnInfo(s);
drawSpeedLimit(s);
drawTurnInfoHud(s);
}
};
bool _right_blinker = false;
bool _left_blinker = false;
class DesireDrawer : ModelDrawer {
protected:
int icon_size = 256;
int blinker_timer = 0;
public:
void draw(const UIState* s, int x, int y) {
blinker_timer = (blinker_timer + 1) % 16;
nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BOTTOM);
SubMaster& sm = *(s->sm);
if (!sm.alive("modelV2") || !sm.alive("carrotMan") || !sm.alive("carState")) return;
auto meta = sm["modelV2"].getModelV2().getMeta();
auto laneChangeDirection = meta.getLaneChangeDirection();
auto laneChangeState = meta.getLaneChangeState();
float desireStateTurnLeft = meta.getDesireState()[1];
float desireStateTurnRight = meta.getDesireState()[2];
float desireStateLaneChangeLeft = meta.getDesireState()[3];
float desireStateLaneChangeRight = meta.getDesireState()[4];
int desireEvent = 0; // TODO
if (desireStateTurnLeft > 0.5) ui_draw_image(s, { x - icon_size / 2, y - icon_size / 2, icon_size, icon_size }, "ic_turn_l", 1.0f);
else if (desireStateTurnRight > 0.5) ui_draw_image(s, { x - icon_size / 2, y - icon_size / 2, icon_size, icon_size }, "ic_turn_r", 1.0f);
else if (desireStateLaneChangeLeft > 0.5) ui_draw_image(s, { x - icon_size / 2, y - icon_size / 2, icon_size, icon_size }, "ic_lane_change_l", 1.0f);
else if (desireStateLaneChangeRight > 0.5) ui_draw_image(s, { x - icon_size / 2, y - icon_size / 2, icon_size, icon_size }, "ic_lane_change_r", 1.0f);
if (desireEvent == 57) {
ui_draw_image(s, { x - icon_size / 2, y - icon_size / 2, icon_size, icon_size }, "ic_lane_change_steer", 1.0f);
ui_draw_image(s, { x - icon_size / 2, y - icon_size / 2, icon_size, icon_size }, "ic_lane_change_l", 1.0f);
}
else if (desireEvent == 58) {
ui_draw_image(s, { x - icon_size / 2, y - icon_size / 2, icon_size, icon_size }, "ic_lane_change_steer", 1.0f);
ui_draw_image(s, { x - icon_size / 2, y - icon_size / 2, icon_size, icon_size }, "ic_lane_change_r", 1.0f);
}
else if (desireEvent == 71) {
if (laneChangeDirection == cereal::LaneChangeDirection::LEFT) {
ui_draw_image(s, { x - icon_size / 2, y - icon_size / 2, icon_size, icon_size }, "ic_lane_change_inhibit", 1.0f);
ui_draw_image(s, { x - icon_size / 2, y - icon_size / 2, icon_size, icon_size }, "ic_lane_change_l", 1.0f);
}
else if (laneChangeDirection == cereal::LaneChangeDirection::RIGHT) {
ui_draw_image(s, { x - icon_size / 2, y - icon_size / 2, icon_size, icon_size }, "ic_lane_change_inhibit", 1.0f);
ui_draw_image(s, { x - icon_size / 2, y - icon_size / 2, icon_size, icon_size }, "ic_lane_change_r", 1.0f);
}
}
if (laneChangeState == cereal::LaneChangeState::PRE_LANE_CHANGE) {
if (laneChangeDirection == cereal::LaneChangeDirection::LEFT) {
ui_draw_image(s, { x - icon_size / 2, y - icon_size / 2, icon_size, icon_size }, "ic_lane_change_steer", 1.0f);
ui_draw_image(s, { x - icon_size / 2, y - icon_size / 2, icon_size, icon_size }, "ic_lane_change_l", 1.0f);
}
else if (laneChangeDirection == cereal::LaneChangeDirection::RIGHT) {
ui_draw_image(s, { x - icon_size / 2, y - icon_size / 2, icon_size, icon_size }, "ic_lane_change_steer", 1.0f);
ui_draw_image(s, { x - icon_size / 2, y - icon_size / 2, icon_size, icon_size }, "ic_lane_change_r", 1.0f);
}
}
const auto carrot_man = sm["carrotMan"].getCarrotMan();
const auto car_state = sm["carState"].getCarState();
QString atc_type = QString::fromStdString(carrot_man.getAtcType());
bool left_blinker = car_state.getLeftBlinker() || atc_type=="fork left" || atc_type =="turn left" || atc_type == "atc left";
bool right_blinker = car_state.getRightBlinker() || atc_type=="fork right" || atc_type =="turn right" || atc_type == "atc right";
_right_blinker = false;
_left_blinker = false;
if (blinker_timer <= 8) {
if (right_blinker) {
_right_blinker = true;
ui_draw_image(s, { x - icon_size / 2, y - icon_size / 2, icon_size, icon_size }, "ic_blinker_r", 1.0f);
}
if (left_blinker) {
_left_blinker = true;
ui_draw_image(s, { x - icon_size / 2, y - icon_size / 2, icon_size, icon_size }, "ic_blinker_l", 1.0f);
}
}
}
};
class BlindSpotDrawer : ModelDrawer{
protected:
QPolygonF lane_barrier_vertices[2];
protected:
void ui_draw_bsd(const UIState* s, const QPolygonF& vd, NVGcolor* color, bool right) {
int index = vd.length();
float x[4], y[4];
for (int i = 0; i < index / 2 - 2; i += 2) {
if (right) {
x[0] = vd[i + 0].x();
y[0] = vd[i + 0].y();
x[1] = vd[i + 1].x();
y[1] = vd[i + 1].y();
x[2] = vd[index - i - 3].x();
y[2] = vd[index - i - 3].y();
x[3] = vd[index - i - 2].x();
y[3] = vd[index - i - 2].y();
}
else {
x[0] = vd[i + 0].x();
y[0] = vd[i + 0].y();
x[1] = vd[i + 1].x();
y[1] = vd[i + 1].y();
x[2] = vd[index - i - 3].x();
y[2] = vd[index - i - 3].y();
x[3] = vd[index - i - 2].x();
y[3] = vd[index - i - 2].y();
}
ui_draw_line2(s, x, y, 4, color, nullptr, 3.0f);
}
}
bool make_data(const UIState* s) {
SubMaster& sm = *(s->sm);
if (!sm.alive("modelV2")) return false;
const cereal::ModelDataV2::Reader& model = sm["modelV2"].getModelV2();
auto model_position = model.getPosition();
int max_idx_barrier_l = get_path_length_idx(model_position, 40.0);
int max_idx_barrier_r = get_path_length_idx(model_position, 40.0);
update_line_data(s, model_position, 0, 1.2 - 0.05, 1.2 - 0.6, &lane_barrier_vertices[0], max_idx_barrier_l, false, -1.7); // 차선폭을 알면 좋겠지만...
update_line_data(s, model_position, 0, 1.2 - 0.05, 1.2 - 0.6, &lane_barrier_vertices[1], max_idx_barrier_r, false, 1.7);
return true;
}
public:
void draw(const UIState* s) {
if (!make_data(s)) return;
NVGcolor color = nvgRGBA(255, 215, 0, 150);
NVGcolor color2 = nvgRGBA(0, 204, 0, 150);
SubMaster& sm = *(s->sm);
auto car_state = sm["carState"].getCarState();
bool left_blindspot = car_state.getLeftBlindspot();
bool right_blindspot = car_state.getRightBlindspot();
auto lead_left = sm["radarState"].getRadarState().getLeadLeft();
auto lead_right = sm["radarState"].getRadarState().getLeadRight();
auto meta = sm["modelV2"].getModelV2().getMeta();
auto laneChangeState = meta.getLaneChangeState();
auto laneChangeDirection = meta.getLaneChangeDirection();
bool rightLaneChange = (laneChangeState == cereal::LaneChangeState::PRE_LANE_CHANGE) &&
(laneChangeDirection == cereal::LaneChangeDirection::RIGHT);
bool leftLaneChange = (laneChangeState == cereal::LaneChangeState::PRE_LANE_CHANGE) &&
(laneChangeDirection == cereal::LaneChangeDirection::LEFT);
#if 0
left_blindspot = right_blindspot = true;
#endif
if (left_blindspot) {
ui_draw_bsd(s, lane_barrier_vertices[0], &color, false);
}
else if (lead_left.getStatus() && lead_left.getDRel() < car_state.getVEgo() * 3.0 && leftLaneChange) {
ui_draw_bsd(s, lane_barrier_vertices[0], &color2, false);
}
if (right_blindspot) {
ui_draw_bsd(s, lane_barrier_vertices[1], &color, true);
}
else if (lead_right.getStatus() && lead_right.getDRel() < car_state.getVEgo() * 3.0 && rightLaneChange) {
ui_draw_bsd(s, lane_barrier_vertices[1], &color2, true);
}
}
};
class PathDrawer : ModelDrawer {
private:
QPolygonF track_vertices;
int show_path_mode = 13;
int show_path_color = 14;
int show_path_mode_normal = 13;
int show_path_color_normal = 14;
int show_path_mode_lane = 13;
int show_path_color_lane = 14;
int show_path_mode_cruise_off = 13;
int show_path_color_cruise_off = 14;
float show_path_width = 1.0;
Params params;
int params_count = 0;
bool active_lane_line = false;
protected:
void update_line_data2(const UIState* s, const cereal::XYZTData::Reader& line,
float width_apply, float z_off_start, float z_off_end, QPolygonF* pvd, int max_idx, bool allow_invert = true) {
const auto line_x = line.getX(), line_y = line.getY(), line_z = line.getZ();
QPolygonF left_points, right_points;
left_points.reserve(max_idx + 1);
right_points.reserve(max_idx + 1);
for (int i = 0; i <= max_idx; i++) {
// highly negative x positions are drawn above the frame and cause flickering, clip to zy plane of camera
if (line_x[i] < 0) continue;
float z_off = interp<float>((float)line_x[i], { 0.0f, 100.0 }, { z_off_start, z_off_end }, false);
float y_off = interp<float>(z_off, { -3.0f, 0.0f, 3.0f }, { 1.5f, 0.5f, 1.5f }, false);
y_off *= width_apply;
QPointF left, right;
bool l = _model->mapToScreen(line_x[i], line_y[i] - y_off, line_z[i] + z_off, &left);
bool r = _model->mapToScreen(line_x[i], line_y[i] + y_off, line_z[i] + z_off, &right);
if (l && r) {
// For wider lines the drawn polygon will "invert" when going over a hill and cause artifacts
if (!allow_invert && left_points.size() && left.y() > left_points.back().y()) {
continue;
}
left_points.push_back(left);
right_points.push_front(right);
}
}
*pvd = left_points + right_points;
}
void update_line_data_dist(const UIState* s, const cereal::XYZTData::Reader& line,
float width_apply, float z_off_start, float z_off_end, QPolygonF* pvd, float max_dist, bool allow_invert = true) {
const auto line_x = line.getX(), line_y = line.getY(), line_z = line.getZ();
QPolygonF left_points, right_points;
left_points.reserve(40 + 1);
right_points.reserve(40 + 1);
float idxs[line_x.size()], line_xs[line_x.size()], line_ys[line_x.size()], line_zs[line_x.size()];
float x_prev = 0;
for (int i = 0; i < line_x.size(); i++) {
idxs[i] = (float)i;
if (i > 0 && line_x[i] < x_prev) {
//printf("plan data error.\n");
line_xs[i] = x_prev;
}
else line_xs[i] = line_x[i];
x_prev = line_xs[i];
line_ys[i] = line_y[i];
line_zs[i] = line_z[i];
}
float dist = 2.0;// , dist_dt = 1.;
bool exit = false;
//printf("\ndist = ");
for (; !exit; dist = dist + dist * 0.15) {
//dist_dt += (i*0.05);
if (dist >= max_dist) {
dist = max_dist;
exit = true;
}
//printf("%.0f ", dist);
float z_off = interp<float>(dist, { 0.0f, 100.0 }, { z_off_start, z_off_end }, false);
float y_off = interp<float>(z_off, { -3.0f, 0.0f, 3.0f }, { 1.5f, 0.5f, 1.5f }, false);
y_off *= width_apply;
float idx = interp<float>(dist, line_xs, idxs, line_x.size(), false);
if (idx >= line_x.size()) break;
float line_y1 = interp<float>(idx, idxs, line_ys, line_x.size(), false);
float line_z1 = interp<float>(idx, idxs, line_zs, line_x.size(), false);
QPointF left, right;
bool l = _model->mapToScreen(dist, line_y1 - y_off, line_z1 + z_off, &left);
bool r = _model->mapToScreen(dist, line_y1 + y_off, line_z1 + z_off, &right);
if (l && r) {
// For wider lines the drawn polygon will "invert" when going over a hill and cause artifacts
if (!allow_invert && left_points.size() && left.y() > left_points.back().y()) {
//printf("invert...\n");
continue;
}
left_points.push_back(left);
right_points.push_front(right);
}
//else printf("range out..\n");
}
*pvd = left_points + right_points;
}
float dist_function(float t, float max_dist) {
float dist = 3.0 * pow(1.2, t);
return (dist >= max_dist) ? max_dist : dist;
}
float pos_t = 0.0f;
void update_line_data_dist3(const UIState* s, const cereal::XYZTData::Reader& line,
float width_apply, float z_off_start, float z_off_end, QPolygonF* pvd, float max_dist, bool allow_invert = true) {
const auto line_x = line.getX(), line_y = line.getY(), line_z = line.getZ();
QPolygonF left_points, right_points;
left_points.reserve(41);
right_points.reserve(41);
// Prepare line data (correcting x-axis if needed)
std::vector<float> idxs(line_x.size()), line_xs(line_x.size()), line_ys(line_x.size()), line_zs(line_x.size());
float x_prev = 0;
for (int i = 0; i < line_x.size(); i++) {
idxs[i] = static_cast<float>(i);
line_xs[i] = (i > 0 && line_x[i] < x_prev) ? x_prev : line_x[i];
x_prev = line_xs[i];
line_ys[i] = line_y[i];
line_zs[i] = line_z[i];
}
// Get simulation state
SubMaster& sm = *(s->sm);
//auto controls_state = sm["controlsState"].getControlsState();
auto car_state = sm["carState"].getCarState();
float v_ego_kph = car_state.getVEgoCluster() * MS_TO_KPH;
// Calculate time step and position
//float pos_t = 0.0f;
float dt = std::min(v_ego_kph * 0.01f, (show_path_mode >= 10) ? 0.6f : 1.0f);
if (v_ego_kph < 1) pos_t = 4.0f;
else if (dt < 0.2f) dt = 0.2f;
pos_t += dt;
if (pos_t > 24.0f) pos_t = pos_t - 24.0f;
// Add time points based on path mode
std::vector<float> draw_t;
draw_t.reserve(50);
draw_t.push_back(pos_t);
auto add_time_points = [&](int count, float interval) {
for (int i = 0; i < count; i++) {
float t = draw_t.back();
draw_t.push_back((t + interval > 24.0f) ? t + interval - 24.0f : t + interval);
}
};
switch (show_path_mode) {
case 9:
add_time_points(1, 3.0f);
add_time_points(1, 10.0f);
add_time_points(1, 3.0f);
break;
case 10:
add_time_points(7, 3.0f);
break;
case 11:
add_time_points(5, 3.0f);
break;
case 12: {
int n = std::clamp(static_cast<int>(v_ego_kph * 0.058f - 0.5f), 0, 7);
add_time_points(n, 3.0f);
break;
}
}
// Find the smallest time value
auto min_elem = std::min_element(draw_t.begin(), draw_t.end());
int draw_t_idx = std::distance(draw_t.begin(), min_elem);
// Process each time point and calculate points
bool exit = false;
for (int i = 0; i <= draw_t.size() && !exit; i++) {
float t = draw_t[draw_t_idx];
draw_t_idx = (draw_t_idx + 1) % draw_t.size();
if (t < 3.0f) continue;
float dist = dist_function(t, max_dist);
if (dist == max_dist) exit = true;
for (int j = 2; j >= 0; j--) {
dist = exit ? dist_function(100, max_dist) : dist_function(t - j * 1.0f, max_dist);
float z_off = interp<float>(dist, { 0.0f, 100.0f }, { z_off_start, z_off_end }, false);
float y_off = interp<float>(z_off, { -3.0f, 0.0f, 3.0f }, { 1.5f, 0.5f, 1.5f }, false) * width_apply;
float idx = interp<float>(dist, line_xs.data(), idxs.data(), line_x.size(), false);
if (idx >= line_x.size()) {
printf("index... %.1f\n", idx);
break;
}
float line_y1 = interp<float>(idx, idxs.data(), line_ys.data(), line_x.size(), false);
float line_z1 = interp<float>(idx, idxs.data(), line_zs.data(), line_x.size(), false);
QPointF left, right;
bool l = _model->mapToScreen(dist, line_y1 - y_off, line_z1 + z_off, &left);
bool r = _model->mapToScreen(dist, line_y1 + y_off, line_z1 + z_off, &right);
if (l && r) {
left_points.push_back(left);
right_points.push_front(right);
}
if (exit) break;
}
}
// Combine left and right points into the polygon
*pvd = left_points + right_points;
}
protected:
bool make_data(const UIState* s) {
SubMaster& sm = *(s->sm);
if (!sm.alive("modelV2") || !sm.alive("carState")) return false;
const cereal::ModelDataV2::Reader& model = sm["modelV2"].getModelV2();
active_lane_line = sm["controlsState"].getControlsState().getActiveLaneLine();
auto model_position = model.getPosition();
if (active_lane_line) {
model_position = sm["lateralPlan"].getLateralPlan().getPosition();
}
float max_distance = s->max_distance;
max_distance -= 2.0;
int max_idx = 32;// show path test... get_path_length_idx(model_position, max_distance);
auto selfdrive_state = sm["selfdriveState"].getSelfdriveState();
bool longActive = selfdrive_state.getEnabled();
if (longActive == false) {
show_path_mode = show_path_mode_cruise_off;
show_path_color = show_path_color_cruise_off;
}
else {
if (active_lane_line) {
show_path_mode = show_path_mode_lane;
show_path_color = show_path_color_lane;
}
else {
show_path_mode = show_path_mode_normal;
show_path_color = show_path_color_normal;
}
}
if (show_path_mode == 0) {
update_line_data2(s, model_position, show_path_width, 1.22, 1.22, &track_vertices, max_idx);
}
else if (show_path_mode < 9 || show_path_mode == 13 || show_path_mode == 14 || show_path_mode == 15) {
update_line_data_dist(s, model_position, show_path_width, 1.22, 1.22, &track_vertices, max_distance, false);
}
else
update_line_data_dist3(s, model_position, show_path_width, 1.22, 1.22, &track_vertices, max_distance, false);
return true;
}
int use_lane_line_speed_apply = 0;
public:
void draw(const UIState* s, float& pathDrawSeq) {
params_count = (params_count + 1) % 20;
if (params_count == 0) {
show_path_mode_normal = params.getInt("ShowPathMode");
show_path_color_normal = params.getInt("ShowPathColor");
show_path_mode_lane = params.getInt("ShowPathModeLane");
show_path_color_lane = params.getInt("ShowPathColorLane");
show_path_mode_cruise_off = params.getInt("ShowPathModeCruiseOff");
show_path_color_cruise_off = params.getInt("ShowPathColorCruiseOff");
}
if (!make_data(s)) return;
int temp = params.getInt("UseLaneLineSpeedApply");
if (temp != use_lane_line_speed_apply) {
ui_draw_text_a(s, 0, 0, (temp>0)?"LaneMode":"Laneless", 30, COLOR_GREEN, BOLD);
use_lane_line_speed_apply = temp;
}
static bool forward = true;
int alpha = 120;
NVGcolor colors[10] = {
COLOR_RED_ALPHA(alpha), nvgRGBA(255, 153, 0, alpha),
COLOR_YELLOW_ALPHA(alpha), COLOR_GREEN_ALPHA(alpha),
COLOR_BLUE_ALPHA(alpha), nvgRGBA(0, 0, 128, alpha),
nvgRGBA(0x8b, 0, 0xff, alpha), COLOR_OCHRE_ALPHA(alpha),
COLOR_WHITE_ALPHA(alpha), COLOR_BLACK_ALPHA(alpha),
};
SubMaster& sm = *(s->sm);
auto car_state = sm["carState"].getCarState();
bool brake_valid = car_state.getBrakeLights();
if (show_path_mode == 0) {
ui_draw_line(s, track_vertices, &colors[show_path_color % 10], nullptr,
(show_path_color >= 10 || brake_valid) ? 2.0 : 0.0,
brake_valid ? COLOR_RED : COLOR_WHITE);
}
else if (show_path_mode >= 13 && show_path_mode <= 15) {
drawSpecialModes(s, show_path_mode, show_path_color, brake_valid, colors);
}
else if (show_path_mode >= 9) {
drawComplexPath(s, show_path_color, brake_valid, colors);
}
else {
drawAnimatedPath(s, car_state, pathDrawSeq, forward, show_path_mode, show_path_color, brake_valid, colors);
}
}
private:
void drawSpecialModes(const UIState* s, int mode, int color_idx,
bool brake_valid, NVGcolor* colors) {
int track_vertices_len = track_vertices.length();
float xp[3][128], yp[3][128];
float g = 0.05f, gc = 0.4f;
if (mode == 13) {
g = 0.2f;
gc = 0.10f;
}
else if (mode == 14) {
g = 0.45f;
gc = 0.05f;
}
else if (mode == 15) {
gc = g;
}
int glen = track_vertices_len / 2 - 1;
for (int i = 0; i < glen; i++) {
int e = track_vertices_len - i - 1;
int ge = glen * 2 - 1 - i;
float x1 = track_vertices[i].x();
float y1 = track_vertices[i].y();
float x2 = track_vertices[e].x();
float y2 = track_vertices[e].y();
xp[0][i] = x1;
yp[0][i] = y1;
xp[0][ge] = x1 + (x2 - x1) * g;
yp[0][ge] = y1 + (y2 - y1) * g;
xp[1][i] = x1 + (x2 - x1) * (0.5f - gc);
yp[1][i] = y1 + (y2 - y1) * (0.5f - gc);
xp[1][ge] = x1 + (x2 - x1) * (0.5f + gc);
yp[1][ge] = y1 + (y2 - y1) * (0.5f + gc);
xp[2][i] = x1 + (x2 - x1) * (1.0f - g);
yp[2][i] = y1 + (y2 - y1) * (1.0f - g);
xp[2][ge] = x2;
yp[2][ge] = y2;
}
if (mode == 13 || mode == 14) {
ui_draw_line2(s, xp[0], yp[0], glen * 2, &colors[color_idx % 10], nullptr,
(color_idx >= 10 || brake_valid) ? 2.0 : 0.0,
brake_valid ? COLOR_RED : COLOR_WHITE);
}
if (mode == 13 || mode == 15) {
ui_draw_line2(s, xp[1], yp[1], glen * 2, &colors[color_idx % 10], nullptr,
(color_idx >= 10 || brake_valid) ? 2.0 : 0.0,
brake_valid ? COLOR_RED : COLOR_WHITE);
}
if (mode == 13 || mode == 14) {
ui_draw_line2(s, xp[2], yp[2], glen * 2, &colors[color_idx % 10], nullptr,
(color_idx >= 10 || brake_valid) ? 2.0 : 0.0,
brake_valid ? COLOR_RED : COLOR_WHITE);
}
}
void drawComplexPath(const UIState* s, int color_idx, bool brake_valid,
NVGcolor* colors) {
int track_vertices_len = track_vertices.length();
float x[6], y[6];
int color_n = 0;
for (int i = 0; i < track_vertices_len / 2 - 1; i += 3) {
int e = track_vertices_len - i - 1;
x[0] = track_vertices[i].x();
y[0] = track_vertices[i].y();
x[1] = track_vertices[i + 1].x();
y[1] = track_vertices[i + 1].y();
x[2] = (track_vertices[i + 2].x() + track_vertices[e - 2].x()) / 2;
y[2] = (track_vertices[i + 2].y() + track_vertices[e - 2].y()) / 2;
x[3] = track_vertices[e - 1].x();
y[3] = track_vertices[e - 1].y();
x[4] = track_vertices[e].x();
y[4] = track_vertices[e].y();
x[5] = (x[1] + x[3]) / 2;
y[5] = (y[1] + y[3]) / 2;
ui_draw_line2(s, x, y, 6, &colors[color_idx % 10], nullptr,
(color_idx >= 10 || brake_valid) ? 2.0 : 0.0,
brake_valid ? COLOR_RED : COLOR_WHITE);
if (++color_n > 6) color_n = 0;
}
}
void drawAnimatedPath(const UIState* s, const cereal::CarState::Reader& car_state,
float& pathDrawSeq, bool& forward, int mode, int color_idx, bool brake_valid,
NVGcolor* colors) {
int track_vertices_len = track_vertices.length();
float accel = car_state.getAEgo();
float v_ego_kph = car_state.getVEgo() * MS_TO_KPH;
#ifdef __TEST
v_ego_kph = 20.0f;
accel = -1.2f;
#endif
float seq = std::max(0.3f, v_ego_kph / 100.0f);
if (accel < -1.0f) forward = false;
if (accel > -0.5f) forward = true;
int max_seq = std::min(track_vertices_len / 4 + 3, 16);
static int pathDrawSeq2 = -1;
if (forward) {
pathDrawSeq += seq;
if (pathDrawSeq > max_seq) {
pathDrawSeq = (pathDrawSeq2 >= 0) ? pathDrawSeq2 : 0;
}
}
else {
pathDrawSeq -= seq;
if (pathDrawSeq < 0) {
pathDrawSeq = (pathDrawSeq2 >= 0) ? pathDrawSeq2 : max_seq;
}
}
pathDrawSeq2 = (max_seq > 15) ? ((int)pathDrawSeq - max_seq / 2 + max_seq) % max_seq : -5;
//float x[6], y[6];
switch (mode) {
case 1:
case 2:
case 5:
case 6:
drawMode1To6(s, pathDrawSeq, pathDrawSeq2, mode, color_idx, brake_valid, colors);
break;
case 3:
case 4:
case 7:
case 8:
drawMode3To8(s, pathDrawSeq, pathDrawSeq2, mode, color_idx, brake_valid, colors);
break;
}
}
void drawMode1To6(const UIState* s, float pathDrawSeq, int pathDrawSeq2, int mode,
int color_idx, bool brake_valid, NVGcolor* colors) {
int track_vertices_len = track_vertices.length();
float x[4], y[4];
for (int i = 0, color_n = 0; i < track_vertices_len / 2 - 4; i += 2) {
x[0] = track_vertices[i].x();
y[0] = track_vertices[i].y();
x[1] = track_vertices[i + 2].x();
y[1] = track_vertices[i + 2].y();
x[2] = track_vertices[track_vertices_len - i - 3].x();
y[2] = track_vertices[track_vertices_len - i - 3].y();
x[3] = track_vertices[track_vertices_len - i - 1].x();
y[3] = track_vertices[track_vertices_len - i - 1].y();
bool draw = ((int)pathDrawSeq == i / 2 || (int)pathDrawSeq == i / 2 - 2 ||
pathDrawSeq2 == i / 2 || pathDrawSeq2 == i / 2 - 2);
if (track_vertices_len / 2 < 8 || mode == 5 || mode == 6) draw = true;
if (draw) {
int idx = (mode == 2 || mode == 6) ? color_n : color_idx % 10;
ui_draw_line2(s, x, y, 4, &colors[idx], nullptr,
(color_idx >= 10 || brake_valid) ? 2.0 : 0.0,
brake_valid ? COLOR_RED : COLOR_WHITE);
}
if (i > 1) color_n = (color_n + 1) % 7;
}
}
void drawMode3To8(const UIState* s, float pathDrawSeq, int pathDrawSeq2, int mode,
int color_idx, bool brake_valid, NVGcolor* colors) {
int track_vertices_len = track_vertices.length();
float x[6], y[6];
for (int i = 0, color_n = 0; i < track_vertices_len / 2 - 4; i += 2) {
x[0] = track_vertices[i].x();
y[0] = track_vertices[i].y();
x[1] = track_vertices[i + 2].x();
y[1] = track_vertices[i + 2].y();
x[2] = (track_vertices[i + 4].x() + track_vertices[track_vertices_len - i - 5].x()) / 2;
y[2] = (track_vertices[i + 4].y() + track_vertices[track_vertices_len - i - 5].y()) / 2;
x[3] = track_vertices[track_vertices_len - i - 3].x();
y[3] = track_vertices[track_vertices_len - i - 3].y();
x[4] = track_vertices[track_vertices_len - i - 1].x();
y[4] = track_vertices[track_vertices_len - i - 1].y();
x[5] = (x[1] + x[3]) / 2;
y[5] = (y[1] + y[3]) / 2;
bool draw = ((int)pathDrawSeq == i / 2 || (int)pathDrawSeq == i / 2 - 2 ||
pathDrawSeq2 == i / 2 || pathDrawSeq2 == i / 2 - 2);
if (track_vertices_len / 2 < 8 || mode == 7 || mode == 8) draw = true;
if (draw) {
int idx = (mode == 4 || mode == 8) ? color_n : color_idx % 10;
ui_draw_line2(s, x, y, 6, &colors[idx], nullptr,
(color_idx >= 10 || brake_valid) ? 2.0 : 0.0,
brake_valid ? COLOR_RED : COLOR_WHITE);
}
if (i > 1) color_n = (color_n + 1) % 7;
}
}
};
#include <QJsonDocument>
#include <QJsonObject>
#include <QJsonValue>
#include <QJsonArray>
typedef struct {
float x, y, d, v, y_rel, v_lat, radar;
} lead_vertex_data;
char carrot_man_debug[128] = "";
class DrawCarrot : public QObject {
Q_OBJECT
protected:
Params params;
Params params_memory{ "/dev/shm/params" };
int icon_size = 256;
public:
DrawCarrot() {
}
float v_cruise = 0.0;
float v_ego = 0.0;
float apply_speed = 250.0;
QString apply_source = "";
bool latActive = false;
bool longActive = false;
int xState = 0;
int trafficState = 0;
int trafficState_carrot = 0;
int active_carrot = 0;
jLeadfactor.... a_change_cost + long control, disp gps state
2025-04-19 13:45:09 +09:00
float cruiseTarget = 0.0;
v8-wip4
2024-09-03 16:09:00 +09:00
int myDrivingMode = 1;
QString szPosRoadName = "";
int nRoadLimitSpeed = 30;
int nGoPosDist = 0;
int xSpdLimit = 0;
int xSignType = -1;
QPointF nav_path_vertex[150];
QPointF nav_path_vertex_xy[150];
int nav_path_vertex_count = 0;
bool nav_path_display = false;
std::vector<lead_vertex_data> lead_vertices_side;
void updateState(UIState *s) {
const SubMaster& sm = *(s->sm);
const bool cs_alive = sm.alive("carState");
const bool car_state_alive = sm.alive("carState");
const bool car_control_alive = sm.alive("carControl");
const bool carrot_man_alive = sm.alive("carrotMan");
const bool lp_alive = sm.alive("longitudinalPlan");
//const auto cs = sm["controlsState"].getControlsState();
const auto car_state = sm["carState"].getCarState();
const auto car_control = sm["carControl"].getCarControl();
const auto lp = sm["longitudinalPlan"].getLongitudinalPlan();
const auto carrot_man = sm["carrotMan"].getCarrotMan();
const cereal::ModelDataV2::Reader& model = sm["modelV2"].getModelV2();
const auto radar_state = sm["radarState"].getRadarState();
auto lead_one = radar_state.getLeadOne();
auto model_position = model.getPosition();
const auto lane_lines = model.getLaneLines();
nav_path_display = params.getInt("ShowRouteInfo");
if (!cs_alive || !car_control_alive || !car_state_alive || !lp_alive) return;
auto selfdrive_state = sm["selfdriveState"].getSelfdriveState();
longActive = selfdrive_state.getEnabled();
latActive = car_control.getLatActive();
v_cruise = car_state.getVCruiseCluster();
v_ego = car_state.getVEgoCluster();
if (carrot_man_alive) {
active_carrot = carrot_man.getActiveCarrot();
apply_speed = carrot_man.getDesiredSpeed();
apply_source = QString::fromStdString(carrot_man.getDesiredSource());
if (apply_speed >= v_cruise) apply_source = "";
szPosRoadName = QString::fromStdString(carrot_man.getSzPosRoadName());
nRoadLimitSpeed = carrot_man.getNRoadLimitSpeed();
xSpdLimit = carrot_man.getXSpdLimit();
xSignType = carrot_man.getXSpdType();
nGoPosDist = carrot_man.getNGoPosDist();
QString atcType = QString::fromStdString(carrot_man.getAtcType());
trafficState_carrot = carrot_man.getTrafficState();
const auto velocity = model.getVelocity();
if (nav_path_display) {
QString naviPaths = QString::fromStdString(carrot_man.getNaviPaths());
QStringList pairs = naviPaths.split(";");
nav_path_vertex_count = 0;
int max_z = lane_lines[2].getZ().size();
float z_offset = 0.0;
foreach(const QString & pair, pairs) {
QStringList xy = pair.split(","); // ","로 x와 y 구분
if (xy.size() == 3) {
//printf("coords = x: %.1f, y: %.1f, d:%.1f\n", xy[0].toFloat(), xy[1].toFloat(), xy[2].toFloat());
float x = xy[0].toFloat();
float y = xy[1].toFloat();
float d = xy[2].toFloat();
int idx = get_path_length_idx(lane_lines[2], d);
if (idx >= max_z) z_offset -= 0.05;
nav_path_vertex_xy[nav_path_vertex_count] = QPointF(y, -x);
_model->mapToScreen((x < 3.0) ? 5.0 : x, y, lane_lines[2].getZ()[idx] + z_offset, &nav_path_vertex[nav_path_vertex_count++]);
if (nav_path_vertex_count >= 150) break;
}
}
}
auto meta = sm["modelV2"].getModelV2().getMeta();
QString desireLog = QString::fromStdString(meta.getDesireLog());
sprintf(carrot_man_debug, "model_kph= %d, %s, %dkm/h TBT(%d): %dm, CAM(%d): %dkm/h, %dm, ATC(%s), T(%d)",
(int)(velocity.getX()[32] * 3.6),
desireLog.toStdString().c_str(),
carrot_man.getDesiredSpeed(),
carrot_man.getXTurnInfo(),
carrot_man.getXDistToTurn(),
carrot_man.getXSpdType(),
carrot_man.getXSpdLimit(),
carrot_man.getXSpdDist(),
atcType.toStdString().c_str(),
carrot_man.getTrafficState());
}
fix lanemode , roadlimitspeed from car(canfd), (#178) * enable detect redlight during regen decel.. * button mode 2 decel.. * lanemode timeadjust 0.05 -> 0.07 * limit_accel_in_turn: unstable decel during cornering * roadLimitSpeed from Car. * fix roadlimitSpeed display.. * fix lanemode model condition.. * fix.. typo * fix. * test for SK3 * fix.. standstill * Revert "test for SK3" This reverts commit b7ab995c5ef2e482b65704fb18c7bc5a3bcf1171. * fix.. loadLimitSpeed.. * fix roadSpeedLimit(hkg can) * initial "gas" state for bump * fix.. lane mode adjust time.. 5 -> 7 -> 6 * test lanemode (curve -> straight) * fix.. lane * revert * fix. laneline curv -> straight.. * fix.. revert * fix.. lanemode... again.. * canfd 0x1b5
2025-05-29 19:51:47 +09:00
else {
active_carrot = 0;
apply_speed = 250.0;
apply_source = "";
carrot_man_debug[0] = 0;
szPosRoadName = "";
nRoadLimitSpeed = 30;
nGoPosDist = 0;
}
v8-wip4
2024-09-03 16:09:00 +09:00
xState = lp.getXState();
trafficState = lp.getTrafficState();
jLeadfactor.... a_change_cost + long control, disp gps state
2025-04-19 13:45:09 +09:00
cruiseTarget = lp.getCruiseTarget();
v8-wip4
2024-09-03 16:09:00 +09:00
myDrivingMode = lp.getMyDrivingMode();
s->max_distance = std::clamp(*(model_position.getX().end() - 1),
MIN_DRAW_DISTANCE, MAX_DRAW_DISTANCE);
if (lead_one.getStatus()) {
const float lead_d = lead_one.getDRel();
s->max_distance = std::clamp((float)lead_d, 0.0f, s->max_distance);
}
lead_vertices_side.clear();
for (auto const& rs : { radar_state.getLeadsLeft(), radar_state.getLeadsRight(), radar_state.getLeadsCenter() }) {
for (auto const& l : rs) {
lead_vertex_data vd;
QPointF vtmp;
float z = lane_lines[2].getZ()[get_path_length_idx(lane_lines[2], l.getDRel())];
if (_model->mapToScreen(l.getDRel(), -l.getYRel(), z - 0.61, &vtmp)) {
vd.x = vtmp.x();
vd.y = vtmp.y();
//printf("(%.1f,%.1f,%.1f) -> (%.1f, %.1f)\n", l.getDRel(), -l.getYRel(), z, vd.x, vd.y);
vd.d = l.getDRel();
vd.v = l.getVLeadK();
vd.y_rel = l.getDPath();// l.getYRel();
vd.v_lat = l.getVLat();
vd.radar = l.getRadar();
lead_vertices_side.push_back(vd);
}
}
}
fix lanemode , roadlimitspeed from car(canfd), (#178) * enable detect redlight during regen decel.. * button mode 2 decel.. * lanemode timeadjust 0.05 -> 0.07 * limit_accel_in_turn: unstable decel during cornering * roadLimitSpeed from Car. * fix roadlimitSpeed display.. * fix lanemode model condition.. * fix.. typo * fix. * test for SK3 * fix.. standstill * Revert "test for SK3" This reverts commit b7ab995c5ef2e482b65704fb18c7bc5a3bcf1171. * fix.. loadLimitSpeed.. * fix roadSpeedLimit(hkg can) * initial "gas" state for bump * fix.. lane mode adjust time.. 5 -> 7 -> 6 * test lanemode (curve -> straight) * fix.. lane * revert * fix. laneline curv -> straight.. * fix.. revert * fix.. lanemode... again.. * canfd 0x1b5
2025-05-29 19:51:47 +09:00
}
v8-wip4
2024-09-03 16:09:00 +09:00
void drawRadarInfo(UIState* s) {
char str[128];
int show_radar_info = params.getInt("ShowRadarInfo");
if (show_radar_info > 0) {
int wStr = 40;
for (auto const& vrd : lead_vertices_side) {
auto [rx, ry, rd, rv, ry_rel, v_lat, radar] = vrd;
if (rv < -1.0 || rv > 1.0) {
fix buttonMode[0,1], BSD desire, torque always , MPH, etc
2025-05-11 12:45:26 +09:00
sprintf(str, "%.0f", (s->scene.is_metric)? rv * MS_TO_KPH : rv * MS_TO_MPH);
v8-wip4
2024-09-03 16:09:00 +09:00
wStr = 35 * (strlen(str) + 0);
ui_fill_rect(s->vg, { (int)(rx - wStr / 2), (int)(ry - 35), wStr, 42 }, (!radar) ? COLOR_BLUE : (rv > 0.) ? COLOR_GREEN : COLOR_RED, 15);
ui_draw_text(s, rx, ry, str, 40, COLOR_WHITE, BOLD);
if (show_radar_info >= 2) {
sprintf(str, "%.1f", ry_rel);
ui_draw_text(s, rx, ry - 40, str, 30, COLOR_WHITE, BOLD);
//sprintf(str, "%.2f", v_lat);
//ui_draw_text(s, rx, ry + 30, str, 30, COLOR_WHITE, BOLD);
}
}
#if 0
else if (v_lat < -1.0 || v_lat > 1.0) {
sprintf(str, "%.0f", (rv + v_lat) * 3.6);
wStr = 35 * (strlen(str) + 0);
ui_fill_rect(s->vg, { (int)(rx - wStr / 2), (int)(ry - 35), wStr, 42 }, COLOR_ORANGE, 15);
ui_draw_text(s, rx, ry, str, 40, COLOR_WHITE, BOLD);
if (s->show_radar_info >= 2) {
sprintf(str, "%.1f", ry_rel);
ui_draw_text(s, rx, ry - 40, str, 30, COLOR_WHITE, BOLD);
}
}
#endif
else if (show_radar_info >= 3) {
strcpy(str, "*");
ui_draw_text(s, rx, ry, str, 40, COLOR_WHITE, BOLD);
}
}
}
}
void drawDebug(UIState* s) {
if (params.getInt("ShowDebugUI") > 1) {
nvgTextAlign(s->vg, NVG_ALIGN_RIGHT | NVG_ALIGN_BOTTOM);
ui_draw_text(s, s->fb_w, s->fb_h - 10, carrot_man_debug, 35, COLOR_WHITE, BOLD, 1.0f, 1.0f);
}
}
void drawNaviPath(UIState* s) {
if (!nav_path_display) return;
#if 0
if (nav_path_vertex_count > 0) {
nvgBeginPath(s->vg);
nvgMoveTo(s->vg, nav_path_vertex[0].x(), nav_path_vertex[0].y());
for (int i = 1; i < nav_path_vertex_count; i++) {
float x = nav_path_vertex[i].x();
float y = nav_path_vertex[i].y();
if (isnan(x) || isnan(y)) continue;
nvgLineTo(s->vg, x, y);
}
nvgStrokeColor(s->vg, COLOR_GREEN);
nvgStrokeWidth(s->vg, 20.0f);
nvgStroke(s->vg);
}
#elif 1
if (nav_path_vertex_count > 1) {
for(int i = 1; i < nav_path_vertex_count; i++) {
float x = nav_path_vertex[i].x();
float y = nav_path_vertex[i].y();
if(isnan(x) || isnan(y)) continue;
nvgBeginPath(s->vg);
nvgCircle(s->vg, x, y, 10);
nvgFillColor(s->vg, COLOR_GREEN);
nvgFill(s->vg);
}
}
#else
if (nav_path_vertex_count) {
nvgBeginPath(s->vg);
int bx = s->fb_w - 400;
int by = s->fb_h - 400;
float scale = 1.0;
nvgSave(s->vg);
nvgScissor(s->vg, bx - 350, by - 250, 700, 300);
nvgMoveTo(s->vg, bx + nav_path_vertex_xy[0].x() * scale, by + nav_path_vertex_xy[0].y() * scale);
for (int i = 1; i < nav_path_vertex_count; i++) {
float x = nav_path_vertex_xy[i].x();
float y = nav_path_vertex_xy[i].y();
if (isnan(x) || isnan(y)) continue;
nvgLineTo(s->vg, bx + x * scale, by + y * scale);
}
nvgStrokeColor(s->vg, COLOR_GREEN);
nvgStrokeWidth(s->vg, 10.0f);
nvgStroke(s->vg);
nvgRestore(s->vg);
}
#endif
}
char cruise_speed_last[32] = "";
char driving_mode_str_last[32] = "";
int gap_last = 0;
char gear_str_last[32] = "";
int blink_timer = 0;
float cpuTemp = 0.0f;
float cpuUsage = 0.0f;
int memoryUsage = 0;
float freeSpace = 0.0f;
void drawHud(UIState* s) {
int show_device_state = params.getInt("ShowDeviceState");
blink_timer = (blink_timer + 1) % 16;
nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BOTTOM);
int x = 140;// 120;
int y = s->fb_h - 500;// 300;// 410;
int bx = x;
int by = y + 270;
#ifdef __UI_TEST
xSpdLimit = 50;
xSignType = 1;
#endif
bool cam_detected = false;
if (xSpdLimit > 0 && xSignType != 22 && xSignType != 4) cam_detected = true;
NVGcolor stroke_color = COLOR_WHITE;
NVGcolor bg_color = (cam_detected && blink_timer > 8)?COLOR_RED_ALPHA(180):COLOR_BLACK_ALPHA(90);
if (show_device_state > 0) {
ui_fill_rect(s->vg, { bx - 120, by - 270, 475, 495 }, bg_color, 30, 2, &stroke_color);
}
else {
ui_fill_rect(s->vg, { bx - 120, by - 270 + 140, 475, 495 - 140 }, bg_color, 30, 2, &stroke_color);
}
// draw traffic light
int icon_red = icon_size;
int icon_green = icon_size;
bool red_light = trafficState == 1;
bool green_light = trafficState == 2;
if(trafficState_carrot == 1) {
red_light = true;
icon_red *= 1.5;
}
else if(trafficState_carrot == 2) {
green_light = true;
icon_green *= 1.5;
}
if (red_light) ui_draw_image(s, { x - icon_red / 2, y - icon_red / 2 + 270, icon_red, icon_red }, "ic_traffic_red", 1.0f);
else if (green_light) ui_draw_image(s, { x - icon_green / 2, y - icon_green / 2 + 270, icon_green, icon_green }, "ic_traffic_green", 1.0f);
// draw speed
char speed[32];
fix buttonMode[0,1], BSD desire, torque always , MPH, etc
2025-05-11 12:45:26 +09:00
sprintf(speed, "%.0f", (s->scene.is_metric)? v_ego * MS_TO_KPH : v_ego * MS_TO_MPH);
v8-wip4
2024-09-03 16:09:00 +09:00
ui_draw_text(s, bx, by + 50, speed, 120, COLOR_WHITE, BOLD, 3.0f, 8.0f);
ui_draw_image(s, { bx - 100, by - 60, 350, 150 }, "ic_speed_bg", 1.0f);
// draw cruise speed
char cruise_speed[32];
int cruise_x = bx + 170;
int cruise_y = by + 15;
fix buttonMode[0,1], BSD desire, torque always , MPH, etc
2025-05-11 12:45:26 +09:00
if(longActive) sprintf(cruise_speed, "%.0f", (s->scene.is_metric)?v_cruise: v_cruise * KM_TO_MILE);
v8-wip4
2024-09-03 16:09:00 +09:00
else sprintf(cruise_speed, "--");
if (strcmp(cruise_speed_last, cruise_speed) != 0) {
strcpy(cruise_speed_last, cruise_speed);
if(strcmp(cruise_speed, "--"))
ui_draw_text_a(s, cruise_x, cruise_y, cruise_speed, 60, COLOR_GREEN, BOLD);
}
ui_draw_text(s, cruise_x, cruise_y, cruise_speed, 60, COLOR_GREEN, BOLD, 1.0, 5.0, COLOR_BLACK, COLOR_BLACK);
// draw apply speed
NVGcolor textColor = COLOR_GREEN;
NVGcolor white_color = COLOR_WHITE;
char apply_speed_str[32];
int apply_x = bx + 250;
int apply_y = by - 50;
if (apply_source.length()) {
fix MPH & AEB(canfd) (#164)
2025-05-10 16:48:19 +09:00
sprintf(apply_speed_str, "%.0f", (s->scene.is_metric)?apply_speed:apply_speed * KM_TO_MILE);
v8-wip4
2024-09-03 16:09:00 +09:00
textColor = COLOR_OCHRE; // apply speed가 작동되면... 색을 바꾸자.
ui_draw_text(s, apply_x, apply_y, apply_speed_str, 50, textColor, BOLD, 1.0, 5.0, COLOR_BLACK, COLOR_BLACK);
ui_draw_text(s, apply_x, apply_y - 50, apply_source.toStdString().c_str(), 30, textColor, BOLD, 1.0, 5.0, COLOR_BLACK, COLOR_BLACK);
}
jLeadfactor.... a_change_cost + long control, disp gps state
2025-04-19 13:45:09 +09:00
else if(abs(cruiseTarget - v_cruise) > 0.5) {
fix MPH & AEB(canfd) (#164)
2025-05-10 16:48:19 +09:00
sprintf(apply_speed_str, "%.0f", (s->scene.is_metric)?cruiseTarget: cruiseTarget * KM_TO_MILE);
v8-wip4
2024-09-03 16:09:00 +09:00
ui_draw_text(s, apply_x, apply_y, apply_speed_str, 50, textColor, BOLD, 1.0, 5.0, COLOR_BLACK, COLOR_BLACK);
ui_draw_text(s, apply_x, apply_y - 50, "eco", 30, textColor, BOLD, 1.0, 5.0, COLOR_BLACK, COLOR_BLACK);
}
const SubMaster& sm = *(s->sm);
// draw gap info
char driving_mode_str[32] = "연비";
int driving_mode = myDrivingMode;// params.getInt("MyDrivingMode");
NVGcolor mode_color = COLOR_GREEN_ALPHA(210);
NVGcolor text_color = COLOR_WHITE;
switch (driving_mode) {
case 1: strcpy(driving_mode_str, tr("ECO").toStdString().c_str()); mode_color = COLOR_GREEN_ALPHA(210); break;
case 2: strcpy(driving_mode_str, tr("SAFE").toStdString().c_str()); mode_color = COLOR_ORANGE_ALPHA(210); text_color = COLOR_WHITE; break;
case 3: strcpy(driving_mode_str, tr("NORM").toStdString().c_str()); mode_color = COLOR_GREY_ALPHA(210); text_color = COLOR_WHITE; break;
case 4: strcpy(driving_mode_str, tr("FAST").toStdString().c_str()); mode_color = COLOR_RED_ALPHA(210); break;
default: strcpy(driving_mode_str, tr("ERRM").toStdString().c_str()); break;
}
int dx = bx - 50;
int dy = by + 175;
ui_fill_rect(s->vg, { dx - 55, dy - 38, 110, 48 }, mode_color, 15, 2);
ui_draw_text(s, dx, dy, driving_mode_str, 35, text_color, BOLD);
if (strcmp(driving_mode_str, driving_mode_str_last)) ui_draw_text_a(s, dx, dy, driving_mode_str, 30, COLOR_WHITE, BOLD);
strcpy(driving_mode_str_last, driving_mode_str);
auto locationd = sm["liveLocationKalman"].getLiveLocationKalman();
liveLocationKalman, livePose, comfortBrake2.4, korean message (#159) * fix routeinfo... * livePose -> liveLocationKalman * fix.. * for debug.. socketmaster * fix.. SCC track * fix.. carrotman * comfortBrake 2.5 -> 2.4 * fix.. * test gps debug * test gps * revert * test gps msg * test restart * ff * ff * ff * liveDelay(SAD), latSmoothSeconds (#158) * SAD default: 0 (use liveDelay)
2025-05-04 15:56:43 +09:00
bool is_gps_valid = sm.valid("liveLocationKalman") && locationd.getGpsOK();
v8-wip4
2024-09-03 16:09:00 +09:00
if (is_gps_valid) {
ui_draw_text(s, dx, dy - 45, "GPS", 30, COLOR_GREEN, BOLD);
}
char gap_str[32];
int gap = params.getInt("LongitudinalPersonality") + 1;
dx = bx + 220;
dy = by + 77;
sprintf(gap_str, "%d", gap);
ui_draw_text(s, dx, dy, gap_str, 40, COLOR_WHITE, BOLD);
if (gap_last != gap) ui_draw_text_a(s, dx, dy, gap_str, 40, COLOR_WHITE, BOLD);
gap_last = gap;
dx = bx + 300 - 30;
dy = by + 175 + 10;// -38;
//float ddx = 70 / 4.;
float ddy = 80 / 4.;
#ifdef __UI_TEST
gap = 3;
#endif
for (int i = 0; i < gap; i++) {
//ui_fill_rect(s->vg, { (int)(dx + i * ddx), (int)dy, (int)ddx - 2, 48 }, COLOR_GREEN_ALPHA(180), 4, 3);
ui_fill_rect(s->vg, { (int)(dx), (int)(dy - ddy*(i+1) + 2), (int)70, (int)ddy-2}, COLOR_GREEN_ALPHA(210), 4, 3, &white_color);
}
char gear_str[32] = "R";
dx = bx + 305;
dy = by + 60;
//const SubMaster& sm = *(s->sm);
auto carState = sm["carState"].getCarState();
if (carState.getGearShifter() == cereal::CarState::GearShifter::UNKNOWN) strcpy(gear_str, "U");
else if (carState.getGearShifter() == cereal::CarState::GearShifter::PARK) strcpy(gear_str, "P");
else if (carState.getGearShifter() == cereal::CarState::GearShifter::DRIVE) {
if (carState.getGearStep() > 0)
sprintf(gear_str, "%d", carState.getGearStep());
else
strcpy(gear_str, "D");
}
else if(carState.getGearShifter() == cereal::CarState::GearShifter::NEUTRAL) strcpy(gear_str, "N");
else if (carState.getGearShifter() == cereal::CarState::GearShifter::REVERSE) strcpy(gear_str, "R");
else if (carState.getGearShifter() == cereal::CarState::GearShifter::SPORT) strcpy(gear_str, "S");
else if(carState.getGearShifter() == cereal::CarState::GearShifter::LOW) strcpy(gear_str, "L");
else if (carState.getGearShifter() == cereal::CarState::GearShifter::BRAKE) strcpy(gear_str, "B");
else if (carState.getGearShifter() == cereal::CarState::GearShifter::ECO) strcpy(gear_str, "E");
else strcpy(gear_str, "M");
ui_fill_rect(s->vg, { dx - 35, dy - 70, 70, 80 }, COLOR_GREEN_ALPHA(210), 15, 3, &white_color);
ui_draw_text(s, dx, dy, gear_str, 70, COLOR_WHITE, BOLD);
if (strcmp(gear_str, gear_str_last)) {
ui_draw_text_a(s, dx, dy, gear_str, 70, COLOR_WHITE, BOLD);
strcpy(gear_str_last, gear_str);
}
dx = bx + 200;
dy = by + 175;
#ifdef __UI_TEST
active_carrot = 2;
#endif
if (active_carrot >= 2) {
ui_fill_rect(s->vg, { dx - 55, dy - 38, 110, 48 }, COLOR_GREEN, 15, 2);
ui_draw_text(s, dx, dy, "APN", 40, COLOR_WHITE, BOLD);
}
else if (active_carrot >= 1) {
ui_fill_rect(s->vg, { dx - 55, dy - 38, 110, 48 }, COLOR_BLUE_ALPHA(210), 15, 2);
ui_draw_text(s, dx, dy, "APM", 40, COLOR_WHITE, BOLD);
}
if (nav_path_vertex_count > 1) {
ui_draw_text(s, dx, dy - 45, "ROUTE", 30, COLOR_WHITE, BOLD);
}
#ifdef __UI_TEST
active_carrot = 2;
nRoadLimitSpeed = 30;
xSpdLimit = 50;
xSignType = 1;
#endif
fix lanemode , roadlimitspeed from car(canfd), (#178) * enable detect redlight during regen decel.. * button mode 2 decel.. * lanemode timeadjust 0.05 -> 0.07 * limit_accel_in_turn: unstable decel during cornering * roadLimitSpeed from Car. * fix roadlimitSpeed display.. * fix lanemode model condition.. * fix.. typo * fix. * test for SK3 * fix.. standstill * Revert "test for SK3" This reverts commit b7ab995c5ef2e482b65704fb18c7bc5a3bcf1171. * fix.. loadLimitSpeed.. * fix roadSpeedLimit(hkg can) * initial "gas" state for bump * fix.. lane mode adjust time.. 5 -> 7 -> 6 * test lanemode (curve -> straight) * fix.. lane * revert * fix. laneline curv -> straight.. * fix.. revert * fix.. lanemode... again.. * canfd 0x1b5
2025-05-29 19:51:47 +09:00
//if (active_carrot >= 2 || nGoPosDist > 0) {
if (true) {
v8-wip4
2024-09-03 16:09:00 +09:00
dx = bx + 75;
dy = by + 175;
int disp_speed = 0;
NVGcolor limit_color = COLOR_GREEN_ALPHA(210);
if (xSpdLimit > 0 && xSignType != 22) {
disp_speed = xSpdLimit;
limit_color = (blink_timer <= 8) ? COLOR_RED_ALPHA(210) : COLOR_YELLOW_ALPHA(210);
ui_draw_text(s, dx, dy-45, "CAM", 30, COLOR_WHITE, BOLD);
}
else {
disp_speed = nRoadLimitSpeed;
limit_color = (v_ego * 3.6 > nRoadLimitSpeed + 2) ? COLOR_RED_ALPHA(210) : COLOR_WHITE_ALPHA(210);
ui_draw_text(s, dx, dy - 45, "LIMIT", 30, COLOR_WHITE, BOLD);
}
ui_fill_rect(s->vg, { dx - 55, dy - 38, 110, 48 }, limit_color, 15, 2);
ui_draw_text(s, dx, dy, QString::number(disp_speed).toStdString().c_str(), 40, COLOR_WHITE, BOLD);
}
if (show_device_state) {
char str[128];
dx = bx - 35;
dy = by - 200;
mode_color = COLOR_GREEN_ALPHA(190);
ui_fill_rect(s->vg, { dx - 65, dy - 38, 130, 90 }, (cpuTemp>80 && blink_timer<=8)?COLOR_RED : mode_color, 15, 2);
ui_draw_text(s, dx, dy-5, "CPU", 25, COLOR_WHITE, BOLD);
sprintf(str, "%.0f\u00B0C", cpuTemp);
ui_draw_text(s, dx, dy + 40, str, 40, COLOR_WHITE, BOLD);
dx += 150;
ui_fill_rect(s->vg, { dx - 65, dy - 38, 130, 90 }, (memoryUsage > 85 && blink_timer <= 8) ? COLOR_RED : mode_color, 15, 2);
ui_draw_text(s, dx, dy-5, "MEM", 25, COLOR_WHITE, BOLD);
sprintf(str, "%d%%", memoryUsage);
ui_draw_text(s, dx, dy + 40, str, 40, COLOR_WHITE, BOLD);
dx += 150;
ui_fill_rect(s->vg, { dx - 65, dy - 38, 130, 90 }, mode_color, 15, 2);
ui_draw_text(s, dx, dy-5, "DISK", 25, COLOR_WHITE, BOLD);
sprintf(str, "%.0f%%", 100 - freeSpace);
ui_draw_text(s, dx, dy + 40, str, 40, COLOR_WHITE, BOLD);
}
}
void drawDateTime(const UIState* s) {
char str[128];
// 시간표시
int show_datetime = params.getInt("ShowDateTime");
if (show_datetime) {
time_t now = time(nullptr);
struct tm* local = localtime(&now);
int x = 170;// s->fb_w - 300;
int y = 120;// 150;
int nav_y = y + 50;
nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BOTTOM);
if (show_datetime == 1 || show_datetime == 2) {
strftime(str, sizeof(str), "%H:%M", local);
ui_draw_text(s, x, y, str, 100, COLOR_WHITE, BOLD, 3.0f, 8.0f);
}
if (show_datetime == 1 || show_datetime == 3) {
//strftime(str, sizeof(str), "%m-%d-%a", local);
const char* weekdays_ko[] = { "일", "월", "화", "수", "목", "금", "토" };
strftime(str, sizeof(str), "%m-%d", local); // 날짜만 가져옴
int weekday_index = local->tm_wday; // tm_wday: 0=일, 1=월, ..., 6=토
snprintf(str + strlen(str), sizeof(str) - strlen(str), "(%s)", weekdays_ko[weekday_index]);
ui_draw_text(s, x, y + 70, str, 60, COLOR_WHITE, BOLD, 3.0f, 8.0f);
nav_y += 70;
}
if (false && szPosRoadName.size() > 0) {
nvgTextAlign(s->vg, NVG_ALIGN_RIGHT | NVG_ALIGN_BOTTOM);
ui_draw_text(s, x, nav_y, szPosRoadName.toStdString().c_str(), 35, COLOR_WHITE, BOLD, 3.0f, 8.0f);
}
}
}
void drawConnInfo(const UIState* s) {
int y = 10;
int x = 30;
if (false) {
ui_draw_image(s, { x, y, 120, 54 }, "ic_scc2", 1.0f);
x += 135;
}
if (active_carrot >= 2) {
ui_draw_image(s, { x, y, 120, 54 }, "ic_apn", 1.0f);
x += 135;
}
//else if (hda_speedLimit > 0 && hda_speedLimitDistance > 0) {
// ui_draw_image(s, { x, y, 120, 54 }, "ic_hda", 1.0f);
// x += 135;
//}
else if (active_carrot >= 1) {
ui_draw_image(s, { x, y, 120, 54 }, "ic_apm", 1.0f);
x += 135;
}
else if (false) {
ui_draw_image(s, { x, y, 120, 54 }, "ic_hda", 1.0f);
x += 135;
}
if (false) {
ui_draw_image(s, { x, y, 240, 54 }, "ic_radartracks", 1.0f);
x += (120 + 135);
}
}
NVGcolor get_tpms_color(float tpms) {
if (tpms < 5 || tpms > 60) // N/A
return nvgRGBA(255, 255, 255, 220);
if (tpms < 31)
return nvgRGBA(255, 90, 90, 220);
return nvgRGBA(255, 255, 255, 220);
}
const char* get_tpms_text(float tpms) {
if (tpms < 5 || tpms > 60) return " -";
static char str[32];
snprintf(str, sizeof(str), "%.0f", round(tpms));
return str;
}
void drawTpms(const UIState* s) {
nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BOTTOM);
SubMaster& sm = *(s->sm);
auto car_state = sm["carState"].getCarState();
int bx = (192 - 24) / 2 + UI_BORDER_SIZE + 10;
int by = s->fb_h - 280 / 2 + 10;
auto tpms = car_state.getTpms();
float fl = tpms.getFl();
float fr = tpms.getFr();
float rl = tpms.getRl();
float rr = tpms.getRr();
ui_draw_text(s, bx - 90, by - 55, get_tpms_text(fl), 38, get_tpms_color(fl), BOLD);
ui_draw_text(s, bx + 90, by - 55, get_tpms_text(fr), 38, get_tpms_color(fr), BOLD);
ui_draw_text(s, bx - 90, by + 80, get_tpms_text(rl), 38, get_tpms_color(rl), BOLD);
ui_draw_text(s, bx + 90, by + 80, get_tpms_text(rr), 38, get_tpms_color(rr), BOLD);
}
void drawTpms2(const UIState* s) {
nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BOTTOM);
SubMaster& sm = *(s->sm);
auto car_state = sm["carState"].getCarState();
int bx = s->fb_w - 125;
int by = 130;
auto tpms = car_state.getTpms();
float fl = tpms.getFl();
float fr = tpms.getFr();
float rl = tpms.getRl();
float rr = tpms.getRr();
#ifdef __UI_TEST
fl = fr = rl = rr = 29;
#endif
int dw = 80;
ui_draw_text(s, bx - dw, by - 55, get_tpms_text(fl), 40, get_tpms_color(fl), BOLD);
ui_draw_text(s, bx + dw, by - 55, get_tpms_text(fr), 40, get_tpms_color(fr), BOLD);
ui_draw_text(s, bx - dw, by + 70, get_tpms_text(rl), 40, get_tpms_color(rl), BOLD);
ui_draw_text(s, bx + dw, by + 70, get_tpms_text(rr), 40, get_tpms_color(rr), BOLD);
}
void makeDeviceInfo(const UIState* s) {
SubMaster& sm = *(s->sm);
auto deviceState = sm["deviceState"].getDeviceState();
freeSpace = deviceState.getFreeSpacePercent();
memoryUsage = deviceState.getMemoryUsagePercent();
const auto cpuTempC = deviceState.getCpuTempC();
const auto cpuUsagePercent = deviceState.getCpuUsagePercent();
int size = sizeof(cpuTempC) / sizeof(cpuTempC[0]);
if (size > 0) {
for (int i = 0; i < size; i++) {
cpuTemp += cpuTempC[i];
}
cpuTemp /= static_cast<float>(size);
}
size = sizeof(cpuUsagePercent) / sizeof(cpuUsagePercent[0]);
if (size > 0) {
int cpu_size = 0;
for (cpu_size = 0; cpu_size < size; cpu_size++) {
if (cpuUsagePercent[cpu_size] <= 0) break;
cpuUsage += cpuUsagePercent[cpu_size];
}
if (cpu_size > 0) cpuUsage /= cpu_size;
}
}
void drawDeviceInfo(const UIState* s) {
#ifdef WSL2
return;
#endif
makeDeviceInfo(s);
if (params.getInt("ShowDebugUI") == 0) return;
nvgTextAlign(s->vg, NVG_ALIGN_RIGHT | NVG_ALIGN_TOP);
QString str = "";
str.sprintf("MEM:%d%% DISK:%.0f%% CPU:%.0f%%,%.0f\u00B0C", memoryUsage, freeSpace, cpuUsage, cpuTemp);
NVGcolor top_right_color = (cpuTemp > 85.0 || memoryUsage > 85.0) ? COLOR_ORANGE : COLOR_WHITE;
ui_draw_text(s, s->fb_w - 10, 2, str.toStdString().c_str(), 30, top_right_color, BOLD, 3.0f, 1.0f);
}
};
#if 0
#include "msgq/visionipc/visionipc_server.h"
#include <QImage>
#include <QPainter>
#include <QPainterPath> // 추가
class MapRenderer {
public:
MapRenderer() : image_data(WIDTH* HEIGHT * 4) { initialize(); }
void initialize() {
const int NUM_VIPC_BUFFERS = 4;
vipc_server = std::make_unique<VisionIpcServer>("navd");
vipc_server->create_buffers(VisionStreamType::VISION_STREAM_MAP, NUM_VIPC_BUFFERS, WIDTH, HEIGHT);
vipc_server->start_listener();
}
void render(QPointF nav_path_vertex_xy[], int vertex_count) {
// QImage 생성 및 초기화
QImage image(WIDTH, HEIGHT, QImage::Format_RGBA8888); // RGBA 형식
image.fill(Qt::black); // 검정 배경
QPainter painter(&image);
painter.setPen(QPen(Qt::white, 15)); // 경로 색상 및 두께 설정
painter.setRenderHint(QPainter::Antialiasing, true);
// 경로 그리기
if (vertex_count > 2) {
QPainterPath path;
path.moveTo(nav_path_vertex_xy[0].x() * pixels_per_meter + WIDTH / 2.0,
nav_path_vertex_xy[0].y() * pixels_per_meter + HEIGHT / 2.0);
for (int i = 1; i < vertex_count; ++i) {
path.lineTo(nav_path_vertex_xy[i].x() * pixels_per_meter + WIDTH / 2.0,
nav_path_vertex_xy[i].y() * pixels_per_meter + HEIGHT / 2.0);
}
painter.drawPath(path);
}
painter.end();
// QImage 데이터를 NanoVG에 전달
memcpy(image_data.data(), image.bits(), WIDTH * HEIGHT * 4); // RGBA 크기
}
bool publish() {
double cur_t = millis_since_boot();
if (cur_t > vipc_sent_t + 500) {
vipc_sent_t = cur_t;
// Vision IPC 버퍼에 데이터 전송
VisionBuf* buf = vipc_server->get_buffer(VisionStreamType::VISION_STREAM_MAP);
uint8_t* dst = (uint8_t*)buf->addr;
memset(dst, 128, buf->len); // 초기화
for (int i = 0; i < WIDTH * HEIGHT; i++) {
dst[i] = image_data[i * 4]; // R 값을 사용하여 grayscale 변환
}
VisionIpcBufExtra extra = {
.frame_id = frame_id,
.timestamp_sof = nanos_since_boot(),
.timestamp_eof = nanos_since_boot(),
.valid = true
};
vipc_server->send(buf, &extra);
frame_id++;
return true;
}
return false;
}
void test_draw(NVGcontext* vg) {
static int tex_id = -1; // 텍스처 ID 저장
if (tex_id == -1) {
tex_id = nvgCreateImageRGBA(vg, WIDTH, HEIGHT, 0, image_data.data());
}
else {
// 텍스처 업데이트
nvgUpdateImage(vg, tex_id, image_data.data());
}
// 텍스처를 화면에 그리기
float x_offset = 200;
float y_offset = 200;
NVGpaint img_paint = nvgImagePattern(vg, x_offset, y_offset, WIDTH, HEIGHT, 0, tex_id, 1.0f);
nvgBeginPath(vg);
nvgRect(vg, x_offset, y_offset, WIDTH, HEIGHT); // 텍스처를 그릴 영역
nvgFillPaint(vg, img_paint);
nvgFill(vg);
}
private:
std::unique_ptr<VisionIpcServer> vipc_server;
uint32_t frame_id = 0;
double vipc_sent_t = 0.0;
float pixels_per_meter = 0.5f; // 픽셀 단위 조정
const int HEIGHT = 256, WIDTH = 256;
std::vector<unsigned char> image_data;
};
#endif
DrawPlot drawPlot;
DrawCarrot drawCarrot;
BlindSpotDrawer drawBlindSpot;
PathDrawer drawPath;
LaneLineDrawer drawLaneLine;
PathEndDrawer drawPathEnd;
DesireDrawer drawDesire;
TurnInfoDrawer drawTurnInfo;
OnroadAlerts::Alert alert;
NVGcolor alert_color;
//MapRenderer mapRenderer;
void ui_draw(UIState *s, ModelRenderer* model_renderer, int w, int h) {
_model = model_renderer;
if (s->fb_w != w || s->fb_h != h) {
ui_resize(s, w, h);
}
glEnable(GL_BLEND);
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
nvgBeginFrame(s->vg, s->fb_w, s->fb_h, 1.0f);
nvgScissor(s->vg, 0, 0, s->fb_w, s->fb_h);
//nvgFontSize(s->vg, 170);
//nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_MIDDLE);
//ui_draw_text(s, 500, 500, "Carrot", 100, COLOR_GREEN, BOLD);
Params params;
drawCarrot.updateState(s);
drawCarrot.drawNaviPath(s);
static float pathDrawSeq = 0.0;
int show_lane_info = params.getInt("ShowLaneInfo");
if(show_lane_info >= 0) drawPath.draw(s, pathDrawSeq);
drawLaneLine.draw(s, show_lane_info);
if (params.getInt("ShowPathEnd") > 0) drawPathEnd.draw(s);
int path_x = drawPathEnd.getPathX();
int path_y = drawPathEnd.getPathY();
drawDesire.draw(s, path_x, path_y - 135);
drawPlot.draw(s);
drawBlindSpot.draw(s);
drawCarrot.drawRadarInfo(s);
drawCarrot.drawHud(s);
drawCarrot.drawDebug(s);
drawCarrot.drawDateTime(s);
//drawCarrot.drawConnInfo(s);
drawCarrot.drawDeviceInfo(s);
drawCarrot.drawTpms2(s);
drawTurnInfo.draw(s);
ui_draw_text_a2(s);
ui_draw_alert(s);
#if 0
if (drawCarrot.nav_path_vertex_count > 1) {
mapRenderer.render(drawCarrot.nav_path_vertex_xy, drawCarrot.nav_path_vertex_count);
mapRenderer.publish();
mapRenderer.test_draw(s->vg);
}
#endif
nvgResetScissor(s->vg);
nvgEndFrame(s->vg);
glDisable(GL_BLEND);
}
class BorderDrawer {
protected:
float a_ego_width = 0.0;
float steering_angle_pos = 0.0;
NVGcolor get_tpms_color(float tpms) {
if (tpms < 5 || tpms > 60) // N/A
return COLOR_GREEN;
if (tpms < 31)
return COLOR_RED;
return COLOR_GREEN;
}
const char* get_tpms_text(float tpms) {
if (tpms < 5 || tpms > 60) return "-";
static char str[32];
snprintf(str, sizeof(str), "%.0f", round(tpms));
return str;
}
public:
void drawTpms(UIState* s, int w, int h) {
NVGcontext* vg = s->vg_border;
nvgTextAlign(vg, NVG_ALIGN_CENTER | NVG_ALIGN_BOTTOM);
SubMaster& sm = *(s->sm);
if (!sm.alive("carState")) return;
auto car_state = sm["carState"].getCarState();
auto tpms = car_state.getTpms();
float fl = tpms.getFl();
float fr = tpms.getFr();
float rl = tpms.getRl();
float rr = tpms.getRr();
//fl = fr = rl = rr = 29;
//fl = fr = 40;
ui_draw_text_vg(vg, 25, 30, get_tpms_text(fl), 30, get_tpms_color(fl), BOLD, 3);
ui_draw_text_vg(vg, w - 25, 30, get_tpms_text(fr), 30, get_tpms_color(fr), BOLD, 3);
ui_draw_text_vg(vg, 25, h, get_tpms_text(rl), 30, get_tpms_color(rl), BOLD, 3);
ui_draw_text_vg(vg, w - 25, h, get_tpms_text(rr), 30, get_tpms_color(rr), BOLD, 3);
}
void draw(UIState *s, int w, int h, NVGcolor bg, NVGcolor bg_long) {
NVGcontext* vg = s->vg_border;
ui_fill_rect(vg, { 0,0, w, h / 2 - 100}, bg, 15);
ui_fill_rect(vg, { 0, h / 2 + 100, w, h }, bg_long, 15);
ui_fill_rect(vg, {w - 50, h/2 - 95, 50, 190}, (_right_blinker)?COLOR_ORANGE:COLOR_BLACK, 15);
ui_fill_rect(vg, {0, h/2 - 95, 50, 190}, (_left_blinker)?COLOR_ORANGE:COLOR_BLACK, 15);
const SubMaster& sm = *(s->sm);
auto car_state = sm["carState"].getCarState();
float a_ego = car_state.getAEgo();
a_ego_width = a_ego_width * 0.5 + (w * std::abs(a_ego) / 4.0) * 0.5;
ui_fill_rect(vg, { w/2 - (int)(a_ego_width / 2), h - 30, (int)a_ego_width, 30 }, (a_ego >= 0)? COLOR_YELLOW : COLOR_RED, 15);
steering_angle_pos = steering_angle_pos * 0.5 + (w / 2. - w / 2. * car_state.getSteeringAngleDeg() / 90) * 0.5;
int x_st = (int)steering_angle_pos - 50;
int x_ed = (int)steering_angle_pos + 50;
if (x_st < 0) x_st = 0;
if (x_ed < 50) x_ed = 50;
if (x_st > w - 50) x_st = w - 50;
if (x_ed > w) x_ed = w;
ui_fill_rect(vg, { x_st, 0, x_ed - x_st, 30 }, COLOR_ORANGE, 15);
char top[256] = "", top_left[256] = "", top_right[256] = "";
char bottom[256] = "", bottom_left[256] = "", bottom_right[256] = "";
Params params = Params();
QString str;
// top
str = QString::fromStdString(car_state.getLogCarrot());
sprintf(top, "%s", str.toStdString().c_str());
// top_right
liveLocationKalman, livePose, comfortBrake2.4, korean message (#159) * fix routeinfo... * livePose -> liveLocationKalman * fix.. * for debug.. socketmaster * fix.. SCC track * fix.. carrotman * comfortBrake 2.5 -> 2.4 * fix.. * test gps debug * test gps * revert * test gps msg * test restart * ff * ff * ff * liveDelay(SAD), latSmoothSeconds (#158) * SAD default: 0 (use liveDelay)
2025-05-04 15:56:43 +09:00
const auto live_delay = sm["liveDelay"].getLiveDelay();
v8-wip4
2024-09-03 16:09:00 +09:00
const auto live_torque_params = sm["liveTorqueParameters"].getLiveTorqueParameters();
const auto live_params = sm["liveParameters"].getLiveParameters();
liveLocationKalman, livePose, comfortBrake2.4, korean message (#159) * fix routeinfo... * livePose -> liveLocationKalman * fix.. * for debug.. socketmaster * fix.. SCC track * fix.. carrotman * comfortBrake 2.5 -> 2.4 * fix.. * test gps debug * test gps * revert * test gps msg * test restart * ff * ff * ff * liveDelay(SAD), latSmoothSeconds (#158) * SAD default: 0 (use liveDelay)
2025-05-04 15:56:43 +09:00
str.sprintf("LD[%d,%.2f],LT[%.0f,%s](%.2f/%.2f), SR(%.1f,%.1f)",
live_delay.getValidBlocks(), live_delay.getLateralDelay(),
v8-wip4
2024-09-03 16:09:00 +09:00
live_torque_params.getTotalBucketPoints(), live_torque_params.getLiveValid() ? "ON" : "OFF",
live_torque_params.getLatAccelFactorFiltered(), live_torque_params.getFrictionCoefficientFiltered(),
live_params.getSteerRatio(), params.getFloat("CustomSR")/10.0);
sprintf(top_right, "%s", str.toStdString().c_str());
//top_left
QString carName = QString::fromStdString(params.get("CarName"));
bool longitudinal_control = sm["carParams"].getCarParams().getOpenpilotLongitudinalControl();
if (params.getInt("HyundaiCameraSCC") > 0) {
carName += "(CAMERA SCC)";
}
else if (longitudinal_control) {
carName += " - OP Long";
}
sprintf(top_left, "%s", carName.toStdString().c_str());
// bottom
const auto lat_plan = sm["lateralPlan"].getLateralPlan();
str = lat_plan.getLatDebugText().cStr();
strcpy(bottom, str.toStdString().c_str());
// bottom_left
QString gitBranch = QString::fromStdString(params.get("GitBranch"));
sprintf(bottom_left, "%s", gitBranch.toStdString().c_str());
// bottom_right
Params params_memory = Params("/dev/shm/params");
if (false && carrot_man_debug[0] != 0 && params.getInt("ShowDebugUI") > 0) {
strcpy(bottom_right, carrot_man_debug);
}
else {
QString ipAddress = QString::fromStdString(params_memory.get("NetworkAddress"));
//extern QString gitBranch;
sprintf(bottom_right, "%s", ipAddress.toStdString().c_str());
}
int text_margin = 30;
// top
nvgTextAlign(vg, NVG_ALIGN_CENTER | NVG_ALIGN_TOP);
ui_draw_text_vg(vg, w / 2, 0, top, 30, COLOR_WHITE, BOLD);
// top left
nvgTextAlign(vg, NVG_ALIGN_LEFT | NVG_ALIGN_TOP);
ui_draw_text_vg(vg, text_margin, 0, top_left, 30, COLOR_WHITE, BOLD);
// top right
nvgTextAlign(vg, NVG_ALIGN_RIGHT | NVG_ALIGN_TOP);
ui_draw_text_vg(vg, w - text_margin, 0, top_right, 30, COLOR_WHITE, BOLD);
// bottom
nvgTextAlign(vg, NVG_ALIGN_CENTER | NVG_ALIGN_BOTTOM);
ui_draw_text_vg(vg, w / 2, h, bottom, 30, COLOR_WHITE, BOLD);
// bottom left
nvgTextAlign(vg, NVG_ALIGN_LEFT | NVG_ALIGN_BOTTOM);
ui_draw_text_vg(vg, text_margin, h, bottom_left, 30, COLOR_WHITE, BOLD);
// bottom right
nvgTextAlign(vg, NVG_ALIGN_RIGHT | NVG_ALIGN_BOTTOM);
ui_draw_text_vg(vg, w- text_margin, h, bottom_right, 30, COLOR_WHITE, BOLD);
//drawTpms(s, w, h);
}
};
NVGcolor QColorToNVGcolor(const QColor& color) {
return nvgRGBAf(color.redF(), color.greenF(), color.blueF(), color.alphaF());
}
BorderDrawer borderDrawer;
void ui_draw_border(UIState* s, int w, int h, QColor bg, QColor bg_long) {
if (s->vg_border == nullptr) {
s->vg_border = nvgCreate(NVG_ANTIALIAS | NVG_STENCIL_STROKES | NVG_DEBUG);
std::pair<const char*, const char*> fonts[] = {
{"KaiGenGothicKR-Bold", "../assets/addon/font/KaiGenGothicKR-Bold.ttf"},
};
for (auto [name, file] : fonts) {
int font_id = nvgCreateFont(s->vg_border, name, file);
assert(font_id >= 0);
}
}
glEnable(GL_BLEND);
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
glViewport(0, 0, w, h);
nvgBeginFrame(s->vg_border, w, h, 1.0f);
nvgScissor(s->vg_border, 0, 0, w, h);
borderDrawer.draw(s, w, h, QColorToNVGcolor(bg), QColorToNVGcolor(bg_long));
nvgResetScissor(s->vg_border);
nvgEndFrame(s->vg_border);
glDisable(GL_BLEND);
}
void ui_draw_alert(UIState* s) {
if (alert.size != cereal::SelfdriveState::AlertSize::NONE) {
alert_color = COLOR_ORANGE;
nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BOTTOM);
ui_draw_text(s, s->fb_w / 2, s->fb_h - 300, alert.text1.toStdString().c_str(), 100, alert_color, BOLD, 3.0f, 8.0f);
ui_draw_text(s, s->fb_w / 2, s->fb_h - 200, alert.text2.toStdString().c_str(), 70, alert_color, BOLD, 3.0f, 8.0f);
}
}
void ui_update_alert(const OnroadAlerts::Alert& a) {
//alert_color = nvgRGBA(color.red(), color.green(), color.blue(), color.alpha());
//printf("r=%d, g=%d, b=%d\n", color.red(), color.green(), color.blue());
alert = a;
}
void ui_nvg_init(UIState *s) {
// on EON, we enable MSAA
//s->vg = Hardware::EON() ? nvgCreate(0) : nvgCreate(NVG_ANTIALIAS | NVG_STENCIL_STROKES | NVG_DEBUG);
s->vg = nvgCreate(NVG_ANTIALIAS | NVG_STENCIL_STROKES | NVG_DEBUG);
assert(s->vg);
// init fonts
std::pair<const char *, const char *> fonts[] = {
//{"sans-regular", "../assets/fonts/opensans_regular.ttf"},
//{"sans-semibold", "../assets/fonts/opensans_semibold.ttf"},
//{"sans-bold", "../assets/fonts/opensans_bold.ttf"},
//{"KaiGenGothicKR-Normal", "../assets/addon/font/KaiGenGothicKR-Normal.ttf"},
//{"KaiGenGothicKR-Medium", "../assets/addon/font/KaiGenGothicKR-Medium.ttf"},
{"KaiGenGothicKR-Bold", "../assets/addon/font/KaiGenGothicKR-Bold.ttf"},
//{"Inter-Bold", "../assets/fonts/Inter-Bold.ttf"},
};
for (auto [name, file] : fonts) {
int font_id = nvgCreateFont(s->vg, name, file);
assert(font_id >= 0);
}
// init images
std::vector<std::pair<const char *, const char *>> images = {
{"ic_lane_change_l", "../assets/images/lane_change_l.png"},
{"ic_lane_change_r", "../assets/images/lane_change_r.png"},
{"ic_lane_change_inhibit", "../assets/images/lane_change_inhibit.png"},
{"ic_lane_change_steer", "../assets/images/lane_change_steer.png"},
{"ic_bsd_l", "../assets/images/bsd_l.png"},
{"ic_bsd_r", "../assets/images/bsd_r.png"},
{"ic_turn_l", "../assets/images/turn_l.png"},
{"ic_turn_r", "../assets/images/turn_r.png"},
{"ic_turn_u", "../assets/images/turn_u.png"},
{"ic_blinker_l", "../assets/images/blink_l.png"},
{"ic_blinker_r", "../assets/images/blink_r.png"},
{"ic_speed_bg", "../assets/images/speed_bg.png"},
{"ic_traffic_green", "../assets/images/traffic_green.png"},
{"ic_traffic_red", "../assets/images/traffic_red.png"},
{"ic_tire", "../assets/images/img_tire.png"},
{"ic_road_speed", "../assets/images/road_speed.png"},
{"ic_speed_bump", "../assets/images/speed_bump.png"},
{"ic_nda", "../assets/images/img_nda.png"},
{"ic_navi","../assets/images/img_navi.png"},
{"ic_scc2", "../assets/images/img_scc2.png"},
{"ic_radartracks", "../assets/images/img_radartracks.png"},
{"ic_apm", "../assets/images/img_apm.png"},
{"ic_apn", "../assets/images/img_apn.png"},
{"ic_hda", "../assets/images/img_hda.png"},
{"ic_navi_point", "../assets/images/navi_point.png"}
};
for (auto [name, file] : images) {
s->images[name] = nvgCreateImage(s->vg, file, 1);
assert(s->images[name] != 0);
}
}
void ui_resize(UIState *s, int width, int height) {
s->fb_w = width;
s->fb_h = height;
}
#include "selfdrive/ui/carrot.moc"
Reference in New Issue Copy Permalink