launch control added

Author: Demon16th

GEMstack/knowledge/defaults/current.yaml

@@ -16,6 +16,7 @@ control:
         lookahead_scale: 2.0
         crosstrack_gain: 0.3
         desired_speed: trajectory
+        launch_control:     1   # to enable racing features put launch_control to 1 
     longitudinal_control:
         pid_p: 0.8
         pid_i: 0.03
@@ -30,8 +31,7 @@ planning:
         max_deceleration:   6.0
         deceleration:       2.0
         min_deceleration:   0.5
-        yield_deceleration: 0.5
-
+        yield_deceleration: 0.5        
 
 #configure the simulator, if using
 simulator:

GEMstack/knowledge/vehicle/gem_e4.yaml

@@ -7,7 +7,7 @@ max_command_rate : 10.0          #for hardware, max rate of commands to send to
 #using !include directives helps maintain reuse of common settings
 geometry: !include gem_e4_geometry.yaml
 dynamics: !include gem_e4_dynamics.yaml
-limits: !include gem_e2_slow_limits.yaml
+limits: !include gem_e2_fast_limits.yaml
 control_defaults: !include gem_e2_control_defaults.yaml
 calibration: !include ../calibration/gem_e4.yaml
 sensors: !include gem_e4_sensor_environment.yaml

GEMstack/onboard/interface/gem_hardware.py

@@ -101,6 +101,13 @@ def __init__(self):
         /pacmod/as_rx/wiper_cmd
         """
 
+        # Launch Control Timing
+        # 0 - 5 Seconds 100% brake 0% throttle
+        # 5 - 6 Seconds 100% brake 100% throttle
+        # 6 - 7 Seconds 0% brake 100% throttle 
+        # TODO add option launch control
+        self.start_time = rospy.get_time()   
+
         #TODO: publish TwistStamped to /front_radar/front_radar/vehicle_motion to get better radar tracks
 
         #subscribers should go last because the callback might be called before the object is initialized
@@ -318,6 +325,22 @@ 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.brake_cmd.f64_cmd = maxbrake
+        # self.accel_cmd.f64_cmd = 0
+        
 
         self.gear_cmd.ui16_cmd = PacmodCmd.SHIFT_FORWARD
         self.gear_cmd.enable = True

GEMstack/onboard/planning/pure_pursuit.py

@@ -19,6 +19,7 @@ def __init__(self, lookahead = None, lookahead_scale = None, crosstrack_gain = N
         self.wheelbase  = settings.get('vehicle.geometry.wheelbase')
         self.wheel_angle_range = [settings.get('vehicle.geometry.min_wheel_angle'),settings.get('vehicle.geometry.max_wheel_angle')]
         self.steering_angle_range = [settings.get('vehicle.geometry.min_steering_angle'),settings.get('vehicle.geometry.max_steering_angle')]
+        self.launch_control = settings.get('control.pure_pursuit.launch_control')
 
         if desired_speed is not None:
             self.desired_speed_source = desired_speed
@@ -180,6 +181,15 @@ def compute(self, state : VehicleState, component : Component = None):
 
 
         output_accel = self.pid_speed.advance(e = desired_speed - speed, t = t, feedforward_term=feedforward_accel)
+
+
+        if self.launch_control:
+            if t < 4:
+                output_accel = self.max_accel
+
+        # self.brake_cmd.f64_cmd = maxbrake
+        # self.accel_cmd.f64_cmd = 0
+
         if component is not None:
             component.debug('curr pt',(curr_x,curr_y))
             component.debug('curr param',self.current_path_parameter)