diff --git a/src/vcm.c b/src/vcm.c index 2fd0860..b2ac2fc 100644 --- a/src/vcm.c +++ b/src/vcm.c @@ -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) {