move launch control logic out of gem.py and gem_hardware.py and into the planners (pure_pursuit and stanley)

Author: Demon16th

GEMstack/onboard/interface/gem.py

@@ -135,7 +135,7 @@ def hardware_faults(self) -> List[str]:
         """
         raise NotImplementedError()
 
-    def simple_command(self, acceleration_mps2: float, steering_wheel_angle: float, state: VehicleState = None, brake_override: Optional[float] = None, launch_control: bool = False) -> GEMVehicleCommand:
+    def simple_command(self, acceleration_mps2: float, steering_wheel_angle: float, state: VehicleState = None) -> GEMVehicleCommand:
         """"
         Returns a command according to a desired acceleration and steering angle
 
@@ -147,29 +147,8 @@ def simple_command(self, acceleration_mps2: float, steering_wheel_angle: float,
         pitch = state.pose.pitch if state is not None and state.pose.pitch is not None else 0.0
         v = state.v if state is not None else 0.0
         gear = state.gear if state is not None else 1
-        stage_duration = settings.get('control.launch_control.stage_duration', 0.5)
-
-        if launch_control and v < 0.1:
-            if not hasattr(self,'_launch_start_time'):
-                self._launch_start_time = self.time()
-            elapsed = self.time() - self._launch_start_time
-            if elapsed < stage_duration:
-                acc_pos = 0.0
-                brake_pos = 1.0
-            elif elapsed < 2 * stage_duration:
-                acc_pos = 1.0
-                brake_pos = 1.0
-            else:
-                acc_pos = 1.0
-                brake_pos = 0.0
-        else:
-            if hasattr(self, '_launch_start_time'):
-                del self._launch_start_time
-            if brake_override is not None:
-                acc_pos = np.clip(acceleration_mps2 / self.max_accel, 0.0, 1.0)
-                brake_pos = np.clip(brake_override, 0.0, 1.0)
-            else:
-                acc_pos, brake_pos, gear = acceleration_to_pedal_positions(acceleration_mps2, v, pitch, gear)
+
+        acc_pos, brake_pos, gear = acceleration_to_pedal_positions(acceleration_mps2, v, pitch, gear)
 
 
         # acc_pos,brake_pos,gear = acceleration_to_pedal_positions(acceleration_mps2, v, pitch, gear)

GEMstack/onboard/interface/gem_hardware.py

@@ -101,13 +101,6 @@ 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
@@ -335,39 +328,6 @@ def send_command(self, command : GEMVehicleCommand):
         self.accel_cmd.enable  = True
         self.accel_cmd.clear   = False
         self.accel_cmd.ignore  = False
-
-        # enable_launch_control = settings.get('control.launch_control.enable', False)
-        # stage_duration = 0.5
-        # currTime = rospy.get_time() - self.start_time
-
-
-        # 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
-        
-        # if command.launch_control and self.last_reading.speed == 0:
-        #     self.brake_cmd.f64_cmd = command.brake_pedal_position
-        #     self.accel_cmd.f64_cmd = command.accelerator_pedal_position
-        # else:
-        #     self.accel_cmd.f64_cmd = command.accelerator_pedal_position
-        #     if command.brake_pedal_position > 0.0:
-        #         self.accel_cmd.f64_cmd = 0.0
-        #     self.brake_cmd.f64_cmd = command.brake_pedal_position
-
-        # self.brake_cmd.f64_cmd = maxbrake
-        # self.accel_cmd.f64_cmd = 0
 
 
         self.gear_cmd.ui16_cmd = PacmodCmd.SHIFT_FORWARD

GEMstack/onboard/planning/pure_pursuit.py

@@ -8,6 +8,7 @@
 from ..interface.gem import GEMVehicleCommand
 from ..component import Component
 import numpy as np
+import rospy
 
 class PurePursuit(object):   
     """Implements a pure pursuit controller on a second-order Dubins vehicle."""
@@ -241,6 +242,8 @@ class PurePursuitTrajectoryTracker(Component):
     def __init__(self,vehicle_interface=None, **args):
         self.pure_pursuit = PurePursuit(**args)
         self.vehicle_interface = vehicle_interface
+        self.enable_launch_control = settings.get('control.launch_control.enable', False) # and vehicle.v < 0.1
+        self.stage_duration = settings.get('control.launch_control.stage_duration', 0.5)
 
     def rate(self):
         return 50.0
@@ -258,8 +261,26 @@ def update(self, vehicle : VehicleState, trajectory: Trajectory):
         steering_angle = np.clip(front2steer(wheel_angle), self.pure_pursuit.steering_angle_range[0], self.pure_pursuit.steering_angle_range[1])
         #print("Desired steering angle",steering_angle)
 
-        launch_control = self.pure_pursuit.enable_launch_control and vehicle.v < 0.1
-        self.vehicle_interface.send_command(self.vehicle_interface.simple_command(accel,steering_angle, vehicle, launch_control=launch_control))
+        cmd = self.vehicle_interface.simple_command(accel, steering_angle, vehicle)
+
+        if self.enable_launch_control:
+            print("launch control active")
+            if not hasattr(self,'_launch_start_time'):
+                self._launch_start_time = rospy.get_time()  
+            elapsed = rospy.get_time() - self._launch_start_time
+            if elapsed < self.stage_duration:
+                cmd.accelerator_pedal_position = 0.0
+                cmd.brake_pedal_position = 1.0
+            elif elapsed < 2 * self.stage_duration:
+                cmd.accelerator_pedal_position = 1.0
+                cmd.brake_pedal_position = 1.0
+            elif elapsed < 3 * self.stage_duration:
+                cmd.accelerator_pedal_position = 1.0
+                cmd.brake_pedal_position = 0.0
+            else:
+                self.enable_launch_control = False
+
+        self.vehicle_interface.send_command(cmd)
 
     def healthy(self):
         return self.pure_pursuit.path is not None

GEMstack/onboard/planning/stanley.py

@@ -1,4 +1,5 @@
 import numpy as np
+import rospy
 from math import sin, cos, atan2
 
 # These imports match your existing project structure
@@ -70,7 +71,6 @@ def __init__(
         i = settings.get('control.longitudinal_control.pid_i')
         self.pid_speed = PID(p, d, i, windup_limit=20)
 
-        self.enable_launch_control = settings.get('control.launch_control.enable', False)
         self.stage_duration = settings.get('control.launch_control.stage_duration', 0.5)
         self.launch_start_time = None
 
@@ -306,6 +306,8 @@ def __init__(self, vehicle_interface=None, **kwargs):
         self.stanley = Stanley(**kwargs)
         self.vehicle_interface = vehicle_interface
         self.desired_speed_source = settings.get('control.stanley.desired_speed', 'path')
+        self.stage_duration = settings.get('control.launch_control.stage_duration', 0.5)
+        self.enable_launch_control = settings.get('control.launch_control.enable', False) # and vehicle.v < 0.1
 
     def rate(self):
         """Control frequency in Hz."""
@@ -337,7 +339,6 @@ def update(self, vehicle: VehicleState, trajectory: Trajectory):
         else:
             self.stanley.set_path(trajectory)
         accel, f_delta = self.stanley.compute(vehicle, self)
-        launch_control = self.stanley.enable_launch_control and vehicle.v < 0.01
 
         # If your low-level interface expects steering wheel angle:
         steering_angle = front2steer(f_delta)
@@ -347,7 +348,26 @@ def update(self, vehicle: VehicleState, trajectory: Trajectory):
             settings.get('vehicle.geometry.max_steering_angle',  0.5)
         )
 
-        cmd = self.vehicle_interface.simple_command(accel, steering_angle, vehicle, launch_control=launch_control)
+        cmd = self.vehicle_interface.simple_command(accel, steering_angle, vehicle)
+        
+        if self.enable_launch_control:
+            print("launch control active")
+            if not hasattr(self,'_launch_start_time'):
+                self._launch_start_time = rospy.get_time()  
+            elapsed = rospy.get_time() - self._launch_start_time
+            if elapsed < self.stage_duration:
+                cmd.accelerator_pedal_position = 0.0
+                cmd.brake_pedal_position = 1.0
+            elif elapsed < 2 * self.stage_duration:
+                cmd.accelerator_pedal_position = 1.0
+                cmd.brake_pedal_position = 1.0
+            elif elapsed < 3 * self.stage_duration:
+                cmd.accelerator_pedal_position = 1.0
+                cmd.brake_pedal_position = 0.0
+            else:
+                self.enable_launch_control = False
+
+
         self.vehicle_interface.send_command(cmd)
 
     def healthy(self):