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

DCM requests

This commit is contained in:
emanuel 2022-02-11 17:28:48 +00:00
parent 54d5b968bc
commit 16c8f385e1
1 changed files with 66 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

73
src/dcm.c
View File

@ -51,6 +51,8 @@ static int are_vehicles_intersecting(
int dcm_check(void* fc, DCM_t* dcm) { int dcm_check(void* fc, DCM_t* dcm) {
facilities_t* facilities = (facilities_t*) fc; facilities_t* facilities = (facilities_t*) fc;
DCM_t* dcm_req = NULL;
it2s_tender_st_s trajectoryA[TRAJECTORY_MAX_LEN+1]; /* ego trajectory */ it2s_tender_st_s trajectoryA[TRAJECTORY_MAX_LEN+1]; /* ego trajectory */
it2s_tender_st_s trajectoryB[TRAJECTORY_MAX_LEN+1]; /* neighbour trajectory */ it2s_tender_st_s trajectoryB[TRAJECTORY_MAX_LEN+1]; /* neighbour trajectory */
uint16_t trajectoryA_len = 0; uint16_t trajectoryA_len = 0;
@ -86,19 +88,76 @@ int dcm_check(void* fc, DCM_t* dcm) {
++trajectoryB_len; ++trajectoryB_len;
} }
bool intersecting = false;
int index = 0;
if (trajectoryA_len > 1 && trajectoryB_len > 1) { if (trajectoryA_len > 1 && trajectoryB_len > 1) {
int index = 0; intersecting = are_vehicles_intersecting(
if (are_vehicles_intersecting(
trajectoryA, trajectoryA_len, facilities->vehicle.length, facilities->vehicle.width, trajectoryA, trajectoryA_len, facilities->vehicle.length, facilities->vehicle.width,
trajectoryB, trajectoryB_len, dcm->dcm.vehicleDimensions.length, dcm->dcm.vehicleDimensions.width, trajectoryB, trajectoryB_len, dcm->dcm.vehicleDimensions.length, dcm->dcm.vehicleDimensions.width,
&index) &index);
) {
syslog_info("[facilities] [dc] intersection danger with %ld @ (%d, %d) in %ld ms",
dcm->header.stationID, trajectoryA[index].latitude, trajectoryA[index].longitude, trajectoryA[index].timestamp - now);
}
} }
if (!intersecting) goto cleanup;
syslog_info("[facilities] [dc] intersection danger with %ld @ (%d, %d) in %ld ms",
dcm->header.stationID, trajectoryA[index].latitude, trajectoryA[index].longitude, trajectoryA[index].timestamp - now);
// Initiate conflict resolution
dcm = calloc(1, sizeof(DCM_t));
it2s_tender_st_s trajectory[TRAJECTORY_MAX_LEN];
dcm->header.messageID = 43;
dcm->header.protocolVersion = 1;
dcm->header.stationID = 1;
pthread_mutex_lock(&facilities->id.lock);
dcm->header.stationID = facilities->id.station_id;
pthread_mutex_unlock(&facilities->id.lock);
dcm->dcm.currentPosition.latitude = lat;
dcm->dcm.currentPosition.longitude = lon;
asn_ulong2INTEGER(&dcm->dcm.currentPosition.timestamp, now);
// Vehicle Dimensions
dcm->dcm.vehicleDimensions.length= facilities->vehicle.length;
dcm->dcm.vehicleDimensions.width = facilities->vehicle.width;
// Planned trajectory
dcm->dcm.plannedTrajectory.list.count = --trajectoryA_len;
dcm->dcm.plannedTrajectory.list.size = trajectoryA_len * sizeof(void*);
dcm->dcm.plannedTrajectory.list.array = malloc(trajectoryA_len * sizeof(void*));
for (int i = 0; i < trajectoryA_len; ++i) {
dcm->dcm.plannedTrajectory.list.array[i] = calloc(1, sizeof(STPoint_t));
dcm->dcm.plannedTrajectory.list.array[i]->latitude = trajectory[i+1].latitude;
dcm->dcm.plannedTrajectory.list.array[i]->longitude = trajectory[i+1].longitude;
asn_ulong2INTEGER(&dcm->dcm.plannedTrajectory.list.array[i]->timestamp, trajectory[i+1].timestamp);
}
// Desired trajectory
dcm->dcm.negotiation = calloc(1, sizeof(CoordinationNegotiation_t));
dcm->dcm.negotiation->present = CoordinationNegotiation_PR_request;
dcm->dcm.negotiation->choice.request.desiredTrajectories.list.count = 1;
dcm->dcm.negotiation->choice.request.desiredTrajectories.list.size = sizeof(void*);
dcm->dcm.negotiation->choice.request.desiredTrajectories.list.array = malloc(1*sizeof(void*));
dcm->dcm.negotiation->choice.request.desiredTrajectories.list.array[0] = calloc(1,sizeof(ProposedTrajectory_t));
ProposedTrajectory_t* pt = dcm->dcm.negotiation->choice.request.desiredTrajectories.list.array[0];
pt->trajectory.list.count = trajectoryA_len;
pt->trajectory.list.size = sizeof(void*) * trajectoryA_len;
pt->trajectory.list.array = malloc(sizeof(void*) * trajectoryA_len);
for (int i = 0; i < trajectoryA_len; ++i) {
pt->trajectory.list.array[i] = calloc(1, sizeof(STPoint_t));
pt->trajectory.list.array[i]->latitude = trajectory[i+1].latitude;
pt->trajectory.list.array[i]->longitude = trajectory[i+1].longitude;
asn_ulong2INTEGER(&pt->trajectory.list.array[i]->timestamp, trajectory[i+1].timestamp);
}
pt->offer = 5;
pt->priority = 1;
pt->id = 0;
cleanup: cleanup:
ASN_STRUCT_FREE(asn_DEF_DCM, dcm_req);
return 0; return 0;
} }