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

Only reply to VCM directed to ego

This commit is contained in:
emanuel 2022-06-28 17:52:24 +01:00
parent 750ffe0601
commit 3bc140e1f1
1 changed files with 39 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

65
src/vcm.c
View File

@ -55,7 +55,24 @@ static int vcm_check_handle_request(facilities_t* facilities, VCM_t* vcm, mc_nei
coordination_s* coordination = &facilities->coordination;
pthread_mutex_lock(&facilities->id.lock);
uint64_t my_station_id = facilities->id.station_id;
pthread_mutex_unlock(&facilities->id.lock);
// Is request for me?
CoordinationRequest_t* request = &vcm->vcm.maneuverContainer.choice.vehicle.negotiation->choice.request;
bool is_req4me = false;
for (int i = 0; i < request->desiredTrajectories.list.count && !is_req4me; ++i) {
for (int j = 0; j < request->desiredTrajectories.list.array[i]->affectingStations.list.count; ++j) {
if (my_station_id == *request->desiredTrajectories.list.array[i]->affectingStations.list.array[j]) {
is_req4me = true;
break;
}
}
}
if (!is_req4me) {
return 0;
}
VCM_t* vcm_rep = NULL;
TransportRequest_t* tr = NULL;
@ -66,6 +83,8 @@ static int vcm_check_handle_request(facilities_t* facilities, VCM_t* vcm, mc_nei
it2s_tender_st_s trajectoryA[TRAJECTORY_MAX_LEN+1]; /* ego trajectory */
ssize_t trajectoryA_len = 0;
it2s_tender_st_s trajectoryB[TRAJECTORY_MAX_LEN+1]; /* neighbour trajectory */
uint16_t trajectoryB_len = 0;
uint64_t now = it2s_tender_get_clock(&facilities->epv);
@ -79,24 +98,11 @@ static int vcm_check_handle_request(facilities_t* facilities, VCM_t* vcm, mc_nei
lon = facilities->epv.space.longitude;
it2s_tender_unlock_space(&facilities->epv);
it2s_tender_lock_trajectory(&facilities->epv);
trajectoryA_len = facilities->epv.trajectory.len;
memcpy(trajectoryA + 1, facilities->epv.trajectory.path, trajectoryA_len * sizeof(it2s_tender_st_s));
it2s_tender_unlock_trajectory(&facilities->epv);
trajectoryA[0].latitude = lat;
trajectoryA[0].longitude = lon;
trajectoryA[0].timestamp = now;
++trajectoryA_len;
vcm_rep = calloc(1, sizeof(VCM_t));
vcm_rep->header.messageID = 43;
vcm_rep->header.protocolVersion = 1;
vcm_rep->header.stationID = 1;
pthread_mutex_lock(&facilities->id.lock);
vcm_rep->header.stationID = facilities->id.station_id;
pthread_mutex_unlock(&facilities->id.lock);
vcm_rep->header.stationID = my_station_id;
vcm_rep->vcm.currentPosition.latitude = lat;
vcm_rep->vcm.currentPosition.longitude = lon;
@ -112,12 +118,12 @@ static int vcm_check_handle_request(facilities_t* facilities, VCM_t* vcm, mc_nei
}
vcm_rep->vcm.maneuverContainer.present = ManeuverContainer_PR_vehicle;
ManeuverVehicleContainer_t* mvc = &vcm_rep->vcm.maneuverContainer.choice.vehicle;
ManeuverVehicleContainer_t* mvc_rep = &vcm_rep->vcm.maneuverContainer.choice.vehicle;
// Vehicle DimensionsDimensions
mvc->vehicleLength.vehicleLengthValue= facilities->vehicle.length;
mvc->vehicleLength.vehicleLengthConfidenceIndication = 0;
mvc->vehicleWidth = facilities->vehicle.width;
mvc_rep->vehicleLength.vehicleLengthValue= facilities->vehicle.length;
mvc_rep->vehicleLength.vehicleLengthConfidenceIndication = 0;
mvc_rep->vehicleWidth = facilities->vehicle.width;
// Accepted trajectory
if (!request->desiredTrajectories.list.count) {
@ -127,14 +133,16 @@ static int vcm_check_handle_request(facilities_t* facilities, VCM_t* vcm, mc_nei
}
ProposedTrajectory_t* pt = request->desiredTrajectories.list.array[0];
mvc->negotiation = calloc(1, sizeof(CoordinationNegotiation_t));
mvc->negotiation->present = CoordinationNegotiation_PR_reply;
mvc->negotiation->choice.reply.acceptedTrajectoriesIds.list.count = 1;
mvc->negotiation->choice.reply.acceptedTrajectoriesIds.list.size = sizeof(void*);
mvc->negotiation->choice.reply.acceptedTrajectoriesIds.list.array = malloc(1*sizeof(void*));
mvc->negotiation->choice.reply.acceptedTrajectoriesIds.list.array[0] = malloc(sizeof(long));
*mvc->negotiation->choice.reply.acceptedTrajectoriesIds.list.array[0] = pt->id;
mvc->negotiation->choice.reply.requesterId = vcm->header.stationID;
// TODO check if vehicles are really going to intersect
mvc_rep->negotiation = calloc(1, sizeof(CoordinationNegotiation_t));
mvc_rep->negotiation->present = CoordinationNegotiation_PR_reply;
mvc_rep->negotiation->choice.reply.acceptedTrajectoriesIds.list.count = 1;
mvc_rep->negotiation->choice.reply.acceptedTrajectoriesIds.list.size = sizeof(void*);
mvc_rep->negotiation->choice.reply.acceptedTrajectoriesIds.list.array = malloc(1*sizeof(void*));
mvc_rep->negotiation->choice.reply.acceptedTrajectoriesIds.list.array[0] = malloc(sizeof(long));
*mvc_rep->negotiation->choice.reply.acceptedTrajectoriesIds.list.array[0] = pt->id;
mvc_rep->negotiation->choice.reply.requesterId = vcm->header.stationID;
asn_enc_rval_t enc = uper_encode_to_buffer(&asn_DEF_VCM, NULL, vcm_rep, buf, buf_len);
if (enc.encoded == -1) {
@ -352,6 +360,11 @@ static int vcm_check_intersection_detected(facilities_t* facilities, VCM_t* vcm,
pt->offer = 5;
pt->priority = 1;
pt->id = 0;
pt->affectingStations.list.count = 1;
pt->affectingStations.list.size = 1* sizeof(void*);
pt->affectingStations.list.array = malloc(sizeof(void*));
pt->affectingStations.list.array[0] = malloc(sizeof(long long));
*pt->affectingStations.list.array[0] = neighbour->station_id;
asn_enc_rval_t enc = uper_encode_to_buffer(&asn_DEF_VCM, NULL, vcm_req, buf, buf_len);
if (enc.encoded == -1) {