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

Add measurement loggings

This commit is contained in:
Yu Asabe 2022-01-15 16:14:17 +09:00
parent 64064a9a75
commit b631397fa2
3 changed files with 39 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
src/cpm_application.cpp
View File

@ -70,12 +70,15 @@ namespace v2x
return btp::ports::CPM;
}
void CpmApplication::indicate(const DataIndication &indication, UpPacketPtr packet)
{
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::Time current_time = node_->now();
RCLCPP_INFO(node_->get_logger(), "[CpmApplication::indicate] [measure] T_receive_r2 %ld", current_time.nanoseconds());
asn1::Cpm message = *cpm;
ItsPduHeader_t &header = message->header;
@ -172,8 +175,7 @@ namespace v2x
void CpmApplication::updateGenerationDeltaTime(int *gdt, long long *gdt_timestamp)
{
generationDeltaTime_ = *gdt;
gdt_timestamp_ = *gdt_timestamp;
// RCLCPP_INFO(node_->get_logger(), "[updateGDT] %d %lu", generationDeltaTime_, gdt_timestamp_);
gdt_timestamp_ = *gdt_timestamp; // ETSI-epoch milliseconds timestamp
}
void CpmApplication::updateHeading(double *yaw)
@ -230,10 +232,15 @@ namespace v2x
object.yawAngle = std::lround((yaw * 180.0 / M_PI) * 10.0); // 0 - 3600
}
long long timestamp = msg->header.stamp.sec * 1e9 + msg->header.stamp.nanosec;
object.timeOfMeasurement = (gdt_timestamp_ - timestamp) / 1e6;
long long msg_timestamp_sec = msg->header.stamp.sec;
long long msg_timestamp_nsec = msg->header.stamp.nanosec;
msg_timestamp_sec -= 1072915200; // convert to etsi-epoch
long long msg_timestamp_msec = msg_timestamp_sec * 1000 + msg_timestamp_nsec / 1000000;
// long long timestamp = msg->header.stamp.sec * 1e9 + msg->header.stamp.nanosec;
object.timeOfMeasurement = gdt_timestamp_ - msg_timestamp_msec;
// RCLCPP_INFO(node_->get_logger(), "[updateObjectsStack] %ld %ld %d", gdt_timestamp_, timestamp, object.timeOfMeasurement);
if (object.timeOfMeasurement < -1500 || object.timeOfMeasurement > 1500) {
RCLCPP_INFO(node_->get_logger(), "[updateObjectsStack] timeOfMeasurement out of bounds: %d", object.timeOfMeasurement);
continue;
}
objectsStack.push_back(object);
@ -248,8 +255,7 @@ namespace v2x
updating_objects_stack_ = false;
}
void CpmApplication::send()
{
void CpmApplication::send() {
sending_ = true;
RCLCPP_INFO(node_->get_logger(), "[SEND] Sending CPM...");
@ -377,5 +383,7 @@ namespace v2x
// }
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());
}
}

5
src/v2x_app.cpp
View File

@ -40,6 +40,9 @@ namespace v2x
void V2XApp::objectsCallback(const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr msg) {
// RCLCPP_INFO(node_->get_logger(), "V2XApp: msg received");
if (!tf_received_) {
RCLCPP_WARN(node_->get_logger(), "[V2XApp::objectsCallback] tf not received yet");
}
if (tf_received_ && cp_started_) {
cp->updateObjectsStack(msg);
}
@ -62,7 +65,7 @@ namespace v2x
// long long gdt_timestamp_msec_etsi_epoch = gdt_timestamp_msec - 1072915200000;
// int gdt = gdt_timestamp_msec_etsi_epoch % 65536; // milliseconds
int gdt = timestamp_msec % 65536;
RCLCPP_INFO(node_->get_logger(), "[tfCallback] %d,%lld,%lld,%lld", gdt, timestamp_msec, timestamp_sec, timestamp_nsec);
// RCLCPP_INFO(node_->get_logger(), "[tfCallback] %d,%lld,%lld,%lld", gdt, timestamp_msec, timestamp_sec, timestamp_nsec);
double rot_x = msg->transforms[0].transform.rotation.x;
double rot_y = msg->transforms[0].transform.rotation.y;

36
src/v2x_node.cpp
View File

@ -25,14 +25,13 @@ using namespace std::chrono;
namespace v2x
{
V2XNode::V2XNode(const rclcpp::NodeOptions &node_options) : rclcpp::Node("autoware_v2x_node", node_options), received_time(this->now()), cpm_received(false)
{
V2XNode::V2XNode(const rclcpp::NodeOptions &node_options) : rclcpp::Node("autoware_v2x_node", node_options) {
using std::placeholders::_1;
subscription_ = this->create_subscription<autoware_perception_msgs::msg::DynamicObjectArray>("/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));
publisher_ = create_publisher<autoware_perception_msgs::msg::DynamicObjectArray>("/perception/object_recognition/objects", rclcpp::QoS{10});
publisher_ = create_publisher<autoware_perception_msgs::msg::DynamicObjectArray>("/v2x/cpm/objects", rclcpp::QoS{10});
this->declare_parameter<std::string>("network_interface", "vmnet1");
@ -40,13 +39,16 @@ namespace v2x
boost::thread v2xApp(boost::bind(&V2XApp::start, app));
RCLCPP_INFO(get_logger(), "V2X Node Launched");
rclcpp::Time current_time = this->now();
RCLCPP_INFO(get_logger(), "[V2XNode::V2XNode] [measure] T_R1 %ld", current_time.nanoseconds());
}
void V2XNode::publishObjects(std::vector<CpmApplication::Object> *objectsStack) {
autoware_perception_msgs::msg::DynamicObjectArray output_dynamic_object_msg;
std_msgs::msg::Header header;
rclcpp::Time current_time = this->now();
received_time = current_time;
output_dynamic_object_msg.header.frame_id = "map";
output_dynamic_object_msg.header.stamp = current_time;
@ -79,27 +81,25 @@ namespace v2x
output_dynamic_object_msg.objects.push_back(object);
}
cpm_received = true;
current_time = this->now();
RCLCPP_INFO(get_logger(), "[V2XNode::publishObjects] [measure] T_obj_r2 %ld", current_time.nanoseconds());
publisher_->publish(output_dynamic_object_msg);
}
void V2XNode::objectsCallback(const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr msg)
{
void V2XNode::objectsCallback(const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr msg) {
rclcpp::Time current_time = this->now();
// RCLCPP_INFO(get_logger(), "%ld", current_time.nanoseconds() - received_time.nanoseconds());
if (cpm_received) {
// if time difference is more than 10ms
if (current_time - received_time > rclcpp::Duration(std::chrono::nanoseconds(10000000))) {
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_obj_receive %ld", current_time.nanoseconds());
app->objectsCallback(msg);
}
cpm_received = false;
} else {
app->objectsCallback(msg);
}
}
void V2XNode::tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr msg)
{
void V2XNode::tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr msg) {
app->tfCallback(msg);
}
}