Skip to content

Commit

Permalink
WIP: Last Minute Fixes
Browse files Browse the repository at this point in the history
  • Loading branch information
blu3r4y committed Oct 10, 2018
1 parent 2e8a48d commit adc1cf0
Show file tree
Hide file tree
Showing 9 changed files with 371 additions and 20 deletions.
6 changes: 3 additions & 3 deletions configuration_files/Maneuverlist.xml
Original file line number Diff line number Diff line change
@@ -1,12 +1,12 @@
<?xml version="1.0" encoding="iso-8859-1" standalone="no"?>
<AADC-Maneuver-List description="Teststrecke">
<AADC-Sector id="0">
<AADC-Maneuver id="0" action="pull_out_left" />
<AADC-Maneuver id="0" action="right" />
<AADC-Maneuver id="1" action="straight" />
<AADC-Maneuver id="2" action="straight" />
<AADC-Maneuver id="2" action="right" />
</AADC-Sector>
<AADC-Sector id="1">
<AADC-Maneuver id="3" action="left" />
<AADC-Maneuver id="3" action="straight" />
<AADC-Maneuver id="4" action="straight" />
<AADC-Maneuver id="5" action="right" />
<AADC-Maneuver id="6" action="straight" />
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -102,7 +102,7 @@ Part 7
<roadSign id="3" x="2.644" y="9.865" radius="1.0" direction="0" name="Intersection" />
<roadSign id="3" x="4.356" y="11.135" radius="1.0" direction="180" name="Intersection" />
<roadSign id="1" x="2.865" y="11.356" radius="1.0" direction="-90" name="Stop"/>
<roadSign id="1" x="4.135" y="9.644" radius="1.0" direction="90" name="Stop"/>
<roadSign id="23" x="4.135" y="9.644" radius="5.0" direction="90" name="Stop" init="1" />

<roadSign id="3" x="2.644" y="12.865" radius="1.0" direction="0" name="Intersection" />
<roadSign id="1" x="4.135" y="12.644" radius="1.0" direction="90" name="Stop"/>
Expand Down
283 changes: 283 additions & 0 deletions src/aadcUserPython/litdrive/Untitled.ipynb
Original file line number Diff line number Diff line change
@@ -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
}
16 changes: 11 additions & 5 deletions src/aadcUserPython/litdrive/litdrive/selfdriving/__main__.py
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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


Expand Down
14 changes: 8 additions & 6 deletions src/aadcUserPython/litdrive/litdrive/selfdriving/commander.py
Original file line number Diff line number Diff line change
Expand Up @@ -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):
Expand Down
2 changes: 1 addition & 1 deletion src/aadcUserPython/litdrive/litdrive/selfdriving/jury.py
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand Down
Loading

0 comments on commit adc1cf0

Please sign in to comment.