S2025 simulation

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:

GEMstack/mathutils/transforms.py

@@ -72,11 +72,11 @@ 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
-        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"""

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

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)

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

main.py

@@ -2,19 +2,25 @@
 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
             break
     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")

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}
 

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}