adapt global simulation, optimaized route planning, optimized parking lot finding

Author: derekzyn

GEMstack/knowledge/defaults/ReedsShepp_param.yaml

@@ -6,13 +6,13 @@ vehicle:
 reeds_shepp_parking:
     shift_from_center_to_rear_axis: 1.25 # in meter
     search_step_size: 0.1 # in meter
-    closest: True # If True, the closest parking spot will be selected, otherwise the farthest one will be selected
+    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
+    add_static_vertical_curb_as_obstacle: False
+    add_static_horizontal_curb_as_obstacle: False
     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/routes/summoning_roadgraph_highbay.json

@@ -54,3 +54,45 @@ def update(self, vehicle : VehicleState) -> Dict[str, AgentState]:
             if ped_num > 0:
                 print("\nDetected", ped_num, "pedestrians")
             return res
+
+
+class SummoningGlobalSimAgentDetector(Component):
+    """Obtains agent detections from a simulator"""
+
+    def __init__(self, vehicle_interface: GEMInterface):
+        self.vehicle_interface = vehicle_interface
+        self.agents = {}
+        self.lock = threading.Lock()
+
+        self.start_pose = None
+
+    def rate(self):
+        return 4.0
+
+    def state_inputs(self):
+        return ['vehicle']
+
+    def state_outputs(self):
+        return ['agents']
+
+    def initialize(self):
+        self.vehicle_interface.subscribe_sensor('agent_detector', self.agent_callback, AgentState)
+
+    def agent_callback(self, name: str, agent: AgentState):
+        with self.lock:
+            self.agents[name] = agent
+
+    def update(self, vehicle : VehicleState) -> Dict[str, AgentState]:
+        with self.lock:
+            res = {}
+            ped_num = 0
+
+            for n, a in self.agents.items():
+                # Define in start frame.
+                a.pose.frame = ObjectFrameEnum.START
+                res[n] = a
+                if a.type == AgentEnum.PEDESTRIAN:
+                    ped_num += 1
+            if ped_num > 0:
+                print("\nDetected", ped_num, "pedestrians")
+            return res

GEMstack/onboard/perception/agent_detection.py

@@ -1,6 +1,6 @@
 from dataclasses import replace
 import math
-from typing import List
+from typing import List, Optional
 from ...utils import settings
 from ...mathutils import transforms
 from ...state.vehicle import VehicleState,VehicleGearEnum
@@ -75,7 +75,6 @@ def __init__(self, vehicle_interface : GEMInterface):
         if 'gnss' not in vehicle_interface.sensors():
             raise RuntimeError("GNSS sensor not available")
         vehicle_interface.subscribe_sensor('gnss',self.fake_gnss_callback)
-        self.vehicle_state = None
 
     # Get GNSS information
     def fake_gnss_callback(self, vehicle_state):

GEMstack/onboard/perception/state_estimation.py

@@ -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):
@@ -37,7 +36,7 @@ def next_state(self):
 class SummoningMissionPlanner(Component):
     def __init__(self, use_webapp, webapp_url, goal=None, state_machine=None):
         self.state_machine = StateMachine([eval(s) for s in state_machine])
-        
+
         # 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
@@ -46,6 +45,7 @@ def __init__(self, use_webapp, webapp_url, goal=None, state_machine=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"
@@ -113,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)
@@ -128,11 +130,15 @@ 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)
-
-            # For global map test in simulation only
-            start_pose_global = ObjectPose(frame=ObjectFrameEnum.GLOBAL, t=time.time(), x=-88.235252, y=40.0927516, yaw=-1.57079633)
-            self.goal_pose = self.goal_pose.to_frame(ObjectFrameEnum.START, start_pose_abs=start_pose_global)
+            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
@@ -159,6 +165,8 @@ def update(self, state: AllState):
                         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:
@@ -171,12 +179,24 @@ def update(self, state: AllState):
                         mission.type = self.state_machine.next_state()
                         self.new_goal = False
                         self.goal_pose = None
+                        self.goal_location = None
+                        self.goal_frame = None
                         mission.goal_pose = self.goal_pose
                         print("============== Next state:", mission.type)
+            else:
+                self.search_count += 1
 
         else:
             raise ValueError("Invalid mission type")
 
+        # Can not find a path, stop mission.
+        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 = {