From adc1cf042bc7d29e1461d18ee4758cb89114159e Mon Sep 17 00:00:00 2001 From: Mario Kahlhofer Date: Wed, 10 Oct 2018 17:58:55 +0200 Subject: [PATCH] WIP: Last Minute Fixes --- configuration_files/Maneuverlist.xml | 6 +- .../aadc_testevent_2018_signs_v_2_0.xml | 2 +- src/aadcUserPython/litdrive/Untitled.ipynb | 283 ++++++++++++++++++ .../litdrive/litdrive/selfdriving/__main__.py | 16 +- .../litdrive/selfdriving/commander.py | 14 +- .../litdrive/litdrive/selfdriving/jury.py | 2 +- .../litdrive/selfdriving/planning/planner.py | 26 +- .../litdrive/selfdriving/util/xml_parser.py | 1 + src/aadcUserPython/litdrive/polynomials.ipynb | 41 ++- 9 files changed, 371 insertions(+), 20 deletions(-) create mode 100644 src/aadcUserPython/litdrive/Untitled.ipynb diff --git a/configuration_files/Maneuverlist.xml b/configuration_files/Maneuverlist.xml index 513ea0a..96438f0 100644 --- a/configuration_files/Maneuverlist.xml +++ b/configuration_files/Maneuverlist.xml @@ -1,12 +1,12 @@ - + - + - + diff --git a/configuration_files/maps/AADC_2018_Testevent_maps/aadc_testevent_2018_signs_v_2_0.xml b/configuration_files/maps/AADC_2018_Testevent_maps/aadc_testevent_2018_signs_v_2_0.xml index 1589ffc..690f14e 100644 --- a/configuration_files/maps/AADC_2018_Testevent_maps/aadc_testevent_2018_signs_v_2_0.xml +++ b/configuration_files/maps/AADC_2018_Testevent_maps/aadc_testevent_2018_signs_v_2_0.xml @@ -102,7 +102,7 @@ Part 7 - + diff --git a/src/aadcUserPython/litdrive/Untitled.ipynb b/src/aadcUserPython/litdrive/Untitled.ipynb new file mode 100644 index 0000000..168cbb3 --- /dev/null +++ b/src/aadcUserPython/litdrive/Untitled.ipynb @@ -0,0 +1,283 @@ +{ + "cells": [ + { + "cell_type": "code", + "execution_count": 1, + "metadata": {}, + "outputs": [], + "source": [ + "import zmq" + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": {}, + "outputs": [], + "source": [ + "ctx = zmq.Context()" + ] + }, + { + "cell_type": "code", + "execution_count": 5, + "metadata": {}, + "outputs": [], + "source": [ + "sock = ctx.socket(zmq.REQ)" + ] + }, + { + "cell_type": "code", + "execution_count": 6, + "metadata": {}, + "outputs": [], + "source": [ + "sock.connect(\"tcp://127.0.0.1:5562\")" + ] + }, + { + "cell_type": "code", + "execution_count": 8, + "metadata": {}, + "outputs": [], + "source": [ + "sock.send(b\"hey\")" + ] + }, + { + "cell_type": "code", + "execution_count": 57, + "metadata": {}, + "outputs": [], + "source": [ + "import enum\n", + "class ManeuverAction(enum.IntEnum):\n", + " Undefined = 0\n", + " Left = 1\n", + " Right = 2\n", + " Straight = 4\n", + " ParallelParking = 5\n", + " CrossParking = 6\n", + " PullOutLeft = 7\n", + " PullOutRight = 8\n", + " MergeLeft = 9\n", + " MergeRight = 10\n", + " \n", + "class RoadSignType(enum.IntEnum):\n", + " UnmarkedIntersection = 0\n", + " StopAndGiveWay = 1\n", + " ParkingArea = 2\n", + " HaveWay = 3\n", + " AheadOnly = 4\n", + " GiveWay = 5\n", + " PedestrianCrossing = 6\n", + " Roundabout = 7\n", + " NoOvertaking = 8\n", + " NoEntryVehicularTraffic = 9\n", + " TestCourseA9 = 10\n", + " OneWayStreet = 11\n", + " RoadWorks = 12\n", + " KMH50 = 13\n", + " KMH100 = 14\n", + " Undefined = 99\n", + " \n", + "def map_maneuver_string(s):\n", + " if s == \"left\":\n", + " return ManeuverAction.Left\n", + " elif s == \"right\":\n", + " return ManeuverAction.Right\n", + " elif s == \"straight\":\n", + " return ManeuverAction.Straight\n", + " elif s == \"parallel_parking\":\n", + " return ManeuverAction.ParallelParking\n", + " elif s == \"cross_parking\":\n", + " return ManeuverAction.CrossParking\n", + " elif s == \"pull_out_left\":\n", + " return ManeuverAction.PullOutLeft\n", + " elif s == \"pull_out_right\":\n", + " return ManeuverAction.PullOutRight\n", + " elif s == \"merge_left\":\n", + " return ManeuverAction.MergeLeft\n", + " elif s == \"merge_right\":\n", + " return ManeuverAction.MergeRight\n", + " else:\n", + " return ManeuverAction.Undefined\n", + " \n", + "def map_roadsign_type(i):\n", + " if 0 <= i <= 15:\n", + " return RoadSignType(i)\n", + " else:\n", + " return RoadSignType.Undefined\n", + " \n", + "path = r\"C:\\data\\Dropbox\\AADC\\adtf\\configuration_files\\Maneuverlist.xml\"\n", + "path2 = r\"C:\\data\\Dropbox\\AADC\\adtf\\configuration_files\\maps\\AADC_2018_Testevent_maps\\aadc_testevent_2018_signs_v_2_0.xml\"\n", + "from xml.etree import ElementTree" + ] + }, + { + "cell_type": "code", + "execution_count": 70, + "metadata": {}, + "outputs": [], + "source": [ + "def parse_maneuver(path: str):\n", + " tree = ElementTree.parse(path)\n", + " sectors = []\n", + " for s in tree.getroot():\n", + " sid = s.attrib['id']\n", + " maneuvers = []\n", + " for m in sector:\n", + " mid = m.attrib['id']\n", + " action = map_maneuver_string(m.attrib['action'])\n", + " extra = int(m.attrib['extra']) if ('extra' in m.attrib and m.attrib['extra'].isnumeric()) else None\n", + " maneuvers.append((action, extra))\n", + " sectors.append(maneuvers)\n", + " return sectors\n", + "\n", + "def parse_roadsigns(path: str):\n", + " pass\n", + "\n", + "#parse_maneuver(path)\n", + "parse_roadsigns(path2)\n", + "\n", + "from collections import namedtuple\n", + "RoadSign = namedtuple('RoadSign', [\"id\", \"x\", \"y\", \"radius\", \"direction\", \"type\", \"init\"])\n", + "ParkingSpace = namedtuple('ParkingSpace', [\"id\", \"x\", \"y\", \"status\", \"direction\"])" + ] + }, + { + "cell_type": "code", + "execution_count": 72, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[ParkingSpace(id=8, x=18.0, y=8.25, status=0, direction=180), ParkingSpace(id=7, x=18.0, y=8.75, status=0, direction=180), ParkingSpace(id=6, x=18.0, y=9.25, status=0, direction=180), ParkingSpace(id=5, x=18.0, y=9.75, status=0, direction=180), ParkingSpace(id=4, x=16.0, y=8.75, status=0, direction=180), ParkingSpace(id=3, x=16.0, y=8.25, status=0, direction=180), ParkingSpace(id=2, x=16.0, y=7.75, status=0, direction=180), ParkingSpace(id=1, x=16.0, y=7.25, status=0, direction=180)]\n" + ] + } + ], + "source": [ + "tree = ElementTree.parse(path2)\n", + "signs = []\n", + "parking = []\n", + "for e in tree.getroot():\n", + " if e.tag == \"roadSign\":\n", + " id_ = int(e.attrib[\"id\"])\n", + " x_ = float(e.attrib[\"x\"])\n", + " y_ = float(e.attrib[\"y\"])\n", + " radius_ = float(e.attrib[\"radius\"])\n", + " direction_ = int(e.attrib[\"direction\"])\n", + " type_ = map_roadsign_type(id_)\n", + " init_ = \"init\" in e.attrib and e.attrib[\"init\"].isnumeric() and int(e.attrib[\"init\"]) == 1\n", + " rs = RoadSign(id_, x_, y_, radius_, direction_, type_, init_)\n", + " signs.append(rs) \n", + " elif e.tag == \"parkingSpace\":\n", + " id_ = int(e.attrib[\"id\"])\n", + " x_ = float(e.attrib[\"x\"])\n", + " y_ = float(e.attrib[\"y\"])\n", + " status_ = int(e.attrib[\"status\"])\n", + " direction_ = int(e.attrib[\"direction\"])\n", + " ps = ParkingSpace(id_, x_, y_, status_, direction_)\n", + " parking.append(ps)\n", + "\n", + "#print(signs)\n", + "print(parking)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [] + }, + { + "cell_type": "code", + "execution_count": 73, + "metadata": {}, + "outputs": [], + "source": [ + "import time" + ] + }, + { + "cell_type": "code", + "execution_count": 77, + "metadata": {}, + "outputs": [], + "source": [ + "start = time.time()" + ] + }, + { + "cell_type": "code", + "execution_count": 84, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "24" + ] + }, + "execution_count": 84, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "end = time.time()\n", + "int(end - start)" + ] + }, + { + "cell_type": "code", + "execution_count": 80, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "4.511585712432861" + ] + }, + "execution_count": 80, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [] + } + ], + "metadata": { + "kernelspec": { + "display_name": "Python 3", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.6.6" + } + }, + "nbformat": 4, + "nbformat_minor": 2 +} diff --git a/src/aadcUserPython/litdrive/litdrive/selfdriving/__main__.py b/src/aadcUserPython/litdrive/litdrive/selfdriving/__main__.py index f3c8ac7..33ecd19 100644 --- a/src/aadcUserPython/litdrive/litdrive/selfdriving/__main__.py +++ b/src/aadcUserPython/litdrive/litdrive/selfdriving/__main__.py @@ -76,7 +76,7 @@ def __init__(self, address: str, car: Car, commander: Commander): ] outputs = [ "tSignalValue", # desired speed - "tTrajectory", # desired trajectory + "tTrajectoryArray", # desired trajectory "tBoolSignalValue", # turn_signal_right "tBoolSignalValue", # turn_signal_left "tBoolSignalValue", # hazard_light @@ -131,11 +131,17 @@ def _process(car: Car, commander: Commander, break_signal = siren or lidar_break or car.THREAD_jury_stop_signal # Should be called last, so that new decisions can already be taken into account in this call. - out_trajectories = None - if controller_leverage and controller_feedback: - out_trajectories = car.planner.update(int(controller_feedback["id"]), int(controller_leverage["id"]), - float(controller_leverage["p"])) + leverage_p = controller_leverage["p"] if controller_leverage else 0 + leverage_id = controller_leverage["id"] if controller_leverage else 0 + feedback = controller_feedback["id"] if controller_feedback else 0 + + out_trajectories = car.planner.update(int(feedback), int(leverage_id), + float(leverage_p)) + + print("SPEED: " + str(commander.out_speed)) + print(out_trajectories) + print(car.planner) return (0, 0 if break_signal else commander.out_speed), out_trajectories diff --git a/src/aadcUserPython/litdrive/litdrive/selfdriving/commander.py b/src/aadcUserPython/litdrive/litdrive/selfdriving/commander.py index 40347d1..dc56b7c 100644 --- a/src/aadcUserPython/litdrive/litdrive/selfdriving/commander.py +++ b/src/aadcUserPython/litdrive/litdrive/selfdriving/commander.py @@ -21,12 +21,14 @@ def decide(self): self._car.position["f32y"], self._car.position["f32heading"]) # send maneuvers - for maneuver in self._car.THREAD_maneuvers: - mapped = _map_maneuver_state(maneuver.action) - mapped = [e for e in mapped if e != ManeuverState.INVALID] - self._car.planner.addManeuver(mapped) - - self.out_speed = 0.5 + for section in self._car.THREAD_maneuvers: + for maneuver in section: + print(maneuver) + mapped = _map_maneuver_state(maneuver.action) + if mapped != ManeuverState.INVALID: + self._car.planner.addManeuver(mapped) + self.out_speed = 0.3 + self._car.running_previous = self._car.THREAD_running def _map_maneuver_state(m): diff --git a/src/aadcUserPython/litdrive/litdrive/selfdriving/jury.py b/src/aadcUserPython/litdrive/litdrive/selfdriving/jury.py index 4cd7b3b..1548632 100644 --- a/src/aadcUserPython/litdrive/litdrive/selfdriving/jury.py +++ b/src/aadcUserPython/litdrive/litdrive/selfdriving/jury.py @@ -59,7 +59,7 @@ def read_files(self): roadsigns = parse_roadsigns(self._config["roadSignsFile"]) print(roadsigns) print("Reading maneuver list ...") - maneuver = parse_roadsigns(self._config["maneuverListFile"]) + maneuver = parse_maneuver(self._config["maneuverListFile"]) print(maneuver) self._received_files = True with self._lock: diff --git a/src/aadcUserPython/litdrive/litdrive/selfdriving/planning/planner.py b/src/aadcUserPython/litdrive/litdrive/selfdriving/planning/planner.py index 9595a17..4a59b8d 100644 --- a/src/aadcUserPython/litdrive/litdrive/selfdriving/planning/planner.py +++ b/src/aadcUserPython/litdrive/litdrive/selfdriving/planning/planner.py @@ -58,7 +58,23 @@ def addManeuver(self, maneuver:ManeuverState): if(len(self.decisions_to_submit)>0): self.state=PlannerState.FORWARD_NORMAL + @staticmethod + def build_trajectory_array_buffer(trajectories): + if not trajectories or len(trajectories) == 0: + return None + # assert len(trajectories) <= TRAJECTORY_ARRAY_SIZE + print(trajectories) + trajectories = trajectories[0:10] + buffer = ([[0] * TRAJECTORY_NUM_FIELDS] * TRAJECTORY_ARRAY_SIZE) + + for i, trajectory in enumerate(trajectories): + buffer[i] = trajectory + + def _flatten(data): + return [item for sublist in data for item in sublist] + + return [len(trajectories)] + _flatten(buffer) def update(self, controller_done_id, controller_current_id, controller_current_p): if(self.state<=PlannerState.INVALID ): @@ -112,12 +128,16 @@ def update(self, controller_done_id, controller_current_id, controller_current_p print("New controller ids from {} to {}".format(last_controller_id, last_controller_id+len(ids))) self.ids_in_controller.extend(controller_ids) - buffer = ([0] * TRAJECTORY_NUM_FIELDS * TRAJECTORY_ARRAY_SIZE) + buffer = [0] * TRAJECTORY_NUM_FIELDS * TRAJECTORY_ARRAY_SIZE + poly_buffer = [] for i in range(0, len(ids)): x_poly, y_poly = lanes[i].getPolys(False) - buffer[i*TRAJECTORY_NUM_FIELDS:(i+1)*TRAJECTORY_NUM_FIELDS]=[controller_ids[i], *tuple(x_poly)[::-1], *tuple(y_poly)[::-1], 0.0, 1.0, False] + #buffer[i*TRAJECTORY_NUM_FIELDS:(i+1)*TRAJECTORY_NUM_FIELDS]=[controller_ids[i], *tuple(x_poly)[::-1], *tuple(y_poly)[::-1], 0.0, 1.0, False] + poly = [controller_ids[i], *tuple(x_poly)[::-1], *tuple(y_poly)[::-1], 0.0, 1.0, False] + poly_buffer.append(poly) - return buffer + #return [len(ids)] + buffer + return Planner.build_trajectory_array_buffer(poly_buffer) return None diff --git a/src/aadcUserPython/litdrive/litdrive/selfdriving/util/xml_parser.py b/src/aadcUserPython/litdrive/litdrive/selfdriving/util/xml_parser.py index e3af686..a285cc6 100644 --- a/src/aadcUserPython/litdrive/litdrive/selfdriving/util/xml_parser.py +++ b/src/aadcUserPython/litdrive/litdrive/selfdriving/util/xml_parser.py @@ -36,6 +36,7 @@ def parse_roadsigns(path: str): def parse_maneuver(path: str): + print(path) tree = ElementTree.parse(path) sectors = [] for s in tree.getroot(): diff --git a/src/aadcUserPython/litdrive/polynomials.ipynb b/src/aadcUserPython/litdrive/polynomials.ipynb index a51ac3c..6c22fcb 100644 --- a/src/aadcUserPython/litdrive/polynomials.ipynb +++ b/src/aadcUserPython/litdrive/polynomials.ipynb @@ -11,7 +11,21 @@ "from litdrive.zeromq.server import ZmqServer\n", "\n", "TRAJECTORY_ARRAY_SIZE = 10\n", - "TRAJECTORY_NUM_FIELDS = 12" + "TRAJECTORY_NUM_FIELDS = 12\n", + "def build_trajectory_array_buffer(trajectories):\n", + " if not trajectories or len(trajectories) == 0:\n", + " return None\n", + "\n", + " assert len(trajectories) <= TRAJECTORY_ARRAY_SIZE\n", + " buffer = ([[0] * TRAJECTORY_NUM_FIELDS] * TRAJECTORY_ARRAY_SIZE)\n", + "\n", + " for i, trajectory in enumerate(trajectories):\n", + " buffer[i] = trajectory\n", + "\n", + " def _flatten(data):\n", + " return [item for sublist in data for item in sublist]\n", + "\n", + " return [len(trajectories)] + _flatten(buffer)" ] }, { @@ -48,6 +62,31 @@ " [\"tTrajectory\", \"tTrajectoryArray\"])" ] }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "1" + ] + }, + "execution_count": 3, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "from collections import namedtuple\n", + "Maneuver = namedtuple('Maneuver', [\"id\", \"action\", \"extra\"])\n", + "\n", + "\n", + "a = Maneuver(1, 2, 3)\n", + "a.id" + ] + }, { "cell_type": "code", "execution_count": null,