pp speeds

Author: Demon16th

GEMstack/mathutils/transforms.py

@@ -117,7 +117,7 @@ def lat_lon_to_xy(lat : float, lon : float, lat_reference : float, lon_reference
 
     Returns (x,y), where x is east in m, y is north in m.
     """
-    import alvinxy.alvinxy as axy 
+    import alvinxy.alvinxy as axy
     # convert GNSS waypoints into local fixed frame reprented in x and y
     east_x, north_y = axy.ll2xy(lat, lon, lat_reference, lon_reference)
     return east_x, north_y

GEMstack/onboard/planning/pure_pursuit.py

@@ -171,16 +171,17 @@ def compute(self, state : VehicleState, component : Component = None):
             curve_radius = self.path.fit_curve_radius((curr_x,curr_y), curve_points)
             # map curve radius to desired_speed. Note curve_radius can be inf if all points are colinear
             print("curve_radius: ", curve_radius)
-            if curve_radius < 10:
-                desired_speed = 2.5
-            elif curve_radius < 15:
-                desired_speed = 3
-            elif curve_radius < 20:
-                desired_speed = 4
-            elif curve_radius < 30:
-                desired_speed = 5
-            else:
-                desired_speed = 10
+            #if curve_radius < 10:
+            #    desired_speed = 2.5
+            #elif curve_radius < 15:
+            #    desired_speed = 3
+            #elif curve_radius < 20:
+            #    desired_speed = 4
+            #elif curve_radius < 30:
+            #    desired_speed = 5
+            #else:
+            #    desired_speed = 10
+            desired_speed = (.5*curve_radius)**.5
         else:
             #decay speed when crosstrack error is high
             desired_speed *= np.exp(-abs(ct_error)*0.8)

launch/fixed_route.yaml

@@ -16,14 +16,14 @@ drive:
     planning:
         route_planning:
             type: StaticRoutePlanner
-            args: [!relative_path '../GEMstack/knowledge/routes/xyhead_highbay_backlot_p.csv']
+            args: [!relative_path '../GEMstack/knowledge/routes/turn_right2.csv']
         motion_planning:
             type: RouteToTrajectoryPlanner
             args: [null]  #desired speed in m/s.  If null, this will keep the route untimed for the trajectory tracker
         trajectory_tracking:
             type: pure_pursuit.PurePursuitTrajectoryTracker
-            args: {desired_speed: 2.5}  #approximately 5mph
-            print: False
+            args: {desired_speed: 10}  #approximately 5mph
+            print: True
 log:
     # Specify the top-level folder to save the log files.  Default is 'logs'
     #folder : 'logs' 

testing/test_gg_diagram.py

@@ -8,6 +8,7 @@
 import json
 import matplotlib.pyplot as plt
 import numpy as np
+from GEMstack.mathutils.transforms import lat_lon_to_xy
 
 def parse_behavior_log(filename):
     """
@@ -23,9 +24,11 @@ def parse_behavior_log(filename):
     speeds = []
     xs = []
     ys = []
+    ref_lat = 0.0
+    ref_long = 0.0
 
     with open(filename, 'r') as f:
-        for line in f:
+        for idx, line in enumerate(f):
             try:
                 entry = json.loads(line)
             except json.JSONDecodeError:
@@ -43,6 +46,10 @@ def parse_behavior_log(filename):
                 frame = vehicle_data["pose"].get("frame")
                 x = vehicle_data["pose"].get("x")
                 y = vehicle_data["pose"].get("y")
+                if idx == 0:
+                    ref_lat = x
+                    ref_long = y
+                x, y = lat_lon_to_xy(x,y,ref_lat,ref_long)
                 # Only add if all fields are available
                 if None not in (t, acceleration, heading_rate, speed)  and frame != 3:
                     times.append(t)
@@ -143,7 +150,7 @@ def plot_accelerations(axis, accelerations, time):
 def plot_gg_diagram(axis, longitudinal_gs, lateral_gs):
     """Plots gg diagram"""
     # Plot G-G diagram
-    axis.scatter(longitudinal_gs, lateral_gs, alpha=0.5, label="Data Points")
+    axis.scatter(lateral_gs, longitudinal_gs, alpha=0.5, label="Data Points")
 
     # Draw friction ellipse (assuming µ = 1.0)
     mu = 1.0