add_aw_pose_covariance_modifier_node

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

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"/>
@@ -113,4 +121,5 @@
       <arg name="input_pointcloud" value="$(var override_input_pointcloud)"/>
     </include>
   </group>
+
 </launch>

localization/aw_pose_covariance_modifier_node/CMakeLists.txt

@@ -0,0 +1,33 @@
+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(std_srvs 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 std_srvs)
+
+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,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 .. "/>
+  <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,26 @@
+<?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="[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>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+
+  <depend>geometry_msgs</depend>
+  <depend>std_srvs</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,90 @@
+// 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<std_srvs::srv::SetBool>("/localization/pose_estimator/covariance_modifier");
+
+    startNDTCovModifier = AWPoseCovarianceModifierNode::callNDTCovarianceModifier();
+    if(startNDTCovModifier == 1){
+        RCLCPP_INFO(get_logger(), "NDT pose covariance modifier activated ...");
+
+    }
+
+}
+
+bool AWPoseCovarianceModifierNode::callNDTCovarianceModifier(){
+
+    while (!client_->wait_for_service(std::chrono::seconds(1))) {
+        if (!rclcpp::ok()) {
+            RCLCPP_ERROR(get_logger(), "Interrupted while waiting for the service. Exiting.");
+            return false;
+        }
+        RCLCPP_INFO(get_logger(), "Service not available, waiting again...");
+    }
+
+    auto request = std::make_shared<std_srvs::srv::SetBool::Request>();
+    request->data = true;
+
+    auto future_result = client_->async_send_request(request);
+    if (rclcpp::spin_until_future_complete(get_node_base_interface(), future_result) ==
+        rclcpp::FutureReturnCode::SUCCESS)
+    {
+        auto response = future_result.get();
+        RCLCPP_INFO(get_logger(), "Response: %d", response->success);
+        return true;
+    }
+    else {
+        RCLCPP_ERROR(get_logger(), "Failed to receive response.");
+        return false;
+    }
+}
+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,42 @@
+// 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 <rclcpp/rclcpp.hpp>
+#include <std_srvs/srv/set_bool.hpp>
+#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
+#include <string>
+
+using namespace std::chrono_literals;
+
+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<std_srvs::srv::SetBool>::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 startNDTCovModifier = 0;
+};
+
+
+#endif  // AW_POSE_COVARIANCE_MODIFIER_NODE_HPP_

localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp

@@ -83,14 +83,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 activate_pose_covariance_modifier(
+    const std_srvs::srv::SetBool::Request::SharedPtr req,
+    std_srvs::srv::SetBool::Response::SharedPtr res);
   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);
@@ -136,8 +140,9 @@ class NDTScanMatcher : public rclcpp::Node
   rclcpp::TimerBase::SharedPtr map_update_timer_;
   rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr initial_pose_sub_;
   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
-    regularization_pose_sub_;
+    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 +177,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<std_srvs::srv::SetBool>::SharedPtr  server_covariance_modifier_;
 
   tf2_ros::TransformBroadcaster tf2_broadcaster_;
   tf2_ros::Buffer tf2_buffer_;
@@ -193,10 +199,20 @@ 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_avarage_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);
+
   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)"/>