Launch control PR

Author: Adi9103

GEMstack/knowledge/defaults/current.yaml

@@ -1,34 +1,48 @@
 # ********* Main settings entry point for behavior stack ***********
-
-# Configure settings for the vehicle / vehicle model
-vehicle:  !include ../vehicle/gem_e4.yaml
-
-#arguments for algorithm components here
-model_predictive_controller:
-    dt: 0.1
-    lookahead: 20
-control:
-    recovery:
-        brake_amount : 0.5
-        brake_speed : 2.0
-    pure_pursuit:
-        lookahead: 2.0
-        lookahead_scale: 3.0
-        crosstrack_gain: 1.0
-        desired_speed: trajectory
-    longitudinal_control:
-        pid_p: 1.0
-        pid_i: 0.1
-        pid_d: 0.0
-
-#configure the simulator, if using
-simulator:
-    dt: 0.01
-    real_time_multiplier: 1.0    # make the simulator run faster than real time by making this > 1
-    gnss_emulator: 
-        dt: 0.1    #10Hz
-        #position_noise: 0.1  #10cm noise
-        #orientation_noise: 0.04  #2.3 degrees noise
-        #velocity_noise:
-        #    constant: 0.04  #4cm/s noise
-        #    linear: 0.02    #2% noise
+ 
+ # Configure settings for the vehicle / vehicle model
+ vehicle:  !include ../vehicle/gem_e4.yaml
+ 
+ #arguments for algorithm components here
+ model_predictive_controller:
+     dt: 0.1
+     lookahead: 20
+ control:
+     recovery:
+         brake_amount : 0.5
+         brake_speed : 2.0
+     pure_pursuit:
+         lookahead: 2.0
+         lookahead_scale: 2.0
+         crosstrack_gain: 0.3
+         desired_speed: trajectory #racing
+     launch_control:
+         enabled: false
+         stage_duration: 0.5
+     longitudinal_control:
+         pid_p: 0.8
+         pid_i: 0.03
+         pid_d: 0.0
+ planning:
+     longitudinal_plan:
+         mode:               'real' # 'real' or 'sim'
+         yielder:            'expert' # 'expert', 'analytic', or 'simulation'
+         planner:            'dt' # 'milestone', 'dt', or 'dx'
+         desired_speed:      1.0
+         acceleration:       0.5
+         max_deceleration:   6.0
+         deceleration:       2.0
+         min_deceleration:   0.5
+         yield_deceleration: 0.5        
+ 
+ #configure the simulator, if using
+ simulator:
+     dt: 0.01
+     real_time_multiplier: 1.0    # make the simulator run faster than real time by making this > 1
+     gnss_emulator: 
+         dt: 0.05    #10Hz
+         #position_noise: 0.1  #10cm noise
+         #orientation_noise: 0.04  #2.3 degrees noise
+         #velocity_noise:
+         #    constant: 0.04  #4cm/s noise
+         #    linear: 0.02    #2% noise

GEMstack/onboard/interface/gem_hardware.py

@@ -316,6 +316,19 @@ def send_command(self, command : GEMVehicleCommand):
         self.accel_cmd.enable  = True
         self.accel_cmd.clear   = False
         self.accel_cmd.ignore  = False
+
+        # Launch control 
+        currTime = rospy.get_time() - self.start_time
+        if currTime < 5:
+            self.brake_cmd.f64_cmd = maxbrake
+            self.accel_cmd.f64_cmd = 0
+        elif  currTime < 6:
+            self.brake_cmd.f64_cmd = maxbrake
+            self.accel_cmd.f64_cmd = maxacc
+        elif currTime < 10:
+            self.brake_cmd.f64_cmd = 0
+            self.accel_cmd.f64_cmd = maxacc
+        
 
         self.gear_cmd.ui16_cmd = PacmodCmd.SHIFT_FORWARD
         self.gear_cmd.enable = True

GEMstack/onboard/planning/pure_pursuit.py

@@ -168,6 +168,7 @@ def compute(self, state : VehicleState, component : Component = None):
         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)
+
         if component is not None:
             component.debug('curr pt',(curr_x,curr_y))
             component.debug('curr param',self.current_path_parameter)