diff --git a/components/entitiy_ident/entity_ident_service/entity_ident_server.py b/components/entitiy_ident/entity_ident_service/entity_ident_server.py index a87fd2f..3409e5e 100644 --- a/components/entitiy_ident/entity_ident_service/entity_ident_server.py +++ b/components/entitiy_ident/entity_ident_service/entity_ident_server.py @@ -54,16 +54,19 @@ def get_traffic_lights_geo(): }}, {"$redact": { "$cond": { - "if": {"$lte": ["$calculatedRange", "$range"]}, + "if": { + "$lte": ["$calculatedRange", "$range"] + }, "then": "$$KEEP", "else": "$$PRUNE" } }} ] ) + if lat < traffic_light['location'][1] ] - return json_util.dumps({'cursor': traffic_lights}) + return json_util.dumps({'cursor': traffic_lights[:1]}) if __name__ == '__main__': diff --git a/components/entitiy_ident/mongo/traffic_lights.json b/components/entitiy_ident/mongo/traffic_lights.json index 8dc949c..cf91b92 100644 --- a/components/entitiy_ident/mongo/traffic_lights.json +++ b/components/entitiy_ident/mongo/traffic_lights.json @@ -2,22 +2,22 @@ { "id": "1", "location": [16.20719, 47.89584], - "range": 2000, - "switchingTime": 500, + "range": 542, + "switchingTime": 15, "initialColor": "RED" }, { "id": "2", "location": [16.20814, 47.90937], - "range": 800, - "switchingTime": 240, + "range": 725, + "switchingTime": 20, "initialColor": "GREEN" }, { "id": "3", "location": [16.20917, 47.92703], - "range": 1000, - "switchingTime": 360, + "range": 910, + "switchingTime": 25, "initialColor": "RED" } ] \ No newline at end of file diff --git a/components/i_feed/devices/traffic_light.py b/components/i_feed/devices/traffic_light.py index 6470df7..5c13adb 100644 --- a/components/i_feed/devices/traffic_light.py +++ b/components/i_feed/devices/traffic_light.py @@ -11,7 +11,7 @@ from dse_shared_libs.message_broker_wrapper import MBWrapper from dse_shared_libs.traffic_light_state import TrafficLightState from pika.exceptions import AMQPConnectionError -SWITCHING_TIME = 10 +SWITCHING_TIME = 15 # Scale speed of switching by factor x SCALING = 1 diff --git a/components/i_feed/devices/vehicle.py b/components/i_feed/devices/vehicle.py index cae687c..64d0b39 100644 --- a/components/i_feed/devices/vehicle.py +++ b/components/i_feed/devices/vehicle.py @@ -16,18 +16,18 @@ from dse_shared_libs.target_velocity import TargetVelocity from pika.exceptions import AMQPConnectionError STARTING_POINT = geopy.Point(47.89053, 16.20703) -# in km/h -STARTING_VELOCITY = 130 # Driving direction in degrees: 0=N, 90=E, 180=S, 270=W BEARING = 0 # Scale speed of vehicles by factor x -SCALING = 2 +SCALING = 1 +# in km/h +STARTING_VELOCITY = 130 * SCALING # Interval between status updates in seconds (is not scaled) UPDATE_INTERVAL = 1 # At x km the NCE shall happen NCE_KM = 2.4 # Time in seconds to recover from NCE (will be scaled) -TIME_TO_RECOVER = 10 +TIME_TO_RECOVER = 5 # Resets vehicle at km x RESET_KM = 4 diff --git a/components/orchestration/orchestrator.py b/components/orchestration/orchestrator.py index 36f8f5a..d0f6cf0 100644 --- a/components/orchestration/orchestrator.py +++ b/components/orchestration/orchestrator.py @@ -3,6 +3,8 @@ import pickle import sys from random import randrange from typing import List, Dict +from datetime import datetime, timedelta +from math import floor, ceil import requests from dse_shared_libs import daf, traffic_light_state, traffic_light_color, target_velocity @@ -11,6 +13,7 @@ from dse_shared_libs.message_broker_wrapper import MBWrapper # necessary to unpickle daf object from dse_shared_libs.target_velocity import TargetVelocity from dse_shared_libs.traffic_light_state import TrafficLightState +from dse_shared_libs.traffic_light_color import TrafficLightColor from requests import Session sys.modules['daf'] = daf @@ -18,7 +21,6 @@ sys.modules['traffic_light_state'] = traffic_light_state sys.modules['traffic_light_color'] = traffic_light_color sys.modules['target_velocity'] = target_velocity - ENTITY_IDENT_URL = 'http://entityident:5002/api/v1/resources/' @@ -53,7 +55,8 @@ class Orchestrator: for traffic_light in traffic_lights['cursor']: self.tls[traffic_light['id']] = {'color': traffic_light['initialColor'], - 'switching_time': traffic_light['switchingTime']} + 'switching_time': traffic_light['switchingTime'], + 'last_switch': datetime.now()} def setup_msg_queues(self): # spawn the vehicle related message broker channels @@ -82,28 +85,47 @@ class Orchestrator: """ received_daf_object: DAF = pickle.loads(pickle_binary) loc = received_daf_object.gps_location - print(received_daf_object) + print('Received DAF object: {}'.format(received_daf_object)) - # TODO ask entity ident if tl in range?! - # something like the following below ... response = self._session.get(ENTITY_IDENT_URL + 'traffic_lights_geo', params={'lat': loc.latitude, 'lon': loc.longitude}) traffic_lights_geo = response.json() - print('TLGeo', traffic_lights_geo) - # TODO in the best case we get a traffic light ID and some sort of visibility range here? - # we need to calculate new speed regarding current speed and position and visibility of TL to ensure a - # green wave - # keep NCE in mind ... vehicle will not adapt to target velocity if still recovering from NCE - - # TODO use the data from the traffic lights (self.tls) - # to transmit a new target velocity for this vehicle to achieve a green wave + current_vel = received_daf_object.velocity + target_vel = 130 + print('Nearest traffic lights: {}'.format(traffic_lights_geo['cursor'])) + print('Nearest traffic light count: {}'.format(len(traffic_lights_geo['cursor']))) + for traffic_light in traffic_lights_geo['cursor']: + # Should only ever contain one traffic light + tl_id = traffic_light['id'] + distance = traffic_light['calculatedRange'] + next_switch_time = self.tls[tl_id]['last_switch'] + timedelta(seconds=self.tls[tl_id]['switching_time']) + time_until_switch = next_switch_time - datetime.now() + print('Distance to TL: {}'.format(distance)) + print('Time until switch in seconds: {}'.format(time_until_switch.total_seconds())) + if self.tls[tl_id]['color'] is TrafficLightColor.RED: + # Ceil because we want to get there AFTER the light changed + speed_needed = (distance / float(time_until_switch.total_seconds())) * 3.6 + if current_vel > speed_needed: + print('Current velocity too high ({}), adjusting to {}'.format(current_vel, speed_needed)) + target_vel = speed_needed + else: + print('Current velocity lower ({})'.format(current_vel)) + target_vel = current_vel + else: + # Check if we can cross in time with max speed (130) + speed_needed = (distance / float(time_until_switch.total_seconds())) * 3.6 + if speed_needed >= 130: + # Cannot make it in time, wait for next green + print('Wait on green -> red -> green switch') + time_until_green = time_until_switch + timedelta(seconds=self.tls[tl_id]['switching_time']) + target_vel = (distance / float(time_until_green.total_seconds())) * 3.6 + print('Speed should be way lower than 130: {}'.format(target_vel)) response_channel = self._velocity_mbs[received_daf_object.vehicle_identification_number] - target_vel = randrange(0, 130) print('Target velocity: {}'.format(target_vel)) response_channel.send(pickle.dumps( TargetVelocity(vin=received_daf_object.vehicle_identification_number, target_velocity=target_vel, - timestamp=datetime.datetime.now()))) + timestamp=datetime.now()))) def handle_tl_state_receive(self, msg): """ @@ -117,4 +139,5 @@ class Orchestrator: """ tl_state: TrafficLightState = pickle.loads(msg) self.tls[tl_state.tlid]['color'] = tl_state.color + self.tls[tl_state.tlid]['last_switch'] = tl_state.last_switch print(tl_state)