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

fix navInstructions

Browse Source
This commit is contained in:
ajouatom 2025-02-26 13:20:06 +09:00
parent 3bf25ea4b6
commit 900adb25de
3 changed files with 133 additions and 134 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

213
selfdrive/carrot/carrot_man.py
View File

@ -28,6 +28,56 @@ except ImportError:
NetworkType = log.DeviceState.NetworkType 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 ################ 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.] #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************************************************") print("************************************************CarrotMan init************************************************")
self.params = Params() self.params = Params()
self.params_memory = Params("/dev/shm/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.pm = messaging.PubMaster(['carrotMan', "navRoute", "navInstructionCarrot"])
self.carrot_serv = CarrotServ() self.carrot_serv = CarrotServ()
@ -295,6 +345,7 @@ class CarrotMan:
if remote_addr is None: if remote_addr is None:
print(f"Broadcasting: {self.broadcast_ip}:{msg}") print(f"Broadcasting: {self.broadcast_ip}:{msg}")
if not self.navd_active: if not self.navd_active:
#print("clear path_points: navd_active: ", self.navd_active)
self.navi_points = [] self.navi_points = []
self.navi_points_active = False self.navi_points_active = False
@ -314,15 +365,21 @@ class CarrotMan:
def carrot_navi_route(self): 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): 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}") #print(f"navi_points_active: {self.navi_points_active}, active_carrot: {self.carrot_serv.active_carrot}")
#haversine_cache.clear() if self.navi_points_active:
#curvature_cache.clear() print("navi_points_active: ", self.navi_points_active, "active_carrot: ", self.carrot_serv.active_carrot, "navd_active: ", self.navd_active)
self.navi_points = [] #haversine_cache.clear()
self.navi_points_active = False #curvature_cache.clear()
if self.active_carrot_last > 1: self.navi_points = []
#self.params.remove("NavDestination") self.navi_points_active = False
pass if self.active_carrot_last > 1:
#self.params.remove("NavDestination")
pass
self.active_carrot_last = self.carrot_serv.active_carrot self.active_carrot_last = self.carrot_serv.active_carrot
return [],[],300 return [],[],300
@ -1365,6 +1422,24 @@ class CarrotServ:
return atc_desired, atc_type, atc_speed, atc_dist 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): def update_navi(self, remote_ip, sm, pm, vturn_speed, coords, distances, route_speed):
self.update_params() self.update_params()
@ -1399,6 +1474,7 @@ class CarrotServ:
self.nTBTTurnType = self.nTBTTurnTypeNext = -1 self.nTBTTurnType = self.nTBTTurnTypeNext = -1
self.roadcate = 8 self.roadcate = 8
self.nGoPosDist = 0 self.nGoPosDist = 0
self.update_nav_instruction(sm)
if self.xSpdType < 0 or self.xSpdDist <= 0: if self.xSpdType < 0 or self.xSpdDist <= 0:
self.xSpdType = -1 self.xSpdType = -1
@ -1561,101 +1637,52 @@ class CarrotServ:
msg.carrotMan.naviPaths = coords_str msg.carrotMan.naviPaths = coords_str
msg.carrotMan.leftSec = int(self.carrot_left_sec) msg.carrotMan.leftSec = int(self.carrot_left_sec)
pm.send('carrotMan', msg) pm.send('carrotMan', msg)
inst = messaging.new_message('navInstructionCarrot')
if self.active_carrot > 1:
inst.valid = True
nav_type_mapping = { instruction = inst.navInstructionCarrot
12: ("turn", "left", 1), instruction.distanceRemaining = self.nGoPosDist
16: ("turn", "sharp left", 1), instruction.timeRemaining = self.nGoPosTime
13: ("turn", "right", 2), instruction.speedLimit = self.nRoadLimitSpeed / 3.6 if self.nRoadLimitSpeed > 0 else 0
19: ("turn", "sharp right", 2), instruction.maneuverDistance = float(self.nTBTDist)
102: ("off ramp", "slight left", 3), instruction.maneuverSecondaryText = self.szNearDirName
105: ("off ramp", "slight left", 3), if self.szFarDirName and len(self.szFarDirName):
112: ("off ramp", "slight left", 3), instruction.maneuverSecondaryText += "[{}]".format(self.szFarDirName)
115: ("off ramp", "slight left", 3), instruction.maneuverPrimaryText = self.szTBTMainText
101: ("off ramp", "slight right", 4), instruction.timeRemainingTypical = self.nGoPosTime
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') navType, navModifier, xTurnInfo1 = "invalid", "", -1
msg.valid = True if self.nTBTTurnType in nav_type_mapping:
navType, navModifier, xTurnInfo1 = nav_type_mapping[self.nTBTTurnType]
navTypeNext, navModifierNext, xTurnInfoNext = "invalid", "", -1
if self.nTBTTurnTypeNext in nav_type_mapping:
navTypeNext, navModifierNext, xTurnInfoNext = nav_type_mapping[self.nTBTTurnTypeNext]
instruction = msg.navInstructionCarrot instruction.maneuverType = navType
instruction.distanceRemaining = self.nGoPosDist instruction.maneuverModifier = navModifier
instruction.timeRemaining = self.nGoPosTime
instruction.speedLimit = self.nRoadLimitSpeed / 3.6 if self.nRoadLimitSpeed > 0 else 0
instruction.maneuverDistance = float(self.nTBTDist)
instruction.maneuverSecondaryText = self.szNearDirName
if self.szFarDirName and len(self.szFarDirName):
instruction.maneuverSecondaryText += "[{}]".format(self.szFarDirName)
instruction.maneuverPrimaryText = self.szTBTMainText
instruction.timeRemainingTypical = self.nGoPosTime
navType, navModifier, xTurnInfo1 = "invalid", "", -1 maneuvers = []
if self.nTBTTurnType in nav_type_mapping: if self.nTBTTurnType >= 0:
navType, navModifier, xTurnInfo1 = nav_type_mapping[self.nTBTTurnType]
navTypeNext, navModifierNext, xTurnInfoNext = "invalid", "", -1
if self.nTBTTurnTypeNext in nav_type_mapping:
navTypeNext, navModifierNext, xTurnInfoNext = nav_type_mapping[self.nTBTTurnTypeNext]
instruction.maneuverType = navType
instruction.maneuverModifier = navModifier
maneuvers = []
if self.nTBTTurnType >= 0:
maneuver = {}
maneuver['distance'] = float(self.xDistToTurn)
maneuver['type'] = navType
maneuver['modifier'] = navModifier
maneuvers.append(maneuver)
if self.nTBTDistNext >= self.nTBTDist:
maneuver = {} maneuver = {}
maneuver['distance'] = float(self.nTBTDistNext) maneuver['distance'] = float(self.xDistToTurn)
maneuver['type'] = navTypeNext maneuver['type'] = navType
maneuver['modifier'] = navModifierNext maneuver['modifier'] = navModifier
maneuvers.append(maneuver) maneuvers.append(maneuver)
if self.nTBTDistNext >= self.nTBTDist:
maneuver = {}
maneuver['distance'] = float(self.nTBTDistNext)
maneuver['type'] = navTypeNext
maneuver['modifier'] = navModifierNext
maneuvers.append(maneuver)
instruction.allManeuvers = maneuvers 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): def _update_system_time(self, epoch_time_remote, timezone_remote):
epoch_time = int(time.time()) epoch_time = int(time.time())

