VCM: moved timeout checks to before intersection check

src/vcm.c

@@ -12,7 +12,7 @@
 #include <itss-management/ManagementRequest.h>
 #include <vcm/VCM.h>
 
-static int are_vehicles_intersecting(
+static int do_paths_intersect(
         itss_st_t* tA, int tA_len, uint16_t vA_length, uint16_t vA_width, 
         itss_st_t* tB, int tB_len, uint16_t vB_length, uint16_t vB_width,
         int* index
@@ -306,7 +306,7 @@ static int vcm_check_handle_reply(VCM_t* vcm, mc_neighbour_s* neighbour) {
         char fin[50];
         fin[0] = 0;
         if (all_rep) {
-            free(coordination->session.req);
+            ASN_STRUCT_FREE(asn_DEF_VCM, coordination->session.req);
             coordination->session.req = NULL;
             memset(coordination->session.affs, 0, sizeof(coordination->session.affs));
             memset(coordination->session.n_affs_neighs, 0, sizeof(coordination->session.n_affs_neighs));
@@ -348,18 +348,6 @@ static int intersection_detected(VCM_t* vcm, mc_neighbour_s* neighbour) {
 
     uint64_t now = itss_time_get();
 
-    if (now < neighbour->t_iid/1000 + MC_RESOLUTION_TIMEOUT ||
-        now < neighbour->t_proposal + MC_RESOLUTION_TIMEOUT) {
-        if (coordination->session.req) {
-            free(coordination->session.req);
-            coordination->session.req = NULL;
-            memset(coordination->session.affs, 0, sizeof(coordination->session.affs));
-            memset(coordination->session.n_affs_neighs, 0, sizeof(coordination->session.n_affs_neighs));
-            coordination->session.n_affs_trjs = 0;
-        }
-        rv = 0;
-        goto cleanup;
-    }
 
     int32_t lat, lon;
     itss_space_lock();
@@ -464,7 +452,7 @@ static int intersection_detected(VCM_t* vcm, mc_neighbour_s* neighbour) {
         }
 
         int index;
-        if (are_vehicles_intersecting(
+        if (do_paths_intersect(
                 trajectoryA, trajectoryA_len,
                 facilities.vehicle.length, facilities.vehicle.length, 
                 coordination->neighbours[n].trajectory, coordination->neighbours[n].trajectory_len, 
@@ -574,18 +562,15 @@ cleanup:
     return rv;
 }
 
-int vcm_check(VCM_t* vcm) {
+static void intersection_check(VCM_t* vcm, mc_neighbour_s* neighbour) {
-    coordination_t* coordination = &facilities.coordination;
 
-    int rv = 0;
+    ManeuverVehicleContainer_t* mvc = &vcm->vcm.maneuverContainer.choice.vehicle;
 
     itss_st_t trajectoryA[TRAJECTORY_MAX_LEN+1]; /* ego trajectory */
     itss_st_t trajectoryB[TRAJECTORY_MAX_LEN+1]; /* neighbour trajectory */
     uint16_t trajectoryA_len = 0;
     uint16_t trajectoryB_len = 0;
 
-    uint64_t now = itss_time_get();
-
     int32_t lat, lon;
     itss_space_lock();
     itss_space_get();
@@ -593,40 +578,7 @@ int vcm_check(VCM_t* vcm) {
     lon = epv.space.longitude;
     itss_space_unlock();
 
-    pthread_mutex_lock(&coordination->lock);
+    uint64_t now = itss_time_get();
-
-    // Find neighbours
-    int ni = -1;
-    for (int i = 0; i < coordination->neighbours_len; ++i) {
-        if (coordination->neighbours[i].station_id == vcm->header.stationID) {
-            ni = i;
-            break;
-        }
-    }
-
-    // Add it if not found
-    if (ni == -1 && coordination->neighbours_len < MC_MAX_NEIGHBOURS) {
-        coordination->neighbours[coordination->neighbours_len].station_id = vcm->header.stationID;
-        ni = coordination->neighbours_len;
-        ++coordination->neighbours_len;
-    }
-
-    switch (vcm->vcm.maneuverContainer.present) {
-        case ManeuverContainer_PR_vehicle:
-            ;
-            ManeuverVehicleContainer_t* mvc = &vcm->vcm.maneuverContainer.choice.vehicle;
-            if (mvc->negotiation) {
-                switch (mvc->negotiation->present) {
-                    case CoordinationNegotiation_PR_request:
-                        vcm_check_handle_request(vcm, &coordination->neighbours[ni]);
-                        break; 
-                    case CoordinationNegotiation_PR_reply:
-                        vcm_check_handle_reply(vcm, &coordination->neighbours[ni]);
-                        break; 
-                    default:
-                        break;
-                }
-            } else { 
 
     itss_trajectory_lock();
     trajectoryA_len = epv.trajectory.len;
@@ -650,21 +602,18 @@ int vcm_check(VCM_t* vcm) {
     }
 
     // Save neighbour trajectory
-                memcpy(coordination->neighbours[ni].trajectory, trajectoryB, trajectoryB_len * sizeof(itss_st_t));
+    memcpy(neighbour->trajectory, trajectoryB, trajectoryB_len * sizeof(itss_st_t));
-                coordination->neighbours[ni].trajectory_len = trajectoryB_len;
+    neighbour->trajectory_len = trajectoryB_len;
-                coordination->neighbours[ni].length = mvc->vehicleLength.vehicleLengthValue;
+    neighbour->length = mvc->vehicleLength.vehicleLengthValue;
-                coordination->neighbours[ni].width = mvc->vehicleWidth;
+    neighbour->width = mvc->vehicleWidth;
 
-                bool intersecting = false;
     int index = -1;
 
     if (trajectoryA_len > 1 && trajectoryB_len > 1) {
-                    intersecting = are_vehicles_intersecting(
+        bool intersecting = do_paths_intersect(
                 trajectoryA, trajectoryA_len, facilities.vehicle.length, facilities.vehicle.width, 
                 trajectoryB, trajectoryB_len, mvc->vehicleLength.vehicleLengthValue, mvc->vehicleWidth,
                 &index);
-                }
-
         if (intersecting) {
             log_info("[vc] intersection danger with %lld @ (%f, %f) in %ld ms", 
                     vcm->header.stationID, 
@@ -672,7 +621,80 @@ int vcm_check(VCM_t* vcm) {
                     trajectoryA[index].longitude/1.0e7, 
                     trajectoryA[index].timestamp - now);
 
-                    intersection_detected(vcm, &coordination->neighbours[ni]);
+            intersection_detected(vcm, neighbour);
+        }
+    }
+}
+
+int vcm_check(VCM_t* vcm) {
+    coordination_t* coordination = &facilities.coordination;
+
+    int rv = 0;
+
+
+    uint64_t now = itss_time_get();
+
+
+    pthread_mutex_lock(&coordination->lock);
+
+    // Find neighbours
+    int ni = -1;
+    for (int i = 0; i < coordination->neighbours_len; ++i) {
+        if (coordination->neighbours[i].station_id == vcm->header.stationID) {
+            ni = i;
+            break;
+        }
+    }
+
+    // Add it if not found
+    if (ni == -1 && coordination->neighbours_len < MC_MAX_NEIGHBOURS) {
+        coordination->neighbours[coordination->neighbours_len].station_id = vcm->header.stationID;
+        ni = coordination->neighbours_len;
+        ++coordination->neighbours_len;
+    }
+
+    if (ni == -1) {
+        rv = 1;
+        goto cleanup;
+    }
+
+    mc_neighbour_s* neighbour = &coordination->neighbours[ni];
+
+    switch (vcm->vcm.maneuverContainer.present) {
+        case ManeuverContainer_PR_vehicle:
+            ;
+            ManeuverVehicleContainer_t* mvc = &vcm->vcm.maneuverContainer.choice.vehicle;
+            if (mvc->negotiation) {
+                switch (mvc->negotiation->present) {
+                    case CoordinationNegotiation_PR_request:
+                        vcm_check_handle_request(vcm, neighbour);
+                        break; 
+
+                    case CoordinationNegotiation_PR_reply:
+                        vcm_check_handle_reply(vcm, neighbour);
+                        break; 
+
+                    default:
+                        break;
+                }
+            } else { 
+                if (
+                    now < neighbour->t_iid/1000 + MC_RESOLUTION_TIMEOUT ||
+                    now < neighbour->t_proposal + MC_RESOLUTION_TIMEOUT
+                    ) { /* in maneuver */
+                    log_warn("[coordination] received basic VCM of vehicle while maneuvering with it"); 
+
+                } else { /* not in maneuver OR timeout reached */
+                    if (coordination->session.req) { /* clear previous request if exists */
+                        ASN_STRUCT_FREE(asn_DEF_VCM, coordination->session.req);
+                        coordination->session.req = NULL;
+                        memset(coordination->session.affs, 0, sizeof(coordination->session.affs));
+                        memset(coordination->session.n_affs_neighs, 0, sizeof(coordination->session.n_affs_neighs));
+                        coordination->session.n_affs_trjs = 0;
+                    }
+
+                    /* check if received VCM intersects with ego trajectory */
+                    intersection_check(vcm, neighbour);
                 }
             }
             break;
@@ -683,6 +705,7 @@ int vcm_check(VCM_t* vcm) {
             rv = 1;
     }
 
+cleanup:
     pthread_mutex_unlock(&coordination->lock);
 
     return rv;