update mission planning

Author: derekzyn

GEMstack/knowledge/defaults/computation_graph.yaml

@@ -40,24 +40,15 @@ components:
       inputs: all
   - mission_execution:
       outputs: mission
+  - mission_planning:
+      inputs: all
+      outputs: mission
   - route_planning:
-      inputs: [vehicle, roadgraph, mission]
+      inputs: all
       outputs: route
   - driving_logic:
       inputs: 
       outputs: intent
-  - parking_component: # one way
-      inputs: all
-      outputs: mission_plan
-  - summon_component: # one way
-      inputs: all
-      outputs: route
-  - summoning_mission_planning:
-      inputs: all
-      outputs: mission_plan
-  - route_planning_component: # one way
-      inputs: all
-      outputs: route
   - motion_planning:
       inputs: all
       outputs: trajectory

GEMstack/onboard/planning/mission_planning.py

@@ -0,0 +1,178 @@
+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
+
+
+def check_distance(goal_pose: Union[ObjectPose, list], current_pose : ObjectPose):
+    if isinstance(goal_pose, ObjectPose):
+        goal = np.array([goal_pose.x, goal_pose.y])
+    else:
+        goal = np.array([goal_pose[0], goal_pose[1]])
+    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, use_webapp, goal, state_machine):
+        self.state_machine = StateMachine([eval(s) for s in state_machine])
+        self.goal_location = goal['location']
+        self.goal_frame = goal['frame']
+        self.new_goal = False
+        self.goal_pose = None
+        self.start_pose = None
+        self.start_time = time.time()
+
+        self.flag_use_webapp = use_webapp
+
+        if self.flag_use_webapp:
+            # Initialize the state in the server
+            url = "http://localhost:8000/api/status"
+            data = {
+                "status": "IDLE"
+            }
+            response = requests.post(url=url, json=data)
+            if response.status_code == 200:
+                print("Status updated successfully")
+            else:
+                print("Failed to update status:", response.status_code)
+            url = "http://localhost:8000/api/summon"
+            data = {
+                "lat": 0,
+                "lon": 0
+            }
+            response = requests.post(url=url, json=data)
+            if response.status_code == 200:
+                print("Initialize goal location successfully")
+            else:
+                print("Failed to initialize goal location:", response.status_code)
+
+    def state_inputs(self):
+        return ['all']
+
+    def state_outputs(self) -> List[str]:
+        return ['mission']
+
+    def rate(self):
+        return 1.0
+
+    def update(self, state: AllState):
+        vehicle = state.vehicle
+        mission = state.mission
+        route = state.route
+
+        if self.flag_use_webapp:
+            goal_location = None
+            goal_frame = None
+            url = "http://localhost:8000/api/summon"
+            response = requests.get(url)
+            print("GET:", response)
+            if response.status_code == 200:
+                data = response.json()
+                if data['lat'] == 0 and data['lon'] == 0:   # TODO: lat and lon equal to 0 is meaningful, should change
+                    print("No goal location received")
+                    goal_location = None
+                    goal_frame = None
+                else:
+                    goal_location = [data['lat'] , data['lon']]
+                    goal_frame = 'global'
+                    print("Goal location:", goal_location)
+                    print("Goal frame:", goal_frame)
+
+            if self.goal_location == goal_location:
+                self.new_goal = False
+            else:
+                self.new_goal = True
+                self.goal_location = goal_location
+                self.goal_frame = goal_frame
+        else:
+            self.new_goal = True
+            goal_location = self.goal_location
+            goal_frame = self.goal_frame
+
+        if goal_frame == 'global':
+            self.goal_pose = ObjectPose(t=time.time(), x=goal_location[0], y=goal_location[1],
+                                        frame=ObjectFrameEnum.GLOBAL)
+        elif goal_frame == 'cartesian':
+            self.goal_pose = ObjectPose(t=time.time(), x=goal_location[0], y=goal_location[1],
+                                    frame=ObjectFrameEnum.ABSOLUTE_CARTESIAN)
+        elif goal_frame == 'start':
+            self.goal_pose = ObjectPose(t=time.time() - self.start_time, x=goal_location[0], y=goal_location[1],
+                                        frame=ObjectFrameEnum.START)
+        elif goal_frame is None:
+            pass
+        else:
+            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)
+
+        # Initiate state
+        if mission is None:
+            mission = MissionObjective()
+            mission.type = self.state_machine.initial_state
+
+        # Receive goal location from the server and start driving
+        elif mission.type == MissionEnum.IDLE:
+            if self.new_goal:
+                mission.goal_pose = self.goal_pose
+                mission.type = self.state_machine.next_state()
+                print("============== Next state:", mission.type)
+
+        # Reach the end of the route, begin to search for parking
+        elif mission.type == MissionEnum.SUMMONING_DRIVE:
+            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)
+
+        # 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)
+
+        else:
+            raise ValueError("Invalid mission type")
+
+
+        if self.flag_use_webapp:
+            url = "http://localhost:8000/api/status"
+            data = {
+                "status": mission.type.name
+            }
+            print("POST:", data)
+            response = requests.post(url=url, json=data)
+            if response.status_code == 200:
+                print("Status updated successfully")
+            else:
+                print("Failed to update status:", response.status_code)
+
+        return mission