From eea1f89a55d6755d9053df24a5f90bc5891e6c58 Mon Sep 17 00:00:00 2001 From: emanuel Date: Mon, 14 Feb 2022 18:35:44 +0000 Subject: [PATCH] DCM comm efforts --- src/dcm.c | 247 ++++++++++++++++++++++++++++++++++++++++++++++-------- 1 file changed, 213 insertions(+), 34 deletions(-) diff --git a/src/dcm.c b/src/dcm.c index 320dd74..a0c9721 100644 --- a/src/dcm.c +++ b/src/dcm.c @@ -49,21 +49,19 @@ static int are_vehicles_intersecting( return 0; } -int dcm_check(void* fc, DCM_t* dcm) { - facilities_t* facilities = (facilities_t*) fc; - +static int dcm_check_handle_request(facilities_t* facilities, DCM_t* dcm) { int rv = 0; - DCM_t* dcm_req = NULL; + CoordinationRequest_t* coo_req = &dcm->dcm.negotiation->choice.request; + + DCM_t* dcm_rep = NULL; TransportRequest_t* tr = NULL; const ssize_t buf_len = 512; uint8_t buf[buf_len]; it2s_tender_st_s trajectoryA[TRAJECTORY_MAX_LEN+1]; /* ego trajectory */ - it2s_tender_st_s trajectoryB[TRAJECTORY_MAX_LEN+1]; /* neighbour trajectory */ - uint16_t trajectoryA_len = 0; - uint16_t trajectoryB_len = 0; + ssize_t trajectoryA_len = 0; uint64_t now = it2s_tender_get_clock(&facilities->epv); @@ -84,35 +82,143 @@ int dcm_check(void* fc, DCM_t* dcm) { trajectoryA[0].timestamp = now; ++trajectoryA_len; - trajectoryB[0].latitude = dcm->dcm.currentPosition.latitude; - trajectoryB[0].longitude = dcm->dcm.currentPosition.longitude; - asn_INTEGER2ulong(&dcm->dcm.currentPosition.timestamp, &trajectoryB[0].timestamp); - ++trajectoryB_len; - for (int i = 0; i < dcm->dcm.plannedTrajectory.list.count && i < TRAJECTORY_MAX_LEN; ++i) { - trajectoryB[i+1].latitude = dcm->dcm.plannedTrajectory.list.array[i]->latitude; - trajectoryB[i+1].longitude = dcm->dcm.plannedTrajectory.list.array[i]->longitude; - asn_INTEGER2ulong(&dcm->dcm.plannedTrajectory.list.array[i]->timestamp, &trajectoryB[i+1].timestamp); - ++trajectoryB_len; + dcm_rep = calloc(1, sizeof(DCM_t)); + + dcm_rep->header.messageID = 43; + dcm_rep->header.protocolVersion = 1; + dcm_rep->header.stationID = 1; + pthread_mutex_lock(&facilities->id.lock); + dcm_rep->header.stationID = facilities->id.station_id; + pthread_mutex_unlock(&facilities->id.lock); + + dcm_rep->dcm.currentPosition.latitude = lat; + dcm_rep->dcm.currentPosition.longitude = lon; + asn_ulong2INTEGER(&dcm_rep->dcm.currentPosition.timestamp, now); + + // Vehicle Dimensions + dcm_rep->dcm.vehicleDimensions.length= facilities->vehicle.length; + dcm_rep->dcm.vehicleDimensions.width = facilities->vehicle.width; + + // Accepted trajectory + if (!coo_req->desiredTrajectories.list.count) { + syslog_debug("[facilities] [dc] received DCM request has no desired trajectories"); + rv = 1; + goto cleanup; + } + ProposedTrajectory_t* pt = coo_req->desiredTrajectories.list.array[0]; + + dcm_rep->dcm.negotiation = calloc(1, sizeof(CoordinationNegotiation_t)); + dcm_rep->dcm.negotiation->present = CoordinationNegotiation_PR_reply; + dcm_rep->dcm.negotiation->choice.reply.acceptedTrajectories.list.count = 1; + dcm_rep->dcm.negotiation->choice.reply.acceptedTrajectories.list.size = sizeof(void*); + dcm_rep->dcm.negotiation->choice.reply.acceptedTrajectories.list.array = malloc(1*sizeof(void*)); + dcm_rep->dcm.negotiation->choice.reply.acceptedTrajectories.list.array[0] = calloc(1,sizeof(AcceptedTrajectory_t)); + AcceptedTrajectory_t* at = dcm_rep->dcm.negotiation->choice.reply.acceptedTrajectories.list.array[0]; + + at->trajectory.list.count = pt->trajectory.list.count; + at->trajectory.list.size = sizeof(void*) * pt->trajectory.list.count; + at->trajectory.list.array = malloc(sizeof(void*) * pt->trajectory.list.count); + for (int i = 0; i < pt->trajectory.list.count; ++i) { + at->trajectory.list.array[i] = calloc(1, sizeof(STPoint_t)); + at->trajectory.list.array[i]->latitude = pt->trajectory.list.array[i]->latitude; + at->trajectory.list.array[i]->longitude = pt->trajectory.list.array[i]->longitude; + at->trajectory.list.array[i]->timestamp.buf = malloc(pt->trajectory.list.array[i]->timestamp.size); + memcpy(at->trajectory.list.array[i]->timestamp.buf, pt->trajectory.list.array[i]->timestamp.buf, pt->trajectory.list.array[i]->timestamp.size); + at->trajectory.list.array[i]->timestamp.size = pt->trajectory.list.array[i]->timestamp.size; } - bool intersecting = false; - int index = 0; + at->id = pt->id; - if (trajectoryA_len > 1 && trajectoryB_len > 1) { - intersecting = are_vehicles_intersecting( - trajectoryA, trajectoryA_len, facilities->vehicle.length, facilities->vehicle.width, - trajectoryB, trajectoryB_len, dcm->dcm.vehicleDimensions.length, dcm->dcm.vehicleDimensions.width, - &index); + asn_enc_rval_t enc = asn_encode_to_buffer(NULL, ATS_UNALIGNED_CANONICAL_PER, &asn_DEF_DCM, dcm_rep, buf, buf_len); + if (enc.encoded == -1) { + syslog_err("[facilities] [dc] DCM request encode failure"); + rv = 1; + goto cleanup; } - if (!intersecting) goto cleanup; + 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; - syslog_info("[facilities] [dc] intersection danger with %ld @ (%d, %d) in %ld ms", - dcm->header.stationID, trajectoryA[index].latitude, trajectoryA[index].longitude, trajectoryA[index].timestamp - now); + bpr->btpType = BTPType_btpB; + + 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(512); + memcpy(bpr->data.buf, buf, buf_len); + bpr->data.size = (enc.encoded + 7) / 8; + + 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) { + syslog_err("[facilities] [dc] TR DCM request encode failure"); + rv = 1; + goto cleanup; + } + + queue_send(facilities->tx_queue, buf, enc.encoded+1, 3); + +cleanup: + ASN_STRUCT_FREE(asn_DEF_DCM, dcm_rep); + ASN_STRUCT_FREE(asn_DEF_TransportRequest, tr); + return rv; +} + +static int dcm_check_handle_reply(facilities_t* facilities, DCM_t* dcm) { + int rv = 0; + + CoordinationReply_t* coo_rep = &dcm->dcm.negotiation->choice.reply; + + syslog_info("[facilities] [dc] received coordination reply with %d accepted trajectories", coo_rep->acceptedTrajectories.list.count); + + return rv; +} + +static int dcm_check_intersection_detected(facilities_t* facilities, DCM_t* dcm) { + + int rv = 0; + + DCM_t* dcm_req = NULL; + TransportRequest_t* tr = NULL; + + const ssize_t buf_len = 512; + uint8_t buf[buf_len]; + + it2s_tender_st_s trajectoryA[TRAJECTORY_MAX_LEN+1]; /* ego trajectory */ + uint16_t trajectoryA_len = 0; + + int32_t lat, lon; + it2s_tender_lock_space(&facilities->epv); + it2s_tender_get_space(&facilities->epv); + lat = facilities->epv.space.latitude; + 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); + + uint64_t now = it2s_tender_get_clock(&facilities->epv); + + trajectoryA[0].latitude = lat; + trajectoryA[0].longitude = lon; + trajectoryA[0].timestamp = now; + ++trajectoryA_len; // Initiate conflict resolution dcm_req = calloc(1, sizeof(DCM_t)); - it2s_tender_st_s trajectory[TRAJECTORY_MAX_LEN]; dcm_req->header.messageID = 43; dcm_req->header.protocolVersion = 1; @@ -135,9 +241,9 @@ int dcm_check(void* fc, DCM_t* dcm) { dcm_req->dcm.plannedTrajectory.list.array = malloc(trajectoryA_len * sizeof(void*)); for (int i = 0; i < trajectoryA_len; ++i) { dcm_req->dcm.plannedTrajectory.list.array[i] = calloc(1, sizeof(STPoint_t)); - dcm_req->dcm.plannedTrajectory.list.array[i]->latitude = trajectory[i+1].latitude; - dcm_req->dcm.plannedTrajectory.list.array[i]->longitude = trajectory[i+1].longitude; - asn_ulong2INTEGER(&dcm_req->dcm.plannedTrajectory.list.array[i]->timestamp, trajectory[i+1].timestamp); + dcm_req->dcm.plannedTrajectory.list.array[i]->latitude = trajectoryA[i+1].latitude; + dcm_req->dcm.plannedTrajectory.list.array[i]->longitude = trajectoryA[i+1].longitude; + asn_ulong2INTEGER(&dcm_req->dcm.plannedTrajectory.list.array[i]->timestamp, trajectoryA[i+1].timestamp); } // Desired trajectory @@ -154,9 +260,9 @@ int dcm_check(void* fc, DCM_t* dcm) { pt->trajectory.list.array = malloc(sizeof(void*) * trajectoryA_len); for (int i = 0; i < trajectoryA_len; ++i) { pt->trajectory.list.array[i] = calloc(1, sizeof(STPoint_t)); - pt->trajectory.list.array[i]->latitude = trajectory[i+1].latitude; - pt->trajectory.list.array[i]->longitude = trajectory[i+1].longitude; - asn_ulong2INTEGER(&pt->trajectory.list.array[i]->timestamp, trajectory[i+1].timestamp); + pt->trajectory.list.array[i]->latitude = trajectoryA[i+1].latitude; + pt->trajectory.list.array[i]->longitude = trajectoryA[i+1].longitude; + asn_ulong2INTEGER(&pt->trajectory.list.array[i]->timestamp, trajectoryA[i+1].timestamp); } pt->offer = 5; @@ -209,6 +315,79 @@ cleanup: return rv; } +int dcm_check(void* fc, DCM_t* dcm) { + facilities_t* facilities = (facilities_t*) fc; + + int rv = 0; + + it2s_tender_st_s trajectoryA[TRAJECTORY_MAX_LEN+1]; /* ego trajectory */ + it2s_tender_st_s trajectoryB[TRAJECTORY_MAX_LEN+1]; /* neighbour trajectory */ + uint16_t trajectoryA_len = 0; + uint16_t trajectoryB_len = 0; + + uint64_t now = it2s_tender_get_clock(&facilities->epv); + + int32_t lat, lon; + it2s_tender_lock_space(&facilities->epv); + it2s_tender_get_space(&facilities->epv); + lat = facilities->epv.space.latitude; + 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; + + trajectoryB[0].latitude = dcm->dcm.currentPosition.latitude; + trajectoryB[0].longitude = dcm->dcm.currentPosition.longitude; + asn_INTEGER2ulong(&dcm->dcm.currentPosition.timestamp, &trajectoryB[0].timestamp); + ++trajectoryB_len; + for (int i = 0; i < dcm->dcm.plannedTrajectory.list.count && i < TRAJECTORY_MAX_LEN; ++i) { + trajectoryB[i+1].latitude = dcm->dcm.plannedTrajectory.list.array[i]->latitude; + trajectoryB[i+1].longitude = dcm->dcm.plannedTrajectory.list.array[i]->longitude; + asn_INTEGER2ulong(&dcm->dcm.plannedTrajectory.list.array[i]->timestamp, &trajectoryB[i+1].timestamp); + ++trajectoryB_len; + } + + bool intersecting = false; + int index = 0; + + if (dcm->dcm.negotiation) { + switch (dcm->dcm.negotiation->present) { + case CoordinationNegotiation_PR_request: + dcm_check_handle_request(facilities, dcm); + break; + case CoordinationNegotiation_PR_reply: + dcm_check_handle_reply(facilities, dcm); + break; + default: + break; + } + } else { + if (trajectoryA_len > 1 && trajectoryB_len > 1) { + intersecting = are_vehicles_intersecting( + trajectoryA, trajectoryA_len, facilities->vehicle.length, facilities->vehicle.width, + trajectoryB, trajectoryB_len, dcm->dcm.vehicleDimensions.length, dcm->dcm.vehicleDimensions.width, + &index); + } + + if (intersecting) { + syslog_info("[facilities] [dc] intersection danger with %ld @ (%d, %d) in %ld ms", + dcm->header.stationID, trajectoryA[index].latitude, trajectoryA[index].longitude, trajectoryA[index].timestamp - now); + dcm_check_intersection_detected(facilities, dcm); + } + } + + + return rv; +} + static int mk_dcm(facilities_t* facilities, uint8_t* dcm_uper, uint16_t* dcm_uper_len) { int rv = 0; @@ -309,7 +488,7 @@ void* dc_service(void* fc) { asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_TransportRequest, NULL, tr, tr_oer+1, 1023); if (enc.encoded == -1) { - syslog_err("[facilities] encoding TR for cam failed"); + syslog_err("[facilities] encoding TR for DCM failed"); continue; }