Only reply to VCM directed to ego

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;
+    vcm_rep->header.stationID = my_station_id;
-    pthread_mutex_lock(&facilities->id.lock);
-    vcm_rep->header.stationID = facilities->id.station_id;
-    pthread_mutex_unlock(&facilities->id.lock);
             
     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_rep->vehicleLength.vehicleLengthValue= facilities->vehicle.length;
-    mvc->vehicleLength.vehicleLengthConfidenceIndication = 0;
+    mvc_rep->vehicleLength.vehicleLengthConfidenceIndication = 0;
-    mvc->vehicleWidth = facilities->vehicle.width;
+    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));
+    // TODO check if vehicles are really going to intersect
-    mvc->negotiation->present = CoordinationNegotiation_PR_reply;
+
-    mvc->negotiation->choice.reply.acceptedTrajectoriesIds.list.count = 1;
+    mvc_rep->negotiation = calloc(1, sizeof(CoordinationNegotiation_t));
-    mvc->negotiation->choice.reply.acceptedTrajectoriesIds.list.size = sizeof(void*);
+    mvc_rep->negotiation->present = CoordinationNegotiation_PR_reply;
-    mvc->negotiation->choice.reply.acceptedTrajectoriesIds.list.array = malloc(1*sizeof(void*));
+    mvc_rep->negotiation->choice.reply.acceptedTrajectoriesIds.list.count = 1;
-    mvc->negotiation->choice.reply.acceptedTrajectoriesIds.list.array[0] = malloc(sizeof(long));
+    mvc_rep->negotiation->choice.reply.acceptedTrajectoriesIds.list.size = sizeof(void*);
-    *mvc->negotiation->choice.reply.acceptedTrajectoriesIds.list.array[0] = pt->id;
+    mvc_rep->negotiation->choice.reply.acceptedTrajectoriesIds.list.array = malloc(1*sizeof(void*));
-    mvc->negotiation->choice.reply.requesterId = vcm->header.stationID;
+    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) {