diff --git a/src/vcm.c b/src/vcm.c index 521790b..3cd07e7 100644 --- a/src/vcm.c +++ b/src/vcm.c @@ -12,7 +12,7 @@ #include #include -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) { - coordination_t* coordination = &facilities.coordination; +static void intersection_check(VCM_t* vcm, mc_neighbour_s* neighbour) { - 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(); - trajectoryA_len = epv.trajectory.len; - memcpy(trajectoryA + 1, epv.trajectory.path, trajectoryA_len * sizeof(itss_st_t)); - itss_trajectory_unlock(); + } 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; + } - 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(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]); + /* 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;