fix navInstructions
This commit is contained in:
ajouatom
parent
3bf25ea4b6
commit
900adb25de
@ -28,6 +28,56 @@ except ImportError:
|
||||
|
||||
NetworkType = log.DeviceState.NetworkType
|
||||
|
||||
nav_type_mapping = {
|
||||
12: ("turn", "left", 1),
|
||||
16: ("turn", "sharp left", 1),
|
||||
13: ("turn", "right", 2),
|
||||
19: ("turn", "sharp right", 2),
|
||||
102: ("off ramp", "slight left", 3),
|
||||
105: ("off ramp", "slight left", 3),
|
||||
112: ("off ramp", "slight left", 3),
|
||||
115: ("off ramp", "slight left", 3),
|
||||
101: ("off ramp", "slight right", 4),
|
||||
104: ("off ramp", "slight right", 4),
|
||||
111: ("off ramp", "slight right", 4),
|
||||
114: ("off ramp", "slight right", 4),
|
||||
7: ("fork", "left", 3),
|
||||
44: ("fork", "left", 3),
|
||||
17: ("fork", "left", 3),
|
||||
75: ("fork", "left", 3),
|
||||
76: ("fork", "left", 3),
|
||||
118: ("fork", "left", 3),
|
||||
6: ("fork", "right", 4),
|
||||
43: ("fork", "right", 4),
|
||||
73: ("fork", "right", 4),
|
||||
74: ("fork", "right", 4),
|
||||
123: ("fork", "right", 4),
|
||||
124: ("fork", "right", 4),
|
||||
117: ("fork", "right", 4),
|
||||
131: ("rotary", "slight right", 5),
|
||||
132: ("rotary", "slight right", 5),
|
||||
140: ("rotary", "slight left", 5),
|
||||
141: ("rotary", "slight left", 5),
|
||||
133: ("rotary", "right", 5),
|
||||
134: ("rotary", "sharp right", 5),
|
||||
135: ("rotary", "sharp right", 5),
|
||||
136: ("rotary", "sharp left", 5),
|
||||
137: ("rotary", "sharp left", 5),
|
||||
138: ("rotary", "sharp left", 5),
|
||||
139: ("rotary", "left", 5),
|
||||
142: ("rotary", "straight", 5),
|
||||
14: ("turn", "uturn", 5),
|
||||
201: ("arrive", "straight", 5),
|
||||
51: ("notification", "straight", None),
|
||||
52: ("notification", "straight", None),
|
||||
53: ("notification", "straight", None),
|
||||
54: ("notification", "straight", None),
|
||||
55: ("notification", "straight", None),
|
||||
153: ("", "", 6), #TG
|
||||
154: ("", "", 6), #TG
|
||||
249: ("", "", 6) #TG
|
||||
}
|
||||
|
||||
################ CarrotNavi
|
||||
## 국가법령정보센터: 도로설계기준
|
||||
#V_CURVE_LOOKUP_BP = [0., 1./800., 1./670., 1./560., 1./440., 1./360., 1./265., 1./190., 1./135., 1./85., 1./55., 1./30., 1./15.]
|
||||
@ -182,7 +232,7 @@ class CarrotMan:
|
||||
print("************************************************CarrotMan init************************************************")
|
||||
self.params = Params()
|
||||
self.params_memory = Params("/dev/shm/params")
|
||||
self.sm = messaging.SubMaster(['deviceState', 'carState', 'controlsState', 'longitudinalPlan', 'modelV2', 'selfdriveState', 'carControl', 'navRouteNavd', 'liveLocationKalman'])
|
||||
self.sm = messaging.SubMaster(['deviceState', 'carState', 'controlsState', 'longitudinalPlan', 'modelV2', 'selfdriveState', 'carControl', 'navRouteNavd', 'liveLocationKalman', 'navInstruction'])
|
||||
self.pm = messaging.PubMaster(['carrotMan', "navRoute", "navInstructionCarrot"])
|
||||
|
||||
self.carrot_serv = CarrotServ()
|
||||
@ -295,6 +345,7 @@ class CarrotMan:
|
||||
if remote_addr is None:
|
||||
print(f"Broadcasting: {self.broadcast_ip}:{msg}")
|
||||
if not self.navd_active:
|
||||
#print("clear path_points: navd_active: ", self.navd_active)
|
||||
self.navi_points = []
|
||||
self.navi_points_active = False
|
||||
|
||||
@ -314,8 +365,14 @@ class CarrotMan:
|
||||
|
||||
def carrot_navi_route(self):
|
||||
|
||||
if self.carrot_serv.active_carrot > 1:
|
||||
if self.navd_active:
|
||||
self.navd_active = False
|
||||
self.params.remove("NavDestination")
|
||||
if not self.navi_points_active or not SHAPELY_AVAILABLE or (self.carrot_serv.active_carrot <= 1 and not self.navd_active):
|
||||
#print(f"navi_points_active: {self.navi_points_active}, active_carrot: {self.carrot_serv.active_carrot}")
|
||||
if self.navi_points_active:
|
||||
print("navi_points_active: ", self.navi_points_active, "active_carrot: ", self.carrot_serv.active_carrot, "navd_active: ", self.navd_active)
|
||||
#haversine_cache.clear()
|
||||
#curvature_cache.clear()
|
||||
self.navi_points = []
|
||||
@ -1365,6 +1422,24 @@ class CarrotServ:
|
||||
|
||||
return atc_desired, atc_type, atc_speed, atc_dist
|
||||
|
||||
def update_nav_instruction(self, sm):
|
||||
if sm.alive['navInstruction'] and sm.valid['navInstruction']:
|
||||
msg_nav = sm['navInstruction']
|
||||
|
||||
self.nGoPosDist = int(msg_nav.distanceRemaining)
|
||||
self.nGoPosTime = int(msg_nav.timeRemaining)
|
||||
self.nRoadLimitSpeed = max(30, int(msg_nav.speedLimit * 3.6))
|
||||
self.xDistToTurn = int(msg_nav.maneuverDistance)
|
||||
self.szTBTMainText = msg_nav.maneuverPrimaryText
|
||||
self.xTurnInfo = -1
|
||||
for key, value in nav_type_mapping.items():
|
||||
if value[0] == msg_nav.maneuverType and value[1] == msg_nav.maneuverModifier:
|
||||
self.xTurnInfo = value[2]
|
||||
break
|
||||
|
||||
#print(msg_nav)
|
||||
#print(f"navInstruction: {self.xTurnInfo}, {self.xDistToTurn}, {self.szTBTMainText}")
|
||||
|
||||
def update_navi(self, remote_ip, sm, pm, vturn_speed, coords, distances, route_speed):
|
||||
|
||||
self.update_params()
|
||||
@ -1399,6 +1474,7 @@ class CarrotServ:
|
||||
self.nTBTTurnType = self.nTBTTurnTypeNext = -1
|
||||
self.roadcate = 8
|
||||
self.nGoPosDist = 0
|
||||
self.update_nav_instruction(sm)
|
||||
|
||||
if self.xSpdType < 0 or self.xSpdDist <= 0:
|
||||
self.xSpdType = -1
|
||||
@ -1561,64 +1637,13 @@ class CarrotServ:
|
||||
msg.carrotMan.naviPaths = coords_str
|
||||
|
||||
msg.carrotMan.leftSec = int(self.carrot_left_sec)
|
||||
|
||||
pm.send('carrotMan', msg)
|
||||
|
||||
inst = messaging.new_message('navInstructionCarrot')
|
||||
if self.active_carrot > 1:
|
||||
inst.valid = True
|
||||
|
||||
nav_type_mapping = {
|
||||
12: ("turn", "left", 1),
|
||||
16: ("turn", "sharp left", 1),
|
||||
13: ("turn", "right", 2),
|
||||
19: ("turn", "sharp right", 2),
|
||||
102: ("off ramp", "slight left", 3),
|
||||
105: ("off ramp", "slight left", 3),
|
||||
112: ("off ramp", "slight left", 3),
|
||||
115: ("off ramp", "slight left", 3),
|
||||
101: ("off ramp", "slight right", 4),
|
||||
104: ("off ramp", "slight right", 4),
|
||||
111: ("off ramp", "slight right", 4),
|
||||
114: ("off ramp", "slight right", 4),
|
||||
7: ("fork", "left", 3),
|
||||
44: ("fork", "left", 3),
|
||||
17: ("fork", "left", 3),
|
||||
75: ("fork", "left", 3),
|
||||
76: ("fork", "left", 3),
|
||||
118: ("fork", "left", 3),
|
||||
6: ("fork", "right", 4),
|
||||
43: ("fork", "right", 4),
|
||||
73: ("fork", "right", 4),
|
||||
74: ("fork", "right", 4),
|
||||
123: ("fork", "right", 4),
|
||||
124: ("fork", "right", 4),
|
||||
117: ("fork", "right", 4),
|
||||
131: ("rotary", "slight right", 5),
|
||||
132: ("rotary", "slight right", 5),
|
||||
140: ("rotary", "slight left", 5),
|
||||
141: ("rotary", "slight left", 5),
|
||||
133: ("rotary", "right", 5),
|
||||
134: ("rotary", "sharp right", 5),
|
||||
135: ("rotary", "sharp right", 5),
|
||||
136: ("rotary", "sharp left", 5),
|
||||
137: ("rotary", "sharp left", 5),
|
||||
138: ("rotary", "sharp left", 5),
|
||||
139: ("rotary", "left", 5),
|
||||
142: ("rotary", "straight", 5),
|
||||
14: ("turn", "uturn", 5),
|
||||
201: ("arrive", "straight", 5),
|
||||
51: ("notification", "straight", None),
|
||||
52: ("notification", "straight", None),
|
||||
53: ("notification", "straight", None),
|
||||
54: ("notification", "straight", None),
|
||||
55: ("notification", "straight", None),
|
||||
153: ("", "", 6), #TG
|
||||
154: ("", "", 6), #TG
|
||||
249: ("", "", 6) #TG
|
||||
}
|
||||
|
||||
msg = messaging.new_message('navInstructionCarrot')
|
||||
msg.valid = True
|
||||
|
||||
instruction = msg.navInstructionCarrot
|
||||
instruction = inst.navInstructionCarrot
|
||||
instruction.distanceRemaining = self.nGoPosDist
|
||||
instruction.timeRemaining = self.nGoPosTime
|
||||
instruction.speedLimit = self.nRoadLimitSpeed / 3.6 if self.nRoadLimitSpeed > 0 else 0
|
||||
@ -1654,8 +1679,10 @@ class CarrotServ:
|
||||
maneuvers.append(maneuver)
|
||||
|
||||
instruction.allManeuvers = maneuvers
|
||||
elif sm.alive['navInstruction'] and sm.valid['navInstruction']:
|
||||
inst.navInstructionCarrot = sm['navInstruction']
|
||||
|
||||
pm.send('navInstructionCarrot', msg)
|
||||
pm.send('navInstructionCarrot', inst)
|
||||
|
||||
def _update_system_time(self, epoch_time_remote, timezone_remote):
|
||||
epoch_time = int(time.time())
|
||||
|
||||
@ -226,7 +226,7 @@ class RouteEngine:
|
||||
msg.valid = False
|
||||
self.pm.send('navInstruction', msg)
|
||||
return
|
||||
|
||||
#print("navd_navInstruction")
|
||||
step = self.route[self.step_idx]
|
||||
geometry = self.route_geometry[self.step_idx]
|
||||
along_geometry = distance_along_geometry(geometry, self.last_position)
|
||||
|
||||
@ -877,7 +877,6 @@ private:
|
||||
int xDistToTurn = 0;
|
||||
int nRoadLimitSpeed = 20;
|
||||
int active_carrot = 0;
|
||||
bool active_navi_inst = false;
|
||||
|
||||
int nGoPosDist = 0;
|
||||
int nGoPosTime = 0;
|
||||
@ -1118,16 +1117,8 @@ public:
|
||||
return;
|
||||
}
|
||||
const auto carrot_man = sm["carrotMan"].getCarrotMan();
|
||||
float maneuverDistance = 0;
|
||||
if (sm.alive("navInstruction")) {
|
||||
const auto nav_inst = sm["navInstruction"].getNavInstruction();
|
||||
maneuverDistance = nav_inst.getManeuverDistance();
|
||||
}
|
||||
|
||||
active_carrot = carrot_man.getActiveCarrot();
|
||||
//printf("active_carrot: %d\n", active_carrot);
|
||||
if (active_carrot <= 1 || (carrot_man.getNGoPosDist() <= 0 && maneuverDistance > 0)) active_navi_inst = true;
|
||||
else active_navi_inst = false;
|
||||
|
||||
if (active_carrot > 1) {
|
||||
xSpdLimit = carrot_man.getXSpdLimit();
|
||||
@ -1139,7 +1130,7 @@ public:
|
||||
xSpdDist = 0;
|
||||
xSignType = 0;
|
||||
}
|
||||
if (active_carrot > 1 and !active_navi_inst) {
|
||||
if (active_carrot > 1 || carrot_man.getNGoPosDist() > 0) {
|
||||
xTurnInfo = carrot_man.getXTurnInfo();
|
||||
xDistToTurn = carrot_man.getXDistToTurn();
|
||||
nRoadLimitSpeed = carrot_man.getNRoadLimitSpeed();
|
||||
@ -1151,29 +1142,17 @@ public:
|
||||
szPosRoadName = QString::fromStdString(carrot_man.getSzPosRoadName());
|
||||
szTBTMainText = QString::fromStdString(carrot_man.getSzTBTMainText());
|
||||
|
||||
} else if(sm.alive("navInstruction") && sm.valid("navInstruction")) {
|
||||
const auto nav_inst = sm["navInstruction"].getNavInstruction();
|
||||
xTurnInfo = 0;
|
||||
xDistToTurn = nav_inst.getManeuverDistance();
|
||||
nRoadLimitSpeed = nav_inst.getSpeedLimit() * 3.6;
|
||||
QString maneuverType = QString::fromStdString(nav_inst.getManeuverType());
|
||||
QString manuverModifier = QString::fromStdString(nav_inst.getManeuverModifier());
|
||||
if (maneuverType == "turn") {
|
||||
if (manuverModifier == "sharp left" || manuverModifier == "slight left" || manuverModifier == "left") xTurnInfo = 1; // left turn
|
||||
else if (manuverModifier == "sharp right" || manuverModifier == "slight right" || manuverModifier == "right") xTurnInfo = 2;
|
||||
else if (manuverModifier == "uturn") xTurnInfo = 5;
|
||||
}
|
||||
else if (maneuverType == "fork" || maneuverType == "off ramp") {
|
||||
if (manuverModifier == "slight left" || manuverModifier == "left") xTurnInfo = 3; // left turn
|
||||
else if (manuverModifier == "slight right" || manuverModifier == "right") xTurnInfo = 4;
|
||||
}
|
||||
|
||||
atc_type = maneuverType + " " + manuverModifier;
|
||||
nGoPosDist = nav_inst.getDistanceRemaining();
|
||||
nGoPosTime = nav_inst.getTimeRemaining();
|
||||
else {
|
||||
xTurnInfo = -1;
|
||||
xDistToTurn = 0;
|
||||
nRoadLimitSpeed = 20;
|
||||
atc_type = "";
|
||||
nGoPosDist = 0;
|
||||
nGoPosTime = 0;
|
||||
szSdiDescr = "";
|
||||
szPosRoadName = "";
|
||||
szTBTMainText = QString::fromStdString(nav_inst.getManeuverPrimaryText());
|
||||
szTBTMainText = "";
|
||||
}
|
||||
|
||||
#ifdef __UI_TEST
|
||||
@ -1879,7 +1858,6 @@ public:
|
||||
int trafficState = 0;
|
||||
int trafficState_carrot = 0;
|
||||
int active_carrot = 0;
|
||||
bool active_navi_inst = false;
|
||||
float xTarget = 0.0;
|
||||
int myDrivingMode = 1;
|
||||
|
||||
@ -1977,12 +1955,6 @@ public:
|
||||
szPosRoadName = "";
|
||||
nRoadLimitSpeed = 30;
|
||||
}
|
||||
const auto nav_inst = sm["navInstruction"].getNavInstruction();
|
||||
if (active_carrot <= 1 || (carrot_man.getNGoPosDist() <= 0 && nav_inst.getManeuverDistance() > 0)) active_navi_inst = true;
|
||||
else active_navi_inst = false;
|
||||
if (active_navi_inst) {
|
||||
nRoadLimitSpeed = carrot_man.getNRoadLimitSpeed();
|
||||
}
|
||||
|
||||
xState = lp.getXState();
|
||||
trafficState = lp.getTrafficState();
|
||||
@ -2294,7 +2266,7 @@ public:
|
||||
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 > 0) {
|
||||
if (nav_path_vertex_count > 1) {
|
||||
ui_draw_text(s, dx, dy - 45, "ROUTE", 30, COLOR_WHITE, BOLD);
|
||||
}
|
||||
#ifdef __UI_TEST
|
||||
@ -2304,7 +2276,7 @@ public:
|
||||
xSignType = 1;
|
||||
#endif
|
||||
|
||||
if (active_carrot >= 2 || active_navi_inst) {
|
||||
if (active_carrot >= 2) {
|
||||
dx = bx + 75;
|
||||
dy = by + 175;
|
||||
int disp_speed = 0;
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user