From 64064a9a75bf0e1f143f2bc1320c5047ff153ba5 Mon Sep 17 00:00:00 2001 From: Yu Asabe Date: Sat, 15 Jan 2022 16:14:06 +0900 Subject: [PATCH 1/6] Remove unused variables --- include/autoware_v2x/v2x_node.hpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/include/autoware_v2x/v2x_node.hpp b/include/autoware_v2x/v2x_node.hpp index 9fd9cb3..0427084 100644 --- a/include/autoware_v2x/v2x_node.hpp +++ b/include/autoware_v2x/v2x_node.hpp @@ -33,9 +33,6 @@ namespace v2x rclcpp::Subscription::SharedPtr subscription_pos_; rclcpp::Publisher::SharedPtr publisher_; - rclcpp::Time received_time; - bool cpm_received; - double pos_lat_; double pos_lon_; }; From b631397fa2ec36261e1401db449e2632dad3bef0 Mon Sep 17 00:00:00 2001 From: Yu Asabe Date: Sat, 15 Jan 2022 16:14:17 +0900 Subject: [PATCH 2/6] Add measurement loggings --- src/cpm_application.cpp | 24 ++++++++++++++++-------- src/v2x_app.cpp | 5 ++++- src/v2x_node.cpp | 38 +++++++++++++++++++------------------- 3 files changed, 39 insertions(+), 28 deletions(-) diff --git a/src/cpm_application.cpp b/src/cpm_application.cpp index d9dbd92..a94b004 100644 --- a/src/cpm_application.cpp +++ b/src/cpm_application.cpp @@ -70,12 +70,15 @@ namespace v2x return btp::ports::CPM; } - void CpmApplication::indicate(const DataIndication &indication, UpPacketPtr packet) - { + 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::Time current_time = node_->now(); + RCLCPP_INFO(node_->get_logger(), "[CpmApplication::indicate] [measure] T_receive_r2 %ld", current_time.nanoseconds()); + asn1::Cpm message = *cpm; ItsPduHeader_t &header = message->header; @@ -172,8 +175,7 @@ namespace v2x void CpmApplication::updateGenerationDeltaTime(int *gdt, long long *gdt_timestamp) { generationDeltaTime_ = *gdt; - gdt_timestamp_ = *gdt_timestamp; - // RCLCPP_INFO(node_->get_logger(), "[updateGDT] %d %lu", generationDeltaTime_, gdt_timestamp_); + gdt_timestamp_ = *gdt_timestamp; // ETSI-epoch milliseconds timestamp } void CpmApplication::updateHeading(double *yaw) @@ -230,10 +232,15 @@ namespace v2x object.yawAngle = std::lround((yaw * 180.0 / M_PI) * 10.0); // 0 - 3600 } - long long timestamp = msg->header.stamp.sec * 1e9 + msg->header.stamp.nanosec; - object.timeOfMeasurement = (gdt_timestamp_ - timestamp) / 1e6; + long long msg_timestamp_sec = msg->header.stamp.sec; + long long msg_timestamp_nsec = msg->header.stamp.nanosec; + msg_timestamp_sec -= 1072915200; // convert to etsi-epoch + long long msg_timestamp_msec = msg_timestamp_sec * 1000 + msg_timestamp_nsec / 1000000; + // long long timestamp = msg->header.stamp.sec * 1e9 + msg->header.stamp.nanosec; + object.timeOfMeasurement = gdt_timestamp_ - msg_timestamp_msec; // RCLCPP_INFO(node_->get_logger(), "[updateObjectsStack] %ld %ld %d", gdt_timestamp_, timestamp, object.timeOfMeasurement); if (object.timeOfMeasurement < -1500 || object.timeOfMeasurement > 1500) { + RCLCPP_INFO(node_->get_logger(), "[updateObjectsStack] timeOfMeasurement out of bounds: %d", object.timeOfMeasurement); continue; } objectsStack.push_back(object); @@ -248,8 +255,7 @@ namespace v2x updating_objects_stack_ = false; } - void CpmApplication::send() - { + void CpmApplication::send() { sending_ = true; RCLCPP_INFO(node_->get_logger(), "[SEND] Sending CPM..."); @@ -377,5 +383,7 @@ namespace v2x // } 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 8491160..911b014 100644 --- a/src/v2x_app.cpp +++ b/src/v2x_app.cpp @@ -40,6 +40,9 @@ namespace v2x void V2XApp::objectsCallback(const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr msg) { // RCLCPP_INFO(node_->get_logger(), "V2XApp: msg received"); + if (!tf_received_) { + RCLCPP_WARN(node_->get_logger(), "[V2XApp::objectsCallback] tf not received yet"); + } if (tf_received_ && cp_started_) { cp->updateObjectsStack(msg); } @@ -62,7 +65,7 @@ namespace v2x // long long gdt_timestamp_msec_etsi_epoch = gdt_timestamp_msec - 1072915200000; // int gdt = gdt_timestamp_msec_etsi_epoch % 65536; // milliseconds int gdt = timestamp_msec % 65536; - RCLCPP_INFO(node_->get_logger(), "[tfCallback] %d,%lld,%lld,%lld", gdt, timestamp_msec, timestamp_sec, timestamp_nsec); + // RCLCPP_INFO(node_->get_logger(), "[tfCallback] %d,%lld,%lld,%lld", gdt, timestamp_msec, timestamp_sec, timestamp_nsec); double rot_x = msg->transforms[0].transform.rotation.x; double rot_y = msg->transforms[0].transform.rotation.y; diff --git a/src/v2x_node.cpp b/src/v2x_node.cpp index e44aed2..31dbcd9 100644 --- a/src/v2x_node.cpp +++ b/src/v2x_node.cpp @@ -25,14 +25,13 @@ using namespace std::chrono; namespace v2x { - V2XNode::V2XNode(const rclcpp::NodeOptions &node_options) : rclcpp::Node("autoware_v2x_node", node_options), received_time(this->now()), cpm_received(false) - { + V2XNode::V2XNode(const rclcpp::NodeOptions &node_options) : rclcpp::Node("autoware_v2x_node", node_options) { using std::placeholders::_1; subscription_ = this->create_subscription("/perception/object_recognition/objects", 10, std::bind(&V2XNode::objectsCallback, this, _1)); subscription_pos_ = this->create_subscription("/tf", 10, std::bind(&V2XNode::tfCallback, this, _1)); - publisher_ = create_publisher("/perception/object_recognition/objects", rclcpp::QoS{10}); + publisher_ = create_publisher("/v2x/cpm/objects", rclcpp::QoS{10}); this->declare_parameter("network_interface", "vmnet1"); @@ -40,13 +39,16 @@ namespace v2x boost::thread v2xApp(boost::bind(&V2XApp::start, app)); RCLCPP_INFO(get_logger(), "V2X Node Launched"); + + rclcpp::Time current_time = this->now(); + RCLCPP_INFO(get_logger(), "[V2XNode::V2XNode] [measure] T_R1 %ld", current_time.nanoseconds()); + } void V2XNode::publishObjects(std::vector *objectsStack) { autoware_perception_msgs::msg::DynamicObjectArray output_dynamic_object_msg; std_msgs::msg::Header header; rclcpp::Time current_time = this->now(); - received_time = current_time; output_dynamic_object_msg.header.frame_id = "map"; output_dynamic_object_msg.header.stamp = current_time; @@ -79,27 +81,25 @@ namespace v2x output_dynamic_object_msg.objects.push_back(object); } - cpm_received = true; + + current_time = this->now(); + RCLCPP_INFO(get_logger(), "[V2XNode::publishObjects] [measure] T_obj_r2 %ld", current_time.nanoseconds()); + publisher_->publish(output_dynamic_object_msg); } - void V2XNode::objectsCallback(const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr msg) - { + void V2XNode::objectsCallback(const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr msg) { rclcpp::Time current_time = this->now(); - // RCLCPP_INFO(get_logger(), "%ld", current_time.nanoseconds() - received_time.nanoseconds()); - if (cpm_received) { - // if time difference is more than 10ms - if (current_time - received_time > rclcpp::Duration(std::chrono::nanoseconds(10000000))) { - app->objectsCallback(msg); - } - cpm_received = false; - } else { - app->objectsCallback(msg); - } + rclcpp::Time msg_time = msg->header.stamp; // timestamp included in the Autoware Perception Msg. + RCLCPP_INFO(get_logger(), "[V2XNode::objectsCallback] %d objects", msg->objects.size()); + + // Measuring T_A1R1 + RCLCPP_INFO(get_logger(), "[V2XNode::objectsCallback] [measure] T_obj %ld", msg_time.nanoseconds()); + RCLCPP_INFO(get_logger(), "[V2XNode::objectsCallback] [measure] T_obj_receive %ld", current_time.nanoseconds()); + app->objectsCallback(msg); } - void V2XNode::tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr msg) - { + void V2XNode::tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr msg) { app->tfCallback(msg); } } From c91415eba97c1044c0540d0fdf085060bf61dce3 Mon Sep 17 00:00:00 2001 From: Yu Asabe Date: Thu, 20 Jan 2022 19:39:51 +0900 Subject: [PATCH 3/6] Update T_R1R2 logging --- src/cpm_application.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/cpm_application.cpp b/src/cpm_application.cpp index a94b004..e0e4e04 100644 --- a/src/cpm_application.cpp +++ b/src/cpm_application.cpp @@ -87,7 +87,10 @@ namespace v2x const auto time_now = duration_cast (runtime_.now().time_since_epoch()); uint16_t gdt = time_now.count(); int gdt_diff = (65536 + (gdt - gdt_cpm) % 65536) % 65536; - RCLCPP_INFO(node_->get_logger(), "[indicate] gdt: %ld,%u,%d", gdt_cpm, gdt, gdt_diff); + RCLCPP_INFO(node_->get_logger(), "[CpmApplication::indicate] [measure] GDT_CPM: %ld", gdt_cpm); + RCLCPP_INFO(node_->get_logger(), "[CpmApplication::indicate] [measure] GDT: %u", gdt); + RCLCPP_INFO(node_->get_logger(), "[CpmApplication::indicate] [measure] T_R1R2: %d", gdt_diff); + CpmManagementContainer_t &management = message->cpm.cpmParameters.managementContainer; double lat = management.referencePosition.latitude / 1.0e7; From e674134bba3b485f595af614c2c4a1e764c98535 Mon Sep 17 00:00:00 2001 From: yuasabe Date: Fri, 21 Jan 2022 22:07:33 +0900 Subject: [PATCH 4/6] Update to include packet reflection --- include/autoware_v2x/cpm_application.hpp | 2 + src/cpm_application.cpp | 259 +++++++++++------------ src/v2x_app.cpp | 2 +- 3 files changed, 125 insertions(+), 138 deletions(-) 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; From e53d089817ac22d9430ee6713a8a7a4730c1b688 Mon Sep 17 00:00:00 2001 From: yuasabe Date: Sun, 23 Jan 2022 15:38:33 +0900 Subject: [PATCH 5/6] Add T_objstack_updated for measurement --- src/cpm_application.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/cpm_application.cpp b/src/cpm_application.cpp index 099863c..c020da0 100644 --- a/src/cpm_application.cpp +++ b/src/cpm_application.cpp @@ -267,6 +267,8 @@ namespace v2x } } RCLCPP_INFO(node_->get_logger(), "ObjectsStack: %d objects", objectsStack.size()); + rclcpp::Time current_time = node_->now(); + RCLCPP_INFO(node_->get_logger(), "[CpmApplication::updateObjectsStack] [measure] T_objstack_updated %ld", current_time.nanoseconds()); updating_objects_stack_ = false; } From eb6a9b20406c0cb8876f8d1ff0297fe86d32586e Mon Sep 17 00:00:00 2001 From: Yu Asabe Date: Wed, 16 Feb 2022 16:53:45 +0900 Subject: [PATCH 6/6] fixing positioning --- include/autoware_v2x/cpm_application.hpp | 17 +++++++---- launch/v2x.launch.xml | 2 +- src/cpm_application.cpp | 38 ++++++++++-------------- src/v2x_app.cpp | 13 +------- 4 files changed, 29 insertions(+), 41 deletions(-) diff --git a/include/autoware_v2x/cpm_application.hpp b/include/autoware_v2x/cpm_application.hpp index 236576d..98fadb8 100644 --- a/include/autoware_v2x/cpm_application.hpp +++ b/include/autoware_v2x/cpm_application.hpp @@ -58,12 +58,17 @@ namespace v2x vanetza::Runtime &runtime_; vanetza::Clock::duration cpm_interval_; - double ego_x_; - double ego_y_; - double ego_lat_; - double ego_lon_; - double ego_altitude_; - double ego_heading_; + struct Ego_station { + double mgrs_x; + double mgrs_y; + double latitude; + double longitude; + double altitude; + double heading; + }; + + Ego_station ego_; + int generationDeltaTime_; long long gdt_timestamp_; diff --git a/launch/v2x.launch.xml b/launch/v2x.launch.xml index 44032d2..636e94d 100644 --- a/launch/v2x.launch.xml +++ b/launch/v2x.launch.xml @@ -1,6 +1,6 @@ - + \ No newline at end of file diff --git a/src/cpm_application.cpp b/src/cpm_application.cpp index c020da0..bc2b084 100644 --- a/src/cpm_application.cpp +++ b/src/cpm_application.cpp @@ -28,17 +28,11 @@ using namespace vanetza; using namespace vanetza::facilities; using namespace std::chrono; -namespace v2x -{ +namespace v2x { CpmApplication::CpmApplication(V2XNode *node, Runtime &rt) : node_(node), runtime_(rt), - ego_x_(0), - ego_y_(0), - ego_lat_(0), - ego_lon_(0), - ego_altitude_(0), - ego_heading_(0), + ego_(), generationDeltaTime_(0), updating_objects_stack_(false), sending_(false), @@ -181,14 +175,14 @@ namespace v2x } void CpmApplication::updateMGRS(double *x, double *y) { - ego_x_ = *x; - ego_y_ = *y; + ego_.mgrs_x = *x; + ego_.mgrs_y = *y; } void CpmApplication::updateRP(double *lat, double *lon, double *altitude) { - ego_lat_ = *lat; - ego_lon_ = *lon; - ego_altitude_ = *altitude; + ego_.latitude = *lat; + ego_.longitude = *lon; + ego_.altitude = *altitude; } void CpmApplication::updateGenerationDeltaTime(int *gdt, long long *gdt_timestamp) { @@ -197,7 +191,7 @@ namespace v2x } void CpmApplication::updateHeading(double *yaw) { - ego_heading_ = *yaw; + ego_.heading = *yaw; } void CpmApplication::updateObjectsStack(const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr msg) { @@ -226,8 +220,10 @@ namespace v2x object.shape_x = std::lround(obj.shape.dimensions.x * 10.0); object.shape_y = std::lround(obj.shape.dimensions.y * 10.0); object.shape_z = std::lround(obj.shape.dimensions.z * 10.0); - object.xDistance = std::lround(((object.position_x - ego_x_) * cos(-ego_heading_) - (object.position_y - ego_y_) * sin(-ego_heading_)) * 100.0); - object.yDistance = std::lround(((object.position_x - ego_x_) * sin(-ego_heading_) + (object.position_y - ego_y_) * cos(-ego_heading_)) * 100.0); + // object.xDistance = std::lround(((object.position_x - ego_x_) * cos(-ego_heading_) - (object.position_y - ego_y_) * sin(-ego_heading_)) * 100.0); + // object.yDistance = std::lround(((object.position_x - ego_x_) * sin(-ego_heading_) + (object.position_y - ego_y_) * cos(-ego_heading_)) * 100.0); + object.xDistance = std::lround(((object.position_x - ego_.mgrs_x) * cos(-ego_.heading) - (object.position_y - ego_.mgrs_y) * sin(-ego_.heading)) * 100.0); + object.yDistance = std::lround(((object.position_x - ego_.mgrs_x) * sin(-ego_.heading) + (object.position_y - ego_.mgrs_y) * cos(-ego_.heading)) * 100.0); if (object.xDistance < -132768 || object.xDistance > 132767) { continue; } @@ -260,9 +256,7 @@ namespace v2x } objectsStack.push_back(object); ++i; - // RCLCPP_INFO(node_->get_logger(), "Added to stack: %f %f %f", obj.shape.dimensions.x, obj.shape.dimensions.y, obj.shape.dimensions.z); - // RCLCPP_INFO(node_->get_logger(), "Added to stack: %f %f %f", obj.shape.dimensions.x * 10.0, obj.shape.dimensions.y * 10.0, obj.shape.dimensions.z * 10.0); - // RCLCPP_INFO(node_->get_logger(), "Added to stack: %d %d %d", std::lround(obj.shape.dimensions.x * 10.0), std::lround(obj.shape.dimensions.y * 10.0), std::lround(obj.shape.dimensions.z * 10.0)); + RCLCPP_INFO(node_->get_logger(), "Added to stack: #%d (%d, %d) (%d, %d) (%d, %d, %d) (%f: %d)", object.objectID, object.xDistance, object.yDistance, object.xSpeed, object.ySpeed, object.shape_x, object.shape_y, object.shape_z, yaw, object.yawAngle); } } @@ -295,8 +289,8 @@ namespace v2x 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.latitude = ego_.latitude * units::degree; + fix.longitude = ego_.longitude * units::degree; fix.confidence.semi_major = 1.0 * units::si::meter; fix.confidence.semi_minor = fix.confidence.semi_major; copy(fix, management.referencePosition); @@ -308,7 +302,7 @@ namespace v2x 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); + int heading = std::lround(((-ego_.heading * 180.0 / M_PI) + 90.0) * 10.0); if (heading < 0) heading += 3600; ovc.heading.headingValue = heading; ovc.heading.headingConfidence = 1; diff --git a/src/v2x_app.cpp b/src/v2x_app.cpp index 81533e2..3ed234f 100644 --- a/src/v2x_app.cpp +++ b/src/v2x_app.cpp @@ -49,7 +49,7 @@ namespace v2x } void V2XApp::tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr msg) { - // RCLCPP_INFO(node_->get_logger(), "V2XApp: tf msg received"); + tf_received_ = true; double x = msg->transforms[0].transform.translation.x; @@ -60,10 +60,6 @@ namespace v2x long long timestamp_nsec = msg->transforms[0].header.stamp.nanosec; // nanoseconds timestamp_sec -= 1072915200; // convert to etsi-epoch long long timestamp_msec = timestamp_sec * 1000 + timestamp_nsec / 1000000; - // long long gdt_timestamp = gdt_timestamp_sec * 1e9 + gdt_timestamp_nsec; // nanoseconds - // long long gdt_timestamp_msec = gdt_timestamp / 1000000; - // long long gdt_timestamp_msec_etsi_epoch = gdt_timestamp_msec - 1072915200000; - // int gdt = gdt_timestamp_msec_etsi_epoch % 65536; // milliseconds int gdt = timestamp_msec % 65536; // RCLCPP_INFO(node_->get_logger(), "[tfCallback] %d,%lld,%lld,%lld", gdt, timestamp_msec, timestamp_sec, timestamp_nsec); @@ -78,23 +74,16 @@ namespace v2x double roll, pitch, yaw; matrix.getRPY(roll, pitch, yaw); - char mgrs[20]; int zone, prec; bool northp; double x_mgrs, y_mgrs; double lat, lon; sprintf(mgrs, "54SVE%.5d%.5d", (int)x, (int)y); - // RCLCPP_INFO(node_->get_logger(), "MGRS: %s", mgrs); GeographicLib::MGRS::Reverse(mgrs, zone, northp, x_mgrs, y_mgrs, prec); GeographicLib::UTMUPS::Reverse(zone, northp, x_mgrs, y_mgrs, lat, lon); - // RCLCPP_INFO(node_->get_logger(), "Ego Position Lat/Lon: %f, %f", lat, lon); - // RCLCPP_INFO(node_->get_logger(), "Ego Orientation: %f, %f, %f, %f", rot_x, rot_y, rot_z, rot_w); - // RCLCPP_INFO(node_->get_logger(), "Ego Orientation: %f %f %f", roll, pitch, yaw); - // RCLCPP_INFO(node_->get_logger(), "Timestamp: %d, GDT: %d", timestamp, gdt); - if (cp && cp_started_) { cp->updateMGRS(&x, &y); cp->updateRP(&lat, &lon, &z);