add autoware pose covariance modifier node

Signed-off-by: meliketanrikulu <[email protected]>

launch/tier4_localization_launch/launch/localization.launch.xml

@@ -3,6 +3,7 @@
   <arg name="pose_source" description="A string consisting of ndt, yabloc, artag and eagleye joined by underscores no matter the order. e.g. ndt_yabloc"/>
   <arg name="twist_source"/>
   <arg name="system_run_mode"/>
+  <arg name="use_autoware_pose_covariance_modifier" default="false"/>
 
   <!-- parameter paths for ndt -->
   <arg name="ndt_scan_matcher/pointcloud_preprocessor/crop_box_filter_measurement_range_param_path"/>

launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml

@@ -113,4 +113,8 @@
       <arg name="input_pointcloud" value="$(var override_input_pointcloud)"/>
     </include>
   </group>
+
+  <group if="$(var use_autoware_pose_covariance_modifier)">
+    <include file="$(find-pkg-share autoware_pose_covariance_modifier_node)/launch/autoware_pose_covariance_modifier_node.launch.xml" />
+  </group>
 </launch>

launch/tier4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml

@@ -1,9 +1,13 @@
 <?xml version="1.0"?>
 <launch>
+
+  <let name="ekf_input_pose_with_cov_name" if="$(eval &quot;'$(var use_autoware_pose_covariance_modifier)'=='true'&quot;)" value="/localization/autoware_pose_covariance_modifier/pose_with_covariance"/>
+  <let name="ekf_input_pose_with_cov_name" if="$(eval &quot;'$(var use_autoware_pose_covariance_modifier)'=='false'&quot;)" value="/localization/pose_estimator/pose_with_covariance"/>
+
   <group>
     <include file="$(find-pkg-share ekf_localizer)/launch/ekf_localizer.launch.xml">
       <arg name="input_initial_pose_name" value="/initialpose3d"/>
-      <arg name="input_pose_with_cov_name" value="/localization/pose_estimator/pose_with_covariance"/>
+      <arg name="input_pose_with_cov_name" value="$(var ekf_input_pose_with_cov_name)"/>
       <arg name="input_twist_with_cov_name" value="/localization/twist_estimator/twist_with_covariance"/>
       <arg name="output_odom_name" value="kinematic_state"/>
       <arg name="output_pose_name" value="pose"/>

localization/autoware_pose_covariance_modifier_node/CMakeLists.txt

@@ -0,0 +1,22 @@
+cmake_minimum_required(VERSION 3.14)
+project(autoware_pose_covariance_modifier_node)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+find_package(rclcpp REQUIRED)
+find_package(geometry_msgs REQUIRED)
+set(NODE_NAME ${PROJECT_NAME})
+set(EXEC_NAME ${PROJECT_NAME}_exe)
+
+add_executable(${PROJECT_NAME}  src/autoware_pose_covariance_modifier_node.cpp)
+
+ament_target_dependencies(${PROJECT_NAME} rclcpp geometry_msgs rcl_interfaces)
+
+install(TARGETS
+        ${PROJECT_NAME}
+        DESTINATION lib/${PROJECT_NAME}
+)
+include_directories(src/include/)
+
+ament_auto_package(INSTALL_TO_SHARE
+        launch)

localization/autoware_pose_covariance_modifier_node/README.md

@@ -0,0 +1 @@
+# autoware_pose_covariance_modifier

localization/autoware_pose_covariance_modifier_node/launch/autoware_pose_covariance_modifier_node.launch.xml

@@ -0,0 +1,12 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+  <arg name="input_trusted_pose_with_cov_topic" default="/sensing/gnss/pose_with_covariance" description="Trusted pose input topic name. "/>
+  <arg name="input_ndt_pose_with_cov_topic" default="/localization/pose_estimator/pose_with_covariance" description="NDT output pose input topic name. "/>
+  <arg name="output_pose_with_covariance_topic" default="/localization/autoware_pose_covariance_modifier/pose_with_covariance" description="Estimated self position with covariance"/>
+
+  <node pkg="autoware_pose_covariance_modifier_node" exec="autoware_pose_covariance_modifier_node" name="autoware_pose_covariance_modifier_node" output="screen">
+    <remap from="input_trusted_pose_with_cov_topic" to="$(var input_trusted_pose_with_cov_topic)"/>
+    <remap from="input_ndt_pose_with_cov_topic" to="$(var input_ndt_pose_with_cov_topic)"/>
+    <remap from="output_pose_with_covariance_topic" to="$(var output_pose_with_covariance_topic)"/>
+  </node>
+</launch>

