From 86056eb304c89ac957f5e6802af1aa90256d20a9 Mon Sep 17 00:00:00 2001 From: emanuel Date: Mon, 21 Nov 2022 17:53:10 +0000 Subject: [PATCH] VCM reps with planned trajectory --- src/vcm.c | 47 +++++++++++++++++++++++++++++++++++++++++++++-- 1 file changed, 45 insertions(+), 2 deletions(-) diff --git a/src/vcm.c b/src/vcm.c index 6acbd0a..222ed20 100644 --- a/src/vcm.c +++ b/src/vcm.c @@ -135,8 +135,6 @@ static int vcm_check_handle_request(VCM_t* vcm, mc_neighbour_s* neighbour) { itss_st_t trajectoryA[TRAJECTORY_MAX_LEN+1]; /* ego trajectory */ ssize_t trajectoryA_len = 0; - itss_st_t trajectoryB[TRAJECTORY_MAX_LEN+1]; /* neighbour trajectory */ - uint16_t trajectoryB_len = 0; uint64_t now = itss_time_get(); @@ -173,6 +171,27 @@ static int vcm_check_handle_request(VCM_t* vcm, mc_neighbour_s* neighbour) { mvc_rep->vehicleLength.vehicleLengthConfidenceIndication = 0; mvc_rep->vehicleWidth = facilities.vehicle.width; + itss_trajectory_lock(); + trajectoryA_len = epv.trajectory.len; + memcpy(trajectoryA + 1, epv.trajectory.path, trajectoryA_len * sizeof(itss_st_t)); + itss_trajectory_unlock(); + + trajectoryA[0].latitude = lat; + trajectoryA[0].longitude = lon; + trajectoryA[0].timestamp = now; + ++trajectoryA_len; + + // Planned trajectory + mvc_rep->plannedTrajectory.list.count = trajectoryA_len - 1; + mvc_rep->plannedTrajectory.list.size = (trajectoryA_len - 1) * sizeof(void*); + mvc_rep->plannedTrajectory.list.array = malloc((trajectoryA_len - 1) * sizeof(void*)); + for (int i = 0; i < trajectoryA_len - 1; ++i) { + mvc_rep->plannedTrajectory.list.array[i] = calloc(1, sizeof(STPoint_t)); + mvc_rep->plannedTrajectory.list.array[i]->deltaLatitude = trajectoryA[i+1].latitude - trajectoryA[i].latitude; + mvc_rep->plannedTrajectory.list.array[i]->deltaLongitude = trajectoryA[i+1].longitude - trajectoryA[i].longitude; + mvc_rep->plannedTrajectory.list.array[i]->deltaTime = trajectoryA[i+1].timestamp - trajectoryA[i].timestamp; + } + // Accepted trajectory if (!request->desiredTrajectories.list.count) { log_debug("[vc] received VCM request has no desired trajectories"); @@ -665,6 +684,9 @@ static void vcm_reject(VCM_t* vcm, mc_neighbour_s* neighbour) { uint64_t now = itss_time_get(); + itss_st_t trajectoryA[TRAJECTORY_MAX_LEN+1]; /* ego trajectory */ + ssize_t trajectoryA_len = 0; + TransportRequest_t* tr = NULL; FacilitiesIndication_t* fi = NULL; VCM_t* vcm_rep = calloc(1, sizeof(VCM_t)); @@ -700,6 +722,27 @@ static void vcm_reject(VCM_t* vcm, mc_neighbour_s* neighbour) { mvc_rep->vehicleLength.vehicleLengthConfidenceIndication = 0; mvc_rep->vehicleWidth = facilities.vehicle.width; + itss_trajectory_lock(); + trajectoryA_len = epv.trajectory.len; + memcpy(trajectoryA + 1, epv.trajectory.path, trajectoryA_len * sizeof(itss_st_t)); + itss_trajectory_unlock(); + + trajectoryA[0].latitude = lat; + trajectoryA[0].longitude = lon; + trajectoryA[0].timestamp = now; + ++trajectoryA_len; + + // Planned trajectory + mvc_rep->plannedTrajectory.list.count = trajectoryA_len - 1; + mvc_rep->plannedTrajectory.list.size = (trajectoryA_len - 1) * sizeof(void*); + mvc_rep->plannedTrajectory.list.array = malloc((trajectoryA_len - 1) * sizeof(void*)); + for (int i = 0; i < trajectoryA_len - 1; ++i) { + mvc_rep->plannedTrajectory.list.array[i] = calloc(1, sizeof(STPoint_t)); + mvc_rep->plannedTrajectory.list.array[i]->deltaLatitude = trajectoryA[i+1].latitude - trajectoryA[i].latitude; + mvc_rep->plannedTrajectory.list.array[i]->deltaLongitude = trajectoryA[i+1].longitude - trajectoryA[i].longitude; + mvc_rep->plannedTrajectory.list.array[i]->deltaTime = trajectoryA[i+1].timestamp - trajectoryA[i].timestamp; + } + mvc_rep->negotiation = calloc(1, sizeof(CoordinationNegotiation_t)); mvc_rep->negotiation->present = CoordinationNegotiation_PR_reply; mvc_rep->negotiation->choice.reply.acceptedTrajectoriesIds.list.count = 0;