Some basic test cases for astar and parking.

Author: jyl2017

testing/test_astar.py

@@ -8,15 +8,118 @@
 import matplotlib.pyplot as plt
 import numpy as np
 
-def test_astar_planning():
-    heuristic = lambda x,y: np.linalg.norm(np.array(x)-np.array(y))
-    test_configs = ((0,0,0), (5,5,0)) #start and goal configurations
+def plot_path(start_config, goal_config, path=None):
+    """Helper function to visualize the path"""
+    plt.figure(figsize=(8, 8))
+    
+    # Plot start and goal
+    plt.plot(start_config[0], start_config[1], 'go', markersize=10, label='Start')
+    plt.plot(goal_config[0], goal_config[1], 'ro', markersize=10, label='Goal')
+    
+    # Plot path if available
+    if path:
+        path = np.array(path)
+        plt.plot(path[:, 0], path[:, 1], 'b.-', label='Path')
+    
+    plt.grid(True)
+    plt.axis('equal')
+    plt.legend()
+    plt.title('A* Path Planning Test')
+    plt.xlabel('X (meters)')
+    plt.ylabel('Y (meters)')
+    plt.show()
 
-    planner = Astar(None, None, heuristic)
+def euclidean_heuristic(current, goal):
+    """Simple Euclidean distance heuristic"""
+    return np.linalg.norm(np.array(current) - np.array(goal))
 
-    test_traj = planner(*test_configs)
-    print(test_traj)
+def test_straight_line():
+    """Test 1: Simple straight line path"""
+    print("\nTest 1: Straight Line Path")
+    start = (0, 0, 0)
+    goal = (5, 0, 0)
+    
+    planner = Astar(start, goal, euclidean_heuristic)
+    path = planner(start, goal)
+    
+    assert path is not False, "Planner failed to find path"
+    assert len(path) > 0, "Path is empty"
+    
+    print("Path found:", path)
+    plot_path(start, goal, path)
+    return path
 
+def test_diagonal_path():
+    """Test 2: Diagonal path"""
+    print("\nTest 2: Diagonal Path")
+    start = (0, 0, 0)
+    goal = (3, 3, 0)
+    
+    planner = Astar(start, goal, euclidean_heuristic)
+    path = planner(start, goal)
+    
+    assert path is not False, "Planner failed to find path"
+    assert len(path) > 0, "Path is empty"
+    
+    print("Path found:", path)
+    plot_path(start, goal, path)
+    return path
+
+def test_path_length():
+    """Test 3: Verify path length is reasonable"""
+    print("\nTest 3: Path Length Test")
+    start = (0, 0, 0)
+    goal = (2, 2, 0)
+    
+    planner = Astar(start, goal, euclidean_heuristic)
+    path = planner(start, goal)
+    
+    # Check if path exists
+    assert path is not False, "Planner failed to find path"
+    
+    # Check if path length is reasonable
+    path_length = 0
+    for i in range(len(path)-1):
+        path_length += euclidean_heuristic(path[i], path[i+1])
+    
+    # Path length should be close to the direct distance
+    direct_distance = euclidean_heuristic(start, goal)
+    assert path_length <= direct_distance * 1.5, "Path is too long"
+    
+    print(f"Path length: {path_length:.2f}")
+    print(f"Direct distance: {direct_distance:.2f}")
+    plot_path(start, goal, path)
+    return path
+
+def test_start_goal_same():
+    """Test 4: Start and goal at same position"""
+    print("\nTest 4: Start and Goal Same Position")
+    start = (1, 1, 0)
+    goal = (1, 1, 0)
+    
+    planner = Astar(start, goal, euclidean_heuristic)
+    path = planner(start, goal)
+    
+    assert path is not False, "Planner failed to find path"
+    assert len(path) > 0, "Path is empty"
+    assert path[0] == start and path[-1] == goal, "Path should contain at least start/goal"
+    
+    print("Path found:", path)
+    plot_path(start, goal, path)
+    return path
+
+def run_all_tests():
+    """Run all test cases"""
+    try:
+        test_straight_line()
+        test_diagonal_path()
+        test_path_length()
+        test_start_goal_same()
+        print("\nAll tests passed successfully!")
+    except AssertionError as e:
+        print(f"\nTest failed: {str(e)}")
+    except Exception as e:
+        print(f"\nUnexpected error: {str(e)}")
 
 if __name__ == '__main__':