localization/autoware_pose_covariance_modifier_node/package.xml

@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>autoware_pose_covariance_modifier_node</name>
+  <version>1.0.0</version>
+  <description>Converts an autoware_msgs message to autoware_auto_msgs version and publishes it.</description>
+
+  <maintainer email="[email protected]">Melike Tanrikulu</maintainer>
+
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>autoware_cmake</buildtool_depend>
+
+  <build_depend>rosidl_default_generators</build_depend>
+
+  <depend>geometry_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

localization/autoware_pose_covariance_modifier_node/src/autoware_pose_covariance_modifier_node.cpp

@@ -0,0 +1,166 @@
+// Copyright 2024 The Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "include/autoware_pose_covariance_modifier_node.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+
+AutowarePoseCovarianceModifierNode::AutowarePoseCovarianceModifierNode() : Node("AutowarePoseCovarianceModifierNode")
+{
+  trusted_pose_with_cov_sub_ =
+    this->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
+      "input_trusted_pose_with_cov_topic", 10000,
+      std::bind(
+        &AutowarePoseCovarianceModifierNode::trusted_pose_with_cov_callback, this,
+        std::placeholders::_1));
+
+  ndt_pose_with_cov_sub_ =
+          this->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
+                  "input_ndt_pose_with_cov_topic", 10000,
+                  std::bind(
+                          &AutowarePoseCovarianceModifierNode::ndt_pose_with_cov_callback, this,
+                          std::placeholders::_1));
+  new_pose_estimator_pub_ =
+          this->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>(
+                "output_pose_with_covariance_topic", 10);
+
+  pose_source_pub_ =
+          this->create_publisher<std_msgs::msg::String>(
+                  "autoware_pose_covariance_modifier/selected_pose_type",10);
+}
+bool 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):
+            selected_pose_type.data =  "NDT";
+            break;
+        default:
+            selected_pose_type.data =  "Unknown";
+            break;
+    }
+    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 <= 0.10 &&
+            trustedPose.yaw_rmse < std::sqrt(in_ndt_covariance[35])) {
+        pose_source_ = static_cast<int>(AutowarePoseCovarianceModifierNode::PoseSource::GNSS);
+    } else if (trustedPose.pose_average_rmse_xy <= 0.10) {
+        ndt_covariance[0] = 1000000;
+        ndt_covariance[7] = 1000000;
+        pose_source_ = static_cast<int>(AutowarePoseCovarianceModifierNode::PoseSource::GNSS_NDT);
+    } else if (trustedPose.pose_average_rmse_xy <= 0.25) {
+        /*
+         * ndt_min_rmse_meters = in_ndt_rmse
+         * ndt_max_rmse_meters = in_ndt_rmse  * 2
+         * ndt_rmse = ndt_max_rmse_meters - (gnss_rmse * (ndt_max_rmse_meters - ndt_min_rmse_meters) / normalization_coeff)
+         */
+        double normalization_coeff = 0.1;
+        ndt_covariance[0] = std::pow(
+                ((std::sqrt(in_ndt_covariance[0]) * 2) - std::sqrt(trusted_source_pose_with_cov.pose.covariance[0])) *
+                (std::sqrt(in_ndt_covariance[0])) / normalization_coeff,
+                2);
+        ndt_covariance[7] = std::pow(
+                ((std::sqrt(in_ndt_covariance[7]) * 2) - std::sqrt(trusted_source_pose_with_cov.pose.covariance[7])) *
+                (std::sqrt(in_ndt_covariance[7])) / normalization_coeff,
+                2);
+        ndt_covariance[14] = std::pow(
+                ((std::sqrt(in_ndt_covariance[14]) * 2) -
+                 std::sqrt(trusted_source_pose_with_cov.pose.covariance[14])) *
+                (std::sqrt(in_ndt_covariance[14])) / normalization_coeff,
+                2);
+
+        ndt_covariance[0]  = std::max(ndt_covariance[0], in_ndt_covariance[0]);
+        ndt_covariance[7]  = std::max(ndt_covariance[7], in_ndt_covariance[7]);
+        ndt_covariance[14] = std::max(ndt_covariance[14], in_ndt_covariance[14]);
+
+        if (trustedPose.yaw_rmse <= std::sqrt(in_ndt_covariance[35])) {
+            ndt_covariance[35] = 1000000;
+        }
+        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){
+
+  std::array<double, 36> ndt_covariance_in_  = msg->pose.covariance;
+  std::array<double, 36> out_ndt_covariance_ = ndt_covariance_modifier(ndt_covariance_in_);
+
+  geometry_msgs::msg::PoseWithCovarianceStamped ndt_pose_with_covariance = *msg;
+  ndt_pose_with_covariance.pose.covariance = out_ndt_covariance_;
+  if(pose_source_ != 0){
+      new_pose_estimator_pub_->publish(ndt_pose_with_covariance);
+  }
+}
+void AutowarePoseCovarianceModifierNode::trusted_pose_with_cov_callback(
+  const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr & msg)
+{
+  trustedPoseCallbackTime = this->now();
+  trusted_source_pose_with_cov = *msg;
+
+  trustedPose.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 =
+  std::sqrt(trusted_source_pose_with_cov.pose.covariance[35]);
+
+  double yaw_rmse_in_degrees =
+          trustedPose.yaw_rmse * 180 / M_PI;
+
+  geometry_msgs::msg::PoseWithCovarianceStamped out_trusted_pose_with_cov = trusted_source_pose_with_cov;
+
+    if (trustedPose.pose_average_rmse_xy < 0.25 && yaw_rmse_in_degrees >= 0.3) {
+        out_trusted_pose_with_cov.pose.covariance[35] = 1000000;
+    }
+  if(pose_source_ != 2) {
+      new_pose_estimator_pub_->publish(out_trusted_pose_with_cov);
+  }
+}
+int main(int argc, char * argv[])
+{
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<AutowarePoseCovarianceModifierNode>());
+  rclcpp::shutdown();
+  return 0;
+}

