Launch control pr

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

@@ -33,6 +33,8 @@ def __init__(self):
         GEMInterface.__init__(self)
         self.max_send_rate = settings.get('vehicle.max_command_rate',10.0)
         self.ros_sensor_topics = settings.get('vehicle.sensors.ros_topics')
+        self.launch_Control = settings.get('control.launch_control.enabled', False)
+        self.stage_Duration = settings.get('control.launch_control.stage_duration', 2)
         self.last_command_time = 0.0
         self.last_reading = GEMVehicleReading()
         self.last_reading.speed = 0.0
@@ -316,6 +318,20 @@ def send_command(self, command : GEMVehicleCommand):
         self.accel_cmd.enable  = True
         self.accel_cmd.clear   = False
         self.accel_cmd.ignore  = False
+
+        # Launch control 
+        if self.launch_Control:
+            currTime = rospy.get_time() - self.start_time
+            if currTime < self.stage_Duration:
+                self.brake_cmd.f64_cmd = maxbrake
+                self.accel_cmd.f64_cmd = 0
+            elif currTime < self.stage_Duration*2:
+                self.brake_cmd.f64_cmd = maxbrake
+                self.accel_cmd.f64_cmd = maxacc
+            elif currTime < self.stage_Duration*3:
+                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)