Adding a testing scenario. Code needs to be cleaned up

Author: SimonKato

GEMstack/knowledge/routes/parking.csv

@@ -0,0 +1,2 @@
+0.0,0,0
+20,4,0

GEMstack/onboard/planning/astar.py

@@ -18,14 +18,15 @@
 class SearchNode(Generic[T]):
     """Representation of a search node"""
 
-    __slots__ = ("data", "gscore", "fscore", "closed", "came_from", "in_openset", "cache")
+    __slots__ = ("data", "gscore", "fscore", "tscore", "closed", "came_from", "in_openset", "cache")
 
     def __init__(
-        self, data: T, gscore: float = infinity, fscore: float = infinity
+        self, data: T, gscore: float = infinity, fscore: float = infinity, tscore:float = infinity
     ) -> None:
         self.data = data
         self.gscore = gscore
         self.fscore = fscore
+        self.tscore = tscore
         self.closed = False
         self.in_openset = False
         self.came_from: Union[None, SearchNode[T]] = None
@@ -92,6 +93,23 @@ def heuristic_cost_estimate(self, current: T, goal: T) -> float:
         This method must be implemented in a subclass.
         """
         raise NotImplementedError
+    
+    @abstractmethod
+    def terminal_cost_estimate(self, current: T, goal: T) -> float:
+        """Computes the estimated distance between a node and the goal.
+        This function is called after all iterations of A* have been run
+        and is used to determine the closest node to the goal found so far.
+
+        This method must be implemented in a subclass.
+
+        Args:
+            current (T): Current T
+            goal (T): goal T
+
+        Returns:
+            float: _description_
+        """
+        raise NotImplementedError
 
     def distance_between(self, n1: T, n2: T) -> float:
         """
@@ -152,7 +170,7 @@ def _gen():
             return reversed(list(_gen()))
 
     def astar(
-        self, start: T, goal: T, reversePath: bool = False
+        self, start: T, goal: T, reversePath: bool = False, iterations: int = 5000
     ) -> Union[Iterable[T], None]:
         if self.is_goal_reached(start, goal):
             return [start]
@@ -163,8 +181,11 @@ def astar(
             start, gscore=0.0, fscore=self.heuristic_cost_estimate(start, goal)
         )
         openSet.push(startNode)
+        bestNode = startNode
+
+        iteration = 0
 
-        while openSet:
+        while openSet and iteration < iterations:
             current = openSet.pop()
 
             if self.is_goal_reached(current.data, goal):
