Merge branch 's2025_teamA' into EstCal_A

merging team a main to local

Author: AadarshHegde123

.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/onboard/visualization/klampt_visualization.py

@@ -1,4 +1,5 @@
 from ..component import Component
+from klampt import __version__ as klampt_version
 from klampt import vis
 from klampt.math import se3
 from klampt import *
@@ -11,14 +12,16 @@
 import numpy as np
 
 class KlamptVisualization(Component):
-    """Runs a matplotlib visualization at 10Hz. 
-    
-    If save_as is not None, saves the visualization to a file.
+    """Runs a Klampt visualization.
+     
+    Runs at 20Hz by default. 
     """
     def __init__(self, vehicle_interface, rate : float = 20.0, save_as : str = None):
         self.vehicle_interface = vehicle_interface
         self._rate = rate
         self.save_as = save_as
+        if save_as is not None:
+            print("WARNING: automatic saving of KlamptVisualization to movie is not supported yet. You can use Ctrl+M to start / stop saving the movie")
         self.num_updates = 0
         self.last_yaw = None
         self.plot_values = {}
@@ -57,12 +60,20 @@ def state_inputs(self):
     def initialize(self):
         vis.setWindowTitle("GEMstack visualization")
         vp = vis.getViewport()
-        vp.camera.rot[1] = -0.15
-        vp.camera.rot[2] = -math.pi/2
-        vp.camera.dist = 30.0
-        vp.w = 1280
-        vp.h = 720
-        vp.clippingplanes = (0.1,1000)
+        if klampt_version == '0.10.0':
+            vp.controller.rot[1] = -0.15
+            vp.controller.rot[2] = -math.pi/2
+            vp.controller.dist = 30.0
+            vp.resize(1280,720)
+            vp.n = 0.1
+            vp.f = 1000
+        else:
+            vp.camera.rot[1] = -0.15
+            vp.camera.rot[2] = -math.pi/2
+            vp.camera.dist = 30.0
+            vp.w = 1280
+            vp.h = 720
+            vp.clippingplanes = (0.1,1000)
         vis.setViewport(vp)
         vis.add("vehicle_plane",self.world.terrain(0),hide_label=True)
         #note: show() takes over the interrupt handler and sets it to default, so we restore it
@@ -121,9 +132,14 @@ def update(self, state):
                 center_offset = 1.0
                 lookahead = 4.0*v
                 dx,dy = math.cos(tracked_vehicle.pose.yaw)*(lookahead+center_offset),math.sin(tracked_vehicle.pose.yaw)*(lookahead+center_offset)
-                vp.camera.tgt = [tracked_vehicle.pose.x+dx,tracked_vehicle.pose.y+dy,1.5]
-                vp.camera.rot[2] += tracked_vehicle.pose.yaw - self.last_yaw
-                vp.camera.dist += 5.0*(v - self.last_v)
+                if klampt_version == '0.10.0':
+                    vp.controller.tgt = [tracked_vehicle.pose.x+dx,tracked_vehicle.pose.y+dy,1.5]
+                    vp.controller.rot[2] += tracked_vehicle.pose.yaw - self.last_yaw
+                    vp.controller.dist += 5.0*(v - self.last_v)
+                else:
+                    vp.camera.tgt = [tracked_vehicle.pose.x+dx,tracked_vehicle.pose.y+dy,1.5]
+                    vp.camera.rot[2] += tracked_vehicle.pose.yaw - self.last_yaw
+                    vp.camera.dist += 5.0*(v - self.last_v)
                 self.last_v = v
                 vis.setViewport(vp)
 

GEMstack/utils/config.py

@@ -18,7 +18,7 @@ def save_config(fn : str, config : dict) -> None:
 
 
 def load_config_recursive(fn : str) -> dict:
-    """Loads a configuration file with !include directives."""
+    """Loads a configuration file with !include and !relative_path directives."""
     if fn.endswith('yaml') or fn.endswith('yml'):
         with open(fn,'r') as f:
             res = yaml.load(f,_Loader)
@@ -35,7 +35,7 @@ def load_config_recursive(fn : str) -> dict:
 
 
 class _Loader(yaml.SafeLoader):
-    """YAML Loader with `!include` constructor."""
+    """YAML Loader with `!include` and `!relative_path` directives."""
 
     def __init__(self, stream: IO) -> None:
         """Initialise Loader."""
@@ -60,7 +60,7 @@ def _construct_relative_path(loader: _Loader, node: yaml.Node) -> Any:
 yaml.add_constructor('!relative_path', _construct_relative_path, _Loader)
 
 def _load_config_or_text_recursive(fn : str) -> dict:
-    """Loads a configuration file with !include directives."""
+    """Loads a configuration file with !include and !relative_path directives."""
     if fn.endswith('yaml') or fn.endswith('yml'):
         with open(fn,'r') as f:
             res = yaml.load(f,_Loader)

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,8 +202,8 @@ 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):
-    if len(path.points) > 50:
-        vis.add(name,[list(p) for p in path.points[::len(path.points)//50]],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)
 
@@ -214,8 +218,8 @@ 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):
-        if len(seg) > 50:
-            vis.add(name+"_%d" % i,seg[::len(seg)//50],**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)
 
@@ -290,12 +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:
-<<<<<<< HEAD
-        plot_path("route",state.route,color=(1,0.5,0,1))
-    if state.trajectory is not None:
-        plot_path("trajectory",state.trajectory,color=(1,0,0,1),width=2)
-=======
         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=3)
->>>>>>> 32c90b400b9e1f01833a3bd9788abdab03565978

HOMEWORK.md

@@ -54,7 +54,7 @@ Part 1:
 
     Replicate your sensor printing and distress signal code in the BlinkDistress class in blink_component.py.  You will need to adapt your code from using ROS subscribers / publishers to using the `GEMVehicleInterface` and `GEMVehicleReading` objects provided in the template code.  Opening the GEMstack folder in the VSCode IDE will help you find the documentation for these classes.
 
-    Run your code in simulation first.  (If you are running on the vehicle, first open one terminal window and run `roscore`. Keep this running.)  Then, in the GEMstack folder, run `python3 main.py --variant=sim launch/blink_launch.yaml`.  You should see your blinking sequence in a matplotlib window.  Use Ctrl+C to quit.
+    Run your code in simulation first.  (If you are running on the vehicle, first open one terminal window and run `roscore`. Keep this running.)  Then, in the GEMstack folder, run `python3 main.py --variant=sim launch/blink_launch.yaml`.  You should see your blinking sequence in a 3D visualization window.  Use Ctrl+C to quit.
 
     Now, run your code on the real vehicle.  Enable Pacmod control as before, then run `python3 main.py launch/blink_launch.yaml`.
 

homework/blink_launch.yaml

@@ -5,10 +5,16 @@ mission_execution: StandardExecutor
 require_engaged: False
 # Recovery behavior after a component failure
 recovery: 
+    perception:
+        state_estimation : GNSSStateEstimator
+        perception_normalization : StandardPerceptionNormalizer
     planning: 
         trajectory_tracking : blink_component.BlinkDistress
 # Driving behavior for the GEM vehicle.  Runs perception and planner but doesn't execute anything (no controller).
 drive: 
+    perception:
+        state_estimation : GNSSStateEstimator
+        perception_normalization : StandardPerceptionNormalizer
     planning:
         trajectory_tracking : blink_component.BlinkDistress
 log: