fix bug

Author: derekzyn

GEMstack/knowledge/routes/summoning_roadgraph_highbay.json

@@ -0,0 +1 @@
+{"type": "Roadgraph", "data": {"frame": 0, "curves": {}, "lanes": {"0": {"type": 0, "surface": 0, "route_name": "", "left": {"type": 0, "segments": [[[-88.236129, 40.09275451039261, 0.0], [-88.235527, 40.09275451039261, 0.0]]], "crossable": true, "elevation": null, "height": null}, "right": {"type": 0, "segments": [[[-88.236129, 40.092727489607384, 0.0], [-88.235527, 40.092727489607384, 0.0]]], "crossable": true, "elevation": null, "height": null}, "center": null, "begin": null, "end": null}, "1": {"type": 0, "surface": 0, "route_name": "", "left": {"type": 0, "segments": [[[-88.235527, 40.09275451039261, 0.0], [-88.235527, 40.092805489607386, 0.0]]], "crossable": true, "elevation": null, "height": null}, "right": {"type": 0, "segments": [[[-88.235527, 40.09272748960739, 0.0], [-88.235527, 40.092832510392604, 0.0]]], "crossable": true, "elevation": null, "height": null}, "center": null, "begin": null, "end": null}, "2": {"type": 0, "surface": 0, "route_name": "", "left": {"type": 0, "segments": [[[-88.235527, 40.092805489607386, 0.0], [-88.236129, 40.092805489607386, 0.0]]], "crossable": true, "elevation": null, "height": null}, "right": {"type": 0, "segments": [[[-88.235527, 40.09283251039261, 0.0], [-88.236129, 40.09283251039261, 0.0]]], "crossable": true, "elevation": null, "height": null}, "center": null, "begin": null, "end": null}, "4": {"type": 0, "surface": 0, "route_name": "", "left": {"type": 0, "segments": [[[-88.236129, 40.092805489607386, 0.0], [-88.236129, 40.09275451039261, 0.0]]], "crossable": true, "elevation": null, "height": null}, "right": {"type": 0, "segments": [[[-88.236129, 40.092832510392604, 0.0], [-88.236129, 40.09272748960739, 0.0]]], "crossable": true, "elevation": null, "height": null}, "center": null, "begin": null, "end": null}}, "regions": {}, "signs": {}, "static_obstacles": {}, "connections": []}}

GEMstack/onboard/planning/RRT.py

@@ -1,6 +1,8 @@
 import numpy as np
 import random  
 import math
+import time
+
 
 class Obstacle:
     def __init__(self,x=0,y=0,r=0.2):
@@ -17,7 +19,9 @@ def __init__(self,x=0,y=0,heading = None):
         self.cost = float('inf')  # Cost to reach this node
 
 class BiRRT:
-    def __init__(self, start : list, goal : list, obstacles : list, MAP_SIZE : list, OFFSET : float = 0.8):
+    def __init__(self, start : list, goal : list, obstacles : list, MAP_SIZE : list,
+                 OFFSET : float = 0.8, time_limit : float = 1.0, heading_limit = math.pi/6,
+                 step_size = 0.5, search_r = 1.4, MAX_Iteration = 20000):
 
         self.path = []
         self.tree_from_start = []
