Fine tuned pure_pursuit

Author: joe-hung

GEMstack/knowledge/defaults/current.yaml

@@ -17,7 +17,7 @@ control:
         crosstrack_gain: 0.5 # default: 1
         desired_speed: trajectory
     longitudinal_control:
-        pid_p: 1.0
+        pid_p: 1.5
         pid_i: 0.1
         pid_d: 0.0
 

GEMstack/knowledge/vehicle/dynamics.py

@@ -18,11 +18,6 @@ def acceleration_to_pedal_positions(acceleration : float, velocity : float, pitc
     Returns tuple (accelerator_pedal_position, brake_pedal_position, desired_gear)
     """
     model = settings.get('vehicle.dynamics.acceleration_model','hang_v1')
-    
-    model = 'hang_v1'
-    # print("===============")
-    # print(model)
-    # print("===============")
 
     if model == 'hang_v1':
         if gear != 1:

GEMstack/knowledge/vehicle/gem_e4_dynamics.yaml

@@ -12,7 +12,7 @@ max_accelerator_power:  #Watts.  Power at max accelerator pedal, by gear
   - 10000.0
 max_accelerator_power_reverse: 10000.0 #Watts.  Power (backwards) in reverse gear
 
-acceleration_model : kris_v1
+acceleration_model : hang_v1 # kris_v1
 accelerator_active_range : [0.2, 1.0]   #range of accelerator pedal where output acceleration is not flat
 brake_active_range : [0,1]             #range of brake pedal where output deceleration is not flat
 

GEMstack/onboard/planning/pure_pursuit.py

@@ -105,9 +105,13 @@ def compute(self, state : VehicleState, component : Component = None):
         else:
             desired_x,desired_y = self.path.eval(des_parameter)
         desired_yaw = np.arctan2(desired_y-curr_y,desired_x-curr_x)
-        #print("Desired point",(desired_x,desired_y)," with lookahead distance",self.look_ahead + self.look_ahead_scale * speed)
-        #print("Current yaw",curr_yaw,"desired yaw",desired_yaw)
-
+        
+        print("Desired point",(desired_x,desired_y)," with lookahead distance",self.look_ahead + self.look_ahead_scale * speed)
+        print("Current x",curr_x,"Current y",curr_y)
+        print("Current yaw",curr_yaw,"Desired yaw",desired_yaw)
+        print("Parameter : ", self.current_path_parameter)
+        print("Last index : ",  self.path.domain()[1])
+        
         # distance between the desired point and the vehicle
         L = transforms.vector2_dist((desired_x,desired_y),(curr_x,curr_y))
 
@@ -142,6 +146,7 @@ def compute(self, state : VehicleState, component : Component = None):
                 #past the end, just stop
                 desired_speed = 0.0
                 feedforward_accel = -2.0
+                f_delta = 0
             else:
                 if self.desired_speed_source=='path':
                     current_trajectory_time = self.trajectory.parameter_to_time(self.current_path_parameter)
@@ -165,6 +170,17 @@ def compute(self, state : VehicleState, component : Component = None):
         else:
             #decay speed when crosstrack error is high
             desired_speed *= np.exp(-abs(ct_error)*0.4)
+            
+            if len(self.trajectory.points) < 2 or self.current_path_parameter >= self.path.domain()[1]:
+                if component is not None:
+                    component.debug_event('Past the end of trajectory')
+                #past the end, just stop
+                desired_speed = 0.0
+                feedforward_accel = -2.0
+                f_delta = 0
+            
+            
+            
         if desired_speed > self.speed_limit:
             desired_speed = self.speed_limit 
         output_accel = self.pid_speed.advance(e = desired_speed - speed, t = t, feedforward_term=feedforward_accel)

launch/fixed_route.yaml

@@ -2,7 +2,7 @@ description: "Drive the GEM vehicle along a fixed route (currently xyhead_highba
 mode: hardware
 vehicle_interface: gem_hardware.GEMHardwareInterface
 mission_execution: StandardExecutor
-require_engaged: False
+# require_engaged: False
 # Recovery behavior after a component failure
 recovery: 
     planning: 
@@ -17,13 +17,14 @@ drive:
     planning:
         route_planning:
             type: StaticRoutePlanner
-            args: [!relative_path '../GEMstack/knowledge/routes/xyhead_highbay_backlot_p.csv']
-        motion_planning:
-            type: RouteToTrajectoryPlanner
-            args: [null]  #desired speed in m/s.  If null, this will keep the route untimed for the trajectory tracker
+            args: [!relative_path '../GEMstack/knowledge/routes/forward_15m.csv'] #forward_15m
+        motion_planning: longitudinal_planning.YieldTrajectoryPlanner
+            # 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
+            # args: [null]
+            # args: {desired_speed: 2.5}  #approximately 5mph
             print: False
 log:
     # Specify the top-level folder to save the log files.  Default is 'logs'
@@ -33,7 +34,7 @@ log:
     # If suffix is specified, then logs will output to folder/prefix+suffix. Default uses date and time as the suffix
     #suffix : 'test3'  
     # Specify which ros topics to record to vehicle.bag. Default records nothing. This records the "standard" ROS topics.
-    ros_topics : []
+    ros_topics : ['/septentrio_gnss/insnavgeod','/pacmod/parsed_tx/vehicle_speed_rpt']
     # Specify options to pass to rosbag record. Default is no options.
     #rosbag_options : '--split --size=1024' 
     # If True, then record all readings / commands of the vehicle interface. Default False
@@ -57,7 +58,7 @@ computation_graph: !include "../GEMstack/knowledge/defaults/computation_graph.ya
 after:
     show_log_folder: True  #set to false to avoid showing the log folder
 
-#on load, variants will overload the settings structure
+#on load, variants will overload the settingsNone structure
 variants:
     #sim variant doesn't execute on the real vehicle 
     #real variant executes on the real robot