WIP: Integrate server, frontend, and state machine

Author: injustli

.gitignore

@@ -22,6 +22,7 @@ downloads/
 eggs/
 .eggs/
 lib/
+!frontend/*
 lib64/
 parts/
 sdist/

GEMstack/onboard/execution/execution.py

@@ -655,7 +655,6 @@ def run(self):
                     "A component may have hung. Traceback:\n{}",
                     traceback.format_exc(),
                 )
-        print("validated: ", validated)
         if validated:
             self.begin()
             while True:
@@ -819,8 +818,6 @@ def run_until_switch(self):
         dt_min = min([c.dt for c in components if c.dt != 0.0])
         looper = TimedLooper(dt_min, name="main executor")
         while looper and not self.done():
-            response = requests.get("http://localhost:8000/api/inspect")
-            print("data: ", response.json())
             self.state.t = self.vehicle_interface.time()
             self.logging_manager.set_vehicle_time(self.state.t)
             self.last_loop_time = time.time()

GEMstack/onboard/planning/inspection_planning.py

@@ -2,7 +2,17 @@
 import matplotlib.pyplot as plt
 from typing import List, Tuple, Union
 from ..component import Component
-from ...state import AllState, VehicleState, EntityRelation, EntityRelationEnum, Path, Trajectory, Route, ObjectFrameEnum, AgentState
+from ...state import (
+    AllState,
+    VehicleState,
+    EntityRelation,
+    EntityRelationEnum,
+    Path,
+    Trajectory,
+    Route,
+    ObjectFrameEnum,
+    AgentState,
+)
 from ...utils import serialization
 from ...mathutils.transforms import vector_madd
 from ...mathutils.quadratic_equation import quad_root
@@ -15,11 +25,15 @@
 import matplotlib.patches as patches
 import math
 from scipy.optimize import minimize
-from .longitudinal_planning import longitudinal_plan, detect_collision, longitudinal_brake
+from .longitudinal_planning import (
+    longitudinal_plan,
+    detect_collision,
+    longitudinal_brake,
+)
 
 
 def is_inside_geofence(x, y, xmin, xmax, ymin, ymax):
-    return xmin+0.2 < x < xmax-0.2 and ymin+0.2 < y < ymax-0.2
+    return xmin + 0.2 < x < xmax - 0.2 and ymin + 0.2 < y < ymax - 0.2
 
 
 def max_visible_arc(circle_center, radius, geofence):
@@ -70,13 +84,22 @@ def plot_circle_with_geofence_and_arc(circle_center, radius, geofence, rem_pts):
 
     # Plot geofence
     (xmin, ymin), (xmax, ymax) = geofence
-    rect = plt.Rectangle((xmin, ymin), xmax - xmin, ymax - ymin, linewidth=2, edgecolor='red', facecolor='none')
+    rect = plt.Rectangle(
+        (xmin, ymin),
+        xmax - xmin,
+        ymax - ymin,
+        linewidth=2,
+        edgecolor="red",
+        facecolor="none",
+    )
     ax.add_patch(rect)
 
     rem_pts = np.array(rem_pts)
-    ax.plot(rem_pts[:, 0], rem_pts[:, 1], color='blue', linewidth=3, label='Max Valid Arc')
+    ax.plot(
+        rem_pts[:, 0], rem_pts[:, 1], color="blue", linewidth=3, label="Max Valid Arc"
+    )
 
-    ax.set_aspect('equal')
+    ax.set_aspect("equal")
     ax.grid(True)
     plt.legend()
     plt.show()
@@ -90,27 +113,29 @@ def plot_circle_with_geofence_and_arc(circle_center, radius, geofence, rem_pts):
 # rem_points = max_visible_arc(circle_center, radius, geofence)
 # plot_circle_with_geofence_and_arc(circle_center, radius, geofence, rem_points)
 
+
 def check_point_exists(server_url="http://localhost:8000"):
     try:
-        response = requests.get(f"{server_url}/point")
+        response = requests.get(f"{server_url}/api/inspect")
         response.raise_for_status()
-        points = response.json().get("points", [])
-        
+        points = response.json().get("coords", [])
+
         for point in points:
-            return True, [point["x"], point["y"]]
+            return True, [point["lat"], point["lng"]]
         return False, []
 
     except requests.exceptions.RequestException as e:
         print("Error contacting server:", e)
         return False, []
 
+
 class InspectionPlanner(Component):
     """Follows the given route.  Brakes if you have to yield or
     you are at the end of the route, otherwise accelerates to
     the desired speed.
     """
 
-    def __init__(self, mode : str = 'real', params: dict = {'state_machine' :[]}):
+    def __init__(self, mode: str = "real", params: dict = {"state_machine": []}):
         self.route_progress = None
         self.t_last = None
         self.acceleration = 2
@@ -121,35 +146,35 @@ def __init__(self, mode : str = 'real', params: dict = {'state_machine' :[]}):
         self.max_deceleration = 8.0
 
         self.mode = mode
-        self.planner = 'dt'
-        self.state_list = params['state_machine']
-        self.goal = [10,0]
-        self.inspect_goal = [15,0]
+        self.planner = "dt"
+        self.state_list = params["state_machine"]
+        self.goal = [10, 0]
+        self.inspect_goal = [15, 0]
         self.index = 0
         self.mission = self.state_list[self.index]
         self.x = 0
         print(self.mission)
 
     def state_inputs(self):
-        return ['all']
+        return ["all"]
 
     def state_outputs(self) -> List[str]:
-        return ['trajectory']
+        return ["trajectory"]
 
     def rate(self):
         return 10
 
-    def update(self, state : AllState):
+    def update(self, state: AllState):
         start_time = time.time()
 
-        vehicle = state.vehicle # type: VehicleState
-        route = state.route   # type: Route
+        vehicle = state.vehicle  # type: VehicleState
+        route = state.route  # type: Route
         t = state.t
 
         if self.t_last is None:
             self.t_last = t
         dt = t - self.t_last
-  
+
         # Position in vehicle frame (Start (0,0) to (15,0))
         curr_x = vehicle.pose.x
         curr_y = vehicle.pose.y
@@ -160,11 +185,15 @@ def update(self, state : AllState):
 
         if self.route_progress is None:
             self.route_progress = 0.0
-        closest_dist,closest_parameter = state.route.closest_point_local((curr_x,curr_y),[self.route_progress-5.0,self.route_progress+5.0])
+        closest_dist, closest_parameter = state.route.closest_point_local(
+            (curr_x, curr_y), [self.route_progress - 5.0, self.route_progress + 5.0]
+        )
         self.route_progress = closest_parameter
 
         lookahead_distance = max(10, curr_v**2 / (2 * self.deceleration))
-        route_with_lookahead = route.trim(closest_parameter,closest_parameter + lookahead_distance)
+        route_with_lookahead = route.trim(
+            closest_parameter, closest_parameter + lookahead_distance
+        )
         print("Lookahead distance:", lookahead_distance)
         print(self.mission)
 
@@ -175,73 +204,111 @@ def update(self, state : AllState):
             points_found, pts = check_point_exists()
             if points_found:
                 self.goal = pts
-                print(self.state_list[self.index+1])
-                self.mission = self.state_list[self.index+1]
+                print(self.state_list[self.index + 1])
+                self.mission = self.state_list[self.index + 1]
                 self.index += 1
                 print("CHANGING STATES", self.mission)
 
-
         elif self.mission == "NAV":
             print(self.goal)
-            print(abs_x,abs_y)
+            print(abs_x, abs_y)
 
             if abs(abs_x - self.goal[0]) <= 0.1 and abs(abs_y - self.goal[1]) <= 0.1:
                 self.mission = self.state_list[self.index + 1]
                 self.index += 1
                 print("CHANGING STATES", self.mission)
-            
 
             should_yield = False
             yield_deceleration = 0.0
 
             print("Current Speed: ", curr_v)
 
             for r in state.relations:
-                if r.type == EntityRelationEnum.YIELDING and r.obj1 == '':
-                    #get the object we are yielding to
+                if r.type == EntityRelationEnum.YIELDING and r.obj1 == "":
+                    # get the object we are yielding to
                     obj = state.agents[r.obj2]
 
-                    detected, deceleration = detect_collision(abs_x, abs_y, curr_v, obj, self.min_deceleration, self.max_deceleration, self.acceleration, self.desired_speed)
+                    detected, deceleration = detect_collision(
+                        abs_x,
+                        abs_y,
+                        curr_v,
+                        obj,
+                        self.min_deceleration,
+                        self.max_deceleration,
+                        self.acceleration,
+                        self.desired_speed,
+                    )
                     if isinstance(deceleration, list):
                         print("@@@@@ INPUT", deceleration)
                         time_collision = deceleration[1]
                         distance_collision = deceleration[0]
-                        b = 3*time_collision - 2*curr_v
-                        c = curr_v**2 - 3*distance_collision
-                        desired_speed = (-b + (b**2 - 4*c)**0.5)/2
+                        b = 3 * time_collision - 2 * curr_v
+                        c = curr_v**2 - 3 * distance_collision
+                        desired_speed = (-b + (b**2 - 4 * c) ** 0.5) / 2
                         deceleration = 1.5
                         print("@@@@@ YIELDING", desired_speed)
-                        route_yield = route.trim(closest_parameter,closest_parameter + distance_collision)
-                        traj = longitudinal_plan(route_yield, self.acceleration, deceleration, desired_speed, curr_v, self.planner)
+                        route_yield = route.trim(
+                            closest_parameter, closest_parameter + distance_collision
+                        )
+                        traj = longitudinal_plan(
+                            route_yield,
+                            self.acceleration,
+                            deceleration,
+                            desired_speed,
+                            curr_v,
+                            self.planner,
+                        )
                         return traj
                     else:
                         if detected and deceleration > 0:
                             yield_deceleration = deceleration
                             should_yield = True
-                    
+
                     print("should yield: ", should_yield)
 
-            should_accelerate = (not should_yield and curr_v < self.desired_speed)
+            should_accelerate = not should_yield and curr_v < self.desired_speed
 
-            #choose whether to accelerate, brake, or keep at current velocity
+            # choose whether to accelerate, brake, or keep at current velocity
             if should_accelerate:
-                traj = longitudinal_plan(route_to_end, self.acceleration, self.deceleration, self.desired_speed, curr_v, self.planner)
+                traj = longitudinal_plan(
+                    route_to_end,
+                    self.acceleration,
+                    self.deceleration,
+                    self.desired_speed,
+                    curr_v,
+                    self.planner,
+                )
             elif should_yield:
                 traj = longitudinal_brake(route_to_end, yield_deceleration, curr_v)
             else:
-                traj = longitudinal_plan(route_to_end, 0.0, self.deceleration, self.desired_speed, curr_v, self.planner)
+                traj = longitudinal_plan(
+                    route_to_end,
+                    0.0,
+                    self.deceleration,
+                    self.desired_speed,
+                    curr_v,
+                    self.planner,
+                )
+
+            return traj
 
-            return traj 
-        
         elif self.mission == "INSPECT":
 
-            if abs(abs_x - self.inspect_goal[0]) <= 0.1 and abs(abs_y - self.inspect_goal[1]) <= 0.1:
+            if (
+                abs(abs_x - self.inspect_goal[0]) <= 0.1
+                and abs(abs_y - self.inspect_goal[1]) <= 0.1
+            ):
                 self.mission = self.state_list[self.index + 1]
                 self.index += 1
                 print("CHANGING STATES", self.mission)
 
+            traj = longitudinal_plan(
+                route_to_end,
+                self.acceleration,
+                self.deceleration,
+                self.desired_speed,
+                curr_v,
+                self.planner,
+            )
 
-            traj = longitudinal_plan(route_to_end, self.acceleration, self.deceleration, self.desired_speed, curr_v, self.planner)
-
-            return traj 
-        
+            return traj

server/main.py

@@ -62,6 +62,10 @@ class Coordinates(BaseModel):
     lng: float
 
 
+class InspectResponse(BaseModel):
+    coords: List[Coordinates]
+
+
 ### HELPERS ###
 # def create_jwt(username: str) -> str:
 #     payload = {"sub": username, "jti": str(uuid.uuid4())}
@@ -114,16 +118,22 @@ def cancel(req: CancelRequest):
     return CancelResponse(launch_id=req.launch_id, status="cancelled")
 
 
-bounding_box = None
+bounding_box = []
 
 
 @app.post("/api/inspect", status_code=201)
 def get_bounding_box(coords: list[Coordinates]):
     global bounding_box
+    # Check to see if it's a quadrilateral
+    if len(coords) != 4:
+        return JSONResponse(
+            content="Require 4 values for bounding box coordinates", status_code=400
+        )
+
     bounding_box = coords
     return "Successfully retrieved bounding box coords!"
 
 
-@app.get("/api/inspect", response_model=list[Coordinates] | None, status_code=200)
+@app.get("/api/inspect", response_model=InspectResponse, status_code=200)
 def send_bounding_box():
-    return bounding_box
+    return InspectResponse(coords=bounding_box)