From 32c90b400b9e1f01833a3bd9788abdab03565978 Mon Sep 17 00:00:00 2001 From: Kris Hauser Date: Tue, 28 Jan 2025 17:04:38 -0600 Subject: [PATCH 01/17] Fixed Klampt drawing problem onboard... still mysterious --- GEMstack/utils/klampt_visualization.py | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/GEMstack/utils/klampt_visualization.py b/GEMstack/utils/klampt_visualization.py index 0e9219e76..5a73fe95d 100644 --- a/GEMstack/utils/klampt_visualization.py +++ b/GEMstack/utils/klampt_visualization.py @@ -198,7 +198,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) > 50: + vis.add(name,[list(p) for p in path.points[::len(path.points)//50]],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 +214,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) > 50: + vis.add(name+"_%d" % i,seg[::len(seg)//50],**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 +290,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) From f51c6a0e0b9605a7fc56a51c7cd28eeec7d0ab3f Mon Sep 17 00:00:00 2001 From: animesh Date: Sun, 2 Feb 2025 14:50:45 -0600 Subject: [PATCH 02/17] Adding github action for linting and documentation --- .github/workflows/python-app.yml | 59 ++++++++++++++++++++++++++++++++ 1 file changed, 59 insertions(+) create mode 100644 .github/workflows/python-app.yml diff --git a/.github/workflows/python-app.yml b/.github/workflows/python-app.yml new file mode 100644 index 000000000..165bd6c09 --- /dev/null +++ b/.github/workflows/python-app.yml @@ -0,0 +1,59 @@ +# 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 + + +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 + if [ -f requirements.txt ]; then pip install -r requirements.txt; fi + - 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 + if [ -f requirements.txt ]; then pip install -r requirements.txt; fi + - 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@v2 + with: + name: documentation + path: documents/build From 86cd573b8cd17a7ba4c0bceeaf8b97de9dc136d0 Mon Sep 17 00:00:00 2001 From: animesh Date: Sun, 2 Feb 2025 14:56:01 -0600 Subject: [PATCH 03/17] Adding github action trigger on all commits --- .github/workflows/python-app.yml | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/.github/workflows/python-app.yml b/.github/workflows/python-app.yml index 165bd6c09..ab65f49b8 100644 --- a/.github/workflows/python-app.yml +++ b/.github/workflows/python-app.yml @@ -3,6 +3,12 @@ name: Python application +on: + push: + branches: + - '**' + pull_request: + permissions: contents: read From 0d4627c58e7bccd7e1cdf04d4da340fabce95037 Mon Sep 17 00:00:00 2001 From: animesh Date: Sun, 2 Feb 2025 14:57:09 -0600 Subject: [PATCH 04/17] Upgrading version of upload artifact --- .github/workflows/python-app.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/python-app.yml b/.github/workflows/python-app.yml index ab65f49b8..5966ffbd7 100644 --- a/.github/workflows/python-app.yml +++ b/.github/workflows/python-app.yml @@ -59,7 +59,7 @@ jobs: # 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@v2 + uses: actions/upload-artifact@v4 with: name: documentation path: documents/build From daaaa1755fede7fb3be69334d8fc06e13d4c4bec Mon Sep 17 00:00:00 2001 From: animesh Date: Sun, 2 Feb 2025 15:03:47 -0600 Subject: [PATCH 05/17] Optimizing the workflow yaml --- .github/workflows/python-app.yml | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/.github/workflows/python-app.yml b/.github/workflows/python-app.yml index 5966ffbd7..19f573309 100644 --- a/.github/workflows/python-app.yml +++ b/.github/workflows/python-app.yml @@ -28,7 +28,6 @@ jobs: run: | python -m pip install --upgrade pip pip install flake8 flake8-docstrings pep8-naming - if [ -f requirements.txt ]; then pip install -r requirements.txt; fi - name: Lint with flake8 run: | # stop the build if there are Python syntax errors or undefined names @@ -49,6 +48,13 @@ jobs: uses: actions/setup-python@v3 with: python-version: "3.10" + - name: Cache dependencies + - uses: actions/cache@v2 + - with: + path: ~/.cache/pip + keys: ${{ runner.os}}-python-${{hashFiles('**/requirements.txt')}} + restore-keys: | + ${{runner.os}}-python- - name: Install dependencies run: | python -m pip install --upgrade pip From 6100ea594917d2602f1033a8901391ce018e8447 Mon Sep 17 00:00:00 2001 From: animesh Date: Sun, 2 Feb 2025 15:06:04 -0600 Subject: [PATCH 06/17] Optimizing the workflow yaml --- .github/workflows/python-app.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/python-app.yml b/.github/workflows/python-app.yml index 19f573309..c95cb087c 100644 --- a/.github/workflows/python-app.yml +++ b/.github/workflows/python-app.yml @@ -49,8 +49,8 @@ jobs: with: python-version: "3.10" - name: Cache dependencies - - uses: actions/cache@v2 - - with: + uses: actions/cache@v2 + with: path: ~/.cache/pip keys: ${{ runner.os}}-python-${{hashFiles('**/requirements.txt')}} restore-keys: | From 3a2f8e1a784e012ea32c587f5e28fe72b6ee0d10 Mon Sep 17 00:00:00 2001 From: animesh Date: Sun, 2 Feb 2025 15:07:57 -0600 Subject: [PATCH 07/17] Correcting in the key names --- .github/workflows/python-app.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/python-app.yml b/.github/workflows/python-app.yml index c95cb087c..fa040db13 100644 --- a/.github/workflows/python-app.yml +++ b/.github/workflows/python-app.yml @@ -52,7 +52,7 @@ jobs: uses: actions/cache@v2 with: path: ~/.cache/pip - keys: ${{ runner.os}}-python-${{hashFiles('**/requirements.txt')}} + key: ${{ runner.os}}-python-${{hashFiles('**/requirements.txt')}} restore-keys: | ${{runner.os}}-python- - name: Install dependencies From 79c3a683b8def9dbe0f9123061e7bb741afcaaaa Mon Sep 17 00:00:00 2001 From: animesh Date: Sun, 2 Feb 2025 15:13:52 -0600 Subject: [PATCH 08/17] Removing requirements.txt installation in documentation --- .github/workflows/python-app.yml | 8 -------- 1 file changed, 8 deletions(-) diff --git a/.github/workflows/python-app.yml b/.github/workflows/python-app.yml index fa040db13..b452f3c51 100644 --- a/.github/workflows/python-app.yml +++ b/.github/workflows/python-app.yml @@ -48,18 +48,10 @@ jobs: uses: actions/setup-python@v3 with: python-version: "3.10" - - name: Cache dependencies - uses: actions/cache@v2 - with: - path: ~/.cache/pip - key: ${{ runner.os}}-python-${{hashFiles('**/requirements.txt')}} - restore-keys: | - ${{runner.os}}-python- - name: Install dependencies run: | python -m pip install --upgrade pip pip install sphinx sphinx-rtd-theme - if [ -f requirements.txt ]; then pip install -r requirements.txt; fi - name: Generate Documentation run: | # stop the build if there are Python syntax errors or undefined names From ad4af5bf8162b5a0a7512d208abcac6e3e19a779 Mon Sep 17 00:00:00 2001 From: animesh Date: Sun, 2 Feb 2025 15:15:07 -0600 Subject: [PATCH 09/17] Correcting name of build artifact for documentation folder --- .github/workflows/python-app.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/python-app.yml b/.github/workflows/python-app.yml index b452f3c51..541b071a7 100644 --- a/.github/workflows/python-app.yml +++ b/.github/workflows/python-app.yml @@ -60,4 +60,4 @@ jobs: uses: actions/upload-artifact@v4 with: name: documentation - path: documents/build + path: docs/build From e1a0f8ef9a9f85c1c1f834ba7138bec397b3f56f Mon Sep 17 00:00:00 2001 From: Animesh Singh Date: Sun, 2 Feb 2025 15:18:35 -0600 Subject: [PATCH 10/17] Removing action trigger on PR --- .github/workflows/python-app.yml | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/.github/workflows/python-app.yml b/.github/workflows/python-app.yml index 541b071a7..3de575d8e 100644 --- a/.github/workflows/python-app.yml +++ b/.github/workflows/python-app.yml @@ -7,8 +7,7 @@ on: push: branches: - '**' - pull_request: - + permissions: contents: read From 1a262761ebe6ee39d67649c894c8dfe193720dcf Mon Sep 17 00:00:00 2001 From: Kris Hauser Date: Mon, 3 Feb 2025 12:31:56 -0500 Subject: [PATCH 11/17] Better docstring --- GEMstack/utils/config.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/GEMstack/utils/config.py b/GEMstack/utils/config.py index 9112107e9..ac7fbbaa7 100644 --- a/GEMstack/utils/config.py +++ b/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) From 01751744435b632225e0bdbf75534b845f594007 Mon Sep 17 00:00:00 2001 From: Kris Hauser Date: Mon, 3 Feb 2025 12:56:54 -0500 Subject: [PATCH 12/17] Fixed to work with Klampt 0.10 --- .../visualization/klampt_visualization.py | 40 +++++++++++++------ 1 file changed, 28 insertions(+), 12 deletions(-) diff --git a/GEMstack/onboard/visualization/klampt_visualization.py b/GEMstack/onboard/visualization/klampt_visualization.py index 3e07aed23..762a747b4 100644 --- a/GEMstack/onboard/visualization/klampt_visualization.py +++ b/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) From be315de599389a89c33438af9ff33fd555eb1d5e Mon Sep 17 00:00:00 2001 From: Kris Hauser Date: Mon, 3 Feb 2025 12:46:58 -0500 Subject: [PATCH 13/17] Fixed diff that was somehow committed --- GEMstack/utils/klampt_visualization.py | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/GEMstack/utils/klampt_visualization.py b/GEMstack/utils/klampt_visualization.py index 5a73fe95d..063f690c7 100644 --- a/GEMstack/utils/klampt_visualization.py +++ b/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) From 503a403e05e46eb1d09706918ce06c9a146b2bf6 Mon Sep 17 00:00:00 2001 From: Kris Hauser Date: Mon, 17 Feb 2025 09:39:38 -0600 Subject: [PATCH 14/17] Fixed ROS type typo --- GEMstack/onboard/interface/gem_hardware.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/GEMstack/onboard/interface/gem_hardware.py b/GEMstack/onboard/interface/gem_hardware.py index 3f2f26994..e13ff817e 100644 --- a/GEMstack/onboard/interface/gem_hardware.py +++ b/GEMstack/onboard/interface/gem_hardware.py @@ -186,7 +186,7 @@ def callback_with_gnss_reading(msg: INSNavGeod): ) speed = np.sqrt(msg.ve**2 + msg.vn**2) callback(GNSSReading(pose,speed,('error' if msg.error else 'ok'))) - self.gnss_sub = rospy.Subscriber(topic, Inspva, callback_with_gnss_reading) + self.gnss_sub = rospy.Subscriber(topic, INSNavGeod, callback_with_gnss_reading) elif name == 'top_lidar': topic = self.ros_sensor_topics[name] if type is not None and (type is not PointCloud2 and type is not np.ndarray): From 2b413fab941c7ab5ac6e6bc9a7dd37381f90874d Mon Sep 17 00:00:00 2001 From: krishauser Date: Mon, 24 Feb 2025 11:01:11 -0500 Subject: [PATCH 15/17] Added items committed to s2024 --- GEMstack/mathutils/dubins.py | 2 +- GEMstack/mathutils/transforms.py | 21 +++++++++++++++++++++ GEMstack/onboard/interface/gem_hardware.py | 9 ++++++--- 3 files changed, 28 insertions(+), 4 deletions(-) diff --git a/GEMstack/mathutils/dubins.py b/GEMstack/mathutils/dubins.py index cdb4655ab..b840201d9 100644 --- a/GEMstack/mathutils/dubins.py +++ b/GEMstack/mathutils/dubins.py @@ -44,7 +44,7 @@ def derivative(self,x,u): right = [-fwd[1],fwd[0]] phi = u[1] d = u[0] - return np.array([fwd[0]*d,fwd[1]*d,phi]) + return np.array([fwd[0]*d,fwd[1]*d,phi*d]) class DubinsCarIntegrator(ControlSpace): diff --git a/GEMstack/mathutils/transforms.py b/GEMstack/mathutils/transforms.py index 833ecc80d..a29ec48e2 100644 --- a/GEMstack/mathutils/transforms.py +++ b/GEMstack/mathutils/transforms.py @@ -36,6 +36,14 @@ def vector_dist(v1, v2) -> float: """Euclidean distance between two vectors""" return vo.distance(v1,v2) +def vector_dot(v1, v2) -> float: + """Dot product between two vectors""" + return vo.dot(v1,v2) + +def vector_cross(v1, v2) -> float: + """Cross product between two 2D vectors""" + return vo.cross(v1,v2) + def vector2_angle(v1, v2 = None) -> float: """find the ccw angle bewtween two 2d vectors""" if v2 is None: @@ -123,3 +131,16 @@ def xy_to_lat_lon(x_east : float, y_north : float, lat_reference : float, lon_re # convert GNSS waypoints into local fixed frame reprented in x and y lat, lon = axy.xy2ll(x_east, y_north, lat_reference, lon_reference) return lat, lon + +def quaternion_to_euler(x : float, y : float, z : float, w : float): + t0 = +2.0 * (w * x + y * z) + t1 = +1.0 - 2.0 * (x * x + y * y) + roll = np.arctan2(t0, t1) + t2 = +2.0 * (w * y - z * x) + t2 = +1.0 if t2 > +1.0 else t2 + t2 = -1.0 if t2 < -1.0 else t2 + pitch = np.arcsin(t2) + t3 = +2.0 * (w * z + x * y) + t4 = +1.0 - 2.0 * (y * y + z * z) + yaw = np.arctan2(t3, t4) + return [roll, pitch, yaw] diff --git a/GEMstack/onboard/interface/gem_hardware.py b/GEMstack/onboard/interface/gem_hardware.py index e13ff817e..4fb95d4fc 100644 --- a/GEMstack/onboard/interface/gem_hardware.py +++ b/GEMstack/onboard/interface/gem_hardware.py @@ -1,6 +1,7 @@ from .gem import * from ...utils import settings import math +import time # ROS Headers import rospy @@ -151,6 +152,7 @@ def subscribe_sensor(self, name, callback, type = None): if name == 'gnss': topic = self.ros_sensor_topics[name] if topic.endswith('inspva'): + #GEM e2 uses Novatel GNSS if type is not None and (type is not Inspva and type is not GNSSReading): raise ValueError("GEMHardwareInterface GEM e2 only supports Inspva/GNSSReading for GNSS") if type is Inspva: @@ -169,7 +171,7 @@ def callback_with_gnss_reading(inspva_msg: Inspva): callback(GNSSReading(pose,speed,inspva_msg.status)) self.gnss_sub = rospy.Subscriber(topic, Inspva, callback_with_gnss_reading) else: - #assume it's septentrio + #assume it's septentrio on GEM e4 if type is not None and (type is not INSNavGeod and type is not GNSSReading): raise ValueError("GEMHardwareInterface GEM e4 only supports INSNavGeod/GNSSReading for GNSS") if type is INSNavGeod: @@ -177,8 +179,9 @@ def callback_with_gnss_reading(inspva_msg: Inspva): else: def callback_with_gnss_reading(msg: INSNavGeod): pose = ObjectPose(ObjectFrameEnum.GLOBAL, - x=msg.longitude, - y=msg.latitude, + t=time.time(), + x=math.degrees(msg.longitude), #Septentrio GNSS uses radians rather than degrees + y=math.degrees(msg.latitude), z=msg.height, yaw=math.radians(msg.heading), #heading from north in degrees (TODO: maybe?? check this) roll=math.radians(msg.roll), From 43bb3e8c53b6395fecb346d42b010fd052ab493d Mon Sep 17 00:00:00 2001 From: krishauser Date: Mon, 24 Feb 2025 11:04:02 -0500 Subject: [PATCH 16/17] Changed time.time to self.time --- GEMstack/onboard/interface/gem_hardware.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/GEMstack/onboard/interface/gem_hardware.py b/GEMstack/onboard/interface/gem_hardware.py index 4fb95d4fc..836d7ef71 100644 --- a/GEMstack/onboard/interface/gem_hardware.py +++ b/GEMstack/onboard/interface/gem_hardware.py @@ -179,7 +179,7 @@ def callback_with_gnss_reading(inspva_msg: Inspva): else: def callback_with_gnss_reading(msg: INSNavGeod): pose = ObjectPose(ObjectFrameEnum.GLOBAL, - t=time.time(), + t=self.time(), x=math.degrees(msg.longitude), #Septentrio GNSS uses radians rather than degrees y=math.degrees(msg.latitude), z=msg.height, 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 17/17] 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)