From 60eea65de3414f996a4e7043b4be58424d49166c Mon Sep 17 00:00:00 2001
From: Yu Asabe <yuasabe@g.ecc.u-tokyo.ac.jp>
Date: Sun, 27 Feb 2022 12:26:11 +0900
Subject: [PATCH] Add is_sender

---
 launch/v2x.launch.xml |  2 ++
 src/v2x_app.cpp       | 18 ++++--------------
 src/v2x_node.cpp      |  9 +++++----
 3 files changed, 11 insertions(+), 18 deletions(-)

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 @@
 <launch>
   <arg name="network_interface" default="wlp5s0"/>
+  <arg name="is_sender" default="true"/>
   <node pkg="autoware_v2x" exec="autoware_v2x_node" namespace="v2x" output="screen">
     <param name="network_interface" value="$(var network_interface)"/>
+    <param name="is_sender" value="$(var is_sender)"/>
   </node>
 </launch>
\ 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<autoware_perception_msgs::msg::DynamicObjectArray>("/v2x/cpm/objects", rclcpp::QoS{10});
 
     this->declare_parameter<std::string>("network_interface", "vmnet1");
+    this->declare_parameter<bool>("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);
   }