From e7f8b7a21066aed177b52145c086ea38a662ffdf Mon Sep 17 00:00:00 2001
From: Yu Asabe <yuasabe@g.ecc.u-tokyo.ac.jp>
Date: Sun, 14 Nov 2021 14:07:08 +0900
Subject: [PATCH] fix bugs

---
 src/cpm_application.cpp | 38 ++++++++++++++++----------------------
 1 file changed, 16 insertions(+), 22 deletions(-)

diff --git a/src/cpm_application.cpp b/src/cpm_application.cpp
index ff22f7b..8baedc6 100644
--- a/src/cpm_application.cpp
+++ b/src/cpm_application.cpp
@@ -118,14 +118,15 @@ namespace v2x
         object.position_y = y_mgrs + (sin(orientation) * x1 + cos(orientation) * y1);
         RCLCPP_INFO(node_->get_logger(), "cpm object: %f, %f, %f, %f", x1, y1, object.position_x, object.position_y);
         
-        object.shape_x = poc->list.array[i]->planarObjectDimension2->value / 100;
-        object.shape_y = poc->list.array[i]->planarObjectDimension1->value / 100;
-        object.shape_z = poc->list.array[i]->verticalObjectDimension->value / 100;
+        object.shape_x = poc->list.array[i]->planarObjectDimension2->value;
+        object.shape_y = poc->list.array[i]->planarObjectDimension1->value;
+        object.shape_z = poc->list.array[i]->verticalObjectDimension->value;
 
-        object.yawAngle = poc->list.array[i]->yawAngle->value / 10;
+        object.yawAngle = poc->list.array[i]->yawAngle->value;
+        double yaw_radian = (M_PI * object.yawAngle / 10) / 180;
 
         tf2::Quaternion quat;
-        quat.setRPY(0, 0, object.yawAngle);
+        quat.setRPY(0, 0, yaw_radian);
         object.orientation_x = quat.x();
         object.orientation_y = quat.y();
         object.orientation_z = quat.z();
@@ -209,7 +210,7 @@ namespace v2x
         double roll, pitch, yaw;
         matrix.getRPY(roll, pitch, yaw);
         // RCLCPP_INFO(node_->get_logger(), "yaw: %f", yaw);
-        object.yawAngle = (int) (yaw * 180 / M_PI) * 10 ;
+        object.yawAngle = (int) (yaw * 180 / M_PI) * 10 ; // 0 - 3600
 
         object.timeOfMeasurement = 100;
         objectsStack.push_back(object);
@@ -309,22 +310,13 @@ namespace v2x
         (*(pObj->planarObjectDimension2)).confidence = 1;
         (*(pObj->verticalObjectDimension)).value = object.shape_z;
         (*(pObj->verticalObjectDimension)).confidence = 1;
-        // *((int*)pObj->planarObjectDimension1->confidence) = 1;
-        // *((int*)pObj->planarObjectDimension2->value) = object.shape_x;
-        // *((int*)pObj->planarObjectDimension2->confidence) = 1;
-        // *((int*)pObj->verticalObjectDimension->value) = object.shape_z;
-        // *((int*)pObj->verticalObjectDimension->confidence) = 1;
-
-        // RCLCPP_INFO(node_->get_logger(), "%d", *((int*)pObj->planarObjectDimension1->value));
-        // RCLCPP_INFO(node_->get_logger(), "%d", *((int*)pObj->planarObjectDimension2->value));
-        // RCLCPP_INFO(node_->get_logger(), "%d", *((int*)pObj->verticalObjectDimension->value));
 
         pObj->yawAngle = vanetza::asn1::allocate<CartesianAngle>();
         (*(pObj->yawAngle)).value = object.yawAngle;
         (*(pObj->yawAngle)).confidence = 1;
 
         ASN_SEQUENCE_ADD(poc, pObj);
-        RCLCPP_INFO(node_->get_logger(), "ADD: %d, %d, %d, %d, %d, %d", pObj->objectID, pObj->timeOfMeasurement, pObj->xDistance.value, pObj->yDistance.value, pObj->xSpeed.value, pObj->ySpeed.value);
+        // RCLCPP_INFO(node_->get_logger(), "ADD: %d, %d, %d, %d, %d, %d", pObj->objectID, pObj->timeOfMeasurement, pObj->xDistance.value, pObj->yDistance.value, pObj->xSpeed.value, pObj->ySpeed.value);
       }
       Application::DownPacketPtr packet{new DownPacket()};
       std::unique_ptr<geonet::DownPacket> payload{new geonet::DownPacket()};
@@ -337,12 +329,14 @@ namespace v2x
       request.its_aid = aid::CP;
       request.transport_type = geonet::TransportType::SHB;
       request.communication_profile = geonet::CommunicationProfile::ITS_G5;
-      // RCLCPP_INFO(node->get_logger(), "Packet Size: %d", payload->size());
-      // RCLCPP_INFO(node->get_logger(), "Going to Application::request...");
-      Application::DataConfirm confirm = Application::request(request, std::move(payload), node_);
-      if (!confirm.accepted())
-      {
-        throw std::runtime_error("CPM application data request failed");
+
+      try {
+        Application::DataConfirm confirm = Application::request(request, std::move(payload), node_);
+        if (!confirm.accepted()) {
+          throw std::runtime_error("CPM application data request failed");
+        }
+      } catch (...) {
+        RCLCPP_INFO(node_->get_logger(), "Request Failed");
       }
     }
     sending_ = false;