Merge pull request #230 from krishauser/summoning_pr

Summoning team 2nd PR

Author: krishauser

GEMstack/knowledge/defaults/rrt_param.yaml

@@ -11,7 +11,7 @@ map:
 # 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
+    half_width: 0.6 # vehicle half width in meter
 # algorithm parameters
 rrt:
     time_limit: 10 # in sec

GEMstack/knowledge/routes/summoning_roadgraph_highbay.json

@@ -1,12 +1,11 @@
 from typing import List, Union
 from klampt.vis import scene
 from ..component import Component
-from ...utils import serialization
 from ...state import Route, ObjectFrameEnum, AllState, VehicleState, Roadgraph, MissionObjective, MissionEnum, ObjectPose
 import numpy as np
 import requests
-import json
 import time
+import yaml
 
 
 def check_distance(goal_pose: Union[ObjectPose, list], current_pose : ObjectPose):
@@ -35,14 +34,18 @@ def next_state(self):
 
 
 class SummoningMissionPlanner(Component):
-    def __init__(self, use_webapp, webapp_url, goal, state_machine):
+    def __init__(self, use_webapp, webapp_url, goal=None, state_machine=None):
         self.state_machine = StateMachine([eval(s) for s in state_machine])
-        self.goal_location = goal['location']
-        self.goal_frame = goal['frame']
+
+        # if use_webapp is True, goal should be None
+        self.goal_location = goal['location'] if not use_webapp else None
+        self.goal_frame = goal['frame'] if not use_webapp else None
         self.new_goal = False
         self.goal_pose = None
         self.start_pose = None
         self.start_time = time.time()
+        self.end_of_driving_route =  None
+        self.search_count = 0
 
         self.flag_use_webapp = use_webapp
         self.url_status = f"{webapp_url}/api/status"
@@ -94,7 +97,7 @@ def update(self, state: AllState):
                     goal_location = None
                     goal_frame = None
                 else:
-                    goal_location = [data['lat'] , data['lon']]
+                    goal_location = [data['lon'] , data['lat']]
                     goal_frame = 'global'
                     print("Goal location:", goal_location)
                     print("Goal frame:", goal_frame)
@@ -110,6 +113,8 @@ def update(self, state: AllState):
             goal_location = self.goal_location
             goal_frame = self.goal_frame
 
+        start_vehicle_pose = state.start_vehicle_pose
+
         if goal_frame == 'global':
             self.goal_pose = ObjectPose(t=time.time(), x=goal_location[0], y=goal_location[1],
                                         frame=ObjectFrameEnum.GLOBAL)
@@ -125,7 +130,16 @@ def update(self, state: AllState):
             raise ValueError("Invalid frame argument")
 
         if self.goal_pose:
-            self.goal_pose = self.goal_pose.to_frame(ObjectFrameEnum.START, start_pose_abs=state.start_vehicle_pose)
+            if start_vehicle_pose.frame == ObjectFrameEnum.ABSOLUTE_CARTESIAN and goal_frame == 'global':
+                # For global map simulation test
+                with open("scenes/summoning_map_sim_setting.yaml", "r") as f:
+                    data = yaml.safe_load(f)
+                    start= data['start_pose_global']
+                start_pose_global = ObjectPose(frame=ObjectFrameEnum.GLOBAL, t=start_vehicle_pose.t, x=start[0], y=start[1], yaw=start[2])
+                self.goal_pose = self.goal_pose.to_frame(ObjectFrameEnum.START, start_pose_abs=start_pose_global)
+            else:
+                self.goal_pose = self.goal_pose.to_frame(ObjectFrameEnum.START, start_pose_abs=start_vehicle_pose)
+            print("Goal pose:", self.goal_pose)
 
         # Initiate state
         if mission is None:
@@ -137,34 +151,52 @@ def update(self, state: AllState):
             if self.new_goal:
                 mission.goal_pose = self.goal_pose
                 mission.type = self.state_machine.next_state()
+                self.new_goal = False
                 print("============== Next state:", mission.type)
 
         # Reach the end of the route, begin to search for parking
-        elif mission.type == MissionEnum.SUMMONING_DRIVE:
+        # elif mission.type == MissionEnum.SUMMONING_DRIVE:
+        elif mission.type == MissionEnum.SUMMON_DRIVING:
             mission.goal_pose = self.goal_pose
             if route:
                 _, closest_index = route.closest_point([vehicle.pose.x, vehicle.pose.y], edges=False)
-                if closest_index == len(route.points) - 1 or check_distance(mission.goal_pose, vehicle.pose) < 1:
-                    mission.type = self.state_machine.next_state()
-                    print("============== Next state:", mission.type)
+                if closest_index == len(route.points) - 1 or check_distance(mission.goal_pose, vehicle.pose) < 5:
+                    if vehicle.v < 0.1:
+                        mission.type = self.state_machine.next_state()
+                        print("============== Next state:", mission.type)
+                self.end_of_driving_route = route.points[-1]
+            else:
+                self.search_count += 1
 
         # Finish parking, back to idle and wait for the next goal location
         elif mission.type == MissionEnum.PARALLEL_PARKING:
             if route:
