From c28bada4d63cf1df21fcf7180762536ae3796698 Mon Sep 17 00:00:00 2001 From: meliketanrikulu Date: Thu, 7 Mar 2024 15:51:10 +0300 Subject: [PATCH] add autoware pose covariance modifier node Signed-off-by: meliketanrikulu --- .../launch/localization.launch.xml | 1 + .../pose_twist_estimator.launch.xml | 4 + .../pose_twist_fusion_filter.launch.xml | 6 +- .../CMakeLists.txt | 22 +++ .../README.md | 1 + ...e_pose_covariance_modifier_node.launch.xml | 12 ++ .../package.xml | 24 +++ ...autoware_pose_covariance_modifier_node.cpp | 166 ++++++++++++++++++ ...autoware_pose_covariance_modifier_node.hpp | 64 +++++++ 9 files changed, 299 insertions(+), 1 deletion(-) create mode 100644 localization/autoware_pose_covariance_modifier_node/CMakeLists.txt create mode 100644 localization/autoware_pose_covariance_modifier_node/README.md create mode 100755 localization/autoware_pose_covariance_modifier_node/launch/autoware_pose_covariance_modifier_node.launch.xml create mode 100644 localization/autoware_pose_covariance_modifier_node/package.xml create mode 100644 localization/autoware_pose_covariance_modifier_node/src/autoware_pose_covariance_modifier_node.cpp create mode 100644 localization/autoware_pose_covariance_modifier_node/src/include/autoware_pose_covariance_modifier_node.hpp diff --git a/launch/tier4_localization_launch/launch/localization.launch.xml b/launch/tier4_localization_launch/launch/localization.launch.xml index 7f2f446ae1bee..04dbe89dc228c 100644 --- a/launch/tier4_localization_launch/launch/localization.launch.xml +++ b/launch/tier4_localization_launch/launch/localization.launch.xml @@ -3,6 +3,7 @@ + diff --git a/launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml b/launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml index 3042774d16fd3..2060135bef5a3 100644 --- a/launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml +++ b/launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml @@ -113,4 +113,8 @@ + + + + diff --git a/launch/tier4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml b/launch/tier4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml index bdea584bd58ae..bd6d16a5ed2ec 100644 --- a/launch/tier4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml +++ b/launch/tier4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml @@ -1,9 +1,13 @@ + + + + - + diff --git a/localization/autoware_pose_covariance_modifier_node/CMakeLists.txt b/localization/autoware_pose_covariance_modifier_node/CMakeLists.txt new file mode 100644 index 0000000000000..9d520b280bb0b --- /dev/null +++ b/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) diff --git a/localization/autoware_pose_covariance_modifier_node/README.md b/localization/autoware_pose_covariance_modifier_node/README.md new file mode 100644 index 0000000000000..e3231536dbc23 --- /dev/null +++ b/localization/autoware_pose_covariance_modifier_node/README.md @@ -0,0 +1 @@ +# autoware_pose_covariance_modifier diff --git a/localization/autoware_pose_covariance_modifier_node/launch/autoware_pose_covariance_modifier_node.launch.xml b/localization/autoware_pose_covariance_modifier_node/launch/autoware_pose_covariance_modifier_node.launch.xml new file mode 100755 index 0000000000000..d71dc9a7cf385 --- /dev/null +++ b/localization/autoware_pose_covariance_modifier_node/launch/autoware_pose_covariance_modifier_node.launch.xml @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/localization/autoware_pose_covariance_modifier_node/package.xml b/localization/autoware_pose_covariance_modifier_node/package.xml new file mode 100644 index 0000000000000..5461d0719fb62 --- /dev/null +++ b/localization/autoware_pose_covariance_modifier_node/package.xml @@ -0,0 +1,24 @@ + + + + autoware_pose_covariance_modifier_node + 1.0.0 + Converts an autoware_msgs message to autoware_auto_msgs version and publishes it. + + Melike Tanrikulu + + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + rosidl_default_generators + + geometry_msgs + rclcpp + rclcpp_components + + + ament_cmake + + diff --git a/localization/autoware_pose_covariance_modifier_node/src/autoware_pose_covariance_modifier_node.cpp b/localization/autoware_pose_covariance_modifier_node/src/autoware_pose_covariance_modifier_node.cpp new file mode 100644 index 0000000000000..1e974c4ef8be4 --- /dev/null +++ b/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 + +AutowarePoseCovarianceModifierNode::AutowarePoseCovarianceModifierNode() : Node("AutowarePoseCovarianceModifierNode") +{ + trusted_pose_with_cov_sub_ = + this->create_subscription( + "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( + "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( + "output_pose_with_covariance_topic", 10); + + pose_source_pub_ = + this->create_publisher( + "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(AutowarePoseCovarianceModifierNode::PoseSource::GNSS): + selected_pose_type.data = "GNSS"; + break; + case static_cast(AutowarePoseCovarianceModifierNode::PoseSource::GNSS_NDT): + selected_pose_type.data = "GNSS_NDT"; + break; + case static_cast(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 AutowarePoseCovarianceModifierNode::ndt_covariance_modifier( + std::array & in_ndt_covariance) +{ + std::array ndt_covariance = in_ndt_covariance; + + if (AutowarePoseCovarianceModifierNode::checkTrustedPoseTimeout()) { + RCLCPP_WARN(this->get_logger(), "Trusted Pose Timeout"); + pose_source_ = static_cast(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(AutowarePoseCovarianceModifierNode::PoseSource::GNSS); + } else if (trustedPose.pose_average_rmse_xy <= 0.10) { + ndt_covariance[0] = 1000000; + ndt_covariance[7] = 1000000; + pose_source_ = static_cast(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(AutowarePoseCovarianceModifierNode::PoseSource::GNSS_NDT); + } else { + RCLCPP_INFO(this->get_logger(), "NDT input covariance values will be used "); + pose_source_ = static_cast(AutowarePoseCovarianceModifierNode::PoseSource::NDT); + } + + publishPoseSource(); + + return ndt_covariance; +} +void AutowarePoseCovarianceModifierNode::ndt_pose_with_cov_callback(const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr & msg){ + + std::array ndt_covariance_in_ = msg->pose.covariance; + std::array 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()); + rclcpp::shutdown(); + return 0; +} diff --git a/localization/autoware_pose_covariance_modifier_node/src/include/autoware_pose_covariance_modifier_node.hpp b/localization/autoware_pose_covariance_modifier_node/src/include/autoware_pose_covariance_modifier_node.hpp new file mode 100644 index 0000000000000..eca31af2fa946 --- /dev/null +++ b/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 + +#include +#include +#include + +class AutowarePoseCovarianceModifierNode : public rclcpp::Node +{ +public: + AutowarePoseCovarianceModifierNode(); + + rclcpp::Subscription::SharedPtr + trusted_pose_with_cov_sub_; + rclcpp::Subscription::SharedPtr + ndt_pose_with_cov_sub_; + rclcpp::Publisher::SharedPtr + new_pose_estimator_pub_; + rclcpp::Publisher::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 ndt_covariance_modifier(std::array & 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_