@@ -184,6 +205,14 @@ def astar(
                 fscore = gscore + self.heuristic_cost_estimate(
                     neighbor.data, goal
                 )
+                tscore = self.terminal_cost_estimate(
+                    neighbor.data, goal
+                )
+
+                # print(f"Checking node: {neighbor.data} with tscore {tscore}")
+                if tscore < bestNode.tscore:
+                    # print(f"Found a better node: {neighbor.data} with tscore {tscore}")
+                    bestNode = neighbor
 
                 if neighbor.in_openset:
                     if neighbor.fscore < fscore:
@@ -197,10 +226,14 @@ def astar(
                 neighbor.came_from = current
                 neighbor.gscore = gscore
                 neighbor.fscore = fscore
+                neighbor.tscore = tscore
 
                 openSet.push(neighbor)
 
-        return None
+            iteration += 1
+
+        # print("Warning: A* search failed to find a path")
+        return self.reconstruct_path(bestNode, reversePath)
 
 
 ################################################################################

GEMstack/onboard/planning/hybrid_astar.py

@@ -1,6 +1,6 @@
 from typing import List, Tuple, Union, Dict
 from ..component import Component
-from ...state import AllState, VehicleState, Path, Trajectory, Route, ObjectFrameEnum, AgentState, Obstacle, ObjectPose
+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.quadratic_equation import quad_root
@@ -26,9 +26,9 @@ def __init__(self, vehicle=None, obstacles=None, actions=None):
         self._vehicle = None
 
         # SecondOrderDubinsCar() #x = (tx,ty,theta,v,dtheta) and u = (fwd_accel,wheel_angle_rate)
-        self.vehicle_sim = IntegratorControlSpace(SecondOrderDubinsCar(), T=0.75, dt=0.25)
+        self.vehicle_sim = IntegratorControlSpace(SecondOrderDubinsCar(), T=1, dt=0.1)
         #@TODO create a more standardized way to define the actions
-        self._actions = [(1, -0.5), (1, -0.25), (1,0), (1, 0.25), (1,0.5),
+        self._actions = [(2, -0.5), (2, -0.25), (2,0), (2, 0.25), (2,0.5),
                          (0,0), (-1, -0.5), (-1,0), (-1, 0.5)]
 
     @property
@@ -60,19 +60,38 @@ def is_goal_reached(self, current: List[float], goal: List[float]):
         # @TODO Currently, the threshold is just a random number, get rid of magic constants
         # print(f"Current Pose: {current}")
         # print(f"Goal Pose: {goal}")
-        if np.abs(current[3]) > 0.5: return False # car must be stopped, this equality will only work in simulation  
+        if np.abs(current[3]) > 1: return False # car must be stopped, this equality will only work in simulation  
         return np.linalg.norm(np.array([current[0], current[1]]) - np.array([goal[0], goal[1]])) < 0.5
     vehicle
-    def heuristic_cost_estimate(self, vehicle_state_1, vehicle_state_2):
+    def heuristic_cost_estimate(self, state_1, state_2):
         # @TODO Consider creating a more sophisticated heuristic
-        """computes the 'direct' distance between two (x,y) tuples"""
+        """computes the 'direct' distance between two (x,y) tuples.
+        The states here are (x,y,theta,v,dtheta,t)
+        """
+        (x1, y1, theta1, v1, dtheta1,t1) = state_1
+        (x2, y2, theta2, v2, dtheta2, t2) = state_2
 
-        return math.hypot(vehicle_state_2[0] - vehicle_state_1[0], vehicle_state_2[1] - vehicle_state_1[1])
+        return math.hypot(x2 - x1, y2 - y1, 2*(v2-v1))
         #return math.hypot(x2 - x1, y2 - y1, theta2 - theta1, v2-v1, dtheta2-dtheta1)
 
-    def distance_between(self, n1, n2):
-        """this method always returns 1, as two 'neighbors' are always adajcent"""
-        return 1
+    def terminal_cost_estimate(self, state_1, state_2):
+        # @TODO Consider creating a more sophisticated heuristic
+        """computes the 'direct' distance between two (x,y) tuples.
+        The states here are (x,y,theta,v,dtheta,t)
+        """
+        (x1, y1, theta1, v1, dtheta1,t1) = state_1
+        (x2, y2, theta2, v2, dtheta2, t2) = state_2
+
+        return math.hypot(x2 - x1, y2 - y1)
+    
+    def distance_between(self, state_1, state_2):
+        """
+        The states here are (x,y,theta,v,dtheta,t)
+        """
+        (x1, y1, theta1, v1, dtheta1,t1) = state_1
+        (x2, y2, theta2, v2, dtheta2, t2) = state_2
+
+        return math.hypot(x2 - x1, y2 - y1)
 
     def neighbors(self, node):
         """ for a given configuration of the car in the maze, returns up to 4 adjacent(north,east,south,west)
@@ -101,13 +120,44 @@ def is_valid_neighbor(self, path):
             path (_type_): _description_
         """
         for obstacle in self.obstacles:
+            # print(f"Obstacle: {obstacle}")
             for point in path:
+                vehicle_polygon = self.state_to_polygon(point)
+                # print(f"Vehicle Polygon: {vehicle_polygon}")
                 # print(point)
                 # print(obstacle.polygon_parent())
-                if collisions.circle_intersects_polygon_2d(point[:-1], 1, obstacle.polygon_parent()):
+                # print("====================================")
+                # print(f"Vehicle Polygon: {vehicle_polygon}")
+                # print(f"Obstacle Polygon: {obstacle.polygon_parent()}")
+                # print(f"Point: {point}")
+                if collisions.polygon_intersects_polygon_2d(vehicle_polygon, obstacle.polygon_parent()):
                     #polygon_intersects_polygon_2d when we have the acutal car geometry
                     return False
         return True
+
+    def state_to_polygon(self, state):
+        """Convert a state to a polygon. The state is of the form (x,y,theta,v,dtheta,t)
+
+        Args:
+            state (_type_): _description_
+
+        Returns:
+            _type_: _description_
+        """
+        x = state[0]
+        y = state[1]
+        theta = state[2]
+        v = state[3]
+        dtheta = state[4]
+        t = state[5]
+
+        pose = ObjectPose(frame=ObjectFrameEnum.START,t=t, x=x,y=y,z=0,yaw=theta)
+        
+        temp_obj = PhysicalObject(pose=pose,
+                               dimensions=self.vehicle.to_object().dimensions,
+                               outline=self.vehicle.to_object().outline)
+
+        return temp_obj.polygon()
 
 
 class ParkingPlanner(Component):
@@ -171,6 +221,9 @@ def update(self, state : AllState) -> Trajectory:
         """
         vehicle = state.vehicle # type: VehicleState
         obstacles = state.obstacles # type: Dict[str, Obstacle]
+        agents = state.agents # type: Dict[str, AgentState]
+        print(f"Obstacles {obstacles}")
+        print(f"Agents {agents}")
         route = state.route
         goal_point = route.points[-1] # I might need to change this to the same frame as the car?
         goal_pose = ObjectPose(frame=ObjectFrameEnum.START,t=15, x=goal_point[0],y=goal_point[1],z=0,yaw=0)
@@ -184,18 +237,19 @@ def update(self, state : AllState) -> Trajectory:
 
 
         # Update the planner
-        self.planner.obstacles = list(obstacles.values())
+        # self.planner.obstacles = list(obstacles.values())
+        self.planner.obstacles = list(agents.values())
         self.planner.vehicle = vehicle
 
         # Compute the new trajectory and return it 
-        res = list(self.planner.astar(start_state, goal_state))
+        res = list(self.planner.astar(start_state, goal_state, 100))
         points = [state[:2] for state in res]
         times = [state[5] for state in res]
         path = Path(frame=vehicle.pose.frame, points=points)
         traj = Trajectory(path.frame,points,times)
-        print("===========================")
-        print(f"Points: {points}")
-        print(f"Times: {times}")
+        # print("===========================")
+        # print(f"Points: {points}")
+        # print(f"Times: {times}")
         # route = Path(frame=vehicle.pose.frame, points=points)
         # traj = longitudinal_plan(route, 2, -2, 10, vehicle.v, "milestone")
         # print(traj)

GEMstack/onboard/planning/parking_planning.py

@@ -17,7 +17,7 @@ drive:
     planning:
         route_planning:
             type: StaticRoutePlanner
-            args: [!relative_path '../GEMstack/knowledge/routes/forward_15m.csv','start']
+            args: [!relative_path '../GEMstack/knowledge/routes/parking.csv','start']
         motion_planning:
             type: parking_planning.ParkingPlanner
             # args: [null]
@@ -64,7 +64,7 @@ variants:
             vehicle_interface:
                 type: gem_mixed.GEMRealSensorsWithSimMotionInterface
                 args:
-                    scene: !relative_path '../scenes/xyhead_demo.yaml'
+                    scene: !relative_path '../scenes/parking_demo.yaml'
             mission_execution: StandardExecutor
             require_engaged: False
             visualization: !include "klampt_visualization.yaml"
@@ -91,7 +91,7 @@ variants:
             vehicle_interface:
                 type: gem_simulator.GEMDoubleIntegratorSimulationInterface
                 args:
-                    scene: !relative_path '../scenes/xyhead_demo.yaml'
+                    scene: !relative_path '../scenes/parking_demo.yaml'
             visualization: !include "klampt_visualization.yaml"
             drive:    
                 perception:

launch/parking.yaml

@@ -0,0 +1,19 @@
+vehicle_state: [4.0, 5.0, 0.0, 0.0, 0.0]
+agents:
+  ped1:
+    type: medium_truck
+    position: [20.0, 8.0]
+    nominal_velocity: 0
+    target: [15.0, 2.0]
+    behavior: loop
+    yaw: 1.5708
+
+  ped2:
+    type: medium_truck
+    position: [30.0, 8.0]
+    nominal_velocity: 0
+    target: [15.0, 4.0]
+    behavior: loop
+    yaw: 1.5708
+
+