TiagoRG
/
AutowareV2X
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

refactor

This commit is contained in:
Yu Asabe 2022-10-31 18:51:20 +09:00
parent 9c257a0e1e
commit b7339b7cbe
2 changed files with 13 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
include/autoware_v2x/v2x_node.hpp
View File

@ -25,17 +25,17 @@ namespace v2x
V2XApp *app; V2XApp *app;
void publishObjects(std::vector<CpmApplication::Object> *, int cpm_num); void publishObjects(std::vector<CpmApplication::Object> *, int cpm_num);
void publishCpmSenderObject(double, double, double); void publishCpmSenderObject(double, double, double);
std::ofstream latency_log_file; std::ofstream latency_log_file;
private: private:
void objectsCallback(const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg); void objectsCallback(const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg);
void tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr msg); void tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr msg);
rclcpp::Subscription<autoware_auto_perception_msgs::msg::PredictedObjects>::SharedPtr subscription_; rclcpp::Subscription<autoware_auto_perception_msgs::msg::PredictedObjects>::SharedPtr objects_sub_;
rclcpp::Subscription<tf2_msgs::msg::TFMessage>::SharedPtr subscription_pos_; rclcpp::Subscription<tf2_msgs::msg::TFMessage>::SharedPtr tf_sub_;
rclcpp::Publisher<autoware_auto_perception_msgs::msg::PredictedObjects>::SharedPtr publisher_; rclcpp::Publisher<autoware_auto_perception_msgs::msg::PredictedObjects>::SharedPtr cpm_objects_pub_;
rclcpp::Publisher<autoware_auto_perception_msgs::msg::PredictedObjects>::SharedPtr publisher_v2x_cpm_sender_; rclcpp::Publisher<autoware_auto_perception_msgs::msg::PredictedObjects>::SharedPtr cpm_sender_pub_;
double pos_lat_; double pos_lat_;
double pos_lon_; double pos_lon_;

37
src/v2x_node.cpp
View File

@ -32,48 +32,33 @@ namespace v2x
{ {
V2XNode::V2XNode(const rclcpp::NodeOptions &node_options) : rclcpp::Node("autoware_v2x_node", node_options) { V2XNode::V2XNode(const rclcpp::NodeOptions &node_options) : rclcpp::Node("autoware_v2x_node", node_options) {
using std::placeholders::_1; using std::placeholders::_1;
subscription_ = this->create_subscription<autoware_auto_perception_msgs::msg::PredictedObjects>("/perception/object_recognition/objects", 10, std::bind(&V2XNode::objectsCallback, this, _1));
subscription_pos_ = this->create_subscription<tf2_msgs::msg::TFMessage>("/tf", 10, std::bind(&V2XNode::tfCallback, this, _1)); objects_sub_ = this->create_subscription<autoware_auto_perception_msgs::msg::PredictedObjects>("/perception/object_recognition/objects", 10, std::bind(&V2XNode::objectsCallback, this, _1));
tf_sub_ = this->create_subscription<tf2_msgs::msg::TFMessage>("/tf", 10, std::bind(&V2XNode::tfCallback, this, _1));
publisher_ = create_publisher<autoware_auto_perception_msgs::msg::PredictedObjects>("/v2x/cpm/objects", rclcpp::QoS{10}); cpm_objects_pub_ = create_publisher<autoware_auto_perception_msgs::msg::PredictedObjects>("/v2x/cpm/objects", rclcpp::QoS{10});
publisher_v2x_cpm_sender_ = create_publisher<autoware_auto_perception_msgs::msg::PredictedObjects>("/v2x/cpm/sender", rclcpp::QoS{10}); // cpm_sender_pub_ = create_publisher<autoware_auto_perception_msgs::msg::PredictedObjects>("/v2x/cpm/sender", rclcpp::QoS{10});
// Declare Parameters
this->declare_parameter<std::string>("network_interface", "vmnet1"); this->declare_parameter<std::string>("network_interface", "vmnet1");
this->declare_parameter<bool>("is_sender", true); this->declare_parameter<bool>("is_sender", true);
// Launch V2XApp in a new thread
app = new V2XApp(this); app = new V2XApp(this);
boost::thread v2xApp(boost::bind(&V2XApp::start, app)); boost::thread v2xApp(boost::bind(&V2XApp::start, app));
RCLCPP_INFO(get_logger(), "V2X Node Launched"); RCLCPP_INFO(get_logger(), "V2X Node Launched");
//rclcpp::Time current_time = this->now(); // Make latency_log file from current timestamp
//RCLCPP_INFO(get_logger(), "[V2XNode::V2XNode] [measure] T_R1 %ld", current_time.nanoseconds());
time_t t = time(nullptr); time_t t = time(nullptr);
const tm* lt = localtime(&t); const tm* lt = localtime(&t);
std::stringstream s; std::stringstream s;
s<<"20"; s << "20" << lt->tm_year-100 <<"-" << lt->tm_mon+1 << "-" << lt->tm_mday << "_" << lt->tm_hour << ":" << lt->tm_min << ":" << lt->tm_sec;
s<<lt->tm_year-100; //100を引くことで20xxのxxの部分になる
s<<"-";
s<<lt->tm_mon+1; //月を0からカウントしているため
s<<"-";
s<<lt->tm_mday; //そのまま
s<<"_";
s<<lt->tm_hour;
s<<":";
s<<lt->tm_min;
s<<":";
s<<lt->tm_sec;
std::string timestamp = s.str(); std::string timestamp = s.str();
char cur_dir[1024]; char cur_dir[1024];
getcwd(cur_dir, 1024); getcwd(cur_dir, 1024);
std::string latency_log_filename = std::string(cur_dir) + "/latency_logs/latency_log_file_" + timestamp + ".csv"; std::string latency_log_filename = std::string(cur_dir) + "/latency_logs/latency_log_file_" + timestamp + ".csv";
latency_log_file.open(latency_log_filename, std::ios::out); latency_log_file.open(latency_log_filename, std::ios::out);
// latency_log_file << "test hello" << std::endl;
} }
void V2XNode::publishCpmSenderObject(double x_mgrs, double y_mgrs, double orientation) { void V2XNode::publishCpmSenderObject(double x_mgrs, double y_mgrs, double orientation) {
@ -168,13 +153,7 @@ namespace v2x
} }
void V2XNode::objectsCallback(const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg) { void V2XNode::objectsCallback(const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg) {
// rclcpp::Time current_time = this->now();
rclcpp::Time msg_time = msg->header.stamp; // timestamp included in the Autoware Perception Msg. rclcpp::Time msg_time = msg->header.stamp; // timestamp included in the Autoware Perception Msg.
// RCLCPP_INFO(get_logger(), "[V2XNode::objectsCallback] %d objects", msg->objects.size());
// Measuring T_A1R1
// RCLCPP_INFO(get_logger(), "[V2XNode::objectsCallback] [measure] T_obj %ld", msg_time.nanoseconds());
// RCLCPP_INFO(get_logger(), "[V2XNode::objectsCallback] [measure] T_rosmsg %ld", current_time.nanoseconds());
std::chrono::milliseconds ms = std::chrono::duration_cast<std::chrono::milliseconds> ( std::chrono::milliseconds ms = std::chrono::duration_cast<std::chrono::milliseconds> (
std::chrono::system_clock::now().time_since_epoch() std::chrono::system_clock::now().time_since_epoch()