@@ -42,16 +46,19 @@ def __init__(self, start : list, goal : list, obstacles : list, MAP_SIZE : list,
         self.OFFSET = OFFSET # meter
 
         # max iteration size for performing route search
-        self.MAX_Iteration = 20000
-        
+        self.MAX_Iteration = MAX_Iteration
+
+        # max search time
+        self.time_limit = time_limit
+
         # step size for local planner
-        self.step_size = 0.5 # meter
+        self.step_size = step_size # meter
 
         # radius for determine neighbor node
-        self.search_r = 1.4 # meter
+        self.search_r = search_r # meter
 
         # angle limit for vehicle turning per step size
-        self.heading_limit = math.pi/6 # limit the heading change in route
+        self.heading_limit = heading_limit # limit the heading change in route
 
         # Map boundary
         self.MAP_X_LOW = MAP_SIZE[0] # meter
@@ -64,9 +71,13 @@ def search(self):
         # initialize two tree
         self.tree_from_start.append(self.start_point)
         self.tree_from_end.append(self.end_point)
-        
+
+        start_time = time.time()
         # perform search within max number of iterration
         for iterration in range(self.MAX_Iteration):
+            if time.time()-start_time > self.time_limit:
+                break
+
             # uniformly sample a point within in the map
             sample_p = Point(random.uniform(self.MAP_X_LOW,self.MAP_X_HIGH),random.uniform(self.MAP_Y_LOW,self.MAP_Y_HIGH))
             Direction = None

GEMstack/onboard/planning/route_planning_component.py

@@ -8,7 +8,7 @@
 # from GEMstack.state.physical_object import ObjectFrameEnum
 # from GEMstack.state.route import PlannerEnum, Route
 from .rrt_star import RRTStar
-
+from ..interface.gem import GEMInterface
 from ..component import Component
 from ...state import AllState, Roadgraph, Route, PlannerEnum, ObjectFrameEnum, Path, VehicleState, AgentState, AgentEnum
 from .RRT import BiRRT
@@ -93,32 +93,35 @@ def __init__(self, roadgraphfn : str = None, map_frame : str = None):
         else:
             raise ValueError("Unknown roadgraph file extension", ext)
 
-        # TODO: Transform global map to start frame
+        # TODO: Transform global map to start frame?
         if self.map_frame == ObjectFrameEnum.GLOBAL:
-            self.start_pose_global = None      # Read from GNSS
+            self.start_pose_global = None      # Read from GNSS?
             self.roadgraph = self.roadgraph.to_frame(ObjectFrameEnum.START, start_pose_abs=self.start_pose_global)
 
         # If parking route existed, not search anymore. For simulation testing only. TODO: Delete after integration
         self.parking_route_existed = False
 
+        # Used as route searchers' time limit as well as the update rate of the component
+        self.update_rate = 10.0
+
     def state_inputs(self):
         return ["all"]
 
     def state_outputs(self) -> List[str]:
         return ['route']
 
     def rate(self):
-        return 10.0
+        return self.update_rate
 
     def update(self, state: AllState):
         print("Route Planner's mission:", state.mission_plan.planner_type)
         # print("type of mission plan:", type(PlannerEnum.RRT_STAR))
         # print("Route Planner's mission:", state.mission_plan.planner_type.value == PlannerEnum.RRT_STAR.value)
         # print("Route Planner's mission:", state.mission_plan.planner_type.value == PlannerEnum.PARKING.value)
         # print("Mission plan:", state.mission_plan)
-        # print("Vehicle x:", state.vehicle.pose.x)
-        # print("Vehicle y:", state.vehicle.pose.y)
-        # print("Vehicle yaw:", state.vehicle.pose.yaw)
+        print("Vehicle x:", state.vehicle.pose.x)
+        print("Vehicle y:", state.vehicle.pose.y)
+        print("Vehicle yaw:", state.vehicle.pose.yaw)
 
         if state.mission_plan.planner_type.value == PlannerEnum.PARKING.value:
             print("I am in PARKING mode")
@@ -180,7 +183,8 @@ def update(self, state: AllState):
             current_pose = plan_available_pose_on_lane([state.vehicle.pose.x, state.vehicle.pose.y],
                                                     self.roadgraph, current_yaw=state.vehicle.pose.yaw)
             goal_pose = plan_available_pose_on_lane([goal_pose_.x, goal_pose_.y], self.roadgraph)
-            searcher = BiRRT(current_pose, goal_pose, self.lane_points, map_size, OFFSET=0.8)
+            searcher = BiRRT(current_pose, goal_pose, self.lane_points, map_size, OFFSET=0.8, time_limit=0.1,
+                             heading_limit=math.pi/6, step_size = 0.5, search_r = 1.4, MAX_Iteration = 20000)
             waypoints = searcher.search()
 
             # For now, waypoints of [x, y, heading] is not working in longitudinal_planning. Use [x, y] instead.
@@ -200,57 +204,25 @@ def update(self, state: AllState):
             print("I am in PARALLEL_PARKING mode")
             # TODO: Get cones information from perception, should have a AgentEnum for cones, for example AgentEnum.CONE
             # cones = []
-            # for obj in state.agents.values():
-            #     if obj.type == AgentEnum.CONE:
-            #         cones.append(obj)
+            # for agent in state.agents.values():
+            #     if agent.type == AgentEnum.CONE:
+            #         cones.append(agent)
 
             """ BEGIN: For simulation test only. Delete after integration """""""""""""""
-            def generate_turn(pose, turn_angle=45, turn_radius=3.0, turn_direction="right",
-                              acceleration=5.0, deceleration=2.0, max_speed=1.0, num_points=10):
-                theta = np.deg2rad(turn_angle)
+            def generate_turn(pose, step=1.0, num_points=10):
                 x, y, yaw = pose
-                R = turn_radius
-                if turn_direction == "left":
-                    cx = x + R * np.cos(yaw - np.pi / 2)
-                    cy = y + R * np.sin(yaw - np.pi / 2)
-                    start_angle = yaw - np.pi / 2
-                    angles = np.linspace(start_angle, start_angle - theta, num_points)
-                    end_yaw = start_angle - theta
-                elif turn_direction == "right":
-                    cx = x + R * np.cos(yaw + np.pi / 2)
-                    cy = y + R * np.sin(yaw + np.pi / 2)
-                    start_angle = yaw + np.pi / 2
-                    angles = np.linspace(start_angle, start_angle + theta, num_points)
-                    end_yaw = start_angle + theta
-                else:
-                    raise ValueError('Invalid turn direction')
-                end_yaw = (end_yaw + np.pi) % (2 * np.pi) - np.pi  # normalize angle
+                yaw_left = yaw + math.pi / 4  # 左前方偏 45°
                 waypoints = []
-                for angle in angles:
-                    xi = cx + R * np.cos(angle)
-                    yi = cy + R * np.sin(angle)
-                    waypoints.append([xi, yi])
-                end_pose = waypoints[-1][0], waypoints[-1][1], end_yaw
-                return waypoints, end_pose
-
-            ### FOR SIMULATION TEST ONLY ###
-            def generate_pull_over_route(pose, turn_angle=45, turn_radius=3.0, turn_direction="right"):
-                waypoints1, end_pose = generate_turn(pose, turn_angle=turn_angle, turn_radius=turn_radius,
-                                                     turn_direction=turn_direction)
-                if turn_direction == "right":
-                    waypoints2, _ = generate_turn(end_pose, turn_angle=turn_angle, turn_radius=turn_radius,
-                                                  turn_direction="left")
-                else:
-                    waypoints2, _ = generate_turn(end_pose, turn_angle=turn_angle, turn_radius=turn_radius,
-                                                  turn_direction="right")
-                waypoints = waypoints1 + waypoints2
+                for i in range(1, num_points + 1):
+                    dx = step * i * math.cos(yaw_left)
+                    dy = step * i * math.sin(yaw_left)
+                    waypoints.append([x + dx, y + dy])
                 return waypoints
 
             if not self.parking_route_existed:
                 current_pose = [state.vehicle.pose.x, state.vehicle.pose.y, state.vehicle.pose.yaw]
-                waypoints = generate_pull_over_route(current_pose, turn_angle=45, turn_radius=2.0, turn_direction="right")
+                waypoints = generate_turn(current_pose)
                 self.route = Route(frame=ObjectFrameEnum.START, points=waypoints)
-
                 self.parking_route_existed = True
             else:
                 self.route = state.route

GEMstack/onboard/planning/summoning_mission_planning.py

@@ -3,28 +3,48 @@
 from ...utils import serialization
 from ...state import Route, ObjectFrameEnum, AllState, VehicleState, Roadgraph, MissionObjective, MissionEnum, \
     VehicleIntent, VehicleIntentEnum, MissionPlan, PlannerEnum, ObjectPose
+from ..interface.gem import GEMInterface, GNSSReading
 import os
 import numpy as np
 import requests
 import json
 import time
 import math
+from ...utils import settings
 
 
-def check_distance(goal, current_pose):
-    goal = np.array(goal)[:2]
-    current_pose = np.array([current_pose.x, current_pose.y])
-    return np.linalg.norm(goal - current_pose)
+def check_distance(goal_pose : ObjectPose, current_pose : ObjectPose):
+    goal = np.array([goal_pose.x, goal_pose.y])
+    current = np.array([current_pose.x, current_pose.y])
+    return np.linalg.norm(goal - current)
+
+
+class StateMachine:
+    def __init__(self, state_list : List = None):
+        self.state_list = state_list
+        self.state_index = 0
+        self.initial_state = self.state_list[0]
+        self.current_state = self.state_list[self.state_index]
+
+    def next_state(self):
+        if self.state_index < len(self.state_list) - 1:
+            self.state_index += 1
+            return self.state_list[self.state_index]
+        else:
+            self.state_index = 0
+            return self.state_list[0]
 
 
 class SummoningMissionPlanner(Component):
-    def __init__(self, distance_to_goal_to_start_parking, distance_error_of_idle_from_parking, state_machine):
-        print("Initializing SummoningMissionPlanner:", distance_to_goal_to_start_parking, distance_error_of_idle_from_parking, state_machine)
-        self.distance_to_goal_to_start_parking = distance_to_goal_to_start_parking
-        self.distance_error_of_idle_from_parking = distance_error_of_idle_from_parking
-        self.state_list = [eval(s) for s in state_machine]
+    def __init__(self, mode, state_machine):
+        self.state_machine = StateMachine([eval(s) for s in state_machine])
+        self.goal_location = None
+        self.new_goal = False
+        self.goal_pose = None
+        self.start_pose = None
+        self.start_time = time.time()
 
-        self.count = 0      # for simulate a delay to get the gaol location
+        self.count = 0      # for test only, simulate a delay to get the gaol location
 
     def state_inputs(self):
         return ['all']
@@ -38,46 +58,78 @@ def rate(self):
     def update(self, state: AllState):
         vehicle = state.vehicle
         mission_plan = state.mission_plan
-        if mission_plan is None:
-            mission_plan = MissionPlan()
-            mission_plan.planner_type = PlannerEnum.IDLE
 
-        # for simulate a delay to get the gaol location
+        # To simulate a delay to get the goal location
         self.count += 1
         if self.count <3:
             return mission_plan
 
         # TODO: Modify to a GET request to get goal location from the server
+        # current_goal_location = self.goal_location
         # url = ""
         # response = requests.get(url)
         # if response.status_code == 200:
         #     data = response.json()
         #     goal_location = data['goal_location']
-        #     goal_location = ObjectPose(t=time.time(), x=goal_location[0], y=goal_location[1], frame=ObjectFrameEnum.GLOBAL)
+        #     goal_frame = data['goal_frame']
 
-        goal_location = [10, 11]  # for test only
-        self.goal_pose = ObjectPose(t=time.time(), x=goal_location[0], y=goal_location[1],
-                                    frame=ObjectFrameEnum.START)
+        goal_location = [10, 11]  # for simulation test only
+        goal_frame = 'start'
 
-        if mission_plan.planner_type == PlannerEnum.IDLE and self.goal_pose is not None:
-            mission_plan.goal_pose = self.goal_pose
-            mission_plan.planner_type = PlannerEnum.SUMMON_DRIVING
+        if self.goal_location == goal_location:
+            self.new_goal = False
+        else:
+            self.new_goal = True
+            self.goal_location = goal_location
+
+        if self.new_goal:
+            if goal_frame == 'global':
+                self.goal_pose = ObjectPose(t=time.time(), x=self.goal_location[0], y=self.goal_location[1],
+                                            frame=ObjectFrameEnum.GLOBAL)
+            elif goal_frame == 'cartesian':
+                self.goal_pose = ObjectPose(t=time.time(), x=self.goal_location[0], y=self.goal_location[1],
+                                        frame=ObjectFrameEnum.ABSOLUTE_CARTESIAN)
+            elif goal_frame == 'start':
+                self.goal_pose = ObjectPose(t=time.time() - self.start_time, x=self.goal_location[0], y=self.goal_location[1],
+                                            frame=ObjectFrameEnum.START)
+            else:
+                raise ValueError("Invalid frame argument")
+
+        # Initiate state
+        if mission_plan is None:
+            mission_plan = MissionPlan()
+            mission_plan.planner_type = self.state_machine.initial_state
+
+        # Receive goal location from the server and start driving
+        elif mission_plan.planner_type == PlannerEnum.IDLE:
+            if self.new_goal:
+                mission_plan.goal_pose = self.goal_pose
+                mission_plan.start_pose = self.start_pose
+                mission_plan.planner_type = self.state_machine.next_state()
 
+        # Close to the goal location, begin to search for parking
         elif mission_plan.planner_type == PlannerEnum.SUMMON_DRIVING:
-            dist = check_distance([self.goal_pose.x, self.goal_pose.y], vehicle.pose)
-            if dist < self.distance_error_of_idle_from_parking:
-                mission_plan.planner_type = PlannerEnum.PARALLEL_PARKING
+            mission_plan.goal_pose = self.goal_pose
+            mission_plan.start_pose = self.start_pose
+            dist = check_distance(mission_plan.goal_pose, vehicle.pose)
+            print("Distance to the goal:", dist)
+            if dist < 5:
+                mission_plan.planner_type = self.state_machine.next_state()
 
+        # Finish parking, back to idle and wait for the next goal location
         elif mission_plan.planner_type == PlannerEnum.PARALLEL_PARKING:
-            dist = check_distance(state.route.points[-1], vehicle.pose)
-            if dist < self.distance_error_of_idle_from_parking:
-                mission_plan.planner_type = PlannerEnum.IDLE
+            if state.route:
+                dist = check_distance(mission_plan.goal_pose, vehicle.pose)
+                print("Distance to the end point of the route:", dist)
+                if dist < 0.2 and vehicle.v < 0.01:
+                    mission_plan.planner_type = self.state_machine.next_state()
 
+        # No state change
         else:
             mission_plan = state.mission_plan
 
         # TODO: POST request to update status to the server
         # data =
-        # response = requests.post(url=, data=)
+        # response = requests.post(url=url, data=data)
 
         return mission_plan

GEMstack/state/mission_plan.py

@@ -13,6 +13,7 @@ class MissionPlan:
     planner_type : PlannerEnum = PlannerEnum.IDLE
     # other mission-specific parameters can be added here
     goal_pose: Optional[ObjectPose] = None
+    start_pose: Optional[ObjectPose] = None
 
 
 

GEMstack/state/route.py

@@ -6,8 +6,7 @@
 
 from enum import Enum
 
-# @dataclass
-# @register
+
 class PlannerEnum(Enum):
     RRT_STAR = 0                #position / yaw in m / radians relative to starting pose of vehicle
     HYBRID_A_STAR = 1           #position / yaw in m / radians relative to current pose of vehicle