Implement dynamic adjustment of velocity

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"
   }
 ]

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
 

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
 

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)