diff --git a/include/autoware_v2x/cpm_application.hpp b/include/autoware_v2x/cpm_application.hpp index 47916d5..79f8d52 100644 --- a/include/autoware_v2x/cpm_application.hpp +++ b/include/autoware_v2x/cpm_application.hpp @@ -11,13 +11,53 @@ class CpmApplication : public Application { public: - CpmApplication(rclcpp::Node *node); + CpmApplication(rclcpp::Node *node, vanetza::Runtime&); PortType port() override; void indicate(const DataIndication &, UpPacketPtr) override; - void send(const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr, rclcpp::Node *, double, double); + void set_interval(vanetza::Clock::duration); + void updateObjectsStack(const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr); + void updateMGRS(double *, double *); + void updateRP(double *, double *, double *); + void updateGenerationDeltaTime(int *); + void updateHeading(double *); + void send(); private: + void schedule_timer(); + void on_timer(vanetza::Clock::time_point); + rclcpp::Node* node_; + vanetza::Runtime& runtime_; + vanetza::Clock::duration cpm_interval_; + struct Object { + int objectID; // 0-255 + rclcpp::Time timestamp; + double position_x; + double position_y; + double position_z; + double orientation_x; + double orientation_y; + double orientation_z; + double orientation_w; + double xDistance; + double yDistance; + double xSpeed; + double ySpeed; + vanetza::PositionFix position; + int timeOfMeasurement; + }; + std::vector objectsStack; + + double ego_x_; + double ego_y_; + double ego_lat_; + double ego_lon_; + double ego_altitude_; + double ego_heading_; + int generationDeltaTime_; + + bool updating_objects_stack_; + bool sending_; }; #endif /* CPM_APPLICATION_HPP_EUIC2VFR */ diff --git a/src/cpm_application.cpp b/src/cpm_application.cpp index a8d1111..1d98020 100644 --- a/src/cpm_application.cpp +++ b/src/cpm_application.cpp @@ -14,15 +14,47 @@ #include #include +#define _USE_MATH_DEFINES +#include + // This is a very simple application that sends BTP-B messages with the content 0xc0ffee. using namespace vanetza; using namespace vanetza::facilities; using namespace std::chrono; -CpmApplication::CpmApplication(rclcpp::Node *node) : node_(node) +CpmApplication::CpmApplication(rclcpp::Node *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), + generationDeltaTime_(0), + updating_objects_stack_(false), + sending_(false) { RCLCPP_INFO(node_->get_logger(), "CpmApplication started..."); + set_interval(milliseconds(1000)); +} + +void CpmApplication::set_interval(Clock::duration interval) +{ + cpm_interval_ = interval; + runtime_.cancel(this); + 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) +{ + schedule_timer(); + send(); } CpmApplication::PortType CpmApplication::port() @@ -34,9 +66,12 @@ void CpmApplication::indicate(const DataIndication &indication, UpPacketPtr pack { asn1::PacketVisitor visitor; std::shared_ptr cpm = boost::apply_visitor(visitor, *packet); - if (cpm) { + if (cpm) + { RCLCPP_INFO(node_->get_logger(), "Received decodable CPM content"); - } else { + } + else + { RCLCPP_INFO(node_->get_logger(), "Received broken content"); } asn1::Cpm message = *cpm; @@ -50,121 +85,174 @@ void CpmApplication::indicate(const DataIndication &indication, UpPacketPtr pack RCLCPP_INFO(node_->get_logger(), "cpm.(reference position) = %f, %f", lat, lon); } -void CpmApplication::send(const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr msg, rclcpp::Node *node, double pos_lat, double pos_lon) -{ - RCLCPP_INFO(node->get_logger(), "Sending CPM..."); - // Send all objects in 1 CPM message - vanetza::asn1::Cpm message; - - // ITS PDU Header - ItsPduHeader_t &header = message->header; - header.protocolVersion = 1; - header.messageID = 14; - header.stationID = 1; - - const auto time_now = duration_cast(system_clock::now().time_since_epoch()); - uint16_t gen_delta_time = time_now.count(); - - CollectivePerceptionMessage_t &cpm = message->cpm; - cpm.generationDeltaTime = gen_delta_time * GenerationDeltaTime_oneMilliSec; - - // auto position = positioning->position_specify(pos_lat, pos_lon); - - 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 = pos_lat * units::degree; - fix.longitude = pos_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.stationDataContainer = NULL; - // cpm.cpmParameters.perceivedObjectContainer = NULL; - cpm.cpmParameters.numberOfPerceivedObjects = msg->objects.size(); - - 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 = 0; - ovc.heading.headingConfidence = 1; - - PerceivedObjectContainer_t *&poc = cpm.cpmParameters.perceivedObjectContainer; - poc = vanetza::asn1::allocate(); - // cpm.cpmParameters.perceivedObjectContainer = poc; - // PerceivedObjectContainer_t pocc = *poc; - // RCLCPP_INFO(node->get_logger(), "Allocated poc"); - - for (auto object : msg->objects) { - PerceivedObject *pObj = vanetza::asn1::allocate(); - pObj->objectID = 1; - pObj->timeOfMeasurement = 10; - pObj->xDistance.value = 100; - pObj->xDistance.confidence = 1; - pObj->yDistance.value = 200; - pObj->yDistance.confidence = 1; - pObj->xSpeed.value = 5; - pObj->xSpeed.confidence = 1; - pObj->ySpeed.value = 0; - pObj->ySpeed.confidence = 1; - - ASN_SEQUENCE_ADD(poc, pObj); - // RCLCPP_INFO(node->get_logger(), "Added one object to poc->list"); - } - - - 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; - - // RCLCPP_INFO(node->get_logger(), "Packet Size: %d", payload->size()); - - // RCLCPP_INFO(node->get_logger(), "Going to Application::request..."); - Application::DataConfirm confirm = Application::request(request, std::move(payload), node); - if (!confirm.accepted()) - { - throw std::runtime_error("CPM application data request failed"); - } - // RCLCPP_INFO(node->get_logger(), "Application::request END"); +void CpmApplication::updateMGRS(double *x, double *y) { + RCLCPP_INFO(node_->get_logger(), "Update MGRS"); + ego_x_ = *x; + ego_y_ = *y; } -// void HelloApplication::schedule_timer() -// { -// timer_.expires_from_now(interval_); -// timer_.async_wait(std::bind(&HelloApplication::on_timer, this, std::placeholders::_1)); -// } +void CpmApplication::updateRP(double *lat, double *lon, double *altitude) +{ + RCLCPP_INFO(node_->get_logger(), "Update RP"); + ego_lat_ = *lat; + ego_lon_ = *lon; + ego_altitude_ = *altitude; +} -// void HelloApplication::on_timer(const boost::system::error_code& ec) -// { -// if (ec != boost::asio::error::operation_aborted) { -// DownPacketPtr packet { new DownPacket() }; -// packet->layer(OsiLayer::Application) = ByteBuffer { 0xC0, 0xFF, 0xEE }; -// DataRequest request; -// request.transport_type = geonet::TransportType::SHB; -// request.communication_profile = geonet::CommunicationProfile::ITS_G5; -// request.its_aid = aid::CA; -// auto confirm = Application::request(request, std::move(packet)); -// if (!confirm.accepted()) { -// throw std::runtime_error("Hello application data request failed"); -// } +void CpmApplication::updateGenerationDeltaTime(int *gdt) +{ + RCLCPP_INFO(node_->get_logger(), "Update GDT"); + generationDeltaTime_ = *gdt; +} -// schedule_timer(); -// } -// } +void CpmApplication::updateHeading(double *yaw) +{ + RCLCPP_INFO(node_->get_logger(), "Update Heading : %f", *yaw); + ego_heading_ = *yaw; +} + +void CpmApplication::updateObjectsStack(const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr msg) +{ + RCLCPP_INFO(node_->get_logger(), "Update ObjectsStack"); + updating_objects_stack_ = true; + + if (sending_) + { + return; + } + + if (msg->objects.size() > 0) + { + RCLCPP_INFO(node_->get_logger(), "At least 1 object detected"); + + // Initialize ObjectsStack + // std::vector objectsStack; + objectsStack.clear(); + + // Create CpmApplication::Object per msg->object and add it to objectsStack + int i = 0; + for (auto obj : msg->objects) + { + CpmApplication::Object object; + object.objectID = i; + object.timestamp = msg->header.stamp; + object.position_x = obj.state.pose_covariance.pose.position.x; // MGRS + object.position_y = obj.state.pose_covariance.pose.position.y; + object.position_z = obj.state.pose_covariance.pose.position.z; + object.orientation_x = obj.state.pose_covariance.pose.orientation.x; + object.orientation_y = obj.state.pose_covariance.pose.orientation.y; + object.orientation_z = obj.state.pose_covariance.pose.orientation.z; + object.orientation_w = obj.state.pose_covariance.pose.orientation.w; + object.xDistance = ((object.position_x - ego_x_) * cos(-ego_heading_) - (object.position_y - ego_y_) * sin(-ego_heading_)) * 100; + object.yDistance = ((object.position_x - ego_x_) * sin(-ego_heading_) + (object.position_y - ego_y_) * cos(-ego_heading_)) * 100; + // RCLCPP_INFO(node_->get_logger(), "xDistance: %f, yDistance: %f", object.xDistance, object.yDistance); + objectsStack.push_back(object); + ++i; + } + } + RCLCPP_INFO(node_->get_logger(), "%d objects added to objectsStack", objectsStack.size()); + updating_objects_stack_ = false; +} + +void CpmApplication::send() +{ + sending_ = true; + if (!updating_objects_stack_) + { + RCLCPP_INFO(node_->get_logger(), "Sending CPM..."); + + // Send all objects in 1 CPM message + vanetza::asn1::Cpm message; + + // ITS PDU Header + ItsPduHeader_t &header = message->header; + header.protocolVersion = 1; + header.messageID = 14; + header.stationID = 1; + + // const auto time_now = duration_cast(system_clock::now().time_since_epoch()); + // uint16_t gen_delta_time = time_now.count(); + + CollectivePerceptionMessage_t &cpm = message->cpm; + cpm.generationDeltaTime = generationDeltaTime_ * GenerationDeltaTime_oneMilliSec; + + // auto position = positioning->position_specify(pos_lat, pos_lon); + + 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); + + // cpm.cpmParameters.stationDataContainer = NULL; + // cpm.cpmParameters.perceivedObjectContainer = NULL; + cpm.cpmParameters.numberOfPerceivedObjects = objectsStack.size(); + + 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; + ovc.heading.headingValue = (int) std::fmod((1.5708-ego_heading_) * 180 / M_PI, 360.0) * 10; + ovc.heading.headingConfidence = 1; + + PerceivedObjectContainer_t *&poc = cpm.cpmParameters.perceivedObjectContainer; + poc = vanetza::asn1::allocate(); + // cpm.cpmParameters.perceivedObjectContainer = poc; + // PerceivedObjectContainer_t pocc = *poc; + // RCLCPP_INFO(node->get_logger(), "Allocated poc"); + + 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; + + ASN_SEQUENCE_ADD(poc, pObj); + // RCLCPP_INFO(node->get_logger(), "Added one object to poc->list"); + } + + 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; + + // RCLCPP_INFO(node->get_logger(), "Packet Size: %d", payload->size()); + + // RCLCPP_INFO(node->get_logger(), "Going to Application::request..."); + Application::DataConfirm confirm = Application::request(request, std::move(payload), node_); + if (!confirm.accepted()) + { + throw std::runtime_error("CPM application data request failed"); + } + } + sending_ = false; + // RCLCPP_INFO(node->get_logger(), "Application::request END"); +}