From b9d258662e3a59d412f2acab30f8d268c1f7f71b Mon Sep 17 00:00:00 2001 From: Adi Pillai <76452932+Adi9103@users.noreply.github.com> Date: Tue, 15 Apr 2025 17:55:51 -0500 Subject: [PATCH 1/2] Launch control PR --- GEMstack/knowledge/defaults/current.yaml | 80 +++++++++++++--------- GEMstack/onboard/interface/gem_hardware.py | 13 ++++ GEMstack/onboard/planning/pure_pursuit.py | 1 + 3 files changed, 61 insertions(+), 33 deletions(-) diff --git a/GEMstack/knowledge/defaults/current.yaml b/GEMstack/knowledge/defaults/current.yaml index c886288be..bc14fcae0 100644 --- a/GEMstack/knowledge/defaults/current.yaml +++ b/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 \ No newline at end of file + + # 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 diff --git a/GEMstack/onboard/interface/gem_hardware.py b/GEMstack/onboard/interface/gem_hardware.py index 836d7ef71..e8496809f 100644 --- a/GEMstack/onboard/interface/gem_hardware.py +++ b/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 diff --git a/GEMstack/onboard/planning/pure_pursuit.py b/GEMstack/onboard/planning/pure_pursuit.py index e4b6f4f4e..de0ccbfed 100644 --- a/GEMstack/onboard/planning/pure_pursuit.py +++ b/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) From 27be0c019f4e7fb44eefb98d4cf42d36c58c8154 Mon Sep 17 00:00:00 2001 From: Colin Lane Date: Tue, 15 Apr 2025 23:34:43 -0500 Subject: [PATCH 2/2] stage duration + settings readings --- GEMstack/onboard/interface/gem_hardware.py | 23 ++++++++++++---------- 1 file changed, 13 insertions(+), 10 deletions(-) diff --git a/GEMstack/onboard/interface/gem_hardware.py b/GEMstack/onboard/interface/gem_hardware.py index e8496809f..c7c2e9c92 100644 --- a/GEMstack/onboard/interface/gem_hardware.py +++ b/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 @@ -318,16 +320,17 @@ def send_command(self, command : GEMVehicleCommand): 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 + 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