2021-10-27 21:56:21 +00:00
|
|
|
#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"
|
|
|
|
|
|
2021-11-10 15:21:32 +00:00
|
|
|
namespace v2x
|
2021-10-27 21:56:21 +00:00
|
|
|
{
|
2021-11-10 15:21:32 +00:00
|
|
|
class V2XNode;
|
|
|
|
|
class CpmApplication : public Application
|
|
|
|
|
{
|
|
|
|
|
public:
|
|
|
|
|
CpmApplication(V2XNode *node, vanetza::Runtime &);
|
|
|
|
|
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 *);
|
2021-11-23 04:02:54 +00:00
|
|
|
void updateGenerationDeltaTime(int *, long long *);
|
2021-11-10 15:21:32 +00:00
|
|
|
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;
|
2021-11-14 04:26:18 +00:00
|
|
|
int shape_x;
|
|
|
|
|
int shape_y;
|
|
|
|
|
int shape_z;
|
2021-11-10 15:21:32 +00:00
|
|
|
int xDistance;
|
|
|
|
|
int yDistance;
|
2021-11-17 08:45:25 +00:00
|
|
|
int xSpeed;
|
|
|
|
|
int ySpeed;
|
2021-11-14 04:26:18 +00:00
|
|
|
int yawAngle;
|
2021-11-10 15:21:32 +00:00
|
|
|
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_;
|
|
|
|
|
|
|
|
|
|
double ego_x_;
|
|
|
|
|
double ego_y_;
|
|
|
|
|
double ego_lat_;
|
|
|
|
|
double ego_lon_;
|
|
|
|
|
double ego_altitude_;
|
|
|
|
|
double ego_heading_;
|
|
|
|
|
int generationDeltaTime_;
|
2021-11-23 04:02:54 +00:00
|
|
|
long long gdt_timestamp_;
|
2021-11-10 15:21:32 +00:00
|
|
|
|
|
|
|
|
bool updating_objects_stack_;
|
|
|
|
|
bool sending_;
|
2021-11-10 08:14:01 +00:00
|
|
|
};
|
2021-11-10 15:21:32 +00:00
|
|
|
}
|
2021-10-27 21:56:21 +00:00
|
|
|
|
|
|
|
|
#endif /* CPM_APPLICATION_HPP_EUIC2VFR */
|
