diff --git a/include/autoware_v2x/cpm_application.hpp b/include/autoware_v2x/cpm_application.hpp index 8879b3e..236576d 100644 --- a/include/autoware_v2x/cpm_application.hpp +++ b/include/autoware_v2x/cpm_application.hpp @@ -69,6 +69,8 @@ namespace v2x bool updating_objects_stack_; bool sending_; + bool is_sender_; + bool reflect_packet_; }; } diff --git a/src/cpm_application.cpp b/src/cpm_application.cpp index e0e4e04..099863c 100644 --- a/src/cpm_application.cpp +++ b/src/cpm_application.cpp @@ -41,43 +41,43 @@ namespace v2x ego_heading_(0), generationDeltaTime_(0), updating_objects_stack_(false), - sending_(false) + sending_(false), + is_sender_(true), + reflect_packet_(false) { RCLCPP_INFO(node_->get_logger(), "CpmApplication started..."); set_interval(milliseconds(100)); } - void CpmApplication::set_interval(Clock::duration interval) - { + void CpmApplication::set_interval(Clock::duration interval) { cpm_interval_ = interval; runtime_.cancel(this); schedule_timer(); } - void CpmApplication::schedule_timer() - { + void CpmApplication::schedule_timer() { runtime_.schedule(cpm_interval_, std::bind(&CpmApplication::on_timer, this, std::placeholders::_1), this); } - void CpmApplication::on_timer(Clock::time_point) - { + void CpmApplication::on_timer(Clock::time_point) { schedule_timer(); send(); } - CpmApplication::PortType CpmApplication::port() - { + CpmApplication::PortType CpmApplication::port() { return btp::ports::CPM; } void CpmApplication::indicate(const DataIndication &indication, UpPacketPtr packet) { + asn1::PacketVisitor visitor; std::shared_ptr cpm = boost::apply_visitor(visitor, *packet); + if (cpm) { - RCLCPP_INFO(node_->get_logger(), "[INDICATE] Received decodable CPM content"); + RCLCPP_INFO(node_->get_logger(), "[CpmApplication::indicate] Received decodable CPM content"); rclcpp::Time current_time = node_->now(); - RCLCPP_INFO(node_->get_logger(), "[CpmApplication::indicate] [measure] T_receive_r2 %ld", current_time.nanoseconds()); + RCLCPP_INFO(node_->get_logger(), "[CpmApplication::indicate] [measure] T_receive_r1 %ld", current_time.nanoseconds()); asn1::Cpm message = *cpm; ItsPduHeader_t &header = message->header; @@ -155,40 +155,52 @@ namespace v2x } else { RCLCPP_INFO(node_->get_logger(), "[INDICATE] Empty POC"); } + + if (reflect_packet_) { + Application::DownPacketPtr packet{new DownPacket()}; + std::unique_ptr payload{new geonet::DownPacket()}; + + payload->layer(OsiLayer::Application) = std::move(message); + + Application::DataRequest request; + request.its_aid = aid::CP; + request.transport_type = geonet::TransportType::SHB; + request.communication_profile = geonet::CommunicationProfile::ITS_G5; + + Application::DataConfirm confirm = Application::request(request, std::move(payload), node_); + + if (!confirm.accepted()) { + throw std::runtime_error("[CpmApplication::indicate] Packet reflection failed"); + } + } + + } else { RCLCPP_INFO(node_->get_logger(), "[INDICATE] Received broken content"); } } - void CpmApplication::updateMGRS(double *x, double *y) - { - // RCLCPP_INFO(node_->get_logger(), "Update MGRS"); + void CpmApplication::updateMGRS(double *x, double *y) { ego_x_ = *x; ego_y_ = *y; } - void CpmApplication::updateRP(double *lat, double *lon, double *altitude) - { - // RCLCPP_INFO(node_->get_logger(), "Update RP"); + void CpmApplication::updateRP(double *lat, double *lon, double *altitude) { ego_lat_ = *lat; ego_lon_ = *lon; ego_altitude_ = *altitude; } - void CpmApplication::updateGenerationDeltaTime(int *gdt, long long *gdt_timestamp) - { + void CpmApplication::updateGenerationDeltaTime(int *gdt, long long *gdt_timestamp) { generationDeltaTime_ = *gdt; gdt_timestamp_ = *gdt_timestamp; // ETSI-epoch milliseconds timestamp } - void CpmApplication::updateHeading(double *yaw) - { - // RCLCPP_INFO(node_->get_logger(), "Update Heading : %f", *yaw); + void CpmApplication::updateHeading(double *yaw) { ego_heading_ = *yaw; } - void CpmApplication::updateObjectsStack(const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr msg) - { + void CpmApplication::updateObjectsStack(const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr msg) { updating_objects_stack_ = true; if (sending_) { @@ -259,134 +271,107 @@ namespace v2x } void CpmApplication::send() { - sending_ = true; - - RCLCPP_INFO(node_->get_logger(), "[SEND] Sending CPM..."); - vanetza::asn1::Cpm message; + if (is_sender_) { + sending_ = true; + + RCLCPP_INFO(node_->get_logger(), "[CpmApplication::send] Sending CPM..."); - // ITS PDU Header - ItsPduHeader_t &header = message->header; - header.protocolVersion = 1; - header.messageID = 14; - header.stationID = 1; + vanetza::asn1::Cpm message; - // const auto time_now = duration_cast(system_clock::now().time_since_epoch()); - // uint16_t gen_delta_time = time_now.count(); + // ITS PDU Header + ItsPduHeader_t &header = message->header; + header.protocolVersion = 1; + header.messageID = 14; + header.stationID = 1; - CollectivePerceptionMessage_t &cpm = message->cpm; - cpm.generationDeltaTime = generationDeltaTime_ * GenerationDeltaTime_oneMilliSec; + CollectivePerceptionMessage_t &cpm = message->cpm; - // auto position = positioning->position_specify(pos_lat, pos_lon); + // Set GenerationDeltaTime + cpm.generationDeltaTime = generationDeltaTime_ * GenerationDeltaTime_oneMilliSec; - CpmManagementContainer_t &management = cpm.cpmParameters.managementContainer; - management.stationType = StationType_passengerCar; - // management.referencePosition.latitude = pos_lat; - // management.referencePosition.longitude = pos_lon; - // management.referencePosition.positionConfidenceEllipse.semiMajorConfidence = 1.0; - // management.referencePosition.positionConfidenceEllipse.semiMinorConfidence = 1.0; - PositionFix fix; - // fix.timestamp = time_now; - fix.latitude = ego_lat_ * units::degree; - fix.longitude = ego_lon_ * units::degree; - // fix.altitude = ego_altitude_; - fix.confidence.semi_major = 1.0 * units::si::meter; - fix.confidence.semi_minor = fix.confidence.semi_major; - copy(fix, management.referencePosition); + CpmManagementContainer_t &management = cpm.cpmParameters.managementContainer; + management.stationType = StationType_passengerCar; + PositionFix fix; + fix.latitude = ego_lat_ * units::degree; + fix.longitude = ego_lon_ * units::degree; + fix.confidence.semi_major = 1.0 * units::si::meter; + fix.confidence.semi_minor = fix.confidence.semi_major; + copy(fix, management.referencePosition); + cpm.cpmParameters.numberOfPerceivedObjects = objectsStack.size(); - // cpm.cpmParameters.stationDataContainer = NULL; - // cpm.cpmParameters.perceivedObjectContainer = NULL; - cpm.cpmParameters.numberOfPerceivedObjects = objectsStack.size(); + StationDataContainer_t *&sdc = cpm.cpmParameters.stationDataContainer; + sdc = vanetza::asn1::allocate(); + sdc->present = StationDataContainer_PR_originatingVehicleContainer; + OriginatingVehicleContainer_t &ovc = sdc->choice.originatingVehicleContainer; + ovc.speed.speedValue = 0; + ovc.speed.speedConfidence = 1; + int heading = std::lround(((-ego_heading_ * 180 / M_PI) + 90.0) * 10); + if (heading < 0) heading += 3600; + ovc.heading.headingValue = heading; + ovc.heading.headingConfidence = 1; - StationDataContainer_t *&sdc = cpm.cpmParameters.stationDataContainer; - sdc = vanetza::asn1::allocate(); - // RCLCPP_INFO(node->get_logger(), "Allocated sdc"); - sdc->present = StationDataContainer_PR_originatingVehicleContainer; - OriginatingVehicleContainer_t &ovc = sdc->choice.originatingVehicleContainer; - ovc.speed.speedValue = 0; - ovc.speed.speedConfidence = 1; - // ovc.heading.headingValue = (int) (1.5708 - ego_heading_) * M_PI / 180; - // RCLCPP_INFO(node_->get_logger(), "headingValue..."); - // ovc.heading.headingValue = (int)std::fmod((1.5708 - ego_heading_) * 180 / M_PI, 360.0) * 10; - int heading = std::lround(((-ego_heading_ * 180 / M_PI) + 90.0) * 10); - if (heading < 0) heading += 3600; - ovc.heading.headingValue = heading; - ovc.heading.headingConfidence = 1; - // RCLCPP_INFO(node_->get_logger(), "[SEND] headingValue: %f %d", ego_heading_, ovc.heading.headingValue); + if (objectsStack.size() > 0) { + PerceivedObjectContainer_t *&poc = cpm.cpmParameters.perceivedObjectContainer; + poc = vanetza::asn1::allocate(); - if (objectsStack.size() > 0) { - PerceivedObjectContainer_t *&poc = cpm.cpmParameters.perceivedObjectContainer; - poc = vanetza::asn1::allocate(); + for (CpmApplication::Object object : objectsStack) { + PerceivedObject *pObj = vanetza::asn1::allocate(); + pObj->objectID = object.objectID; + pObj->timeOfMeasurement = object.timeOfMeasurement; + pObj->xDistance.value = object.xDistance; + pObj->xDistance.confidence = 1; + pObj->yDistance.value = object.yDistance; + pObj->yDistance.confidence = 1; + pObj->xSpeed.value = object.xSpeed; + pObj->xSpeed.confidence = 1; + pObj->ySpeed.value = object.ySpeed; + pObj->ySpeed.confidence = 1; - for (CpmApplication::Object object : objectsStack) { - // if (object.xDistance > 10000) continue; - // if (object.yDistance > 10000) continue; + pObj->planarObjectDimension1 = vanetza::asn1::allocate(); + pObj->planarObjectDimension2 = vanetza::asn1::allocate(); + pObj->verticalObjectDimension = vanetza::asn1::allocate(); - PerceivedObject *pObj = vanetza::asn1::allocate(); - pObj->objectID = object.objectID; - pObj->timeOfMeasurement = object.timeOfMeasurement; - pObj->xDistance.value = object.xDistance; - pObj->xDistance.confidence = 1; - pObj->yDistance.value = object.yDistance; - pObj->yDistance.confidence = 1; - pObj->xSpeed.value = object.xSpeed; - pObj->xSpeed.confidence = 1; - pObj->ySpeed.value = object.ySpeed; - pObj->ySpeed.confidence = 1; + (*(pObj->planarObjectDimension1)).value = object.shape_y; + (*(pObj->planarObjectDimension1)).confidence = 1; + (*(pObj->planarObjectDimension2)).value = object.shape_x; + (*(pObj->planarObjectDimension2)).confidence = 1; + (*(pObj->verticalObjectDimension)).value = object.shape_z; + (*(pObj->verticalObjectDimension)).confidence = 1; - pObj->planarObjectDimension1 = vanetza::asn1::allocate(); - pObj->planarObjectDimension2 = vanetza::asn1::allocate(); - pObj->verticalObjectDimension = vanetza::asn1::allocate(); + pObj->yawAngle = vanetza::asn1::allocate(); + (*(pObj->yawAngle)).value = object.yawAngle; + (*(pObj->yawAngle)).confidence = 1; - (*(pObj->planarObjectDimension1)).value = object.shape_y; - (*(pObj->planarObjectDimension1)).confidence = 1; - (*(pObj->planarObjectDimension2)).value = object.shape_x; - (*(pObj->planarObjectDimension2)).confidence = 1; - (*(pObj->verticalObjectDimension)).value = object.shape_z; - (*(pObj->verticalObjectDimension)).confidence = 1; + RCLCPP_INFO(node_->get_logger(), "[SEND] Added: #%d (%d, %d) (%d, %d) (%d, %d, %d) %d", object.objectID, object.xDistance, object.yDistance, object.xSpeed, object.ySpeed, object.shape_y, object.shape_x, object.shape_z, object.yawAngle); - pObj->yawAngle = vanetza::asn1::allocate(); - (*(pObj->yawAngle)).value = object.yawAngle; - (*(pObj->yawAngle)).confidence = 1; - - RCLCPP_INFO(node_->get_logger(), "[SEND] Added: #%d (%d, %d) (%d, %d) (%d, %d, %d) %d", object.objectID, object.xDistance, object.yDistance, object.xSpeed, object.ySpeed, object.shape_y, object.shape_x, object.shape_z, object.yawAngle); - - ASN_SEQUENCE_ADD(poc, pObj); + ASN_SEQUENCE_ADD(poc, pObj); + } + } else { + cpm.cpmParameters.perceivedObjectContainer = NULL; + RCLCPP_INFO(node_->get_logger(), "[CpmApplication::send] Empty POC"); } - } else { - cpm.cpmParameters.perceivedObjectContainer = NULL; - RCLCPP_INFO(node_->get_logger(), "[SEND] Empty POC"); + + Application::DownPacketPtr packet{new DownPacket()}; + std::unique_ptr payload{new geonet::DownPacket()}; + + payload->layer(OsiLayer::Application) = std::move(message); + + Application::DataRequest request; + request.its_aid = aid::CP; + request.transport_type = geonet::TransportType::SHB; + request.communication_profile = geonet::CommunicationProfile::ITS_G5; + + Application::DataConfirm confirm = Application::request(request, std::move(payload), node_); + + if (!confirm.accepted()) { + throw std::runtime_error("[CpmApplication::send] CPM application data request failed"); + } + + sending_ = false; + rclcpp::Time current_time = node_->now(); + RCLCPP_INFO(node_->get_logger(), "[CpmApplication::send] [measure] T_depart_r1 %ld", current_time.nanoseconds()); + } - - Application::DownPacketPtr packet{new DownPacket()}; - std::unique_ptr payload{new geonet::DownPacket()}; - // std::shared_ptr message_p = std::make_shared(message); - // std::unique_ptr buffer { new convertible::byte_buffer_impl(&message)}; - - payload->layer(OsiLayer::Application) = std::move(message); - - Application::DataRequest request; - request.its_aid = aid::CP; - request.transport_type = geonet::TransportType::SHB; - request.communication_profile = geonet::CommunicationProfile::ITS_G5; - - Application::DataConfirm confirm = Application::request(request, std::move(payload), node_); - - if (!confirm.accepted()) { - throw std::runtime_error("[SEND] CPM application data request failed"); - } - - // try { - // Application::DataConfirm confirm = Application::request(request, std::move(payload), node_); - // if (!confirm.accepted()) { - // throw std::runtime_error("CPM application data request failed"); - // } - // } catch (...) { - // RCLCPP_INFO(node_->get_logger(), "Request Failed"); - // } - sending_ = false; - // RCLCPP_INFO(node->get_logger(), "Application::request END"); - rclcpp::Time current_time = node_->now(); - RCLCPP_INFO(node_->get_logger(), "[CpmApplication::send] [measure] T_depart_r1 %ld", current_time.nanoseconds()); } } \ No newline at end of file diff --git a/src/v2x_app.cpp b/src/v2x_app.cpp index 911b014..81533e2 100644 --- a/src/v2x_app.cpp +++ b/src/v2x_app.cpp @@ -135,7 +135,7 @@ namespace v2x context.set_link_layer(link_layer.get()); cp = new CpmApplication(node_, trigger.runtime()); - + context.enable(cp); cp_started_ = true;