update function structure

Signed-off-by: meliketanrikulu <melike@leodrive.ai>

Author: meliketanrikulu

localization/autoware_pose_covariance_modifier_node/src/autoware_pose_covariance_modifier_node.cpp

@@ -49,49 +49,23 @@ AutowarePoseCovarianceModifierNode::AutowarePoseCovarianceModifierNode()
   }
 
 }
-bool AutowarePoseCovarianceModifierNode::checkTrustedPoseTimeout()
+void AutowarePoseCovarianceModifierNode::checkTrustedPoseTimeout()
 {
   auto timeDiff = this->now() - trustedPoseCallbackTime;
   if (timeDiff.seconds() > 1.0) {
-    return true;
-  }
-  return false;
-}
-void AutowarePoseCovarianceModifierNode::publishPoseSource()
-{
-  std_msgs::msg::String selected_pose_type;
-  switch (pose_source_) {
-    case static_cast<int>(AutowarePoseCovarianceModifierNode::PoseSource::GNSS):
-      selected_pose_type.data = "GNSS";
-      break;
-    case static_cast<int>(AutowarePoseCovarianceModifierNode::PoseSource::GNSS_NDT):
-      selected_pose_type.data = "GNSS + NDT";
-      break;
-    case static_cast<int>(AutowarePoseCovarianceModifierNode::PoseSource::NDT):
+      RCLCPP_WARN(this->get_logger(), "Trusted Pose Timeout");
+      pose_source_ = static_cast<int>(AutowarePoseCovarianceModifierNode::PoseSource::NDT);
+      std_msgs::msg::String selected_pose_type;
       selected_pose_type.data = "NDT";
-      break;
-    default:
-      selected_pose_type.data = "Unknown";
-      break;
+      pose_source_pub_->publish(selected_pose_type);
   }
-  pose_source_pub_->publish(selected_pose_type);
 }
