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,6 +578,63 @@ int vcm_check(VCM_t* vcm) {
     lon = epv.space.longitude;
     itss_space_unlock();
 
+    uint64_t now = itss_time_get();
+
+    itss_trajectory_lock();
+    trajectoryA_len = epv.trajectory.len;
+    memcpy(trajectoryA + 1, epv.trajectory.path, trajectoryA_len * sizeof(itss_st_t));
+    itss_trajectory_unlock();
+
+    trajectoryA[0].latitude = lat;
+    trajectoryA[0].longitude = lon;
+    trajectoryA[0].timestamp = now;
+    ++trajectoryA_len;
+
+    trajectoryB[0].latitude = vcm->vcm.currentPosition.latitude;
+    trajectoryB[0].longitude = vcm->vcm.currentPosition.longitude;
+    asn_INTEGER2ulong(&vcm->vcm.currentPosition.timestamp, (unsigned long long*) &trajectoryB[0].timestamp);
+    ++trajectoryB_len;
+    for (int i = 1; i < mvc->plannedTrajectory.list.count && i < TRAJECTORY_MAX_LEN; ++i) {
+        trajectoryB[i].latitude = mvc->plannedTrajectory.list.array[i]->deltaLatitude + trajectoryB[i-1].latitude;
+        trajectoryB[i].longitude = mvc->plannedTrajectory.list.array[i]->deltaLongitude + trajectoryB[i-1].longitude;
+        trajectoryB[i].timestamp = mvc->plannedTrajectory.list.array[i]->deltaTime + trajectoryB[i-1].timestamp;
+        ++trajectoryB_len;
+    }
+
+    // Save neighbour trajectory
+    memcpy(neighbour->trajectory, trajectoryB, trajectoryB_len * sizeof(itss_st_t));
+    neighbour->trajectory_len = trajectoryB_len;
+    neighbour->length = mvc->vehicleLength.vehicleLengthValue;
+    neighbour->width = mvc->vehicleWidth;
+
+    int index = -1;
+
+    if (trajectoryA_len > 1 && trajectoryB_len > 1) {
+        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, 
+                    trajectoryA[index].latitude/1.0e7, 
+                    trajectoryA[index].longitude/1.0e7, 
+                    trajectoryA[index].timestamp - now);
+
+            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
@@ -611,6 +653,13 @@ int vcm_check(VCM_t* vcm) {
         ++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:
             ;
@@ -618,61 +667,34 @@ int vcm_check(VCM_t* vcm) {
             if (mvc->negotiation) {
                 switch (mvc->negotiation->present) {
                     case CoordinationNegotiation_PR_request:
-                        vcm_check_handle_request(vcm, &coordination->neighbours[ni]);
+                        vcm_check_handle_request(vcm, neighbour);
                         break; 
+
                     case CoordinationNegotiation_PR_reply:
-                        vcm_check_handle_reply(vcm, &coordination->neighbours[ni]);
+                        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"); 
 
-                itss_trajectory_lock();
+                } else { /* not in maneuver OR timeout reached */
-                trajectoryA_len = epv.trajectory.len;
+                    if (coordination->session.req) { /* clear previous request if exists */
-                memcpy(trajectoryA + 1, epv.trajectory.path, trajectoryA_len * sizeof(itss_st_t));
+                        ASN_STRUCT_FREE(asn_DEF_VCM, coordination->session.req);
-                itss_trajectory_unlock();
+                        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;
+                    }
 
-                trajectoryA[0].latitude = lat;
+                    /* check if received VCM intersects with ego trajectory */
-                trajectoryA[0].longitude = lon;
+                    intersection_check(vcm, neighbour);
-                trajectoryA[0].timestamp = now;
-                ++trajectoryA_len;
-
-                trajectoryB[0].latitude = vcm->vcm.currentPosition.latitude;
-                trajectoryB[0].longitude = vcm->vcm.currentPosition.longitude;
-                asn_INTEGER2ulong(&vcm->vcm.currentPosition.timestamp, (unsigned long long*) &trajectoryB[0].timestamp);
-                ++trajectoryB_len;
-                for (int i = 1; i < mvc->plannedTrajectory.list.count && i < TRAJECTORY_MAX_LEN; ++i) {
-                    trajectoryB[i].latitude = mvc->plannedTrajectory.list.array[i]->deltaLatitude + trajectoryB[i-1].latitude;
-                    trajectoryB[i].longitude = mvc->plannedTrajectory.list.array[i]->deltaLongitude + trajectoryB[i-1].longitude;
-                    trajectoryB[i].timestamp = mvc->plannedTrajectory.list.array[i]->deltaTime + trajectoryB[i-1].timestamp;
-                    ++trajectoryB_len;
-                }
-
-                // Save neighbour trajectory
-                memcpy(coordination->neighbours[ni].trajectory, trajectoryB, trajectoryB_len * sizeof(itss_st_t));
-                coordination->neighbours[ni].trajectory_len = trajectoryB_len;
-                coordination->neighbours[ni].length = mvc->vehicleLength.vehicleLengthValue;
-                coordination->neighbours[ni].width = mvc->vehicleWidth;
-
-                bool intersecting = false;
-                int index = -1;
-
-                if (trajectoryA_len > 1 && trajectoryB_len > 1) {
-                    intersecting = are_vehicles_intersecting(
-                            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, 
-                            trajectoryA[index].latitude/1.0e7, 
-                            trajectoryA[index].longitude/1.0e7, 
-                            trajectoryA[index].timestamp - now);
-
-                    intersection_detected(vcm, &coordination->neighbours[ni]);
                 }
             }
             break;
@@ -683,6 +705,7 @@ int vcm_check(VCM_t* vcm) {
             rv = 1;
     }
 
+cleanup:
     pthread_mutex_unlock(&coordination->lock);
 
     return rv;