2
selfdrive/navd/navd.py
View File

@ -226,7 +226,7 @@ class RouteEngine:
msg.valid = False msg.valid = False
self.pm.send('navInstruction', msg) self.pm.send('navInstruction', msg)
return return
#print("navd_navInstruction")
step = self.route[self.step_idx] step = self.route[self.step_idx]
geometry = self.route_geometry[self.step_idx] geometry = self.route_geometry[self.step_idx]
along_geometry = distance_along_geometry(geometry, self.last_position) along_geometry = distance_along_geometry(geometry, self.last_position)

52
selfdrive/ui/carrot.cc
View File

@ -877,7 +877,6 @@ private:
int xDistToTurn = 0; int xDistToTurn = 0;
int nRoadLimitSpeed = 20; int nRoadLimitSpeed = 20;
int active_carrot = 0; int active_carrot = 0;
bool active_navi_inst = false;
int nGoPosDist = 0; int nGoPosDist = 0;
int nGoPosTime = 0; int nGoPosTime = 0;
@ -1118,16 +1117,8 @@ public:
return; return;
} }
const auto carrot_man = sm["carrotMan"].getCarrotMan(); 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(); 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) { if (active_carrot > 1) {
xSpdLimit = carrot_man.getXSpdLimit(); xSpdLimit = carrot_man.getXSpdLimit();
@ -1139,7 +1130,7 @@ public:
xSpdDist = 0; xSpdDist = 0;
xSignType = 0; xSignType = 0;
} }
if (active_carrot > 1 and !active_navi_inst) { if (active_carrot > 1 || carrot_man.getNGoPosDist() > 0) {
xTurnInfo = carrot_man.getXTurnInfo(); xTurnInfo = carrot_man.getXTurnInfo();
xDistToTurn = carrot_man.getXDistToTurn(); xDistToTurn = carrot_man.getXDistToTurn();
nRoadLimitSpeed = carrot_man.getNRoadLimitSpeed(); nRoadLimitSpeed = carrot_man.getNRoadLimitSpeed();
@ -1151,29 +1142,17 @@ public:
szPosRoadName = QString::fromStdString(carrot_man.getSzPosRoadName()); szPosRoadName = QString::fromStdString(carrot_man.getSzPosRoadName());
szTBTMainText = QString::fromStdString(carrot_man.getSzTBTMainText()); szTBTMainText = QString::fromStdString(carrot_man.getSzTBTMainText());
} else if(sm.alive("navInstruction") && sm.valid("navInstruction")) { }
const auto nav_inst = sm["navInstruction"].getNavInstruction(); else {
xTurnInfo = 0; xTurnInfo = -1;
xDistToTurn = nav_inst.getManeuverDistance(); xDistToTurn = 0;
nRoadLimitSpeed = nav_inst.getSpeedLimit() * 3.6; nRoadLimitSpeed = 20;
QString maneuverType = QString::fromStdString(nav_inst.getManeuverType()); atc_type = "";
QString manuverModifier = QString::fromStdString(nav_inst.getManeuverModifier()); nGoPosDist = 0;
if (maneuverType == "turn") { nGoPosTime = 0;
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();
szSdiDescr = ""; szSdiDescr = "";
szPosRoadName = ""; szPosRoadName = "";
szTBTMainText = QString::fromStdString(nav_inst.getManeuverPrimaryText()); szTBTMainText = "";
} }
#ifdef __UI_TEST #ifdef __UI_TEST
@ -1879,7 +1858,6 @@ public:
int trafficState = 0; int trafficState = 0;
int trafficState_carrot = 0; int trafficState_carrot = 0;
int active_carrot = 0; int active_carrot = 0;
bool active_navi_inst = false;
float xTarget = 0.0; float xTarget = 0.0;
int myDrivingMode = 1; int myDrivingMode = 1;
@ -1977,12 +1955,6 @@ public:
szPosRoadName = ""; szPosRoadName = "";
nRoadLimitSpeed = 30; 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(); xState = lp.getXState();
trafficState = lp.getTrafficState(); 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_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); 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); ui_draw_text(s, dx, dy - 45, "ROUTE", 30, COLOR_WHITE, BOLD);
} }
#ifdef __UI_TEST #ifdef __UI_TEST
@ -2304,7 +2276,7 @@ public:
xSignType = 1; xSignType = 1;
#endif #endif
if (active_carrot >= 2 || active_navi_inst) { if (active_carrot >= 2) {
dx = bx + 75; dx = bx + 75;
dy = by + 175; dy = by + 175;
int disp_speed = 0; int disp_speed = 0;