2021-10-27 21:56:21 +00:00
|
|
|
#include "autoware_v2x/cpm_application.hpp"
|
|
|
|
#include "autoware_v2x/positioning.hpp"
|
|
|
|
#include "autoware_v2x/security.hpp"
|
|
|
|
#include "autoware_v2x/link_layer.hpp"
|
|
|
|
|
|
|
|
#include "rclcpp/rclcpp.hpp"
|
|
|
|
#include <vanetza/btp/ports.hpp>
|
|
|
|
#include <vanetza/asn1/cpm.hpp>
|
|
|
|
#include <vanetza/asn1/packet_visitor.hpp>
|
|
|
|
#include <vanetza/facilities/cpm_functions.hpp>
|
|
|
|
#include <chrono>
|
|
|
|
#include <functional>
|
|
|
|
#include <iostream>
|
|
|
|
#include <sstream>
|
|
|
|
#include <exception>
|
|
|
|
|
2021-11-10 08:14:01 +00:00
|
|
|
#define _USE_MATH_DEFINES
|
|
|
|
#include <math.h>
|
|
|
|
|
2021-10-27 21:56:21 +00:00
|
|
|
using namespace vanetza;
|
|
|
|
using namespace vanetza::facilities;
|
|
|
|
using namespace std::chrono;
|
|
|
|
|
2021-11-10 08:57:37 +00:00
|
|
|
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)
|
2021-10-27 21:56:21 +00:00
|
|
|
{
|
|
|
|
RCLCPP_INFO(node_->get_logger(), "CpmApplication started...");
|
2021-11-10 08:14:01 +00:00
|
|
|
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();
|
2021-10-27 21:56:21 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
CpmApplication::PortType CpmApplication::port()
|
|
|
|
{
|
|
|
|
return btp::ports::CPM;
|
|
|
|
}
|
|
|
|
|
|
|
|
void CpmApplication::indicate(const DataIndication &indication, UpPacketPtr packet)
|
|
|
|
{
|
|
|
|
asn1::PacketVisitor<asn1::Cpm> visitor;
|
|
|
|
std::shared_ptr<const asn1::Cpm> cpm = boost::apply_visitor(visitor, *packet);
|
2021-11-10 08:14:01 +00:00
|
|
|
if (cpm)
|
|
|
|
{
|
2021-10-27 21:56:21 +00:00
|
|
|
RCLCPP_INFO(node_->get_logger(), "Received decodable CPM content");
|
2021-11-10 08:57:37 +00:00
|
|
|
asn1::Cpm message = *cpm;
|
|
|
|
ItsPduHeader_t &header = message->header;
|
|
|
|
|
|
|
|
CpmManagementContainer_t &management = message->cpm.cpmParameters.managementContainer;
|
|
|
|
double lat = management.referencePosition.latitude;
|
|
|
|
double lon = management.referencePosition.longitude;
|
|
|
|
RCLCPP_INFO(node_->get_logger(), "cpm.(reference position) = %f, %f", lat, lon);
|
2021-11-10 08:14:01 +00:00
|
|
|
}
|
|
|
|
else
|
|
|
|
{
|
2021-10-27 21:56:21 +00:00
|
|
|
RCLCPP_INFO(node_->get_logger(), "Received broken content");
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2021-11-10 08:14:01 +00:00
|
|
|
void CpmApplication::updateMGRS(double *x, double *y) {
|
|
|
|
RCLCPP_INFO(node_->get_logger(), "Update MGRS");
|
|
|
|
ego_x_ = *x;
|
|
|
|
ego_y_ = *y;
|
|
|
|
}
|
|
|
|
|
|
|
|
void CpmApplication::updateRP(double *lat, double *lon, double *altitude)
|
2021-10-27 21:56:21 +00:00
|
|
|
{
|
2021-11-10 08:14:01 +00:00
|
|
|
RCLCPP_INFO(node_->get_logger(), "Update RP");
|
|
|
|
ego_lat_ = *lat;
|
|
|
|
ego_lon_ = *lon;
|
|
|
|
ego_altitude_ = *altitude;
|
|
|
|
}
|
2021-10-27 21:56:21 +00:00
|
|
|
|
2021-11-10 08:14:01 +00:00
|
|
|
void CpmApplication::updateGenerationDeltaTime(int *gdt)
|
|
|
|
{
|
|
|
|
RCLCPP_INFO(node_->get_logger(), "Update GDT");
|
|
|
|
generationDeltaTime_ = *gdt;
|
|
|
|
}
|
2021-10-27 21:56:21 +00:00
|
|
|
|
2021-11-10 08:14:01 +00:00
|
|
|
void CpmApplication::updateHeading(double *yaw)
|
|
|
|
{
|
|
|
|
RCLCPP_INFO(node_->get_logger(), "Update Heading : %f", *yaw);
|
|
|
|
ego_heading_ = *yaw;
|
|
|
|
}
|
2021-10-27 21:56:21 +00:00
|
|
|
|
2021-11-10 08:14:01 +00:00
|
|
|
void CpmApplication::updateObjectsStack(const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr msg)
|
|
|
|
{
|
|
|
|
RCLCPP_INFO(node_->get_logger(), "Update ObjectsStack");
|
|
|
|
updating_objects_stack_ = true;
|
2021-10-27 21:56:21 +00:00
|
|
|
|
2021-11-10 08:14:01 +00:00
|
|
|
if (sending_)
|
|
|
|
{
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
|
|
|
if (msg->objects.size() > 0)
|
|
|
|
{
|
|
|
|
RCLCPP_INFO(node_->get_logger(), "At least 1 object detected");
|
2021-10-27 21:56:21 +00:00
|
|
|
|
2021-11-10 08:14:01 +00:00
|
|
|
// Initialize ObjectsStack
|
|
|
|
// std::vector<CpmApplication::Object> objectsStack;
|
|
|
|
objectsStack.clear();
|
2021-10-27 21:56:21 +00:00
|
|
|
|
2021-11-10 08:14:01 +00:00
|
|
|
// 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;
|
2021-11-10 08:57:37 +00:00
|
|
|
object.xDistance = (int)((object.position_x - ego_x_) * cos(-ego_heading_) - (object.position_y - ego_y_) * sin(-ego_heading_)) * 100;
|
|
|
|
object.yDistance = (int)((object.position_x - ego_x_) * sin(-ego_heading_) + (object.position_y - ego_y_) * cos(-ego_heading_)) * 100;
|
|
|
|
RCLCPP_INFO(node_->get_logger(), "xDistance: %d, yDistance: %d", object.xDistance, object.yDistance);
|
|
|
|
object.timeOfMeasurement = 100;
|
2021-11-10 08:14:01 +00:00
|
|
|
objectsStack.push_back(object);
|
|
|
|
++i;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
RCLCPP_INFO(node_->get_logger(), "%d objects added to objectsStack", objectsStack.size());
|
|
|
|
updating_objects_stack_ = false;
|
|
|
|
}
|
2021-10-27 21:56:21 +00:00
|
|
|
|
2021-11-10 08:14:01 +00:00
|
|
|
void CpmApplication::send()
|
|
|
|
{
|
|
|
|
sending_ = true;
|
|
|
|
if (!updating_objects_stack_)
|
2021-10-27 21:56:21 +00:00
|
|
|
{
|
2021-11-10 08:14:01 +00:00
|
|
|
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<milliseconds>(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<StationDataContainer_t>();
|
|
|
|
// 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;
|
2021-11-10 08:57:37 +00:00
|
|
|
RCLCPP_INFO(node_->get_logger(), "headingValue...");
|
2021-11-10 08:14:01 +00:00
|
|
|
ovc.heading.headingValue = (int) std::fmod((1.5708-ego_heading_) * 180 / M_PI, 360.0) * 10;
|
|
|
|
ovc.heading.headingConfidence = 1;
|
|
|
|
|
2021-11-10 08:57:37 +00:00
|
|
|
RCLCPP_INFO(node_->get_logger(), "PerceviedObjectContainer...");
|
2021-11-10 08:14:01 +00:00
|
|
|
PerceivedObjectContainer_t *&poc = cpm.cpmParameters.perceivedObjectContainer;
|
|
|
|
poc = vanetza::asn1::allocate<PerceivedObjectContainer_t>();
|
|
|
|
// 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<PerceivedObject>();
|
|
|
|
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);
|
2021-11-10 08:57:37 +00:00
|
|
|
RCLCPP_INFO(node_->get_logger(), "ADD: %d, %d, %d, %d, %d, %d", pObj->objectID, pObj->timeOfMeasurement, pObj->xDistance.value, pObj->yDistance.value, pObj->xSpeed.value, pObj->ySpeed.value);
|
2021-11-10 08:14:01 +00:00
|
|
|
}
|
|
|
|
|
2021-11-10 08:57:37 +00:00
|
|
|
RCLCPP_INFO(node_->get_logger(), "1");
|
2021-11-10 08:14:01 +00:00
|
|
|
Application::DownPacketPtr packet{new DownPacket()};
|
2021-11-10 08:57:37 +00:00
|
|
|
RCLCPP_INFO(node_->get_logger(), "2");
|
2021-11-10 08:14:01 +00:00
|
|
|
std::unique_ptr<geonet::DownPacket> payload{new geonet::DownPacket()};
|
2021-11-10 08:57:37 +00:00
|
|
|
RCLCPP_INFO(node_->get_logger(), "3");
|
2021-11-10 08:14:01 +00:00
|
|
|
// std::shared_ptr<asn1::Cpm> message_p = std::make_shared<asn1::Cpm>(message);
|
|
|
|
// std::unique_ptr<convertible::byte_buffer> buffer { new convertible::byte_buffer_impl<asn1::Cpm>(&message)};
|
|
|
|
|
|
|
|
payload->layer(OsiLayer::Application) = std::move(message);
|
|
|
|
|
2021-11-10 08:57:37 +00:00
|
|
|
RCLCPP_INFO(node_->get_logger(), "4");
|
|
|
|
|
2021-11-10 08:14:01 +00:00
|
|
|
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...");
|
2021-11-10 08:57:37 +00:00
|
|
|
RCLCPP_INFO(node_->get_logger(), "5");
|
2021-11-10 08:14:01 +00:00
|
|
|
Application::DataConfirm confirm = Application::request(request, std::move(payload), node_);
|
2021-11-10 08:57:37 +00:00
|
|
|
RCLCPP_INFO(node_->get_logger(), "6");
|
2021-11-10 08:14:01 +00:00
|
|
|
if (!confirm.accepted())
|
|
|
|
{
|
|
|
|
throw std::runtime_error("CPM application data request failed");
|
|
|
|
}
|
2021-10-27 21:56:21 +00:00
|
|
|
}
|
2021-11-10 08:14:01 +00:00
|
|
|
sending_ = false;
|
2021-10-27 21:56:21 +00:00
|
|
|
// RCLCPP_INFO(node->get_logger(), "Application::request END");
|
|
|
|
}
|