Launch control pr

.github/workflows/python-app.yml

@@ -0,0 +1,62 @@
+# This workflow will install Python dependencies, run tests and lint with a single version of Python
+# For more information see: https://docs.github.com/en/actions/automating-builds-and-tests/building-and-testing-python
+
+name: Python application
+
+on:
+  push:
+    branches: 
+    - '**'
+
+
+permissions:
+  contents: read
+
+jobs:
+  PEP-Guidelines:
+
+    runs-on: ubuntu-latest
+
+    steps:
+    - uses: actions/checkout@v3
+    - name: Set up Python 3.10
+      uses: actions/setup-python@v3
+      with:
+        python-version: "3.10"
+    - name: Install dependencies
+      run: |
+        python -m pip install --upgrade pip
+        pip install flake8 flake8-docstrings pep8-naming
+    - name: Lint with flake8
+      run: |
+        # stop the build if there are Python syntax errors or undefined names
+        flake8 ./GEMstack --count --select=E9,F63,F7,F82 --show-source --statistics --exclude=__init__.py || exit 1
+        # to enable more advanced checks on the repo, uncomment the lines below (There are around 3000 violations)
+        # flake8 ./GEMstack --ignore=D,C901,E402,E231 --count --max-complexity=10 --max-line-length=127 --statistics --exclude=__init__.py || exit 1
+        # if we want to enable documentation checks, uncomment the line below
+        # flake8 ./GEMstack --ignore=E128,E402,E501,F401 --docstring-convention pep257 --max-line-length=120 --exclude=__init__.py || exit 1
+      continue-on-error: false
+
+  Documentation:
+
+    runs-on: ubuntu-latest
+
+    steps:
+    - uses: actions/checkout@v3
+    - name: Set up Python 3.10
+      uses: actions/setup-python@v3
+      with:
+        python-version: "3.10"
+    - name: Install dependencies
+      run: |
+        python -m pip install --upgrade pip
+        pip install sphinx sphinx-rtd-theme
+    - name: Generate Documentation
+      run: |
+        # stop the build if there are Python syntax errors or undefined names
+        sphinx-build -b html docs docs/build
+    - name: Save Documentation as Artifact
+      uses: actions/upload-artifact@v4
+      with:
+        name: documentation
+        path: docs/build

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

@@ -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

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)

GEMstack/utils/klampt_visualization.py

@@ -6,6 +6,10 @@
 from . import settings
 from ..state import ObjectFrameEnum,ObjectPose,PhysicalObject,VehicleState,VehicleGearEnum,Path,Obstacle,AgentState,AgentEnum,Roadgraph,RoadgraphLane,RoadgraphLaneEnum,RoadgraphCurve,RoadgraphCurveEnum,RoadgraphRegion,RoadgraphRegionEnum,RoadgraphSurfaceEnum,Trajectory,Route,SceneState,AllState
 
+#KH: there is a bug on some system where the visualization crashes with an OpenGL error when drawing curves
+#this is a workaround.  We really should find the source of the bug!
+MAX_POINTS_IN_CURVE = 50
+
 OBJECT_COLORS = {
     AgentEnum.CAR : (1,1,0,1),
     AgentEnum.PEDESTRIAN : (0,1,0,1),
@@ -198,7 +202,10 @@ def plot_vehicle(vehicle : VehicleState, vehicle_model=None, axis_len=1.0):
             vehicle_model.link('rear_left_stop_light_link').appearance().setColor(0.3,0,0,1)
 
 def plot_path(name : str, path : Path, color=(0,0,0), width=1):
-    vis.add(name,[list(p) for p in path.points],color=color,width=width)
+    if len(path.points) > MAX_POINTS_IN_CURVE:  # downsample due to OpenGL error?
+        vis.add(name,[list(p) for p in path.points[::len(path.points)//MAX_POINTS_IN_CURVE]],color=color,width=width)
+    else:
+        vis.add(name,[list(p) for p in path.points],color=color,width=width)
 
 def plot_curve(name : str, curve : RoadgraphCurve, color=None, width=None):
     style = CURVE_TO_STYLE.get(curve.type,CURVE_TO_STYLE[None])
@@ -211,7 +218,10 @@ def plot_curve(name : str, curve : RoadgraphCurve, color=None, width=None):
     if width is not None:
         style['width'] = width
     for i,seg in enumerate(curve.segments):
-        vis.add(name+"_%d" % i,seg,**style)
+        if len(seg) > MAX_POINTS_IN_CURVE:  # downsample due to OpenGL error?
+            vis.add(name+"_%d" % i,seg[::len(seg)//MAX_POINTS_IN_CURVE],**style)
+        else:
+            vis.add(name+"_%d" % i,seg,**style)
 
 def plot_lane(name : str, lane : RoadgraphLane, on_route=False):
     if lane.surface != RoadgraphSurfaceEnum.PAVEMENT:
@@ -284,6 +294,6 @@ def plot_scene(scene : SceneState, ground_truth_vehicle=None, vehicle_model = No
 def plot(state : AllState, ground_truth_vehicle = None, vehicle_model=None, title=None, show=True):
     plot_scene(state, ground_truth_vehicle=ground_truth_vehicle, vehicle_model=vehicle_model, title=title, show=show)
     if state.route is not None:
-        plot_path("route",state.route,color=(1,0.5,0,1))
+        plot_path("route",state.route,color=(1,0.5,0,1),width=2)
     if state.trajectory is not None:
-        plot_path("trajectory",state.trajectory,color=(1,0,0,1),width=2)
+        plot_path("trajectory",state.trajectory,color=(1,0,0,1),width=3)