-    test_astar_planning()
+    run_all_tests()

testing/test_parallel_parking.py

@@ -0,0 +1,93 @@
+import sys
+import os
+sys.path.append(os.getcwd())
+
+from GEMstack.state import Path, ObjectFrameEnum
+from GEMstack.onboard.planning.hybrid_astar import Astar
+import matplotlib.pyplot as plt
+import numpy as np
+
+def plot_parking_scenario(start_config, goal_config, path=None, obstacles=None):
+    """
+    Visualize the parking scenario including start position, goal position, obstacles, and path
+    """
+    plt.figure(figsize=(10, 6))
+    
+    # Plot start position
+    plt.plot(start_config[0], start_config[1], 'go', markersize=10, label='Start')
+    plt.arrow(start_config[0], start_config[1], 
+             np.cos(start_config[2]), np.sin(start_config[2]), 
+             head_width=0.1, color='g')
+    
+    # Plot goal position
+    plt.plot(goal_config[0], goal_config[1], 'ro', markersize=10, label='Goal')
+    plt.arrow(goal_config[0], goal_config[1], 
+             np.cos(goal_config[2]), np.sin(goal_config[2]), 
+             head_width=0.1, color='r')
+    
+    # Plot obstacles if provided
+    if obstacles is not None:
+        for obs in obstacles:
+            plt.plot(obs[0], obs[1], 'ks', markersize=20)
+    
+    # Plot path if provided
+    if path is not None:
+        path = np.array(path)
+        plt.plot(path[:, 0], path[:, 1], 'b-', label='Planned Path')
+    
+    plt.grid(True)
+    plt.axis('equal')
+    plt.legend()
+    plt.title('Parallel Parking Scenario')
+    plt.xlabel('X (meters)')
+    plt.ylabel('Y (meters)')
+    plt.show()
+
+def euclidean_distance_heuristic(current, goal):
+    """
+    Calculate Euclidean distance between current and goal configurations
+    Also considers heading difference as part of the heuristic
+    """
+    pos_distance = np.linalg.norm(np.array(current[:2]) - np.array(goal[:2]))
+    angle_diff = abs(current[2] - goal[2])
+    angle_diff = min(angle_diff, 2*np.pi - angle_diff)
+    
+    # Weight for combining position and angle differences
+    angle_weight = 0.5
+    return pos_distance + angle_weight * angle_diff
+
+def test_parallel_parking():
+    # Define start and goal configurations [x, y, heading]
+    # Start: Vehicle is parallel to the road
+    start_config = (0, 0, 0)  # (x=0, y=0, heading=0 radians)
+    
+    # Goal: Vehicle is in the parking spot
+    goal_config = (-2, -2, -np.pi/2)  # (x=-2, y=-2, heading=-90 degrees)
+    
+    # Create obstacles representing parked cars
+    obstacles = [
+        (-2, 0),   # Car in front of parking spot
+        (-2, -4)   # Car behind parking spot
+    ]
+    
+    # Initialize planner
+    planner = Astar(start_config, goal_config, euclidean_distance_heuristic)
+    
+    # Run the planner
+    path = planner(start_config, goal_config)
+    
+    if path:
+        print("Path found successfully!")
+        print("Path points:", path)
+        
+        # Visualize the scenario and solution
+        plot_parking_scenario(start_config, goal_config, path, obstacles)
+    else:
+        print("No valid path found!")
+
+def main():
+    print("Testing Hybrid A* Parallel Parking Planner...")
+    test_parallel_parking()
+
+if __name__ == '__main__':
+    main()