TiagoRG
/
it2s-itss-facilities
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

VCM: moved timeout checks to before intersection check

This commit is contained in:
emanuel 2022-11-19 18:58:56 +00:00
parent fbe6d5cfb8
commit 22a54a1d06
1 changed files with 89 additions and 66 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

149
src/vcm.c
View File

@ -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) {
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,40 +578,7 @@ int vcm_check(VCM_t* vcm) {
lon = epv.space.longitude;
itss_space_unlock();
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;
}
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 {
uint64_t now = itss_time_get();
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));
coordination->neighbours[ni].trajectory_len = trajectoryB_len;
coordination->neighbours[ni].length = mvc->vehicleLength.vehicleLengthValue;
coordination->neighbours[ni].width = mvc->vehicleWidth;
memcpy(neighbour->trajectory, trajectoryB, trajectoryB_len * sizeof(itss_st_t));
neighbour->trajectory_len = trajectoryB_len;
neighbour->length = mvc->vehicleLength.vehicleLengthValue;
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;