diff --git a/launch/v2x.launch.xml b/launch/v2x.launch.xml index 636e94d..30124ae 100644 --- a/launch/v2x.launch.xml +++ b/launch/v2x.launch.xml @@ -1,6 +1,8 @@ + + \ No newline at end of file diff --git a/src/v2x_app.cpp b/src/v2x_app.cpp index e8a25c1..a56db42 100644 --- a/src/v2x_app.cpp +++ b/src/v2x_app.cpp @@ -61,7 +61,6 @@ namespace v2x timestamp_sec -= 1072915200; // convert to etsi-epoch long long timestamp_msec = timestamp_sec * 1000 + timestamp_nsec / 1000000; int gdt = timestamp_msec % 65536; - // 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; @@ -74,21 +73,12 @@ namespace v2x double roll, pitch, yaw; matrix.getRPY(roll, pitch, yaw); - char mgrs[30]; int zone = 54; int grid_num_x = 4; int grid_num_y = 39; int grid_size = 100000; - int prec; bool northp = true; - double x_mgrs, y_mgrs; - double lat, lon; - // sprintf(mgrs, "54SVE%.3f%.3f", x, y); - // RCLCPP_INFO(node_->get_logger(), "%s", mgrs); - - // Convert MGRS coordinate to UTM coordinate. - // GeographicLib::MGRS::Reverse(mgrs, zone, northp, x_mgrs, y_mgrs, prec); // Reverse projection from UTM to geographic. GeographicLib::UTMUPS::Reverse( @@ -99,9 +89,7 @@ namespace v2x lat, lon ); - - RCLCPP_INFO(node_->get_logger(), "%f, %f", lat, lon); - + if (cp && cp_started_) { cp->updateMGRS(&x, &y); cp->updateRP(&lat, &lon, &z); @@ -141,7 +129,9 @@ namespace v2x context.set_link_layer(link_layer.get()); - cp = new CpmApplication(node_, trigger.runtime()); + bool is_sender; + node_->get_parameter("is_sender", is_sender); + cp = new CpmApplication(node_, trigger.runtime(), is_sender); context.enable(cp); diff --git a/src/v2x_node.cpp b/src/v2x_node.cpp index 31dbcd9..7e4dd0e 100644 --- a/src/v2x_node.cpp +++ b/src/v2x_node.cpp @@ -34,6 +34,7 @@ namespace v2x publisher_ = create_publisher("/v2x/cpm/objects", rclcpp::QoS{10}); this->declare_parameter("network_interface", "vmnet1"); + this->declare_parameter("is_sender", true); app = new V2XApp(this); boost::thread v2xApp(boost::bind(&V2XApp::start, app)); @@ -83,7 +84,7 @@ namespace v2x } current_time = this->now(); - RCLCPP_INFO(get_logger(), "[V2XNode::publishObjects] [measure] T_obj_r2 %ld", current_time.nanoseconds()); + // RCLCPP_INFO(get_logger(), "[V2XNode::publishObjects] [measure] T_obj_r2 %ld", current_time.nanoseconds()); publisher_->publish(output_dynamic_object_msg); } @@ -91,11 +92,11 @@ namespace v2x void V2XNode::objectsCallback(const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr msg) { rclcpp::Time current_time = this->now(); 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()); + // 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()); + // 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); }