Bump tar and npm in /seminar06-planning/simulator

seminar06-planning/py_planning/__init__.py

@@ -0,0 +1,2 @@
+from .py_planning import init, run_planner
+from . import data_types

seminar06-planning/py_planning/data_types.py

@@ -0,0 +1,104 @@
+from dataclasses import dataclass, field
+from typing import List, Optional
+import itertools
+
+@dataclass
+class Position:
+    x: float
+    y: float
+
+@dataclass
+class Size:
+    w: float
+    h: float
+
+@dataclass
+class VehiclePose:
+    pos: Position
+    rot: float
+    velocity: float
+    curv: float
+
+@dataclass
+class MultipleLanePath:
+    centerlines: List[List[Position]]
+    leftBoundaries: List[List[Position]]
+    rightBoundaries: List[List[Position]]
+
+@dataclass
+class LanePath:
+    centerline: List[Position]
+    left_boundaries: List[Position]
+    right_boundaries: List[Position]
+
+@dataclass
+class StaticObstacle:
+    p: List[float]  # координаты препятствия в виде кортежа (x, y)
+    r: float        # угол поворота (вращение)
+    w: float        # ширина препятствия
+    h: float        # высота препятствия
+
+@dataclass
+class DynamicObstacle:
+    type: str
+    startPos: Position
+    velocity: Position
+    size: Size
+    parallel: bool
+
+@dataclass
+class _RawState:
+    vehiclePose: VehiclePose
+    vehicleStation: float
+    lanePath: MultipleLanePath
+    startTime: float
+    dynamicObstacles: List[DynamicObstacle]
+    staticObstacles: List[StaticObstacle]
+
+@dataclass
+class State:
+    vehicle_pose: VehiclePose  # current AV position and velocity
+    vehicle_station: float     # current 'station' that is the distance travelled along the centerline
+    lane_path: LanePath
+    start_time: float
+    dynamic_obstacles: List[DynamicObstacle]
+    static_obstacles: List[StaticObstacle]
+
+@dataclass
+class PlannedState:
+    pos: Position
+    velocity: float
+    acceleration: float = 0
+    rot: Optional[float] = field(default=None)
+    curv: Optional[float] = field(default=None)
+
+@dataclass
+class PlannedPath:
+    states: List[PlannedState]
+
+
+def _merge_multiple_lane_paths(multiple_lane_paths: MultipleLanePath) -> LanePath:
+    return LanePath(
+        centerline=list(p for p in itertools.chain(*multiple_lane_paths.centerlines)),
+        left_boundaries=list(p for p in itertools.chain(*multiple_lane_paths.leftBoundaries)),
+        right_boundaries=list(p for p in itertools.chain(*multiple_lane_paths.rightBoundaries))
+    )
+
+def beatify_state(raw_state: _RawState) -> State:
+    return State(
+        start_time=raw_state.startTime,
+        vehicle_pose=raw_state.vehiclePose,
+        vehicle_station=raw_state.vehicleStation,
+        dynamic_obstacles=raw_state.dynamicObstacles,
+        static_obstacles=raw_state.staticObstacles,
+        lane_path=_merge_multiple_lane_paths(raw_state.lanePath)
+    )
+
+# this is essential for correct json serialization
+def postprocess_planned_path(planned_path: PlannedPath) -> PlannedPath:
+    for i in range(len(planned_path.states)):
+        planned_path.states[i].acceleration = float(planned_path.states[i].acceleration)
+        planned_path.states[i].velocity = float(planned_path.states[i].velocity)
+        planned_path.states[i].pos.x = float(planned_path.states[i].pos.x)
+        planned_path.states[i].pos.y = float(planned_path.states[i].pos.y)
+    return planned_path

seminar06-planning/py_planning/planner.py