-                _, closest_index = route.closest_point([vehicle.pose.x, vehicle.pose.y], edges=False)
-                if closest_index == len(route.points) - 1 or check_distance(route.points[-1], vehicle.pose) < 0.1:
-                    mission.type = self.state_machine.next_state()
-                    self.goal_pose = None
-                    mission.goal_pose = self.goal_pose
-                    print("============== Next state:", mission.type)
+                if route.points[-1] != self.end_of_driving_route:
+                    _, closest_index = route.closest_point([vehicle.pose.x, vehicle.pose.y], edges=False)
+                    if vehicle.v < 0.01 and (closest_index >= len(route.points) - 1 or check_distance(route.points[-1], vehicle.pose) < 0.5):
+                        # Set everything to idle
+                        mission.type = self.state_machine.next_state()
+                        print("============== Next state:", mission.type)
+            else:
+                self.search_count += 1
 
         else:
             raise ValueError("Invalid mission type")
 
+        # Can not find a path, stop mission.
+        if mission.type != MissionEnum.IDLE:
+            print("Route searching times:", self.search_count)
+            if self.search_count > 10:
+                mission.type = MissionEnum.IDLE
+                self.goal_pose = None
+                self.goal_location = None
+                self.goal_frame = None
+                self.search_count = 0
+
 
         if self.flag_use_webapp:
             data = {
-                "status": mission.planner_type.name
+                "status": mission.type.name
             }
             print("POST:", data)
             response = requests.post(url=self.url_status, json=data)

GEMstack/knowledge/routes/summoning_roadgraph_sim.json

@@ -204,6 +204,7 @@ def all_parking_spots_in_parking_lot(self,
             List of (x, y, yaw) poses for each parking spot.
         """
         if len(static_horizontal_curb) != 2:
+            return None
             raise ValueError("Exactly two curb endpoints are required.")
 
         # Extract center points of the curb rectangles
@@ -225,6 +226,7 @@ def all_parking_spots_in_parking_lot(self,
         spacing = spot_length 
 
         if total_length < spot_length:
+            return None
             raise ValueError("Insufficient curb length to place even one spot.")
 
         # Number of full spots that can fit along the curb
@@ -398,6 +400,7 @@ def shift_points_perpendicular_ccw(self, p1, p2, shift_amount):
         # 2) Compute its magnitude |v|
         length = math.hypot(v_x, v_y)
         if length == 0:
+            return None, None, None
             raise ValueError("p1 and p2 must be distinct points to define a direction.")
 
         # 3) Normalize v to get unit direction v̂ = (v_x, v_y) / |v|
@@ -453,6 +456,7 @@ def project_point_on_axis(self, p1, p2, p3):
         dot_vv = v_x * v_x + v_y * v_y      # v · v
 
         if dot_vv == 0:
+            return (None, None)
             raise ValueError("p1 and p2 must be distinct to define an axis.")
 
         # Parameter t gives the position along the line: p_proj = p1 + t * v
@@ -486,6 +490,7 @@ def move_point_along_vector(self, p0, direction, step, positive_direction=True):
         # Compute the length of the direction vector
         length = math.hypot(dx, dy)
         if length == 0:
+            print(None, None)
             raise ValueError("Direction vector must be non-zero to define a movement direction.")
 
         # Normalize the direction vector to unit length
@@ -627,20 +632,28 @@ def find_available_parking_spots_and_search_vector(self, detected_cones=[], vehi
             self.parking_lot_axis_shift_margin
         )
 
+        if self.curb_0_xy_shifted is None:
+            return
+        
         # Horizontal axis search direction
         _ , _, self.horizontal_search_axis_direction = self.shift_points_perpendicular_ccw(
             self.static_horizontal_curb_xy_coordinates[0],
             self.curb_0_xy_shifted,
             self.parking_lot_axis_shift_margin
         )
+        
+        if self.horizontal_search_axis_direction is None:
+            return
 
         # Project vehicle pose onto the search axis
         self.vehicle_pose_proj = self.project_point_on_axis(
             self.curb_0_xy_shifted,
             self.curb_1_xy_shifted,
             self.vehicle_pose[0:2]
         )
-
+        
+        if self.vehicle_pose_proj is None:
+            return
         # Compute bounds for the search axis
         self.upper_bound_xy = self.move_point_along_vector(
             self.curb_1_xy_shifted,
@@ -680,7 +693,9 @@ def find_collision_free_trajectory_to_park(self, detected_cones=[], vehicle_pose
             self.vehicle_pose[0:2]
         )
 
-
+            if self.vehicle_pose_proj is None:
+                return
+        
             while True:
                 # Move projected pose along the search axis
                 self.vehicle_pose_proj = self.move_point_along_vector(
@@ -751,7 +766,8 @@ def find_collision_free_trajectory_to_park(self, detected_cones=[], vehicle_pose
                     # Use self.horizontal_search_axis_direction_var
                     break  # Give up in this direction
 
-        # If both directions fail        
+        # If both directions fail
+        return
         raise ValueError("No collision-free trajectory available in either direction for parking.")   
 
 
@@ -767,7 +783,9 @@ def find_collision_free_trajectory_to_unpark(self, detected_cones=[], vehicle_po
             self.curb_0_xy_shifted,
             self.curb_1_xy_shifted,
             self.vehicle_pose[0:2]
-        )           
+        )      
+        if self.vehicle_pose_proj is None:
+            return
 
         # Move projected pose along the search axis
         self.vehicle_pose_proj = self.move_point_along_vector(
@@ -824,5 +842,6 @@ def find_collision_free_trajectory_to_unpark(self, detected_cones=[], vehicle_po
             if dist_to_upper_bound < self.search_bound_threshold or dist_to_lower_bound < self.search_bound_threshold:
                 break  # Give up in this direction
 
-        # If both directions fail        
+        # If both directions fail 
+        return       
         raise ValueError("No collision-free trajectory available for unparking.")