83 lines
2.4 KiB
C++
83 lines
2.4 KiB
C++
#ifndef CPM_APPLICATION_HPP_EUIC2VFR
|
|
#define CPM_APPLICATION_HPP_EUIC2VFR
|
|
|
|
#include "autoware_v2x/application.hpp"
|
|
#include "rclcpp/rclcpp.hpp"
|
|
#include <boost/asio/io_service.hpp>
|
|
#include <boost/asio/steady_timer.hpp>
|
|
#include "autoware_perception_msgs/msg/dynamic_object_array.hpp"
|
|
#include "autoware_v2x/positioning.hpp"
|
|
|
|
namespace v2x
|
|
{
|
|
class V2XNode;
|
|
class CpmApplication : public Application
|
|
{
|
|
public:
|
|
CpmApplication(V2XNode *node, vanetza::Runtime &, bool is_sender);
|
|
PortType port() override;
|
|
void indicate(const DataIndication &, UpPacketPtr) override;
|
|
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 *, long long *);
|
|
void updateHeading(double *);
|
|
void send();
|
|
|
|
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;
|
|
int shape_x;
|
|
int shape_y;
|
|
int shape_z;
|
|
int xDistance;
|
|
int yDistance;
|
|
int xSpeed;
|
|
int ySpeed;
|
|
int yawAngle;
|
|
vanetza::PositionFix position;
|
|
int timeOfMeasurement;
|
|
};
|
|
std::vector<CpmApplication::Object> objectsStack;
|
|
std::vector<CpmApplication::Object> receivedObjectsStack;
|
|
|
|
private:
|
|
void schedule_timer();
|
|
void on_timer(vanetza::Clock::time_point);
|
|
|
|
V2XNode *node_;
|
|
vanetza::Runtime &runtime_;
|
|
vanetza::Clock::duration cpm_interval_;
|
|
|
|
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_;
|
|
|
|
bool updating_objects_stack_;
|
|
bool sending_;
|
|
bool is_sender_;
|
|
bool reflect_packet_;
|
|
};
|
|
}
|
|
|
|
#endif /* CPM_APPLICATION_HPP_EUIC2VFR */
|