@@ -0,0 +1,30 @@
+
+from shapely.geometry import LineString, Point
+
+def get_index_of_closest_point(line: LineString, point: Point):
+    closest_point_index = None
+    min_distance = float('inf')
+
+    for i, line_point in enumerate(line.coords):
+        line_point = Point(line_point)
+        distance = point.distance(line_point)
+        if distance < min_distance:
+            min_distance = distance
+            closest_point_index = i
+
+    return closest_point_index
+
+
+def do_plan(state):
+    vehicle_pose = state['vehiclePose']
+    vehicle_pos = Point(vehicle_pose['pos']['x'], vehicle_pose['pos']['y'])
+
+    all_centerlines = []
+    for centerline in state['lanePath']['centerlines']:
+        all_centerlines.extend(centerline)
+
+    centerline = LineString([(p['x'], p['y']) for p in all_centerlines])
+
+    closest_index = get_index_of_closest_point(centerline, vehicle_pos)
+
+    return [{'pos': p, 'velocity': vehicle_pose['velocity'], 'acceleration': 0} for p in all_centerlines][max(closest_index-3, 0):max(closest_index-3, 0) + 50]

seminar06-planning/py_planning/planning_server.py

@@ -0,0 +1,153 @@
+import json
+import jsonpickle
+import traceback
+
+from http.server import BaseHTTPRequestHandler, HTTPServer
+from urllib.parse import urlparse
+from dacite import from_dict
+from shapely.geometry import Point
+
+from .data_types import _RawState, beatify_state, PlannedPath, postprocess_planned_path
+
+
+class JSONRequestHandler(BaseHTTPRequestHandler):
+    def __init__(self, do_plan, on_case_status, verify_planned_trajectory, *args, **kwargs):
+        self.do_plan = do_plan
+        self.on_case_status = on_case_status
+        self.verify_planned_trajectory = verify_planned_trajectory
+        super().__init__(*args, **kwargs)
+
+    def do_OPTIONS(self):
+        self.send_response(200)
+        self.end_headers()
+
+    def do_POST(self):
+        # Parse query data & params to find out what was passed
+        parsed_path = urlparse(self.path)
+        try:
+            if parsed_path.path == '/plan':
+                self.plan_request()
+            elif parsed_path.path == '/notify_case_status':
+                self.notify_case_status_request()
+            elif parsed_path.path == '/ping':
+                self.ping_request()
+            else:
+                self.send_response(404)
+        except BrokenPipeError:
+            pass
+
+    def ping_request(self):
+        # Send the "200 OK" response
+        self.send_response(200)
+        self.send_header('Content-type', 'application/json')
+        self.end_headers()
+        self.wfile.write('{"status": "ok"}')
+
+    def plan_request(self):
+        content_length = int(self.headers['Content-Length'])
+        post_data = self.rfile.read(content_length)
+        try:
+            state = from_dict(data_class=_RawState, data=json.loads(post_data))
+            response = self.do_plan(beatify_state(state))
+            if not self.verify_planned_trajectory(response):
+                response = {'status': 'error', 'message': 'trajectory verification failed'}
+        except Exception:
+            print('Exception while planning: ', traceback.format_exc())
+            response = {'status': 'error', 'message': traceback.format_exc()}
+
+        # Send the "200 OK" response
+        self.send_response(200)
+        self.send_header('Content-type', 'application/json')
+        self.end_headers()
+
+        # Send the response
+        self.wfile.write(jsonpickle.dumps(postprocess_planned_path(response)).encode('utf-8'))
+
+    def notify_case_status_request(self):
+        content_length = int(self.headers['Content-Length'])
+        post_data = self.rfile.read(content_length)
+        response = ""
+        status = {}
+        try:
+            status = json.loads(post_data)
+        except json.JSONDecodeError:
+            response = {'status': 'error', 'message': 'Invalid JSON'}
+
+        self.on_case_status(status)
+
+        # Send the "200 OK" response
+        self.send_response(200)
+        self.send_header('Content-type', 'application/json')
+        self.end_headers()
+
+        # Send the response
+        self.wfile.write(jsonpickle.dumps(response).encode('utf-8'))
+
+    def end_headers(self):
+        self.send_header('Access-Control-Allow-Origin', '*')
+        self.send_header('Access-Control-Allow-Methods', 'POST')
+        self.send_header('Access-Control-Allow-Headers', 'Content-Type')
+        BaseHTTPRequestHandler.end_headers(self)
+
+    def log_message(self, format, *args):
+        pass  # Override the log_message method to silence all logs
+
+
+class PlanningServer(HTTPServer):
+    def __init__(self, server_address):
+        super().__init__(server_address, JSONRequestHandler)
+        self.handles_planning_requests = True
+        self.case_completed = False
+        self.fail_reason = ''
+        self.stop_on_fail = True
+
+    def finish_request(self, request, client_address):
+        self.RequestHandlerClass(
+            self.do_plan,
+            self.on_case_status,
+            self.verify_planned_trajectory,
+            request, client_address, self)
+
+    def set_planner(self, do_plan):
+        self.do_plan = do_plan
+
+    def set_stop_on_fail(self, stop_on_fail):
+        self.stop_on_fail = stop_on_fail
+
+    def on_case_status(self, status):
+        status_string = status["status"]
+        self.handles_planning_requests = False
+        if status_string == 'reset':
+            self.fail_reason = ''
+            self.handles_planning_requests = True
+        if status_string == 'completed':
+            self.fail_reason  = ''
+            self.case_completed = True
+        if status_string == 'failed':
+            self.case_completed = False
+            self.fail_reason = status["reason"]
+            if not self.stop_on_fail:
+                self.handles_planning_requests = True
+
+    def verify_planned_trajectory(self, planned_path: PlannedPath):
+        if len(planned_path.states) < 2:
+            print("Invalid planned trajectory: too short.")
+            self.handles_planning_requests = False
+            self.case_completed = False
+            return False
+
+        MAX_VELOCITY = 30.0  # m/s
+        for state in planned_path.states:
+            if state.velocity > MAX_VELOCITY:
+                print("Invalid planned trajectory: too high velocity: ", state.velocity, " > ",MAX_VELOCITY)
+                self.handles_planning_requests = False
+                self.case_completed = False
+                return False
+
+        return True
+
+    def run(self):
+        self.handles_planning_requests = True
+        while self.handles_planning_requests:
+            self.handle_request()
+        return self.case_completed

seminar06-planning/py_planning/py_planning.py

@@ -0,0 +1,44 @@
+from threading import Thread
+from .planning_server import PlanningServer
+
+
+def _run_visualization_server():
+    def server_thread():
+        from http.server import HTTPServer, SimpleHTTPRequestHandler
+
+        class Handler(SimpleHTTPRequestHandler):
+            def __init__(self, *args, **kwargs):
+                super().__init__(*args, directory='./simulator', **kwargs)
+
+            def log_message(self, format, *args):
+                pass  # Override the log_message method to silence all logs
+
+            def end_headers(self):
+                self.send_header('Cache-Control', 'no-store, no-cache, must-revalidate')
+                self.send_header('Pragma', 'no-cache')
+                self.send_header('Expires', '0')
+                SimpleHTTPRequestHandler.end_headers(self)
+
+        httpd = HTTPServer(("127.0.0.1", 8008), Handler)
+        httpd.serve_forever()
+
+    thread = Thread(target=server_thread)
+    thread.start()
+
+
+def init():
+    _run_visualization_server()
+
+    global _p_server
+    _p_server = PlanningServer(('127.0.0.1', 9999))
+
+
+def run_planner(planning_function, stop_on_fail=True):
+    global _p_server
+    _p_server.set_planner(planning_function)
+    _p_server.set_stop_on_fail(stop_on_fail)
+
+    if _p_server.run():
+        print("Congrats! Case completed successfully.")
+    else:
+        print("Case failed: " + _p_server.fail_reason)

seminar06-planning/scenarios/static01.txt

