weird simulation behavior

Author: maoyifei2013605

GEMstack/onboard/planning/parking_planning.py

@@ -137,10 +137,10 @@ def neighbors(self, node):
             next_state = np.append(next_state, node[5] + self.vehicle_sim.T)
             next_state = np.round(next_state, 3)
             if self.is_valid_neighbor([next_state]):
-                print(f"Accepted: {next_state}")
+                # print(f"Accepted: {next_state}")
                 neighbors.append(tuple(next_state))
-            else:
-                print(f"Rejected (collision): {next_state}")
+            # else:
+            #     print(f"Rejected (collision): {next_state}")
         return neighbors
 
     def is_valid_neighbor(self, path):
@@ -196,8 +196,8 @@ def state_to_polygon(self, state):
 
 def generate_action_set():
     return [
-        (1.0, -0.3), (1.0, 0.0), (1.0, 0.3),
-        (-1.0, -0.3), (-1.0, 0.0), (-1.0, 0.3)
+        (0.25, -0.3), (0.25, 0.0), (0.25, 0.3),
+        (-0.25, -0.3), (-0.25, 0.0), (-0.25, 0.3)
     ]
 
 # @TODO Need to change the functions here to use VehicleState
@@ -250,6 +250,7 @@ def is_goal_reached(self, current: List[float], goal: List[float]):
         # return np.linalg.norm(np.array([current[0], current[1]]) - np.array([goal[0], goal[1]])) < 0.5
         pos_dist = np.linalg.norm([current[0] - goal[0], current[1] - goal[1]])
         yaw_dist = abs((current[2] - goal[2] + np.pi) % (2 * np.pi) - np.pi)
+        print("Reach The Goal")
         return pos_dist < 0.5 and yaw_dist < 0.3
 
     def heuristic_cost_estimate(self, state_1, state_2):
@@ -314,10 +315,10 @@ def neighbors(self, node):
             next_state = np.append(next_state, node[3] + self.vehicle_sim.T)
             next_state = np.round(next_state, 3)
             if self.is_valid_neighbor([next_state]):
-                print(f"Accepted: {next_state}")
+                # print(f"Accepted: {next_state}")
                 neighbors.append(tuple(next_state))
-            else:
-                print(f"Rejected first order (collision): {next_state}")
+            # else:
+            #     print(f"Rejected first order (collision): {next_state}")
         return neighbors
 
     def is_valid_neighbor(self, path):

GEMstack/onboard/planning/parking_route_planner.py

@@ -1,16 +1,14 @@
 from typing import List, Tuple, Union, Dict
 from ..component import Component
 from ...state import AllState, VehicleState, Path, Trajectory, Route, ObjectFrameEnum, AgentState, Obstacle, ObjectPose, PhysicalObject
-from ...utils import serialization, settings
-from ...mathutils.transforms import vector_madd
-from ...mathutils import quad_root
 from GEMstack.mathutils.dubins import DubinsCar, SecondOrderDubinsCar, DubinsCarIntegrator
 from ...mathutils.dynamics import IntegratorControlSpace
 from ...mathutils import collisions
 from .astar import AStar
-from .longitudinal_planning import longitudinal_plan
-from testing.reeds_shepp_path import path_length
-import reed_shepp
+# from testing.reeds_shepp_path import path_length
+from .reed_shepp import get_optimal_path
+from .reed_shepp import path_length
+
 
 
 import numpy as np
@@ -296,9 +294,9 @@ def reed_shepp(self, state_1, state_2):
         goal = (x2, y2, theta2)
 
         # Calculate Reeds-Shepp path length
-        path = reed_shepp.get_optimal_path(start, goal)
+        path = get_optimal_path(start, goal)
 
-        return reed_shepp.path_length(path)  # Using turning radius of 1.0
+        return path_length(path)  # Using turning radius of 1.0
 
     def terminal_cost_estimate(self, state_1, state_2):
         """computes the 'direct' distance between two (x,y) tuples"""

launch/parking.yaml

@@ -102,7 +102,7 @@ variants:
                         type: StaticRoutePlanner
                         args: [!relative_path '../GEMstack/knowledge/routes/parking.csv','start']
                     motion_planning:
-                        type: parking_planning.ParkingPlanner
+                        type: parking_route_planner.ParkingPlanner