Kinodynamic RRT* planning added

Author: anandk1999

.github/workflows/python-app.yml

@@ -30,7 +30,7 @@ jobs:
     - 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
+        flake8 ./GEMstack --count --select=E9,F63,F7,F82 --ignore=F824 --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

.gitignore

@@ -22,6 +22,7 @@ downloads/
 eggs/
 .eggs/
 lib/
+!frontend/*
 lib64/
 parts/
 sdist/
@@ -32,6 +33,7 @@ share/python-wheels/
 .installed.cfg
 *.egg
 MANIFEST
+.idea/
 
 # PyInstaller
 #  Usually these files are written by a python script from a template
@@ -134,6 +136,7 @@ venv/
 ENV/
 env.bak/
 venv.bak/
+*.DS_Store
 
 # Spyder project settings
 .spyderproject
@@ -164,14 +167,22 @@ cython_debug/
 #  be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore
 #  and can be added to the global gitignore or merged into this file.  For a more nuclear
 #  option (not recommended) you can uncomment the following to ignore the entire idea folder.
-#.idea/
+
+.idea/
 
 # ZED run files
 **/*.run
 .vscode/
 setup/zed_sdk.run
+
+#Ignore ROS bags
+*.bag
+
 cuda/
-GEMstack/knowledge/detection/yolov8n.pt
-GEMstack/knowledge/detection/yolo11n.pt
+homework/yolov8n.pt
+homework/yolo11n.pt
 yolov8n.pt
-yolo11n.pt
+yolo11n.pt
+
+# Computation  Graph   of Launch File Outputs
+launch_visualization/graph

GEMstack/knowledge/calibration/cameras.yaml

@@ -31,4 +31,5 @@ cameras:
        - [0.99939639, 0.02547917, 0.023615, 1.1]
        - [-0.02530848, 0.99965156, -0.00749882, 0.03773583]
        - [-0.02379784, 0.00689664, 0.999693, 1.95320223]
-       - [0.0,          0.0,          0.0,          1.0 ]
+       - [0.0,          0.0,          0.0,          1.0 ]
+

GEMstack/knowledge/defaults/ReedsShepp_param.yaml

@@ -0,0 +1,19 @@
+# vehicle info 
+vehicle:
+    vehicle_dim: [1.7, 3.2]  # in meter
+    vehicle_turning_radius: 3.657
+# algorithm parameters
+reeds_shepp_parking:
+    shift_from_center_to_rear_axis: 1.25 # in meter
+    search_step_size: 0.1 # in meter
+    closest: False # If True, the closest parking spot will be selected, otherwise the farthest one will be selected
+    parking_lot_axis_shift_margin: 2.44 # in meter
+    search_bound_threshold: 0.5 
+    clearance_step: 0.5
+    clearance: 0
+    add_static_vertical_curb_as_obstacle: True
+    add_static_horizontal_curb_as_obstacle: True
+    static_horizontal_curb_size: [2.44, 0.5]
+    static_vertical_curb_size: [2.44, 24.9]
+    compact_parking_spot_size: [2.44, 4.88] # US Compact Space for parking (2.44, 4.88)
+

GEMstack/knowledge/defaults/computation_graph.yaml

@@ -21,6 +21,9 @@ components:
   - lane_detection:
       inputs: [vehicle, roadgraph]
       outputs: vehicle_lane
+  - parking_detection:
+      inputs: obstacles
+      outputs: [goal, obstacles]
   - sign_detection: 
       inputs: [vehicle, roadgraph]
       outputs: roadgraph.signs
@@ -40,12 +43,25 @@ components:
       inputs: all
   - mission_execution:
       outputs: mission
+  - mission_planning:
+      inputs: all
+      outputs: mission
+  - planning_component_example: # one way
+      inputs: all
+      outputs: mission_plan
   - route_planning:
-      inputs: [vehicle, roadgraph, mission]
+      inputs: all
       outputs: route
-  - driving_logic:
+  - route_planning_component: # one way
+      inputs: [vehicle,mission_plan, agents, obstacles]
+      outputs: route
+  - save_lidar_data:
       inputs: all
+      outputs:
+  - driving_logic:
+      inputs: 
       outputs: intent
+<<<<<<< HEAD
   - parking_component: # one way
       inputs: all
       outputs: mission_plan
@@ -55,6 +71,14 @@ components:
   - route_planning_component: # one way
       inputs: [vehicle,agents,mission_plan, obstacles]
       outputs: route
+=======
+  - _mission_planner: # one way
+      inputs: all
+      outputs: mission_plan
+  - _route_planner: # one way
+      inputs: all
+      outputs: route
+>>>>>>> c2672669e19024ddf6f0ec39238970f2bbca80ed
   - motion_planning:
       inputs: all
       outputs: trajectory
@@ -63,4 +87,8 @@ components:
       outputs: 
   - signaling:
       inputs: [intent]
+<<<<<<< HEAD
+      outputs:
+=======
       outputs:
+>>>>>>> c2672669e19024ddf6f0ec39238970f2bbca80ed

GEMstack/knowledge/defaults/rrt_param.yaml

@@ -0,0 +1,19 @@
+# arguments for BiRRT* algorithm
+# map parameters
+map:
+    lower_x: -15 
+    upper_x: 40 
+    lower_y: -10
+    upper_y: 30
+    grid_resolution: 10 # grids per meter for occupency grid
+    lane_boundary_radius: 0.2 # radius in meter for determining the size of the lane boundary in occupency grid
+    obstacle_radius: 0.2 # radius in meter for determining the size of the obstacle in occupency grid
+# vehicle info 
+vehicle:
+    heading_limit: 0.5235987756  # vehicle heading difference limit per rrt step_size (pi/6 in radians)
+    half_width: 0.8 # vehicle half width in meter
+# algorithm parameters
+rrt:
+    time_limit: 10 # in sec
+    step_size: 0.5 # length in meter for local planner to explore
+    search_r: 1.4 # radius in meter for rewiring

GEMstack/knowledge/routes/summoning_roadgraph_highbay.json

@@ -66,6 +66,11 @@ def acceleration_to_pedal_positions(acceleration : float, velocity : float, pitc
         if abs(acceleration) < acceleration_deadband:
             #deadband?
             return (0,0,gear)
+        
+        #velocity threshold for switching gears
+        if abs(velocity) <= 0.01:
+            velocity = 0
+
         if velocity * acceleration < 0:
             accel_pos = 0
             brake_pos = -acceleration / brake_max

GEMstack/knowledge/routes/summoning_roadgraph_sim.json

@@ -13,7 +13,7 @@ max_accelerator_power:  #Watts.  Power at max accelerator pedal, by gear
 max_accelerator_power_reverse: 10000.0 #Watts.  Power (backwards) in reverse gear
 
 acceleration_model : kris_v1
-accelerator_active_range : [0.2, 1.0]   #range of accelerator pedal where output acceleration is not flat
+accelerator_active_range : [0.32, 1.0]   #range of accelerator pedal where output acceleration is not flat
 brake_active_range : [0,1]             #range of brake pedal where output deceleration is not flat
 
 internal_dry_deceleration: 0.2      #m/s^2: deceleration due to internal dry friction (non-speed dependent)