Update to include packet reflection
This commit is contained in:
yuasabe
parent
c91415eba9
commit
e674134bba
|
|
@ -69,6 +69,8 @@ namespace v2x
|
|||
|
||||
bool updating_objects_stack_;
|
||||
bool sending_;
|
||||
bool is_sender_;
|
||||
bool reflect_packet_;
|
||||
};
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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<asn1::Cpm> visitor;
|
||||
std::shared_ptr<const asn1::Cpm> 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<geonet::DownPacket> 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...");
|
||||
if (is_sender_) {
|
||||
sending_ = true;
|
||||
|
||||
vanetza::asn1::Cpm message;
|
||||
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<milliseconds>(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<StationDataContainer_t>();
|
||||
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<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;
|
||||
// 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<PerceivedObjectContainer_t>();
|
||||
|
||||
if (objectsStack.size() > 0) {
|
||||
PerceivedObjectContainer_t *&poc = cpm.cpmParameters.perceivedObjectContainer;
|
||||
poc = vanetza::asn1::allocate<PerceivedObjectContainer_t>();
|
||||
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;
|
||||
|
||||
for (CpmApplication::Object object : objectsStack) {
|
||||
// if (object.xDistance > 10000) continue;
|
||||
// if (object.yDistance > 10000) continue;
|
||||
pObj->planarObjectDimension1 = vanetza::asn1::allocate<ObjectDimension_t>();
|
||||
pObj->planarObjectDimension2 = vanetza::asn1::allocate<ObjectDimension_t>();
|
||||
pObj->verticalObjectDimension = vanetza::asn1::allocate<ObjectDimension_t>();
|
||||
|
||||
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;
|
||||
(*(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<ObjectDimension_t>();
|
||||
pObj->planarObjectDimension2 = vanetza::asn1::allocate<ObjectDimension_t>();
|
||||
pObj->verticalObjectDimension = vanetza::asn1::allocate<ObjectDimension_t>();
|
||||
pObj->yawAngle = vanetza::asn1::allocate<CartesianAngle>();
|
||||
(*(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<CartesianAngle>();
|
||||
(*(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<geonet::DownPacket> 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<geonet::DownPacket> payload{new geonet::DownPacket()};
|
||||
// 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);
|
||||
|
||||
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());
|
||||
}
|
||||
}
|
||||
Loading…
Reference in New Issue