Update cpm_application
This commit is contained in:
Yu Asabe
parent
440fd202e3
commit
dad222be44
|
|
@ -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<CpmApplication::Object> 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 */
|
||||
|
|
|
|||
|
|
@ -14,15 +14,47 @@
|
|||
#include <sstream>
|
||||
#include <exception>
|
||||
|
||||
#define _USE_MATH_DEFINES
|
||||
#include <math.h>
|
||||
|
||||
// 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<asn1::Cpm> visitor;
|
||||
std::shared_ptr<const asn1::Cpm> 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<milliseconds>(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<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 = 0;
|
||||
ovc.heading.headingConfidence = 1;
|
||||
|
||||
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 (auto object : msg->objects) {
|
||||
PerceivedObject *pObj = vanetza::asn1::allocate<PerceivedObject>();
|
||||
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<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;
|
||||
|
||||
// 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<CpmApplication::Object> 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<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;
|
||||
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<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);
|
||||
// RCLCPP_INFO(node->get_logger(), "Added one object to poc->list");
|
||||
}
|
||||
|
||||
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;
|
||||
|
||||
// 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");
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue