changed launch ctrl to gem

Author: Adi9103

GEMstack/knowledge/defaults/current.yaml

@@ -16,7 +16,10 @@
          lookahead_scale: 2.0
          crosstrack_gain: 0.3
          desired_speed: trajectory #racing
-         launch_control:     0   # to enable racing features put launch_control to 1 
+     launch_control:
+         enabled: false
+         stage_duration: 0.5
+         
      longitudinal_control:
          pid_p: 0.8
          pid_i: 0.03

GEMstack/onboard/interface/gem_hardware.py

@@ -326,17 +326,26 @@ def send_command(self, command : GEMVehicleCommand):
         self.accel_cmd.clear   = False
         self.accel_cmd.ignore  = False
 
-        # Launch control 
+        enable_launch_control = settings.get('control.launch_control.enable', False)
+        stage_duration = 0.5
         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
+
+
+        # Launch control 
+        if enable_launch_control:
+            total_stage_time = 3 * stage_duration
+            if currTime < total_stage_time:
+                if currTime < stage_duration:
+                    self.brake_cmd.f64_cmd = maxbrake
+                    self.accel_cmd.f64_cmd = 0
+                elif currTime < 2 * stage_duration:
+                    self.brake_cmd.f64_cmd = maxbrake
+                    self.accel_cmd.f64_cmd = maxacc
+                else:
+                    self.brake_cmd.f64_cmd = 0  
+                    self.accel_cmd.f64_cmd = maxacc
+            else:
+                enable_launch_control = False
 
         # self.brake_cmd.f64_cmd = maxbrake
         # self.accel_cmd.f64_cmd = 0

GEMstack/onboard/planning/pure_pursuit.py

@@ -19,7 +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')
+        # self.launch_control = settings.get('control.pure_pursuit.launch_control')
 
         if desired_speed is not None:
             self.desired_speed_source = desired_speed
@@ -201,11 +201,6 @@ 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