feat(aw_pose_covariance_modifier_node): add aw_pose_covariance_modifier for using NDT and GNSS together

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

@@ -7,6 +7,9 @@
   <!-- When gnss_enabled is false, automatic_pose_initializer will not run, only manual initial position estimation is available. -->
   <arg name="gnss_enabled" default="true" description="gnss availability for initial position estimation"/>
 
+  <!-- When use_pose_covariance_modifier is true, gnss and NDT poses will be used together in localization-->
+  <arg name="use_pose_covariance_modifier" default="true" description="parameter for using gnss and NDT together with covariance modifier"/>
+
   <!-- split string with underscores -->
   <let name="available_args" value="[\'ndt\',\'yabloc\',\'eagleye\',\'artag\']"/>
   <let name="split_function" value="list(set('$(var pose_source)'.split('_')).intersection($(var available_args)))"/>
@@ -80,6 +83,11 @@
     </include>
   </group>
 
+  <!-- AW Pose Covariance Modifier Node -->
+  <group if="$(var use_pose_covariance_modifier)">
+    <include file="$(find-pkg-share aw_pose_covariance_modifier_node)/launch/aw_pose_covariance_modifier_node.launch.xml"/>
+  </group>
+
   <!-- Util Launch -->
   <group>
     <push-ros-namespace namespace="util"/>

localization/aw_pose_covariance_modifier_node/CMakeLists.txt

@@ -0,0 +1,32 @@
+cmake_minimum_required(VERSION 3.14)
+project(aw_pose_covariance_modifier_node)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+find_package(rclcpp REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(rcl_interfaces REQUIRED)
+set(NODE_NAME ${PROJECT_NAME})
+set(EXEC_NAME ${PROJECT_NAME}_exe)
+
+add_executable(${PROJECT_NAME}  src/aw_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_add_library(${NODE_NAME}
+#  src/include/aw_pose_covariance_modifier_node.hpp
+#  src/aw_pose_covariance_modifier_node.cpp)
+
+#rclcpp_components_register_node(${NODE_NAME}
+#  PLUGIN "aw_pose_covariance_modifier_node::AWPoseCovarianceModifierNode"
+#  EXECUTABLE ${EXEC_NAME})
+
+
+ament_auto_package(INSTALL_TO_SHARE
+        launch)

localization/aw_pose_covariance_modifier_node/README.md

@@ -0,0 +1 @@
+# aw_pose_covariance_modifier

localization/aw_pose_covariance_modifier_node/launch/aw_pose_covariance_modifier_node.launch.xml

@@ -0,0 +1,10 @@
+<?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 .. "/>
+  <arg name="output_pose_with_covariance_topic" default="/localization/pose_estimator/pose_with_covariance" description="Estimated self position with covariance"/>
+
+  <node pkg="aw_pose_covariance_modifier_node" exec="aw_pose_covariance_modifier_node" name="aw_pose_covariance_modifier_node" output="screen">
+    <remap from="input_trusted_pose_with_cov_topic" to="$(var input_trusted_pose_with_cov_topic)"/>
+    <remap from="output_pose_with_covariance_topic" to="$(var output_pose_with_covariance_topic)"/>
+  </node>
+</launch>

localization/aw_pose_covariance_modifier_node/package.xml

@@ -0,0 +1,25 @@
+<?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>aw_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="melike@leodrive.ai">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>rcl_interfaces</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

localization/aw_pose_covariance_modifier_node/src/aw_pose_covariance_modifier_node.cpp

@@ -0,0 +1,112 @@
+// 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/aw_pose_covariance_modifier_node.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+
+AWPoseCovarianceModifierNode::AWPoseCovarianceModifierNode() : Node("AWPoseCovarianceModifierNode")
+{
+  trusted_pose_with_cov_sub_ =
+    this->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
+      "input_trusted_pose_with_cov_topic", 10000,
+      std::bind(
+        &AWPoseCovarianceModifierNode::trusted_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);
+
+  client_ = this->create_client<rcl_interfaces::srv::SetParameters>(
+    "/localization/pose_estimator/ndt_scan_matcher/set_parameters");
+
+  while (!client_->wait_for_service(std::chrono::seconds(1)) && rclcpp::ok()) {
+    RCLCPP_INFO(this->get_logger(), "Waiting for aw_pose_covariance_modifier_node service...");
+  }
+
+  activateNDTCovModifier = AWPoseCovarianceModifierNode::callNDTCovarianceModifier();
+  if (activateNDTCovModifier == 1) {
+    RCLCPP_INFO(get_logger(), "NDT pose covariance modifier activated ...");
+  } else {
+    RCLCPP_WARN(get_logger(), "Failed to enable NDT pose covariance modifier ...");
+  }
+}
+
+bool AWPoseCovarianceModifierNode::callNDTCovarianceModifier()
+{
+  auto request = std::make_shared<rcl_interfaces::srv::SetParameters::Request>();
+
+  rcl_interfaces::msg::Parameter parameter;
+  parameter.name = "aw_pose_covariance_modifier.enable";
+  parameter.value.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL;
+  parameter.value.bool_value = true;
+
+  request->parameters.push_back(parameter);
+
+  client_->async_send_request(
+    request, [this](rclcpp::Client<rcl_interfaces::srv::SetParameters>::SharedFuture result_) {
+      auto result_status = result_.wait_for(std::chrono::seconds(1));
+
+      if (result_status == std::future_status::ready) {
+        auto response = result_.get();
+
+        if (response && response->results.data()) {
+          RCLCPP_INFO(
+            this->get_logger(), "aw_pose_covariance_modifier.enable parameter set successfully.");
+          return true;
+        } else {
+          RCLCPP_ERROR(
+            this->get_logger(),
+            "An error occurred while setting the aw_pose_covariance_modifier.enable parameter.");
+          return false;
+        }
+      } else {
+        RCLCPP_ERROR(
+          this->get_logger(), "The request was not completed within the specified time.");
+        return false;
+      }
+    });
+  return true;
+}
+
+void AWPoseCovarianceModifierNode::trusted_pose_with_cov_callback(
+  const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr & msg)
+{
+  geometry_msgs::msg::PoseWithCovarianceStamped pose_estimator_pose = *msg;
+
+  trusted_pose_rmse_ = (std::sqrt(pose_estimator_pose.pose.covariance[0]) +
+                        std::sqrt(pose_estimator_pose.pose.covariance[7])) /
+                       2;
+  trusted_pose_yaw_rmse_in_degrees_ =
+    std::sqrt(pose_estimator_pose.pose.covariance[35]) * 180 / M_PI;
+
+  if (trusted_pose_rmse_ > 0.25) {
+    RCLCPP_INFO(
+      this->get_logger(),
+      "Trusted Pose RMSE is under the threshold. It will not be used as a pose source.");
+  } else {
+    if (trusted_pose_yaw_rmse_in_degrees_ >= 0.3) {
+      pose_estimator_pose.pose.covariance[35] = 1000000;
+    }
+
+    new_pose_estimator_pub_->publish(pose_estimator_pose);
+  }
+}
+int main(int argc, char * argv[])
+{
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<AWPoseCovarianceModifierNode>());
+  rclcpp::shutdown();
+  return 0;
+}

localization/aw_pose_covariance_modifier_node/src/include/aw_pose_covariance_modifier_node.hpp

@@ -0,0 +1,45 @@
+// 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 AW_POSE_COVARIANCE_MODIFIER_NODE_HPP_
+#define AW_POSE_COVARIANCE_MODIFIER_NODE_HPP_
+
+#include <rcl_interfaces/srv/set_parameters.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
+
+#include <string>
+
+class AWPoseCovarianceModifierNode : public rclcpp::Node
+{
+public:
+  AWPoseCovarianceModifierNode();
+
+  rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
+    trusted_pose_with_cov_sub_;
+  rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
+    new_pose_estimator_pub_;
+  rclcpp::Client<rcl_interfaces::srv::SetParameters>::SharedPtr client_;
+
+  void trusted_pose_with_cov_callback(
+    const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr & msg);
+  bool callNDTCovarianceModifier();
+
+private:
+  double trusted_pose_rmse_;
+  double trusted_pose_yaw_rmse_in_degrees_;
+  bool activateNDTCovModifier = 0;
+};
+
+#endif  // AW_POSE_COVARIANCE_MODIFIER_NODE_HPP_

localization/ndt_scan_matcher/CMakeLists.txt

@@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.14)
 project(ndt_scan_matcher)
 
 find_package(autoware_cmake REQUIRED)
+find_package(rcl_interfaces REQUIRED)
 autoware_package()
 
 # Compile flags for SIMD instructions

localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp

@@ -20,6 +20,7 @@
 #include "localization_util/smart_pose_buffer.hpp"
 #include "ndt_scan_matcher/hyper_parameters.hpp"
 #include "ndt_scan_matcher/map_update_module.hpp"
+#include "rcl_interfaces/srv/set_parameters.hpp"
 
 #include <rclcpp/rclcpp.hpp>
 #include <tier4_autoware_utils/ros/logger_level_configure.hpp>
@@ -83,14 +84,18 @@ class NDTScanMatcher : public rclcpp::Node
   void service_trigger_node(
     const std_srvs::srv::SetBool::Request::SharedPtr req,
     std_srvs::srv::SetBool::Response::SharedPtr res);
-
+  void setParametersCallback(
+    const rcl_interfaces::srv::SetParameters::Request::SharedPtr request,
+    rcl_interfaces::srv::SetParameters::Response::SharedPtr response);
   void callback_timer();
   void callback_sensor_points(
     sensor_msgs::msg::PointCloud2::ConstSharedPtr sensor_points_msg_in_sensor_frame);
   void callback_initial_pose(
     geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr initial_pose_msg_ptr);
   void callback_regularization_pose(
     geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr pose_conv_msg_ptr);
+  void callback_trusted_source_pose(
+    geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr pose_conv_msg_ptr);
 
   geometry_msgs::msg::PoseWithCovarianceStamped align_pose(
     const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_with_cov);
@@ -138,6 +143,8 @@ class NDTScanMatcher : public rclcpp::Node
   rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr sensor_points_sub_;
   rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
     regularization_pose_sub_;
+  rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
+    trusted_source_pose_sub_;
 
   rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr sensor_aligned_pose_pub_;
   rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr no_ground_points_aligned_pose_pub_;
@@ -172,6 +179,7 @@ class NDTScanMatcher : public rclcpp::Node
 
   rclcpp::Service<tier4_localization_msgs::srv::PoseWithCovarianceStamped>::SharedPtr service_;
   rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr service_trigger_node_;
+  rclcpp::Service<rcl_interfaces::srv::SetParameters>::SharedPtr set_parameters_service_;
 
   tf2_ros::TransformBroadcaster tf2_broadcaster_;
   tf2_ros::Buffer tf2_buffer_;
@@ -193,10 +201,27 @@ class NDTScanMatcher : public rclcpp::Node
 
   std::unique_ptr<SmartPoseBuffer> regularization_pose_buffer_;
 
+  geometry_msgs::msg::PoseWithCovarianceStamped trusted_source_pose_;
+  bool activate_pose_covariance_modifier_;
+  bool close_ndt_pose_source_ = false;
+  struct trusted_pose_
+  {
+    double pose_average_rmse_xy = 0.0;
+    double yaw_rmse = 0.0;
+  } trustedPose;
+
   std::atomic<bool> is_activated_;
   std::unique_ptr<MapUpdateModule> map_update_module_;
   std::unique_ptr<tier4_autoware_utils::LoggerLevelConfigure> logger_configure_;
 
+  std::array<double, 36> covariance_modifier(std::array<double, 36> & in_ndt_covariance);
+
+  // To be used for AW Pose Covariance Modifier - Trusted Pose timeout control
+  bool checkTrustedPoseTimeout();
+  rclcpp::Time trustedPoseCallbackTime;
+  std::mutex mutex_cov_modifier_;
+  void createTrustedSourcePoseSubscriber();
+
   HyperParameters param_;
 };
 

localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml

@@ -6,6 +6,7 @@
   <arg name="input_initial_pose_topic" default="ekf_pose_with_covariance" description="Initial position topic to align"/>
   <arg name="input_regularization_pose_topic" default="regularization_pose_with_covariance" description="Regularization pose topic"/>
   <arg name="input_service_trigger_node" default="trigger_node" description="Trigger node service name"/>
+  <arg name="input_covariance_modifier_trusted_pose_topic" default="/sensing/gnss/pose_with_covariance" description="AW Pose cov modifier trusted PoseWithCov topic"/>
 
   <arg name="output_pose_topic" default="ndt_pose" description="Estimated self position"/>
   <arg name="output_pose_with_covariance_topic" default="ndt_pose_with_covariance" description="Estimated self position with covariance"/>
@@ -14,11 +15,12 @@
 
   <arg name="node_name" default="ndt_scan_matcher" description="Use a different name for this node"/>
 
-  <node pkg="ndt_scan_matcher" exec="ndt_scan_matcher" name="$(var node_name)" output="log">
+  <node pkg="ndt_scan_matcher" exec="ndt_scan_matcher" name="$(var node_name)" output="screen">
     <remap from="points_raw" to="$(var input_pointcloud)"/>
     <remap from="ekf_pose_with_covariance" to="$(var input_initial_pose_topic)"/>
     <remap from="regularization_pose_with_covariance" to="$(var input_regularization_pose_topic)"/>
     <remap from="trigger_node_srv" to="$(var input_service_trigger_node)"/>
+    <remap from="input_trusted_pose_topic" to="$(var input_covariance_modifier_trusted_pose_topic)"/>
 
     <remap from="ndt_pose" to="$(var output_pose_topic)"/>
     <remap from="ndt_pose_with_covariance" to="$(var output_pose_with_covariance_topic)"/>

localization/ndt_scan_matcher/package.xml

@@ -28,6 +28,7 @@
   <depend>nav_msgs</depend>
   <depend>ndt_omp</depend>
   <depend>pcl_conversions</depend>
+  <depend>rcl_interfaces</depend>
   <depend>rclcpp</depend>
   <depend>sensor_msgs</depend>
   <depend>std_msgs</depend>