From 55ba9a86a6100c7a265c6d3fbb54c097cc5f39c8 Mon Sep 17 00:00:00 2001 From: harishkumarbalaji Date: Tue, 22 Apr 2025 23:44:17 -0500 Subject: [PATCH 1/7] add install ackermann-msgs in docker --- setup/Dockerfile.cuda11.8 | 2 +- setup/Dockerfile.cuda12 | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/setup/Dockerfile.cuda11.8 b/setup/Dockerfile.cuda11.8 index 464b3b29b..459e7dca0 100644 --- a/setup/Dockerfile.cuda11.8 +++ b/setup/Dockerfile.cuda11.8 @@ -68,7 +68,7 @@ COPY requirements.txt /tmp/requirements.txt RUN pip3 install -r /tmp/requirements.txt # Install other Dependencies -RUN apt-get install -y ros-noetic-septentrio-gnss-driver +RUN apt-get install -y ros-noetic-septentrio-gnss-driver ros-noetic-ackermann-msgs USER ${USER} diff --git a/setup/Dockerfile.cuda12 b/setup/Dockerfile.cuda12 index 32005202f..a7ad71e82 100644 --- a/setup/Dockerfile.cuda12 +++ b/setup/Dockerfile.cuda12 @@ -69,7 +69,7 @@ COPY requirements.txt /tmp/requirements.txt RUN pip3 install -r /tmp/requirements.txt # Install other Dependencies -RUN apt-get install -y ros-noetic-septentrio-gnss-driver +RUN apt-get install -y ros-noetic-septentrio-gnss-driver ros-noetic-ackermann-msgs USER ${USER} From ff6b765778f99b02d0cf492821adb6aa2a8f25df Mon Sep 17 00:00:00 2001 From: harishkumarbalaji Date: Tue, 22 Apr 2025 23:49:57 -0500 Subject: [PATCH 2/7] update documentation --- docs/Gazebo Simulation Documentation.md | 10 +++++- launch/README.md | 48 ------------------------- 2 files changed, 9 insertions(+), 49 deletions(-) delete mode 100644 launch/README.md diff --git a/docs/Gazebo Simulation Documentation.md b/docs/Gazebo Simulation Documentation.md index 4e78bf920..d2f51bb03 100644 --- a/docs/Gazebo Simulation Documentation.md +++ b/docs/Gazebo Simulation Documentation.md @@ -78,5 +78,13 @@ Example command launching the fixed route with e4 vehicle: ```bash python3 main.py --variant=gazebo --vehicle=e4_gazebo launch/fixed_route.yaml ``` - +**Variants:** + - sim + - gazebo + +**Vehicle types:** +- e2 +- e4 +- e2_gazebo +- e4_gazebo --- diff --git a/launch/README.md b/launch/README.md deleted file mode 100644 index 4c9e18fc2..000000000 --- a/launch/README.md +++ /dev/null @@ -1,48 +0,0 @@ -Contains launch scripts for use in the standard executor, e.g., `python main.py launch/fixed_route_sim.yaml`. - - -# Gazebo Simulation Guide lines - -## Gazebo Simulation Setup - -Head over [here](https://github.com/harishkumarbalaji/POLARIS_GEM_Simulator/tree/main) to setup gazebo docker container to run the simulation - -Install dependency packages - -``` -sudo apt-get install -y ros-noetic-ackermann-msgs - -``` - - - - -## Running GEM Stack with Gazebo - -- Open two terminals -- Launch the Gazebo simulator in the first terminal -- In the other terminal navigate to GEM Stack folder and run the following command to launch GEM Stack - -``` -Sample command: - -python3 main.py --variant=gazebo --vehicle=e4_gazebo launch/gazebo_simulation.yaml - - -Other variants: - -python3 main.py --variant={variant_name} --vehicle={vehicle_name} launch/gazebo_simulation.yaml - -Variants: -sim - -Vehicle types: - -e2 -e4 -e2_gazebo -e4_gazebo - -``` - -- The gazebo_simulation.yaml can be editied to add your planning and perception code, more variants can be added on request. \ No newline at end of file From cfcd03b3715b3730dbaef3b49786ccaed7e5488f Mon Sep 17 00:00:00 2001 From: danielzhuang11 Date: Thu, 24 Apr 2025 11:15:00 -0500 Subject: [PATCH 3/7] revert current yaml, fix accel/heading states --- GEMstack/knowledge/defaults/current.yaml | 4 ++-- GEMstack/onboard/interface/gem_gazebo.py | 6 ++++++ 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/GEMstack/knowledge/defaults/current.yaml b/GEMstack/knowledge/defaults/current.yaml index 5c6331b23..c886288be 100644 --- a/GEMstack/knowledge/defaults/current.yaml +++ b/GEMstack/knowledge/defaults/current.yaml @@ -12,8 +12,8 @@ control: brake_amount : 0.5 brake_speed : 2.0 pure_pursuit: - lookahead: 4.0 - lookahead_scale: 1.0 + lookahead: 2.0 + lookahead_scale: 3.0 crosstrack_gain: 1.0 desired_speed: trajectory longitudinal_control: diff --git a/GEMstack/onboard/interface/gem_gazebo.py b/GEMstack/onboard/interface/gem_gazebo.py index 23d328054..b78ea0e06 100644 --- a/GEMstack/onboard/interface/gem_gazebo.py +++ b/GEMstack/onboard/interface/gem_gazebo.py @@ -225,6 +225,12 @@ def send_command(self, command : GEMVehicleCommand): # Get current speed v = self.last_reading.speed + + + #update last reading + self.last_reading.accelerator_pedal_position = command.accelerator_pedal_position + self.last_reading.brake_pedal_position = command.brake_pedal_position + self.last_reading.steering_wheel_angle = command.steering_wheel_angle # Convert pedal to acceleration accelerator_pedal_position = np.clip(command.accelerator_pedal_position, 0.0, 1.0) From 1f9a46cd7e48aac5c2e38fd444600dba3e0587ab Mon Sep 17 00:00:00 2001 From: krishauser Date: Thu, 24 Apr 2025 21:18:44 -0400 Subject: [PATCH 4/7] Fixed math error --- GEMstack/mathutils/transforms.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/GEMstack/mathutils/transforms.py b/GEMstack/mathutils/transforms.py index a29ec48e2..4c00cbff2 100644 --- a/GEMstack/mathutils/transforms.py +++ b/GEMstack/mathutils/transforms.py @@ -72,7 +72,7 @@ def point_segment_distance(x,a,b) -> Tuple[float,float]: udotv = np.dot(u,v) if udotv < 0: return vector_norm(u),0 - elif udotv > vnorm: + elif udotv > vnorm**2: return vector_norm(vector_sub(x,b)),1 else: param = udotv/vnorm From 91784d71db6860df83533cf2c44bbb3c05c8fa29 Mon Sep 17 00:00:00 2001 From: krishauser Date: Thu, 24 Apr 2025 21:22:57 -0400 Subject: [PATCH 5/7] Fixed math error --- GEMstack/mathutils/transforms.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/GEMstack/mathutils/transforms.py b/GEMstack/mathutils/transforms.py index 4c00cbff2..7a5a7a8dd 100644 --- a/GEMstack/mathutils/transforms.py +++ b/GEMstack/mathutils/transforms.py @@ -75,8 +75,8 @@ def point_segment_distance(x,a,b) -> Tuple[float,float]: elif udotv > vnorm**2: return vector_norm(vector_sub(x,b)),1 else: - param = udotv/vnorm - return vector_norm(vector_sub(u,vector_mul(v,param/vnorm))),param + param = udotv/vnorm**2 + return vector_norm(vector_sub(u,vector_mul(v,param))),param def rotate2d(point, angle : float, origin=None): """Rotates a point about the origin by an angle""" From 1e9c5063f8ead768c9349d32fa1ab68afb199766 Mon Sep 17 00:00:00 2001 From: danielzhuang11 Date: Mon, 28 Apr 2025 13:23:18 -0500 Subject: [PATCH 6/7] flipped lat and long --- GEMstack/onboard/interface/gem_gazebo.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/GEMstack/onboard/interface/gem_gazebo.py b/GEMstack/onboard/interface/gem_gazebo.py index b78ea0e06..575334136 100644 --- a/GEMstack/onboard/interface/gem_gazebo.py +++ b/GEMstack/onboard/interface/gem_gazebo.py @@ -133,7 +133,7 @@ def gnss_callback_wrapper(gps_msg: NavSatFix): # Convert IMU's yaw to heading (CW from North), then to navigation yaw (CCW from East) # This handles the coordinate frame differences between Gazebo and the navigation frame # Negate yaw to convert from ROS to heading - heading = transforms.yaw_to_heading(-yaw, degrees=False) + heading = transforms.yaw_to_heading(-yaw - np.pi/2, degrees=False) navigation_yaw = transforms.heading_to_yaw( heading, degrees=False) From f29414aa2573c25adf43bffc0090d31d556d6d22 Mon Sep 17 00:00:00 2001 From: krishauser Date: Wed, 30 Apr 2025 14:54:39 -0400 Subject: [PATCH 7/7] Added ability to override settings --- GEMstack/utils/settings.py | 22 ++++++++++++++++------ main.py | 12 +++++++++--- 2 files changed, 25 insertions(+), 9 deletions(-) diff --git a/GEMstack/utils/settings.py b/GEMstack/utils/settings.py index b2470ce25..65d1ef467 100644 --- a/GEMstack/utils/settings.py +++ b/GEMstack/utils/settings.py @@ -1,23 +1,33 @@ import json -from ..knowledge import defaults import copy from typing import List,Union,Any SETTINGS = None -def load_settings(): +def load_settings(settings_file : str = None): """Loads the settings object for the first time. - Order of operations is to look into defaults.SETTINGS, and then - look through the command line arguments to determine whether the user has - overridden any settings using --KEY=VALUE. + Order of operations is: + - If settings_file is given, load it. + - Otherwise, get the settings from defaults.SETTINGS + - Look through the command line arguments to determine whether the user has + overridden any settings using --KEY=VALUE. """ global SETTINGS if SETTINGS is not None: return import os import sys - SETTINGS = copy.deepcopy(defaults.SETTINGS) + if settings_file is not None: + from .config import load_config_recursive + import os + print("**************************************************************") + print("Loading global settings from",settings_file) + print("**************************************************************") + SETTINGS = load_config_recursive(os.path.abspath(settings_file)) + else: + from ..knowledge import defaults + SETTINGS = copy.deepcopy(defaults.SETTINGS) for arg in sys.argv: if arg.startswith('--'): k,v = arg.split('=',1) diff --git a/main.py b/main.py index 64d6afadc..a30a833be 100644 --- a/main.py +++ b/main.py @@ -2,10 +2,16 @@ import sys if __name__=='__main__': + #check for settings override + for arg in sys.argv[1:]: + if arg.startswith('--settings='): + settings.load_settings(arg[11:]) + break + #get launch file launch_file = None for arg in sys.argv[1:]: if arg.startswith('--run='): - launch_file = arg[9:] + launch_file = arg[6:] break elif not arg.startswith('--'): launch_file = arg @@ -13,8 +19,8 @@ if launch_file is None: runconfig = settings.get('run',None) if runconfig is None: - print("Usage: python3 [--key1=value1 --key2=value2] LAUNCH_FILE.yaml") - print(" Current settings are found in knowledge/defaults/current.yaml") + print("Usage: python3 [--key1=value1 --key2=value2] [--settings=SETTINGS_OVERRIDE.yaml] LAUNCH_FILE.yaml") + print(" Default settings are found in knowledge/defaults/current.yaml") exit(1) else: print("Using default run configuration in knowledge/defaults/current.yaml")