1173 lines
45 KiB
C
1173 lines
45 KiB
C
#include "vcm.h"
|
|
#include "facilities.h"
|
|
|
|
#include <it2s-tender/epv.h>
|
|
#include <it2s-tender/space.h>
|
|
#include <it2s-tender/trajectory.h>
|
|
#include <it2s-tender/geodesy.h>
|
|
#include <it2s-tender/recorder.h>
|
|
#include <it2s-tender/packet.h>
|
|
#include <it2s-asn/itss-transport/TransportRequest.h>
|
|
#include <it2s-asn/itss-facilities/FacilitiesIndication.h>
|
|
#include <it2s-asn/itss-management/ManagementRequest.h>
|
|
#include <it2s-asn/vcm/VCM.h>
|
|
|
|
static int do_paths_intersect(
|
|
itss_st_t* tA, int tA_len, uint16_t vA_length, uint16_t vA_width,
|
|
itss_st_t* tB, int tB_len, uint16_t vB_length, uint16_t vB_width,
|
|
int* index
|
|
) {
|
|
|
|
double A1[2], A2[2], B1[2], B2[2];
|
|
uint64_t tsA, tsB;
|
|
for (int a = 0; a < tA_len-1; ++a) {
|
|
A1[0] = tA[a].latitude;
|
|
A1[1] = tA[a].longitude;
|
|
A2[0] = tA[a+1].latitude;
|
|
A2[1] = tA[a+1].longitude;
|
|
for (int b = 0; b < tB_len-1; ++b) {
|
|
if (tA[a].timestamp > tB[b].timestamp + 2000 ||
|
|
tA[a].timestamp < tB[b].timestamp - 2000) {
|
|
continue;
|
|
}
|
|
|
|
B1[0] = tB[b].latitude;
|
|
B1[1] = tB[b].longitude;
|
|
B2[0] = tB[b+1].latitude;
|
|
B2[1] = tB[b+1].longitude;
|
|
|
|
if (itss_do_segments_intersect(A1, A2, B1, B2)) {
|
|
*index = a;
|
|
return 1;
|
|
}
|
|
}
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
static mc_neighbour_s* get_neighbour(uint32_t station_id) {
|
|
coordination_t* coordination = &facilities.coordination;
|
|
|
|
mc_neighbour_s* neighbour = NULL;
|
|
|
|
// Find neighbours
|
|
for (int i = 0; i < coordination->neighbours_len; ++i) {
|
|
if (coordination->neighbours[i].station_id == station_id) {
|
|
return &coordination->neighbours[i];
|
|
}
|
|
}
|
|
|
|
// Add it if not found
|
|
if (coordination->neighbours_len < MC_MAX_NEIGHBOURS) {
|
|
coordination->neighbours[coordination->neighbours_len].station_id = station_id;
|
|
++coordination->neighbours_len;
|
|
return &coordination->neighbours[coordination->neighbours_len-1];
|
|
}
|
|
|
|
return NULL;
|
|
}
|
|
|
|
|
|
static void tx_vcm(VCM_t* vcm) {
|
|
const uint16_t buf_len = 2048;
|
|
uint8_t buf[buf_len];
|
|
TransportRequest_t* tr = NULL;
|
|
FacilitiesIndication_t* fi = NULL;
|
|
asn_enc_rval_t enc = uper_encode_to_buffer(&asn_DEF_VCM, NULL, vcm, buf, buf_len);
|
|
if (enc.encoded == -1) {
|
|
log_error("[vc] VCM.reply encode failure (%s)", enc.failed_type->name);
|
|
goto cleanup;
|
|
}
|
|
uint16_t vcm_len = (enc.encoded + 7) / 8;
|
|
|
|
tr = calloc(1, sizeof(TransportRequest_t));
|
|
tr->present = TransportRequest_PR_packet;
|
|
tr->choice.packet.present = TransportPacketRequest_PR_btp;
|
|
BTPPacketRequest_t* bpr = &tr->choice.packet.choice.btp;
|
|
bpr->btpType = BTPType_btpB;
|
|
|
|
if (facilities.edm.enabled &&
|
|
vcm->vcm.maneuverContainer.present == ManeuverContainer_PR_vehicle &&
|
|
vcm->vcm.maneuverContainer.choice.vehicle.negotiation
|
|
) {
|
|
edm_encap(buf, &vcm_len, buf_len, 2043);
|
|
bpr->destinationPortInfo = calloc(1, sizeof(OCTET_STRING_t));
|
|
bpr->destinationPortInfo->size = 2;
|
|
bpr->destinationPortInfo->buf = malloc(2);
|
|
*(uint16_t*)bpr->destinationPortInfo->buf = 0xed;
|
|
}
|
|
|
|
bpr->id = itss_id(buf, vcm_len);
|
|
bpr->gn.destinationAddress.buf = malloc(6);
|
|
for (int i = 0; i < 6; ++i) {
|
|
bpr->gn.destinationAddress.buf[i] = 0xff;
|
|
}
|
|
bpr->gn.destinationAddress.size = 6;
|
|
bpr->gn.packetTransportType = PacketTransportType_shb;
|
|
bpr->destinationPort = 2043;
|
|
bpr->gn.trafficClass = 2;
|
|
bpr->data.buf = malloc(vcm_len);
|
|
memcpy(bpr->data.buf, buf, vcm_len);
|
|
bpr->data.size = vcm_len;
|
|
buf[0] = 4;
|
|
enc = asn_encode_to_buffer(NULL, ATS_CANONICAL_OER, &asn_DEF_TransportRequest, tr, buf+1, buf_len-1);
|
|
if (enc.encoded == -1) {
|
|
log_error("[vc] TR VCM.reply encode failure (%s)", enc.failed_type->name);
|
|
goto cleanup;
|
|
}
|
|
|
|
itss_queue_send(facilities.tx_queue, buf, enc.encoded+1, ITSS_TRANSPORT, bpr->id, "TR.packet.btp");
|
|
|
|
fi = calloc(1, sizeof(FacilitiesIndication_t));
|
|
fi->present = FacilitiesIndication_PR_message;
|
|
fi->choice.message.id = bpr->id;
|
|
fi->choice.message.itsMessageType = 2043;
|
|
fi->choice.message.data.size = bpr->data.size;
|
|
fi->choice.message.data.buf = malloc(bpr->data.size);
|
|
memcpy(fi->choice.message.data.buf, bpr->data.buf, bpr->data.size);
|
|
buf[0] = 4;
|
|
enc = asn_encode_to_buffer(NULL, ATS_CANONICAL_OER, &asn_DEF_FacilitiesIndication, fi, buf+1, buf_len-1);
|
|
if (enc.encoded == -1) {
|
|
log_error("[vc] TR VCM.reply encode failure (%s)", enc.failed_type->name);
|
|
goto cleanup;
|
|
}
|
|
|
|
itss_queue_send(facilities.tx_queue, buf, enc.encoded+1, ITSS_APPLICATIONS, bpr->id, "FI.message");
|
|
|
|
facilities.coordination.t_last_send_vcm = itss_time_get();
|
|
|
|
if (facilities.logging.recorder) {
|
|
uint16_t buffer_len = 2048;
|
|
uint8_t buffer[buffer_len];
|
|
int e = itss_management_record_packet_sdu(
|
|
buffer,
|
|
buffer_len,
|
|
bpr->data.buf,
|
|
bpr->data.size,
|
|
bpr->id,
|
|
itss_time_get(),
|
|
ITSS_FACILITIES,
|
|
true);
|
|
if (e != -1) {
|
|
itss_queue_send(facilities.tx_queue, buffer, e, ITSS_MANAGEMENT, bpr->id, "MReq.packet.set");
|
|
}
|
|
}
|
|
|
|
cleanup:
|
|
ASN_STRUCT_FREE(asn_DEF_TransportRequest, tr);
|
|
ASN_STRUCT_FREE(asn_DEF_FacilitiesIndication, fi);
|
|
}
|
|
|
|
static void vcm_reject(VCM_t* vcm, mc_neighbour_s* neighbour) {
|
|
|
|
coordination_t* coordination = &facilities.coordination;
|
|
|
|
pthread_mutex_lock(&facilities.id.lock);
|
|
uint32_t my_station_id = facilities.id.station_id;
|
|
pthread_mutex_unlock(&facilities.id.lock);
|
|
|
|
int32_t lat, lon;
|
|
itss_space_lock();
|
|
itss_space_get();
|
|
lat = epv.space.latitude;
|
|
lon = epv.space.longitude;
|
|
itss_space_unlock();
|
|
|
|
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));
|
|
|
|
int rv = 0;
|
|
const uint16_t buf_len = 2048;
|
|
uint8_t buf1[buf_len];
|
|
|
|
vcm_rep->header.messageID = 43;
|
|
vcm_rep->header.protocolVersion = 1;
|
|
vcm_rep->header.stationID = my_station_id;
|
|
|
|
vcm_rep->vcm.currentPosition.latitude = lat;
|
|
vcm_rep->vcm.currentPosition.longitude = lon;
|
|
asn_ulong2INTEGER(&vcm_rep->vcm.currentPosition.timestamp, now);
|
|
|
|
/*
|
|
if (coordination->chain.enabled && coordination->chain.id) {
|
|
vcm_rep->vcm.chain = calloc(1, sizeof(ChainInformation_t));
|
|
vcm_rep->vcm.chain->id = coordination->chain.id;
|
|
vcm_rep->vcm.chain->link.buf = malloc(32);
|
|
vcm_rep->vcm.chain->link.size = 32;
|
|
memcpy(vcm_rep->vcm.chain->link.buf, coordination->chain.links[0], 32);
|
|
vcm_rep->vcm.chain->area = calloc(1, sizeof(ChainInformationArea_t));
|
|
vcm_rep->vcm.chain->area->trees.list.count = 1;
|
|
vcm_rep->vcm.chain->area->trees.list.size = 1 * sizeof(OCTET_STRING_t*);
|
|
vcm_rep->vcm.chain->area->trees.list.array = malloc(1 * sizeof(OCTET_STRING_t*));
|
|
for (int q = 0; q < 1; ++q) {
|
|
vcm_rep->vcm.chain->area->trees.list.array[q] = calloc(1, sizeof(OCTET_STRING_t));
|
|
vcm_rep->vcm.chain->area->trees.list.array[q]->buf = calloc(1,4);
|
|
vcm_rep->vcm.chain->area->trees.list.array[q]->size = 4;
|
|
}
|
|
}
|
|
*/
|
|
|
|
vcm_rep->vcm.maneuverContainer.present = ManeuverContainer_PR_vehicle;
|
|
ManeuverVehicleContainer_t* mvc_rep = &vcm_rep->vcm.maneuverContainer.choice.vehicle;
|
|
|
|
// Vehicle DimensionsDimensions
|
|
mvc_rep->vehicleLength.vehicleLengthValue= facilities.vehicle.length;
|
|
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;
|
|
mvc_rep->negotiation->choice.reply.acceptedTrajectoriesIds.list.size = 0;
|
|
mvc_rep->negotiation->choice.reply.acceptedTrajectoriesIds.list.array = NULL;
|
|
mvc_rep->negotiation->choice.reply.requesterId = vcm->header.stationID;
|
|
mvc_rep->negotiation->choice.reply.nonce = vcm->vcm.maneuverContainer.choice.vehicle.negotiation->choice.request.nonce;
|
|
|
|
tx_vcm(vcm_rep);
|
|
|
|
cleanup:
|
|
ASN_STRUCT_FREE(asn_DEF_VCM, vcm_rep);
|
|
}
|
|
|
|
static bool is_maneuver_approved() {
|
|
coordination_t* coordination = &facilities.coordination;
|
|
bool all_rep = true;
|
|
for (int t = 0; t < coordination->session.n_affs_trjs; ++t) {
|
|
if (coordination->session.n_affs_neighs[t] != 0) {
|
|
all_rep = false;
|
|
break;
|
|
}
|
|
}
|
|
if (all_rep) {
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
static void session_cleanup() {
|
|
coordination_t* coordination = &facilities.coordination;
|
|
ASN_STRUCT_FREE(asn_DEF_VCM, coordination->session.own_req);
|
|
coordination->session.own_req = NULL;
|
|
coordination->session.ts = itss_time_get();
|
|
coordination->session.nonce = 0;
|
|
coordination->session.requester = NULL;
|
|
memset(coordination->session.affs, 0, sizeof(coordination->session.affs));
|
|
memset(coordination->session.n_affs_neighs, 0, sizeof(coordination->session.n_affs_neighs));
|
|
coordination->session.n_affs_neighs_reqd = 0;
|
|
coordination->session.n_affs_trjs = 0;
|
|
}
|
|
|
|
static bool commit() {
|
|
coordination_t* coordination = &facilities.coordination;
|
|
|
|
if (!is_maneuver_approved()) {
|
|
return false;
|
|
}
|
|
|
|
/* do not send commit if in RR1C and ego not requester */
|
|
if (coordination->protocol == MC_PROTOCOL_VCM_RR1C &&
|
|
!coordination->session.own_req) {
|
|
session_cleanup();
|
|
return false;
|
|
}
|
|
|
|
log_debug("[vc] issuing COMMIT - session %d", coordination->session.nonce);
|
|
|
|
VCM_t* vcm_com = calloc(1, sizeof(VCM_t));
|
|
vcm_com->header.messageID = 43;
|
|
vcm_com->header.protocolVersion = 1;
|
|
pthread_mutex_lock(&facilities.id.lock);
|
|
vcm_com->header.stationID = facilities.id.station_id;
|
|
pthread_mutex_unlock(&facilities.id.lock);
|
|
|
|
int32_t lat, lon;
|
|
itss_space_lock();
|
|
itss_space_get();
|
|
lat = epv.space.latitude;
|
|
lon = epv.space.longitude;
|
|
itss_space_unlock();
|
|
vcm_com->vcm.currentPosition.latitude = lat;
|
|
vcm_com->vcm.currentPosition.longitude = lon;
|
|
|
|
uint64_t now = itss_time_get();
|
|
asn_ulong2INTEGER(&vcm_com->vcm.currentPosition.timestamp, now);
|
|
|
|
vcm_com->vcm.maneuverContainer.present = ManeuverContainer_PR_vehicle;
|
|
ManeuverVehicleContainer_t* mvc = &vcm_com->vcm.maneuverContainer.choice.vehicle;
|
|
|
|
// Vehicle Dimensions
|
|
mvc->vehicleLength.vehicleLengthValue= facilities.vehicle.length;
|
|
mvc->vehicleLength.vehicleLengthConfidenceIndication = 0;
|
|
mvc->vehicleWidth = facilities.vehicle.width;
|
|
|
|
itss_st_t trajectoryA[TRAJECTORY_MAX_LEN+1]; /* ego trajectory */
|
|
uint16_t trajectoryA_len = 0;
|
|
|
|
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;
|
|
|
|
mvc->plannedTrajectory.list.count = trajectoryA_len - 1;
|
|
mvc->plannedTrajectory.list.size = (trajectoryA_len - 1) * sizeof(void*);
|
|
mvc->plannedTrajectory.list.array = malloc((trajectoryA_len - 1) * sizeof(void*));
|
|
for (int i = 0; i < trajectoryA_len - 1; ++i) {
|
|
mvc->plannedTrajectory.list.array[i] = calloc(1, sizeof(STPoint_t));
|
|
mvc->plannedTrajectory.list.array[i]->deltaLatitude = trajectoryA[i+1].latitude - trajectoryA[i].latitude;
|
|
mvc->plannedTrajectory.list.array[i]->deltaLongitude = trajectoryA[i+1].longitude - trajectoryA[i].longitude;
|
|
mvc->plannedTrajectory.list.array[i]->deltaTime = trajectoryA[i+1].timestamp - trajectoryA[i].timestamp;
|
|
}
|
|
|
|
mvc->negotiation = calloc(1, sizeof(CoordinationNegotiation_t));
|
|
mvc->negotiation->present = CoordinationNegotiation_PR_commit;
|
|
mvc->negotiation->choice.commit.nonce = coordination->session.nonce;
|
|
if (coordination->session.own_req) {
|
|
mvc->negotiation->choice.commit.qp.present = CoordinationCommitQP_PR_acceptedTrajectoryId;
|
|
mvc->negotiation->choice.commit.qp.choice.acceptedTrajectoryId = 0;
|
|
} else {
|
|
mvc->negotiation->choice.commit.qp.present = CoordinationCommitQP_PR_requesterId;
|
|
mvc->negotiation->choice.commit.qp.choice.requesterId = coordination->session.requester->station_id;
|
|
}
|
|
|
|
/*
|
|
if (coordination->chain.enabled && coordination->chain.id) {
|
|
vcm_com->vcm.chain = calloc(1, sizeof(ChainInformation_t));
|
|
vcm_com->vcm.chain->id = coordination->chain.id;
|
|
vcm_com->vcm.chain->link.buf = malloc(32);
|
|
vcm_com->vcm.chain->link.size = 32;
|
|
memcpy(vcm_com->vcm.chain->link.buf, coordination->chain.links[0], 32);
|
|
vcm_com->vcm.chain->area = calloc(1, sizeof(ChainInformationArea_t));
|
|
vcm_com->vcm.chain->area->trees.list.count = 1;
|
|
vcm_com->vcm.chain->area->trees.list.size = 1 * sizeof(OCTET_STRING_t*);
|
|
vcm_com->vcm.chain->area->trees.list.array = malloc(1 * sizeof(OCTET_STRING_t*));
|
|
for (int q = 0; q < 1; ++q) {
|
|
vcm_com->vcm.chain->area->trees.list.array[q] = calloc(1, sizeof(OCTET_STRING_t));
|
|
vcm_com->vcm.chain->area->trees.list.array[q]->buf = calloc(1,4);
|
|
vcm_com->vcm.chain->area->trees.list.array[q]->size = 4;
|
|
}
|
|
}
|
|
*/
|
|
|
|
tx_vcm(vcm_com);
|
|
ASN_STRUCT_FREE(asn_DEF_VCM, vcm_com);
|
|
|
|
session_cleanup();
|
|
|
|
return true;
|
|
};
|
|
|
|
|
|
static int vcm_check_handle_request(VCM_t* vcm, mc_neighbour_s* neighbour) {
|
|
int rv = 0;
|
|
|
|
coordination_t* coordination = &facilities.coordination;
|
|
|
|
pthread_mutex_lock(&facilities.id.lock);
|
|
uint32_t my_station_id = facilities.id.station_id;
|
|
pthread_mutex_unlock(&facilities.id.lock);
|
|
|
|
uint64_t now = itss_time_get();
|
|
|
|
|
|
// 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) {
|
|
log_debug("[vc] VCM.request meant for %d", *request->desiredTrajectories.list.array[i]->affectingStations.list.array[j]);
|
|
if (my_station_id == *request->desiredTrajectories.list.array[i]->affectingStations.list.array[j]) {
|
|
is_req4me = true;
|
|
}
|
|
coordination->session.affs[i][j] = get_neighbour(*request->desiredTrajectories.list.array[i]->affectingStations.list.array[j]);
|
|
}
|
|
coordination->session.n_affs_neighs[i] = request->desiredTrajectories.list.array[i]->affectingStations.list.count;
|
|
coordination->session.n_affs_neighs_reqd = coordination->session.n_affs_neighs[i];
|
|
}
|
|
coordination->session.n_affs_trjs = request->desiredTrajectories.list.count;
|
|
|
|
if (!is_req4me) {
|
|
log_debug("[vc] received VCM.request not affecting me");
|
|
return 0;
|
|
}
|
|
|
|
if (coordination->session.own_req && now < coordination->session.ts + MC_RESOLUTION_TIMEOUT) { /* in maneuver */
|
|
log_debug("[vc] rejecting VCM from %d - currently in maneuver", vcm->header.stationID);
|
|
vcm_reject(vcm, neighbour);
|
|
return 1;
|
|
}
|
|
|
|
const ssize_t buf_len = 512;
|
|
uint8_t buf1[buf_len], buf2[buf_len];
|
|
|
|
// Change speed
|
|
// Just check who is closer to mid trajectory point, if ego is closer, accelerate
|
|
// TODO a real trajectory analysis
|
|
STDeltaTrajectory_t* stdt = &request->desiredTrajectories.list.array[0]->trajectory;
|
|
itss_st_t* trj = calloc(stdt->list.count/2 + 1, sizeof(itss_st_t));
|
|
trj[0].latitude = vcm->vcm.currentPosition.latitude;
|
|
trj[0].longitude = vcm->vcm.currentPosition.longitude;
|
|
asn_INTEGER2ulong(&vcm->vcm.currentPosition.timestamp, (unsigned long long*) &trj[0].timestamp);
|
|
int h = 0;
|
|
for (h = 1; h < stdt->list.count/2; ++h) {
|
|
trj[h].latitude = trj[h-1].latitude + stdt->list.array[h-1]->deltaLatitude;
|
|
trj[h].longitude = trj[h-1].longitude + stdt->list.array[h-1]->deltaLongitude;
|
|
trj[h].timestamp = trj[h-1].timestamp + stdt->list.array[h-1]->deltaTime;
|
|
}
|
|
if (h) --h;
|
|
|
|
int32_t lat, lon;
|
|
itss_space_lock();
|
|
itss_space_get();
|
|
lat = epv.space.latitude;
|
|
lon = epv.space.longitude;
|
|
itss_space_unlock();
|
|
|
|
double dreq = itss_haversine(
|
|
trj[h].latitude / 1.0e7, trj[h].longitude / 1.0e7,
|
|
vcm->vcm.currentPosition.latitude / 1.0e7, vcm->vcm.currentPosition.longitude / 1.0e7
|
|
);
|
|
double dego = itss_haversine(
|
|
trj[h].latitude / 1.0e7, trj[h].longitude / 1.0e7,
|
|
lat / 1.0e7, lon / 1.0e7
|
|
);
|
|
free(trj);
|
|
|
|
ManagementRequest_t* mreq = calloc(1, sizeof(ManagementRequest_t));
|
|
mreq->present = ManagementRequest_PR_attributes;
|
|
mreq->choice.attributes.present = ManagementRequestAttributes_PR_set;
|
|
mreq->choice.attributes.choice.set.speed = calloc(1, sizeof(ManagementSpeedSet_t));
|
|
ManagementSpeedSet_t* mgss = mreq->choice.attributes.choice.set.speed;
|
|
mgss->rate = 5; /* km/h/s */
|
|
mgss->temporary = true; /* go back to original speed after a while */
|
|
mgss->type.present = ManagementSpeedSetType_PR_diff; /* differential change set */
|
|
mgss->type.choice.diff = (dreq > dego) ? 10 : -10; /* % */
|
|
asn_enc_rval_t enc = asn_encode_to_buffer(NULL, ATS_CANONICAL_OER, &asn_DEF_ManagementRequest, mreq, buf1, buf_len);
|
|
if (enc.encoded == -1) {
|
|
log_error("[vc] failed to encode MReq.speedSet (%s)", enc.failed_type->name);
|
|
}
|
|
itss_0send(coordination->mgmt_socket, buf1, enc.encoded);
|
|
if (itss_0recv_rt(&coordination->mgmt_socket, buf2, buf_len, buf1, enc.encoded, 500) == -1) {
|
|
log_error("[vc]-> MReq.speedSet ->[management] <TIMEOUT>");
|
|
}
|
|
ASN_STRUCT_FREE(asn_DEF_ManagementRequest, mreq);
|
|
|
|
// Respond
|
|
VCM_t* vcm_rep = NULL;
|
|
TransportRequest_t* tr = NULL;
|
|
FacilitiesIndication_t* fi = NULL;
|
|
|
|
|
|
itss_st_t trajectoryA[TRAJECTORY_MAX_LEN+1]; /* ego trajectory */
|
|
ssize_t trajectoryA_len = 0;
|
|
|
|
neighbour->proposed = true;
|
|
neighbour->session.requested = true;
|
|
neighbour->session.nonce = request->nonce;
|
|
neighbour->station_id = vcm->header.stationID;
|
|
coordination->session.nonce = request->nonce;
|
|
coordination->session.requester = neighbour;
|
|
coordination->session.t_init = now;
|
|
|
|
vcm_rep = calloc(1, sizeof(VCM_t));
|
|
|
|
vcm_rep->header.messageID = 43;
|
|
vcm_rep->header.protocolVersion = 1;
|
|
vcm_rep->header.stationID = my_station_id;
|
|
|
|
vcm_rep->vcm.currentPosition.latitude = lat;
|
|
vcm_rep->vcm.currentPosition.longitude = lon;
|
|
asn_ulong2INTEGER(&vcm_rep->vcm.currentPosition.timestamp, now);
|
|
|
|
/*
|
|
if (coordination->chain.enabled && coordination->chain.id && vcm->vcm.chain) {
|
|
vcm_rep->vcm.chain = calloc(1, sizeof(ChainInformation_t));
|
|
vcm_rep->vcm.chain->id = coordination->chain.id;
|
|
vcm_rep->vcm.chain->link.buf = malloc(32);
|
|
vcm_rep->vcm.chain->link.size = 32;
|
|
|
|
int to_link = 0;
|
|
for (int i = 0; i < coordination->chain.n_links; ++i) {
|
|
if (memcmp(coordination->chain.links[i], vcm->vcm.chain->link.buf, 32)) {
|
|
to_link = i;
|
|
break;
|
|
}
|
|
}
|
|
memcpy(vcm_rep->vcm.chain->link.buf, coordination->chain.links[to_link], 32);
|
|
|
|
vcm_rep->vcm.chain->area = calloc(1, sizeof(ChainInformationArea_t));
|
|
vcm_rep->vcm.chain->area->trees.list.count = 1;
|
|
vcm_rep->vcm.chain->area->trees.list.size = 1 * sizeof(OCTET_STRING_t*);
|
|
vcm_rep->vcm.chain->area->trees.list.array = malloc(1 * sizeof(OCTET_STRING_t*));
|
|
for (int q = 0; q < 1; ++q) {
|
|
vcm_rep->vcm.chain->area->trees.list.array[q] = calloc(1, sizeof(OCTET_STRING_t));
|
|
vcm_rep->vcm.chain->area->trees.list.array[q]->buf = calloc(1,4);
|
|
vcm_rep->vcm.chain->area->trees.list.array[q]->size = 4;
|
|
}
|
|
}
|
|
*/
|
|
|
|
vcm_rep->vcm.maneuverContainer.present = ManeuverContainer_PR_vehicle;
|
|
ManeuverVehicleContainer_t* mvc_rep = &vcm_rep->vcm.maneuverContainer.choice.vehicle;
|
|
|
|
// Vehicle DimensionsDimensions
|
|
mvc_rep->vehicleLength.vehicleLengthValue= facilities.vehicle.length;
|
|
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");
|
|
rv = 1;
|
|
goto cleanup;
|
|
}
|
|
ProposedTrajectory_t* pt = request->desiredTrajectories.list.array[0];
|
|
|
|
// 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;
|
|
mvc_rep->negotiation->choice.reply.nonce = request->nonce;
|
|
|
|
tx_vcm(vcm_rep);
|
|
|
|
for (int t = 0; t < mvc_rep->negotiation->choice.reply.acceptedTrajectoriesIds.list.count; ++t) {
|
|
int tid = *mvc_rep->negotiation->choice.reply.acceptedTrajectoriesIds.list.array[t];
|
|
for (int n = 0; n < coordination->session.n_affs_neighs[tid]; ++n) {
|
|
if (my_station_id == coordination->session.affs[tid][n]->station_id) {
|
|
for (int i = n; i < coordination->session.n_affs_neighs[tid] - 1; ++i) {
|
|
coordination->session.affs[tid][i] = coordination->session.affs[tid][i+1];
|
|
}
|
|
--coordination->session.n_affs_neighs[tid];
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
if (coordination->protocol != MC_PROTOCOL_VCM_RR) {
|
|
commit();
|
|
}
|
|
|
|
cleanup:
|
|
ASN_STRUCT_FREE(asn_DEF_VCM, vcm_rep);
|
|
return rv;
|
|
}
|
|
|
|
static int vcm_check_handle_reply(VCM_t* vcm, mc_neighbour_s* neighbour) {
|
|
int rv = 0;
|
|
|
|
coordination_t* coordination = &facilities.coordination;
|
|
|
|
CoordinationReply_t* reply = &vcm->vcm.maneuverContainer.choice.vehicle.negotiation->choice.reply;
|
|
|
|
if (reply->nonce != coordination->session.nonce) {
|
|
return 1;
|
|
}
|
|
|
|
itss_time_lock();
|
|
uint64_t now_us = itss_ts_get(TIME_MICROSECONDS);
|
|
itss_time_unlock();
|
|
|
|
if (neighbour->intersecting) { /* REQUESTER */
|
|
|
|
if (!coordination->session.own_req) {
|
|
log_debug("[vc] unknown reply context - session expired or another ITS-S rejected");
|
|
return 1;
|
|
}
|
|
|
|
if (reply->acceptedTrajectoriesIds.list.count == 0) {
|
|
session_cleanup();
|
|
return 1;
|
|
}
|
|
|
|
for (int t = 0; t < reply->acceptedTrajectoriesIds.list.count; ++t) {
|
|
if (t > MC_TRAJECTORIES_N_MAX - 1) {
|
|
return 1;
|
|
}
|
|
for (int n = 0; n < coordination->session.n_affs_neighs[t]; ++n) {
|
|
if (neighbour->station_id == coordination->session.affs[t][n]->station_id) {
|
|
for (int i = n; i < coordination->session.n_affs_neighs[t] - 1; ++i) {
|
|
coordination->session.affs[t][i] = coordination->session.affs[t][i+1];
|
|
}
|
|
--coordination->session.n_affs_neighs[t];
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
neighbour->intersecting = false;
|
|
neighbour->proposed = false;
|
|
|
|
if (!is_maneuver_approved()) {
|
|
return 1;
|
|
}
|
|
|
|
log_info("[vc] received VCM.reply from %d (of %d) with %d accepted trajectories - session: %d | took %ld us",
|
|
vcm->header.stationID,
|
|
coordination->session.n_affs_neighs_reqd,
|
|
reply->acceptedTrajectoriesIds.list.count,
|
|
coordination->session.nonce,
|
|
now_us-neighbour->t_iid);
|
|
|
|
if (coordination->protocol == MC_PROTOCOL_VCM_RR) {
|
|
session_cleanup();
|
|
}
|
|
|
|
} else { /* REPLIER */
|
|
log_debug("[vc] received VCM.reply is response to another ITS-S VCM.request - session: %d", coordination->session.nonce);
|
|
|
|
if (reply->acceptedTrajectoriesIds.list.count == 0) {
|
|
session_cleanup();
|
|
return 1;
|
|
}
|
|
|
|
for (int t = 0; t < reply->acceptedTrajectoriesIds.list.count; ++t) {
|
|
int tid = *reply->acceptedTrajectoriesIds.list.array[t];
|
|
if (tid > MC_TRAJECTORIES_N_MAX - 1) {
|
|
return 1;
|
|
}
|
|
for (int n = 0; n < coordination->session.n_affs_neighs[tid]; ++n) {
|
|
if (neighbour->station_id == coordination->session.affs[tid][n]->station_id) {
|
|
for (int i = n; i < coordination->session.n_affs_neighs[tid] - 1; ++i) {
|
|
coordination->session.affs[tid][i] = coordination->session.affs[tid][i+1];
|
|
}
|
|
--coordination->session.n_affs_neighs[tid];
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
if (!is_maneuver_approved()) {
|
|
return 1;
|
|
}
|
|
|
|
if (coordination->protocol == MC_PROTOCOL_VCM_RR) {
|
|
session_cleanup();
|
|
}
|
|
}
|
|
|
|
if (coordination->protocol != MC_PROTOCOL_VCM_RR) {
|
|
commit();
|
|
}
|
|
|
|
return rv;
|
|
}
|
|
|
|
static int intersection_detected(VCM_t* vcm, mc_neighbour_s* neighbour) {
|
|
|
|
int rv = 0;
|
|
|
|
coordination_t* coordination = &facilities.coordination;
|
|
|
|
VCM_t* vcm_req = NULL;
|
|
TransportRequest_t* tr = NULL;
|
|
FacilitiesIndication_t* fi = NULL;
|
|
|
|
const ssize_t buf_len = 1024;
|
|
uint8_t buf[buf_len];
|
|
|
|
itss_st_t trajectoryA[TRAJECTORY_MAX_LEN+1]; /* ego trajectory */
|
|
uint16_t trajectoryA_len = 0;
|
|
|
|
uint64_t now = itss_time_get();
|
|
|
|
|
|
int32_t lat, lon;
|
|
itss_space_lock();
|
|
itss_space_get();
|
|
lat = epv.space.latitude;
|
|
lon = epv.space.longitude;
|
|
itss_space_unlock();
|
|
|
|
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;
|
|
|
|
// Initiate conflict resolution
|
|
vcm_req = calloc(1, sizeof(VCM_t));
|
|
|
|
vcm_req->header.messageID = 43;
|
|
vcm_req->header.protocolVersion = 1;
|
|
pthread_mutex_lock(&facilities.id.lock);
|
|
vcm_req->header.stationID = facilities.id.station_id;
|
|
pthread_mutex_unlock(&facilities.id.lock);
|
|
|
|
vcm_req->vcm.currentPosition.latitude = lat;
|
|
vcm_req->vcm.currentPosition.longitude = lon;
|
|
asn_ulong2INTEGER(&vcm_req->vcm.currentPosition.timestamp, now);
|
|
|
|
/*
|
|
if (coordination->chain.enabled && coordination->chain.id) {
|
|
vcm_req->vcm.chain = calloc(1, sizeof(ChainInformation_t));
|
|
vcm_req->vcm.chain->id = coordination->chain.id;
|
|
vcm_req->vcm.chain->link.buf = malloc(32);
|
|
vcm_req->vcm.chain->link.size = 32;
|
|
memcpy(vcm_req->vcm.chain->link.buf, coordination->chain.links[0], 32);
|
|
vcm_req->vcm.chain->area = calloc(1, sizeof(ChainInformationArea_t));
|
|
vcm_req->vcm.chain->area->trees.list.count = 1;
|
|
vcm_req->vcm.chain->area->trees.list.size = 1 * sizeof(OCTET_STRING_t*);
|
|
vcm_req->vcm.chain->area->trees.list.array = malloc(1 * sizeof(OCTET_STRING_t*));
|
|
for (int q = 0; q < 1; ++q) {
|
|
vcm_req->vcm.chain->area->trees.list.array[q] = calloc(1, sizeof(OCTET_STRING_t));
|
|
vcm_req->vcm.chain->area->trees.list.array[q]->buf = calloc(1,4);
|
|
vcm_req->vcm.chain->area->trees.list.array[q]->size = 4;
|
|
}
|
|
}
|
|
*/
|
|
|
|
vcm_req->vcm.maneuverContainer.present = ManeuverContainer_PR_vehicle;
|
|
ManeuverVehicleContainer_t* mvc = &vcm_req->vcm.maneuverContainer.choice.vehicle;
|
|
|
|
// Vehicle Dimensions
|
|
mvc->vehicleLength.vehicleLengthValue= facilities.vehicle.length;
|
|
mvc->vehicleLength.vehicleLengthConfidenceIndication = 0;
|
|
mvc->vehicleWidth = facilities.vehicle.width;
|
|
|
|
// Planned trajectory
|
|
mvc->plannedTrajectory.list.count = trajectoryA_len - 1;
|
|
mvc->plannedTrajectory.list.size = (trajectoryA_len - 1) * sizeof(void*);
|
|
mvc->plannedTrajectory.list.array = malloc((trajectoryA_len - 1) * sizeof(void*));
|
|
for (int i = 0; i < trajectoryA_len - 1; ++i) {
|
|
mvc->plannedTrajectory.list.array[i] = calloc(1, sizeof(STPoint_t));
|
|
mvc->plannedTrajectory.list.array[i]->deltaLatitude = trajectoryA[i+1].latitude - trajectoryA[i].latitude;
|
|
mvc->plannedTrajectory.list.array[i]->deltaLongitude = trajectoryA[i+1].longitude - trajectoryA[i].longitude;
|
|
mvc->plannedTrajectory.list.array[i]->deltaTime = trajectoryA[i+1].timestamp - trajectoryA[i].timestamp;
|
|
}
|
|
|
|
// Desired trajectory
|
|
mvc->negotiation = calloc(1, sizeof(CoordinationNegotiation_t));
|
|
mvc->negotiation->present = CoordinationNegotiation_PR_request;
|
|
mvc->negotiation->choice.request.desiredTrajectories.list.count = 1;
|
|
mvc->negotiation->choice.request.desiredTrajectories.list.size = sizeof(void*);
|
|
mvc->negotiation->choice.request.desiredTrajectories.list.array = malloc(1*sizeof(void*));
|
|
mvc->negotiation->choice.request.desiredTrajectories.list.array[0] = calloc(1,sizeof(ProposedTrajectory_t));
|
|
ProposedTrajectory_t* pt = mvc->negotiation->choice.request.desiredTrajectories.list.array[0];
|
|
mvc->negotiation->choice.request.requesterId = vcm->header.stationID;
|
|
mvc->negotiation->choice.request.nonce = rand();
|
|
coordination->session.nonce = mvc->negotiation->choice.request.nonce;
|
|
|
|
pt->trajectory.list.count = trajectoryA_len - 1;
|
|
pt->trajectory.list.size = sizeof(void*) * (trajectoryA_len - 1);
|
|
pt->trajectory.list.array = malloc(sizeof(void*) * (trajectoryA_len - 1));
|
|
for (int i = 0; i < trajectoryA_len - 1; ++i) {
|
|
pt->trajectory.list.array[i] = calloc(1, sizeof(STPoint_t));
|
|
pt->trajectory.list.array[i]->deltaLatitude = trajectoryA[i+1].latitude - trajectoryA[i].latitude;
|
|
pt->trajectory.list.array[i]->deltaLongitude = trajectoryA[i+1].longitude - trajectoryA[i].longitude;
|
|
pt->trajectory.list.array[i]->deltaTime = trajectoryA[i+1].timestamp - trajectoryA[i].timestamp;
|
|
}
|
|
|
|
pt->offer = 5;
|
|
pt->priority = 1;
|
|
pt->id = 0;
|
|
|
|
itss_time_lock();
|
|
uint64_t now_us = itss_ts_get(TIME_MICROSECONDS) ;
|
|
itss_time_unlock();
|
|
|
|
neighbour->intersecting = true;
|
|
neighbour->t_iid = now_us;
|
|
|
|
mc_neighbour_s* intneigh[MC_AFF_STATIONS_N_MAX];
|
|
intneigh[0] = neighbour;
|
|
int n_intneigh = 1;
|
|
for (int n = 0; n < coordination->neighbours_len && n_intneigh < MC_AFF_STATIONS_N_MAX; ++n) {
|
|
if (coordination->neighbours[n].station_id == neighbour->station_id) {
|
|
continue;
|
|
}
|
|
|
|
int index;
|
|
if (do_paths_intersect(
|
|
trajectoryA, trajectoryA_len,
|
|
facilities.vehicle.length, facilities.vehicle.length,
|
|
coordination->neighbours[n].trajectory, coordination->neighbours[n].trajectory_len,
|
|
coordination->neighbours[n].length, coordination->neighbours[n].width,
|
|
&index
|
|
)) {
|
|
coordination->neighbours[n].intersecting = true;
|
|
coordination->neighbours[n].t_iid = now_us;
|
|
intneigh[n_intneigh] = &coordination->neighbours[n];
|
|
++n_intneigh;
|
|
}
|
|
}
|
|
|
|
#ifndef NDEBUG
|
|
char buffer[512];
|
|
int wr = 0;
|
|
#endif
|
|
pt->affectingStations.list.count = n_intneigh;
|
|
pt->affectingStations.list.size = n_intneigh*sizeof(void*);
|
|
pt->affectingStations.list.array = malloc(n_intneigh * sizeof(void*));
|
|
for (int i = 0; i < n_intneigh; ++i) {
|
|
pt->affectingStations.list.array[i] = malloc(sizeof(long long));
|
|
*pt->affectingStations.list.array[i] = intneigh[i]->station_id;
|
|
#ifndef NDEBUG
|
|
wr += sprintf(buffer + wr, " %d", intneigh[i]->station_id);
|
|
#endif
|
|
}
|
|
log_debug("[vc] this VCM.request affects %d station(s):%s - session: %d", n_intneigh, buffer, coordination->session.nonce);
|
|
|
|
coordination->session.own_req = vcm_req;
|
|
coordination->session.ts = now;
|
|
coordination->session.n_affs_trjs = 1;
|
|
coordination->session.n_affs_neighs[0] = n_intneigh;
|
|
coordination->session.n_affs_neighs_reqd = n_intneigh;
|
|
memcpy(coordination->session.affs[0], intneigh, sizeof(intneigh));
|
|
|
|
tx_vcm(vcm_req);
|
|
|
|
return rv;
|
|
}
|
|
|
|
static void intersection_check(VCM_t* vcm, mc_neighbour_s* neighbour) {
|
|
|
|
ManeuverVehicleContainer_t* mvc = &vcm->vcm.maneuverContainer.choice.vehicle;
|
|
|
|
itss_st_t trajectoryA[TRAJECTORY_MAX_LEN+1]; /* ego trajectory */
|
|
itss_st_t trajectoryB[TRAJECTORY_MAX_LEN+1]; /* neighbour trajectory */
|
|
uint16_t trajectoryA_len = 0;
|
|
uint16_t trajectoryB_len = 0;
|
|
|
|
int32_t lat, lon;
|
|
itss_space_lock();
|
|
itss_space_get();
|
|
lat = epv.space.latitude;
|
|
lon = epv.space.longitude;
|
|
itss_space_unlock();
|
|
|
|
uint64_t now = itss_time_get();
|
|
|
|
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;
|
|
|
|
trajectoryB[0].latitude = vcm->vcm.currentPosition.latitude;
|
|
trajectoryB[0].longitude = vcm->vcm.currentPosition.longitude;
|
|
asn_INTEGER2ulong(&vcm->vcm.currentPosition.timestamp, (unsigned long long*) &trajectoryB[0].timestamp);
|
|
++trajectoryB_len;
|
|
for (int i = 1; i < mvc->plannedTrajectory.list.count && i < TRAJECTORY_MAX_LEN; ++i) {
|
|
trajectoryB[i].latitude = mvc->plannedTrajectory.list.array[i]->deltaLatitude + trajectoryB[i-1].latitude;
|
|
trajectoryB[i].longitude = mvc->plannedTrajectory.list.array[i]->deltaLongitude + trajectoryB[i-1].longitude;
|
|
trajectoryB[i].timestamp = mvc->plannedTrajectory.list.array[i]->deltaTime + trajectoryB[i-1].timestamp;
|
|
++trajectoryB_len;
|
|
}
|
|
|
|
// Save neighbour trajectory
|
|
memcpy(neighbour->trajectory, trajectoryB, trajectoryB_len * sizeof(itss_st_t));
|
|
neighbour->trajectory_len = trajectoryB_len;
|
|
neighbour->length = mvc->vehicleLength.vehicleLengthValue;
|
|
neighbour->width = mvc->vehicleWidth;
|
|
|
|
int index = -1;
|
|
|
|
if (trajectoryA_len > 1 && trajectoryB_len > 1) {
|
|
bool intersecting = do_paths_intersect(
|
|
trajectoryA, trajectoryA_len, facilities.vehicle.length, facilities.vehicle.width,
|
|
trajectoryB, trajectoryB_len, mvc->vehicleLength.vehicleLengthValue, mvc->vehicleWidth,
|
|
&index);
|
|
if (intersecting) {
|
|
log_info("[vc] intersection danger with %lld @ (%f, %f) in %ld ms",
|
|
vcm->header.stationID,
|
|
trajectoryA[index].latitude/1.0e7,
|
|
trajectoryA[index].longitude/1.0e7,
|
|
trajectoryA[index].timestamp - now);
|
|
|
|
intersection_detected(vcm, neighbour);
|
|
}
|
|
}
|
|
}
|
|
|
|
static int vcm_check_handle_commit(VCM_t* vcm, mc_neighbour_s* neighbour) {
|
|
coordination_t* coordination = &facilities.coordination;
|
|
|
|
if (vcm->vcm.maneuverContainer.choice.vehicle.negotiation->choice.commit.nonce != coordination->session.nonce) {
|
|
return 1;
|
|
}
|
|
|
|
session_cleanup();
|
|
|
|
return 0;
|
|
}
|
|
|
|
static bool in_maneuver() {
|
|
coordination_t* coordination = &facilities.coordination;
|
|
uint64_t now = itss_time_get();
|
|
|
|
if (now > coordination->session.ts + MC_RESOLUTION_TIMEOUT ||
|
|
now > coordination->session.t_init + MC_RESOLUTION_TIMEOUT) {
|
|
session_cleanup();
|
|
return false;
|
|
}
|
|
if (!coordination->session.own_req && !coordination->session.requester) {
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
int vcm_check(VCM_t* vcm) {
|
|
coordination_t* coordination = &facilities.coordination;
|
|
|
|
int rv = 0;
|
|
|
|
uint64_t now = itss_time_get();
|
|
|
|
pthread_mutex_lock(&coordination->lock);
|
|
|
|
mc_neighbour_s* neighbour = get_neighbour(vcm->header.stationID);
|
|
|
|
switch (vcm->vcm.maneuverContainer.present) {
|
|
case ManeuverContainer_PR_vehicle:
|
|
;
|
|
ManeuverVehicleContainer_t* mvc = &vcm->vcm.maneuverContainer.choice.vehicle;
|
|
if (mvc->negotiation) {
|
|
switch (mvc->negotiation->present) {
|
|
case CoordinationNegotiation_PR_request:
|
|
vcm_check_handle_request(vcm, neighbour);
|
|
break;
|
|
|
|
case CoordinationNegotiation_PR_reply:
|
|
vcm_check_handle_reply(vcm, neighbour);
|
|
break;
|
|
|
|
case CoordinationNegotiation_PR_commit:
|
|
vcm_check_handle_commit(vcm, neighbour);
|
|
break;
|
|
|
|
default:
|
|
break;
|
|
}
|
|
} else {
|
|
if (in_maneuver()) {
|
|
log_debug("[vc] ignoring VCM from %d - currently in maneuver", vcm->header.stationID);
|
|
} else {
|
|
if (coordination->session.own_req) { /* clear previous request if exists */
|
|
ASN_STRUCT_FREE(asn_DEF_VCM, coordination->session.own_req);
|
|
coordination->session.own_req = NULL;
|
|
coordination->session.ts = now;
|
|
memset(coordination->session.affs, 0, sizeof(coordination->session.affs));
|
|
memset(coordination->session.n_affs_neighs, 0, sizeof(coordination->session.n_affs_neighs));
|
|
coordination->session.n_affs_neighs_reqd = 0;
|
|
coordination->session.n_affs_trjs = 0;
|
|
}
|
|
|
|
/* check if received VCM intersects with ego trajectory */
|
|
intersection_check(vcm, neighbour);
|
|
}
|
|
}
|
|
break;
|
|
case ManeuverContainer_PR_rsu:
|
|
break;
|
|
default:
|
|
log_debug("[vc] received VCM contains unrecognized ManeuverContainer type");
|
|
rv = 1;
|
|
}
|
|
|
|
cleanup:
|
|
pthread_mutex_unlock(&coordination->lock);
|
|
|
|
return rv;
|
|
}
|
|
|
|
static int mk_vcm() {
|
|
int rv = 0;
|
|
|
|
coordination_t* coordination = &facilities.coordination;
|
|
|
|
itss_st_t trajectory[TRAJECTORY_MAX_LEN];
|
|
|
|
VCM_t* vcm = calloc(1, sizeof(VCM_t));
|
|
|
|
vcm->header.messageID = 43;
|
|
vcm->header.protocolVersion = 1;
|
|
uint64_t now = itss_time_get();
|
|
pthread_mutex_lock(&facilities.id.lock);
|
|
vcm->header.stationID = facilities.id.station_id;
|
|
pthread_mutex_unlock(&facilities.id.lock);
|
|
|
|
int32_t lat, lon;
|
|
uint16_t trajectory_len = 0;
|
|
itss_space_lock();
|
|
itss_space_get();
|
|
lat = epv.space.latitude;
|
|
lon = epv.space.longitude;
|
|
itss_space_unlock();
|
|
|
|
itss_trajectory_lock();
|
|
trajectory_len = epv.trajectory.len;
|
|
memcpy(trajectory, epv.trajectory.path, trajectory_len * sizeof(itss_st_t));
|
|
itss_trajectory_unlock();
|
|
|
|
vcm->vcm.currentPosition.latitude = lat;
|
|
vcm->vcm.currentPosition.longitude = lon;
|
|
asn_ulong2INTEGER(&vcm->vcm.currentPosition.timestamp, now);
|
|
|
|
/*
|
|
if (coordination->chain.enabled && coordination->chain.id) {
|
|
vcm->vcm.chain = calloc(1, sizeof(ChainInformation_t));
|
|
vcm->vcm.chain->id = coordination->chain.id;
|
|
vcm->vcm.chain->link.buf = malloc(32);
|
|
vcm->vcm.chain->link.size = 32;
|
|
memcpy(vcm->vcm.chain->link.buf, coordination->chain.links[0], 32);
|
|
vcm->vcm.chain->area = calloc(1, sizeof(ChainInformationArea_t));
|
|
vcm->vcm.chain->area->trees.list.count = 1;
|
|
vcm->vcm.chain->area->trees.list.size = 1 * sizeof(OCTET_STRING_t*);
|
|
vcm->vcm.chain->area->trees.list.array = malloc(1 * sizeof(OCTET_STRING_t*));
|
|
for (int q = 0; q < 1; ++q) {
|
|
vcm->vcm.chain->area->trees.list.array[q] = calloc(1, sizeof(OCTET_STRING_t));
|
|
vcm->vcm.chain->area->trees.list.array[q]->buf = calloc(1,4);
|
|
vcm->vcm.chain->area->trees.list.array[q]->size = 4;
|
|
}
|
|
}
|
|
*/
|
|
|
|
if (facilities.station_type == StationType_roadSideUnit) {
|
|
vcm->vcm.maneuverContainer.present = ManeuverContainer_PR_rsu;
|
|
ManeuverRSUContainer_t* mrc = &vcm->vcm.maneuverContainer.choice.rsu;
|
|
mrc->recommendedTrajectories = NULL; // TODO
|
|
} else {
|
|
vcm->vcm.maneuverContainer.present = ManeuverContainer_PR_vehicle;
|
|
ManeuverVehicleContainer_t* mvc = &vcm->vcm.maneuverContainer.choice.vehicle;
|
|
// Vehicle Dimensions
|
|
mvc->vehicleLength.vehicleLengthValue= facilities.vehicle.length;
|
|
mvc->vehicleLength.vehicleLengthConfidenceIndication = 0;
|
|
mvc->vehicleWidth = facilities.vehicle.width;
|
|
|
|
mvc->plannedTrajectory.list.count = trajectory_len;
|
|
mvc->plannedTrajectory.list.size = trajectory_len * sizeof(void*);
|
|
mvc->plannedTrajectory.list.array = malloc(trajectory_len * sizeof(void*));
|
|
mvc->plannedTrajectory.list.array[0] = calloc(1, sizeof(STPoint_t));
|
|
mvc->plannedTrajectory.list.array[0]->deltaLatitude = trajectory[0].latitude - lat;
|
|
mvc->plannedTrajectory.list.array[0]->deltaLongitude = trajectory[0].longitude - lon;
|
|
mvc->plannedTrajectory.list.array[0]->deltaTime = trajectory[0].timestamp - now;
|
|
for (int i = 1; i < trajectory_len; ++i) {
|
|
mvc->plannedTrajectory.list.array[i] = calloc(1, sizeof(STPoint_t));
|
|
mvc->plannedTrajectory.list.array[i]->deltaLatitude = trajectory[i].latitude - trajectory[i-1].latitude;
|
|
mvc->plannedTrajectory.list.array[i]->deltaLongitude = trajectory[i].longitude - trajectory[i-1].longitude;
|
|
mvc->plannedTrajectory.list.array[i]->deltaTime = trajectory[i].timestamp - trajectory[i-1].timestamp;
|
|
}
|
|
}
|
|
|
|
tx_vcm(vcm);
|
|
|
|
cleanup:
|
|
ASN_STRUCT_FREE(asn_DEF_VCM, vcm);
|
|
|
|
return rv;
|
|
}
|
|
|
|
static bool vcm_timer_check(coordination_t* coordination, uint64_t now) {
|
|
|
|
bool send = false;
|
|
|
|
if (now > coordination->t_last_send_vcm + coordination->vcm_period_max) {
|
|
send = true;
|
|
}
|
|
|
|
return send;
|
|
}
|
|
|
|
void* vc_service() {
|
|
|
|
coordination_t* coordination = (coordination_t*) &facilities.coordination;
|
|
|
|
int rv;
|
|
|
|
while (!facilities.exit) {
|
|
|
|
uint64_t now = itss_time_get();
|
|
|
|
pthread_mutex_lock(&coordination->lock);
|
|
if (vcm_timer_check(coordination, now)) {
|
|
rv = mk_vcm();
|
|
if (rv) {
|
|
pthread_mutex_unlock(&coordination->lock);
|
|
continue;
|
|
}
|
|
}
|
|
pthread_mutex_unlock(&coordination->lock);
|
|
|
|
usleep(50 * 1000);
|
|
}
|
|
|
|
itss_0close(coordination->mgmt_socket);
|
|
|
|
return NULL;
|
|
}
|
|
|
|
void coordination_init() {
|
|
coordination_t* coo = &facilities.coordination;
|
|
pthread_mutex_init(&coo->lock, NULL);
|
|
coo->mgmt_socket = itss_0connect(facilities.zmq.management_address, ZMQ_REQ);
|
|
}
|