@@ -0,0 +1 @@
+eyJwIjpbLTE1LjM0NDM3LDY0LjEwMjQyLC01LjcyMTk1LDQyLjg5NTkyLDIuMTQ1NzUsMjQuMjk5ODIsMTAuMDEzNjIsNC45ODgxLDE2LjY4OTUyLC0xMi40MTY2MywyMi40MTE3NiwtMjIuNjY4ODksMjguODQ5MzQsLTMyLjkyMTI2LDI5LjgwMzE4LC00Mi40NTg0NCwyOC4zNzI4LC01My42NjQ3NiwyNi40NjU0OCwtNjAuNTc5MzYsMjMuNjA0NDgsLTcxLjA3MDU5XSwicyI6W3sicCI6WzI5LjU2NTE1LC02Ny42MTMyNF0sInIiOi0wLjQxMjAxLCJ3Ijo1LjI0NTM1LCJoIjo0LjUzMDN9LHsicCI6WzI0Ljc5NjE0LC0zMy45OTQxOV0sInIiOjAsInciOjMuMzM3ODksImgiOjMuNTc2NDN9LHsicCI6WzE2LjkyNzg0LC0yLjE2NDQ5XSwiciI6LTAuNTcyMjQsInciOjEuNDMwNSwiaCI6Mi4zODQyMX0seyJwIjpbMTEuMjA1NzcsLTQuMTkxMDddLCJyIjowLCJ3IjoxLjQzMDUxLCJoIjoyLjE0NTc5fSx7InAiOlsyNS43NDk2NSwtMjAuODgwN10sInIiOi0wLjk3MjgxLCJ3Ijo0Ljc2ODM5LCJoIjo0LjA1MzIzfSx7InAiOlsxLjI3NDQ5LDIwLjExNjk1XSwiciI6MCwidyI6Mi4wMzkxOCwiaCI6Mi41NDg5NX1dLCJkIjpbXSwibCI6MTQ1Ljg3NSwiYyI6eyJzIjoiMTAiLCJzbCI6IjEwIiwibHAiOjF9LCJ2IjoxfQ==

seminar06-planning/scenarios/static02.txt

@@ -0,0 +1 @@
+eyJwIjpbLTEwMi40NjA3OCwyNi4zODUxMywtNjguNjk4MjEsMjUuNzk3NzYsLTU1Ljk0OTEzLDE5LjUwNDI3LC0yNS4zMjI4NCwxMi42MTgzLC0xNi42MDI0LDEwLjc3MzksLTYuNzA4LDExLjc4MDEzLDMxLjM2MDU0LDEyLjExNTU0LDQ3LjA0MDU3LDE0Ljc5ODgsNTYuODUwNDgsMjMuMjY3NzYsNTYuODQ5NzksMzUuMjU4MjgsNDYuOTU1MTEsNDMuODk0NjMsMjUuNDg5NzksNDMuNTU5MjQsOC4xMzMyNiw1Mi4yNzk5LC0xNS4zNDQzNyw2NC4xMDI0Ml0sInMiOlt7InAiOlstMzYuMDkyNTQsMTIuMzY5NjNdLCJyIjowLjE0NzI2LCJ3IjozLjQzNzkxLCJoIjoxLjg0NDc1fSx7InAiOlstMTMuNDE2MjUsMTQuMjUzN10sInIiOjAuMTE2NTgsInciOjQuNjk1NjgsImgiOjEuNDI1NDl9LHsicCI6WzQ0LjAwOTMxLDE4LjM4MTY2XSwiciI6MC4yNTc3MSwidyI6NC45NDcxMywiaCI6NC4yNzY0Mn0seyJwIjpbNjIuNzQ2NjksMjkuNDEzMzFdLCJyIjowLCJ3Ijo0LjEwODU1LCJoIjo1Ljg2OTUyfSx7InAiOls0Ni43MTExOSwzOC43NDQ2OV0sInIiOjEuMTQ3NDIsInciOjQuNTI3NzUsImgiOjYuNzA3OTR9LHsicCI6WzE5LjczNTEsOS40MDYyMV0sInIiOjAuNjUwNDEsInciOjEuOTcxNzcsImgiOjEuMDc1NTN9LHsicCI6WzI4Ljk5Njc5LDkuNzE3MjddLCJyIjowLjIwODYyLCJ3IjoxLjk3MTc1LCJoIjoxLjAzMDcyfSx7InAiOlsyNC4xNTIwNSw5LjkzODcyXSwiciI6LTAuMzE5MDcsInciOjEuMjU0NzYsImgiOjAuOTg1OX1dLCJkIjpbXSwibCI6MjU5LjA4OCwiYyI6eyJzIjoiMTAiLCJzbCI6IjEwIiwibHAiOjF9LCJ2IjoxfQ==

seminar06-planning/simulator/LICENSE

@@ -0,0 +1,21 @@
+MIT License
+
+Copyright (c) 2018 Matt Bradley
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.