localization/autoware_pose_covariance_modifier_node/src/include/autoware_pose_covariance_modifier_node.hpp

@@ -0,0 +1,64 @@
+// Copyright 2024 The Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+#ifndef AUTOWARE_POSE_COVARIANCE_MODIFIER_NODE_HPP_
+#define AUTOWARE_POSE_COVARIANCE_MODIFIER_NODE_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
+#include <std_msgs/msg/string.hpp>
+#include <string>
+
+class AutowarePoseCovarianceModifierNode : public rclcpp::Node
+{
+public:
+    AutowarePoseCovarianceModifierNode();
+
+  rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
+    trusted_pose_with_cov_sub_;
+  rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
+    ndt_pose_with_cov_sub_;
+  rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
+    new_pose_estimator_pub_;
+  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr
+    pose_source_pub_;
+
+  void trusted_pose_with_cov_callback(
+    const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr & msg);
+  void ndt_pose_with_cov_callback(
+    const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr & msg);
+  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();
+
+private:
+
+  struct trusted_pose_
+  {
+    double pose_average_rmse_xy = 0.0;
+    double yaw_rmse = 0.0;
+  } trustedPose;
+
+  enum class PoseSource{
+    GNSS = 0,
+    GNSS_NDT = 1,
+    NDT = 2,
+  };
+ void publishPoseSource();
+  int pose_source_;
+  rclcpp::Time trustedPoseCallbackTime;
+};
+
+#endif  // AUTOWARE_POSE_COVARIANCE_MODIFIER_NODE_HPP_