+
 std::array<double, 36> AutowarePoseCovarianceModifierNode::ndt_covariance_modifier(
   std::array<double, 36> & in_ndt_covariance)
 {
   std::array<double, 36> ndt_covariance = in_ndt_covariance;
 
-  if (AutowarePoseCovarianceModifierNode::checkTrustedPoseTimeout()) {
-    RCLCPP_WARN(this->get_logger(), "Trusted Pose Timeout");
-    pose_source_ = static_cast<int>(AutowarePoseCovarianceModifierNode::PoseSource::NDT);
-    return ndt_covariance;
-  }
-
-  if (
-    trustedPose.pose_average_rmse_xy <= gnss_error_reliable_max_ &&
-    trustedPose.yaw_rmse_in_degrees < yaw_error_deg_threshold_) {
-    pose_source_ = static_cast<int>(AutowarePoseCovarianceModifierNode::PoseSource::GNSS);
-  } else if (trustedPose.pose_average_rmse_xy <= gnss_error_unreliable_min_) {
     /*
      * ndt_min_rmse_meters = in_ndt_rmse
      * ndt_max_rmse_meters = in_ndt_rmse  * 2
@@ -119,21 +93,15 @@ std::array<double, 36> AutowarePoseCovarianceModifierNode::ndt_covariance_modifi
     ndt_covariance[7] = std::max(ndt_covariance[7], in_ndt_covariance[7]);
     ndt_covariance[14] = std::max(ndt_covariance[14], in_ndt_covariance[14]);
 
-    pose_source_ = static_cast<int>(AutowarePoseCovarianceModifierNode::PoseSource::GNSS_NDT);
-  } else {
-    RCLCPP_INFO(this->get_logger(), "NDT input covariance values will be used ");
-    pose_source_ = static_cast<int>(AutowarePoseCovarianceModifierNode::PoseSource::NDT);
-  }
-
-  publishPoseSource();
-
   return ndt_covariance;
 }
 void AutowarePoseCovarianceModifierNode::ndt_pose_with_cov_callback(
   const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr & msg)
 {
+  AutowarePoseCovarianceModifierNode::checkTrustedPoseTimeout();
+
   std::array<double, 36> ndt_covariance_in_ = msg->pose.covariance;
-  std::array<double, 36> out_ndt_covariance_ = ndt_covariance_modifier(ndt_covariance_in_);
+  std::array<double, 36> out_ndt_covariance_ = pose_source_ == 1 ? ndt_covariance_modifier(ndt_covariance_in_) : ndt_covariance_in_ ;
 
   geometry_msgs::msg::PoseWithCovarianceStamped ndt_pose_with_covariance = *msg;
   ndt_pose_with_covariance.pose.covariance = out_ndt_covariance_;
@@ -152,23 +120,42 @@ void AutowarePoseCovarianceModifierNode::trusted_pose_with_cov_callback(
   trustedPoseCallbackTime = this->now();
   trusted_source_pose_with_cov = *msg;
 
-  trustedPose.pose_average_rmse_xy = (std::sqrt(trusted_source_pose_with_cov.pose.covariance[0]) +
+  double trusted_pose_average_rmse_xy;
+  double trusted_pose_yaw_rmse_in_degrees;
+    trusted_pose_average_rmse_xy= (std::sqrt(trusted_source_pose_with_cov.pose.covariance[0]) +
                                       std::sqrt(trusted_source_pose_with_cov.pose.covariance[7])) /
                                      2;
 
-  trustedPose.yaw_rmse_in_degrees =
+  trusted_pose_yaw_rmse_in_degrees =
     std::sqrt(trusted_source_pose_with_cov.pose.covariance[35]) * 180 / M_PI;
 
+  selectPositionSource(trusted_pose_average_rmse_xy, trusted_pose_yaw_rmse_in_degrees);
+
   if (pose_source_ != 2) {
     new_pose_estimator_pub_->publish(trusted_source_pose_with_cov);
     if(debug_){
         std_msgs::msg::Float32 out_gnss_rmse;
-        out_gnss_rmse.data = trustedPose.pose_average_rmse_xy;
+        out_gnss_rmse.data = trusted_pose_average_rmse_xy;
         out_gnss_position_rmse_pub_->publish(out_gnss_rmse);
     }
-
   }
 }
+
+void AutowarePoseCovarianceModifierNode::selectPositionSource(double trusted_pose_average_rmse_xy ,double trusted_pose_yaw_rmse_in_degrees){
+    std_msgs::msg::String selected_pose_type;
+    if (trusted_pose_average_rmse_xy <= gnss_error_reliable_max_ &&
+            trusted_pose_yaw_rmse_in_degrees < yaw_error_deg_threshold_) {
+        pose_source_ = static_cast<int>(AutowarePoseCovarianceModifierNode::PoseSource::GNSS);
+        selected_pose_type.data = "GNSS";
+    } else if (trusted_pose_average_rmse_xy <= gnss_error_unreliable_min_) {
+        pose_source_ = static_cast<int>(AutowarePoseCovarianceModifierNode::PoseSource::GNSS_NDT);
+        selected_pose_type.data = "GNSS + NDT";
+    } else {
+        pose_source_ = static_cast<int>(AutowarePoseCovarianceModifierNode::PoseSource::NDT);
+        selected_pose_type.data = "NDT";
+    }
+    pose_source_pub_->publish(selected_pose_type);
+}
 int main(int argc, char * argv[])
 {
   rclcpp::init(argc, argv);

localization/autoware_pose_covariance_modifier_node/src/include/autoware_pose_covariance_modifier_node.hpp

@@ -43,14 +43,11 @@ class AutowarePoseCovarianceModifierNode : public rclcpp::Node
   std::array<double, 36> ndt_covariance_modifier(std::array<double, 36> & in_ndt_covariance);
   geometry_msgs::msg::PoseWithCovarianceStamped trusted_source_pose_with_cov;
 
-  bool checkTrustedPoseTimeout();
+  void checkTrustedPoseTimeout();
 
 private:
-  struct trusted_pose_
-  {
-    double pose_average_rmse_xy = 0.0;
-    double yaw_rmse_in_degrees = 0.0;
-  } trustedPose;
+
+  void selectPositionSource(double trusted_pose_average_rmse_xy ,double trusted_pose_yaw_rmse_in_degrees);
 
   enum class PoseSource {
     GNSS = 0,