diff is red so == good?

This commit is contained in:
discountchubbs
2025-10-23 17:16:29 -07:00
parent 450fcd4d55
commit 39342d7b5e
6 changed files with 10 additions and 46 deletions
+3 -9
View File
@@ -458,15 +458,9 @@ struct Navigationd @0xcb9fd56c7057593a {
upcomingTurn @0 :Text;
currentSpeedLimit @1 :UInt64;
bannerInstructions @2 :Text;
distanceToNextTurn @3 :Float64;
routeProgressPercent @4 :Float64;
distanceFromRoute @5 :Float64;
routePositionCumulative @6 :Float64;
distanceToEndOfStep @7 :Float64;
totalDistanceRemaining @8 :Float64;
totalTimeRemaining @9 :Float64;
allManeuvers @10 :List(Maneuver);
valid @11 :Bool;
distanceFromRoute @3 :Float64;
allManeuvers @4 :List(Maneuver);
valid @5 :Bool;
struct Maneuver {
distance @0 :Float64;
+1 -1
View File
@@ -190,7 +190,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
// Navigation params
{"MapboxToken", {PERSISTENT | BACKUP, STRING}},
{"MapboxSettings", {CLEAR_ON_MANAGER_START, JSON}},
{"MapboxRoute", {CLEAR_ON_MANAGER_START, STRING}},
{"MapboxRoute", {PERSISTENT, STRING}},
{"MapboxRecompute", {PERSISTENT | BACKUP, BOOL, "0"}},
// Neural Network Lateral Control
@@ -31,18 +31,11 @@ class NavigationInstructions:
# The next turn is the next step relative to our cumulative index
next_turn_idx = current_step_idx + 1
next_turn = route['steps'][next_turn_idx] if 0 <= next_turn_idx < len(route['steps']) else None
next_turn_distance = max(0, next_turn['cumulative_distance'] - closest_cumulative) if next_turn else None
current_maxspeed = current_step['maxspeed']
distance_to_end_of_step = max(0, current_step['distance'] - (closest_cumulative - current_step['cumulative_distance']))
# Calculate total remaining distance and time
total_distance_remaining: float = max(0, route['total_distance'] - closest_cumulative)
progress_in_step = (closest_cumulative - current_step['cumulative_distance']) / current_step['distance']
time_left_in_step = (1 - progress_in_step) * current_step['duration']
total_time_remaining: float = time_left_in_step + sum(step['duration'] for step in route['steps'][current_step_idx + 1 :])
all_maneuvers: list = []
max_maneuvers = 2
for idx in range(current_step_idx, min(current_step_idx + max_maneuvers, len(route['steps']))):
@@ -55,17 +48,12 @@ class NavigationInstructions:
return {
'distance_from_route': min_distance,
'route_position_cumulative': closest_cumulative,
'current_step': current_step,
'next_turn': next_turn,
'distance_to_next_turn': next_turn_distance,
'route_progress_percent': (closest_cumulative / max(1, route['total_distance'])) * 100,
'current_maxspeed': current_maxspeed,
'distance_to_end_of_step': distance_to_end_of_step,
'total_distance_remaining': total_distance_remaining,
'total_time_remaining': total_time_remaining,
'all_maneuvers': all_maneuvers,
'current_step_idx': current_step_idx,
'distance_to_end_of_step': distance_to_end_of_step,
}
def get_current_route(self):
@@ -68,15 +68,10 @@ class TestMapbox:
assert progress is not None
assert 'distance_from_route' in progress
assert 'next_turn' in progress
assert 'route_progress_percent' in progress
assert 'current_maxspeed' in progress
assert 'total_distance_remaining' in progress
assert 'total_time_remaining' in progress
assert 'all_maneuvers' in progress
assert 'distance_to_end_of_step' in progress
assert progress['distance_from_route'] >= 0
assert 0 <= progress['route_progress_percent'] <= 100
assert progress['total_distance_remaining'] >= 0
assert progress['total_time_remaining'] >= 0
assert isinstance(progress['all_maneuvers'], list)
speed_limit_metric = self.nav.get_current_speed_limit_from_progress(progress, True)
+4 -13
View File
@@ -73,11 +73,7 @@ class Navigationd:
if parsed:
banner_instructions = parsed['maneuverPrimaryText']
nav_data['distance_to_next_turn'] = progress['distance_to_next_turn']
nav_data['distance_to_end_of_step'] = progress['distance_to_end_of_step']
nav_data['route_progress_percent'] = progress['route_progress_percent']
nav_data['distance_from_route'] = progress['distance_from_route']
nav_data['route_position_cumulative'] = progress['route_position_cumulative']
# Don't recompute in last segment to prevent reroute loops
if self.route:
@@ -85,22 +81,17 @@ class Navigationd:
self.allow_recompute = False
if self.recompute_allowed:
self.reroute_counter += 1 if nav_data['distance_from_route'] > 25 else 0
self.reroute_counter += 1 if nav_data['distance_from_route'] > 100 else 0
return banner_instructions, progress, nav_data
def _build_navigation_message(self, banner_instructions, progress, nav_data):
def _build_navigation_message(self, banner_instructions: str, progress: dict | None, nav_data: dict, valid: bool):
msg = messaging.new_message('navigationd')
msg.valid = valid
msg.navigationd.upcomingTurn = nav_data.get('upcoming_turn', 'none')
msg.navigationd.currentSpeedLimit = nav_data.get('current_speed_limit', 0)
msg.navigationd.bannerInstructions = banner_instructions
msg.navigationd.distanceToNextTurn = nav_data.get('distance_to_next_turn', 0.0)
msg.navigationd.distanceToEndOfStep = nav_data.get('distance_to_end_of_step', 0.0)
msg.navigationd.routeProgressPercent = nav_data.get('route_progress_percent', 0.0)
msg.navigationd.distanceFromRoute = nav_data.get('distance_from_route', 0.0)
msg.navigationd.routePositionCumulative = nav_data.get('route_position_cumulative', 0.0)
msg.navigationd.totalDistanceRemaining = progress['total_distance_remaining'] if progress else 0.0
msg.navigationd.totalTimeRemaining = progress['total_time_remaining'] if progress else 0.0
msg.navigationd.valid = self.valid
all_maneuvers = (
@@ -127,7 +118,7 @@ class Navigationd:
self._update_params()
banner_instructions, progress, nav_data = self._update_navigation()
msg = self._build_navigation_message(banner_instructions, progress, nav_data)
msg = self._build_navigation_message(banner_instructions, progress, nav_data, valid=localizer_valid)
self.pm.send('navigationd', msg)
self.rk.keep_time()
@@ -47,8 +47,6 @@ class TestNavigationd:
nav = Navigationd()
msg = nav._build_navigation_message('', None, {})
assert msg.navigationd.bannerInstructions == ''
assert msg.navigationd.totalDistanceRemaining == 0.0
assert msg.navigationd.totalTimeRemaining == 0.0
assert msg.navigationd.valid is False
else:
sm = messaging.SubMaster(['navigationd'])
@@ -60,6 +58,4 @@ class TestNavigationd:
received_msg = sm['navigationd']
assert received_msg.bannerInstructions == msg.navigationd.bannerInstructions
assert received_msg.totalDistanceRemaining == msg.navigationd.totalDistanceRemaining
assert received_msg.totalTimeRemaining == msg.navigationd.totalTimeRemaining
assert received_msg.valid == msg.navigationd.valid