1414)
1515from fcutils .maths .filtering import line_smoother
1616
17- from proj .utils import interpolate_nans
17+ from proj .utils . misc import interpolate_nans
1818
1919
2020def complete_given_xy (x , y , params , planning_params ):
@@ -36,7 +36,9 @@ def complete_given_xy(x, y, params, planning_params):
3636 )
3737
3838
39- def compute_trajectory_stats (trajectory , duration , planning_params ):
39+ def compute_trajectory_stats (
40+ trajectory , duration , planning_params , min_dist_travelled = 150
41+ ):
4042 # Compute stats
4143 n_points = len (trajectory )
4244 distance_travelled = np .sum (
@@ -96,15 +98,21 @@ def compute_trajectory_stats(trajectory, duration, planning_params):
9698 print (table )
9799
98100 if waypoint_density < 2 or waypoint_density > 3 :
99- print (
100- f"[bold red]Waypoint density of { round (waypoint_density , 3 )} out of range, it should be 2 < wp < 3!"
101+ log .info (
102+ f"[bold red]Waypoint density of { round (waypoint_density , 3 )} out of range, it should be 2 < wp < 3!" ,
103+ extra = {"markdown" : True },
101104 )
102105
103106 if perc_lookahead < 0.05 :
104- print (
105- f"[bold red]Lookahead of { lookahead } is { perc_lookahead } of the # of waypoints, that might be too low. Values closer to 5% are advised."
107+ log .info (
108+ f"[bold red]Lookahead of { lookahead } is { perc_lookahead } of the # of waypoints, that might be too low. Values closer to 5% are advised." ,
109+ extra = {"markdown" : True },
106110 )
107111
112+ if distance_travelled < min_dist_travelled :
113+ log .warning ("Distance travelled below minimal requirement, erroring" )
114+ raise ValueError ("Distance travelled below minimal requirement" )
115+
108116 return trajectory , duration
109117
110118
0 commit comments