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

VCM reps with planned trajectory

This commit is contained in:
emanuel 2022-11-21 17:53:10 +00:00
parent 64453527da
commit 86056eb304
1 changed files with 45 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

47
src/vcm.c
View File

@ -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 */ itss_st_t trajectoryA[TRAJECTORY_MAX_LEN+1]; /* ego trajectory */
ssize_t trajectoryA_len = 0; 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(); 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->vehicleLength.vehicleLengthConfidenceIndication = 0;
mvc_rep->vehicleWidth = facilities.vehicle.width; 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 // Accepted trajectory
if (!request->desiredTrajectories.list.count) { if (!request->desiredTrajectories.list.count) {
log_debug("[vc] received VCM request has no desired trajectories"); 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(); 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; TransportRequest_t* tr = NULL;
FacilitiesIndication_t* fi = NULL; FacilitiesIndication_t* fi = NULL;
VCM_t* vcm_rep = calloc(1, sizeof(VCM_t)); 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->vehicleLength.vehicleLengthConfidenceIndication = 0;
mvc_rep->vehicleWidth = facilities.vehicle.width; 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 = calloc(1, sizeof(CoordinationNegotiation_t));
mvc_rep->negotiation->present = CoordinationNegotiation_PR_reply; mvc_rep->negotiation->present = CoordinationNegotiation_PR_reply;
mvc_rep->negotiation->choice.reply.acceptedTrajectoriesIds.list.count = 0; mvc_rep->negotiation->choice.reply.acceptedTrajectoriesIds.list.count = 0;