From 61f99da9f2681621e88ba835dadd3035a1a1ed03 Mon Sep 17 00:00:00 2001 From: meliketanrikulu Date: Thu, 22 Feb 2024 00:55:54 +0300 Subject: [PATCH] add_aw_pose_covariance_modifier_node Signed-off-by: melike --- .../pose_twist_estimator.launch.xml | 9 ++ .../CMakeLists.txt | 33 +++++++ .../README.md | 1 + ...w_pose_covariance_modifier_node.launch.xml | 12 +++ .../package.xml | 26 ++++++ .../src/aw_pose_covariance_modifier_node.cpp | 90 +++++++++++++++++++ .../aw_pose_covariance_modifier_node.hpp | 42 +++++++++ .../ndt_scan_matcher_core.hpp | 20 ++++- .../launch/ndt_scan_matcher.launch.xml | 4 +- .../src/ndt_scan_matcher_core.cpp | 87 ++++++++++++++++-- 10 files changed, 316 insertions(+), 8 deletions(-) create mode 100644 localization/aw_pose_covariance_modifier_node/CMakeLists.txt create mode 100644 localization/aw_pose_covariance_modifier_node/README.md create mode 100755 localization/aw_pose_covariance_modifier_node/launch/aw_pose_covariance_modifier_node.launch.xml create mode 100644 localization/aw_pose_covariance_modifier_node/package.xml create mode 100644 localization/aw_pose_covariance_modifier_node/src/aw_pose_covariance_modifier_node.cpp create mode 100644 localization/aw_pose_covariance_modifier_node/src/include/aw_pose_covariance_modifier_node.hpp 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 c00c90d467090..4b90e29457d4e 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 @@ -7,6 +7,9 @@ + + + @@ -80,6 +83,11 @@ + + + + + @@ -113,4 +121,5 @@ + diff --git a/localization/aw_pose_covariance_modifier_node/CMakeLists.txt b/localization/aw_pose_covariance_modifier_node/CMakeLists.txt new file mode 100644 index 0000000000000..97c20b35c4e16 --- /dev/null +++ b/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) diff --git a/localization/aw_pose_covariance_modifier_node/README.md b/localization/aw_pose_covariance_modifier_node/README.md new file mode 100644 index 0000000000000..e27aec146d719 --- /dev/null +++ b/localization/aw_pose_covariance_modifier_node/README.md @@ -0,0 +1 @@ +# aw_pose_covariance_modifier \ No newline at end of file diff --git a/localization/aw_pose_covariance_modifier_node/launch/aw_pose_covariance_modifier_node.launch.xml b/localization/aw_pose_covariance_modifier_node/launch/aw_pose_covariance_modifier_node.launch.xml new file mode 100755 index 0000000000000..c5f49fb860697 --- /dev/null +++ b/localization/aw_pose_covariance_modifier_node/launch/aw_pose_covariance_modifier_node.launch.xml @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/localization/aw_pose_covariance_modifier_node/package.xml b/localization/aw_pose_covariance_modifier_node/package.xml new file mode 100644 index 0000000000000..d375e428ad8e1 --- /dev/null +++ b/localization/aw_pose_covariance_modifier_node/package.xml @@ -0,0 +1,26 @@ + + + + aw_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 + + rclcpp + rclcpp_components + + geometry_msgs + std_srvs + + + ament_cmake + + diff --git a/localization/aw_pose_covariance_modifier_node/src/aw_pose_covariance_modifier_node.cpp b/localization/aw_pose_covariance_modifier_node/src/aw_pose_covariance_modifier_node.cpp new file mode 100644 index 0000000000000..d1ae0a137425b --- /dev/null +++ b/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 + + +AWPoseCovarianceModifierNode::AWPoseCovarianceModifierNode() + : Node("AWPoseCovarianceModifierNode") +{ + trusted_pose_with_cov_sub_ = this->create_subscription( + "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("output_pose_with_covariance_topic",10); + + client_ = this->create_client("/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(); + 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()); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/localization/aw_pose_covariance_modifier_node/src/include/aw_pose_covariance_modifier_node.hpp b/localization/aw_pose_covariance_modifier_node/src/include/aw_pose_covariance_modifier_node.hpp new file mode 100644 index 0000000000000..4a6a63e6e5086 --- /dev/null +++ b/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 +#include +#include +#include + +using namespace std::chrono_literals; + +class AWPoseCovarianceModifierNode : public rclcpp::Node +{ +public: + AWPoseCovarianceModifierNode(); + + rclcpp::Subscription::SharedPtr trusted_pose_with_cov_sub_; + rclcpp::Publisher::SharedPtr new_pose_estimator_pub_; + rclcpp::Client::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_ diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp index ca69576061e21..aae40107e47d8 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp @@ -83,7 +83,9 @@ 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); @@ -91,6 +93,8 @@ class NDTScanMatcher : public rclcpp::Node 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::SharedPtr initial_pose_sub_; rclcpp::Subscription::SharedPtr sensor_points_sub_; + rclcpp::Subscription::SharedPtr regularization_pose_sub_; rclcpp::Subscription::SharedPtr - regularization_pose_sub_; + trusted_source_pose_sub_; rclcpp::Publisher::SharedPtr sensor_aligned_pose_pub_; rclcpp::Publisher::SharedPtr no_ground_points_aligned_pose_pub_; @@ -172,6 +177,7 @@ class NDTScanMatcher : public rclcpp::Node rclcpp::Service::SharedPtr service_; rclcpp::Service::SharedPtr service_trigger_node_; + rclcpp::Service::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 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 is_activated_; std::unique_ptr map_update_module_; std::unique_ptr logger_configure_; + std::array covariance_modifier(std::array & in_ndt_covariance); + HyperParameters param_; }; diff --git a/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml b/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml index cbcb0a9f74bc4..33ea5d294a18e 100644 --- a/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml +++ b/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml @@ -6,6 +6,7 @@ + @@ -14,11 +15,12 @@ - + + diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index cd637791f04b6..0f47d1f1488f8 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -71,6 +71,7 @@ NDTScanMatcher::NDTScanMatcher() tf2_listener_(tf2_buffer_), ndt_ptr_(new NormalDistributionsTransform), state_ptr_(new std::map), + activate_pose_covariance_modifier_(false), is_activated_(false), param_(this) { @@ -102,6 +103,12 @@ NDTScanMatcher::NDTScanMatcher() std::bind(&NDTScanMatcher::callback_sensor_points, this, std::placeholders::_1), sensor_sub_opt); + trusted_source_pose_sub_ = this->create_subscription( + "input_trusted_pose_topic", 100, + std::bind(&NDTScanMatcher::callback_trusted_source_pose, this, std::placeholders::_1), + initial_pose_sub_opt); + + // Only if regularization is enabled, subscribe to the regularization base pose if (param_.ndt_regularization_enable) { // NOTE: The reason that the regularization subscriber does not belong to the @@ -175,6 +182,12 @@ NDTScanMatcher::NDTScanMatcher() &NDTScanMatcher::service_trigger_node, this, std::placeholders::_1, std::placeholders::_2), rclcpp::ServicesQoS().get_rmw_qos_profile(), sensor_callback_group); + server_covariance_modifier_ = this->create_service( + "/localization/pose_estimator/covariance_modifier", + std::bind( + &NDTScanMatcher::activate_pose_covariance_modifier, this, std::placeholders::_1, std::placeholders::_2), + rclcpp::ServicesQoS().get_rmw_qos_profile(),sensor_callback_group); + ndt_ptr_->setParams(param_.ndt); initial_pose_buffer_ = std::make_unique( @@ -187,6 +200,15 @@ NDTScanMatcher::NDTScanMatcher() logger_configure_ = std::make_unique(this); } +void NDTScanMatcher::activate_pose_covariance_modifier(const std_srvs::srv::SetBool::Request::SharedPtr req, + std_srvs::srv::SetBool::Response::SharedPtr res) { + activate_pose_covariance_modifier_ = req->data; + RCLCPP_INFO(this->get_logger(), " Pose Covariance Modifier Activated ..."); + res->success = true; + res->message = "Covariance Modifier Activated for NDT"; + +} + void NDTScanMatcher::publish_diagnostic() { diagnostic_msgs::msg::DiagnosticStatus diag_status_msg; @@ -303,6 +325,15 @@ void NDTScanMatcher::callback_regularization_pose( regularization_pose_buffer_->push_back(pose_conv_msg_ptr); } +void NDTScanMatcher::callback_trusted_source_pose( + geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr pose_conv_msg_ptr) +{ + trusted_source_pose_ = *pose_conv_msg_ptr; + trustedPose.pose_avarage_rmse_xy = (std::sqrt(trusted_source_pose_.pose.covariance[0]) + std::sqrt(trusted_source_pose_.pose.covariance[7])) / 2; + trustedPose.yaw_rmse = std::sqrt(trusted_source_pose_.pose.covariance[35]); +} + + void NDTScanMatcher::callback_sensor_points( sensor_msgs::msg::PointCloud2::ConstSharedPtr sensor_points_msg_in_sensor_frame) { @@ -411,15 +442,23 @@ void NDTScanMatcher::callback_sensor_points( const Eigen::Matrix3d map_to_base_link_rotation = map_to_base_link_quat.normalized().toRotationMatrix(); - std::array ndt_covariance = - rotate_covariance(param_.covariance.output_pose_covariance, map_to_base_link_rotation); + std::array ndt_covariance_in_; + if (activate_pose_covariance_modifier_){ + ndt_covariance_in_ = covariance_modifier(param_.covariance.output_pose_covariance); + } + else{ + ndt_covariance_in_ = param_.covariance.output_pose_covariance; + } + std::array ndt_covariance = + rotate_covariance(ndt_covariance_in_, map_to_base_link_rotation); if (is_converged && param_.covariance.covariance_estimation.enable) { const auto estimated_covariance = estimate_covariance(ndt_result, initial_pose_matrix, sensor_ros_time); ndt_covariance = estimated_covariance; } + const auto exe_end_time = std::chrono::system_clock::now(); const auto duration_micro_sec = std::chrono::duration_cast(exe_end_time - exe_start_time).count(); @@ -532,6 +571,37 @@ void NDTScanMatcher::publish_tf( tier4_autoware_utils::pose2transform(result_pose_stamped_msg, param_.frame.ndt_base_frame)); } +std::array NDTScanMatcher::covariance_modifier(std::array & in_ndt_covariance){ + std::array ndt_covariance; + ndt_covariance = in_ndt_covariance; + close_ndt_pose_source_ = false; + + if(trustedPose.pose_avarage_rmse_xy <= 0.10 && trustedPose.yaw_rmse < 0.3){ + close_ndt_pose_source_ = true; + } + else if(trustedPose.pose_avarage_rmse_xy <= 0.10){ + ndt_covariance[0] = 1000000; + ndt_covariance[7] = 1000000; + } + else if(trustedPose.pose_avarage_rmse_xy <= 0.25){ + ndt_covariance[0] = std::pow((std::sqrt(in_ndt_covariance[0]) * 2) - std::sqrt(trusted_source_pose_.pose.covariance[0]),2); + ndt_covariance[7] = std::pow((std::sqrt(in_ndt_covariance[7]) * 2) - std::sqrt(trusted_source_pose_.pose.covariance[7]),2); + ndt_covariance[14] = std::pow((std::sqrt(in_ndt_covariance[14]) * 2) - std::sqrt(trusted_source_pose_.pose.covariance[14]),2); + + ndt_covariance[0] = (ndt_covariance[0] >= in_ndt_covariance[0]) ? ndt_covariance[0] : in_ndt_covariance[0]; + ndt_covariance[7] = (ndt_covariance[7] >= in_ndt_covariance[7]) ? ndt_covariance[7] : in_ndt_covariance[0]; + ndt_covariance[14] = (ndt_covariance[14] >= in_ndt_covariance[14]) ? ndt_covariance[14] : in_ndt_covariance[0]; + + if (trustedPose.yaw_rmse <= 0.3){ + ndt_covariance[35] = 1000000; + } + } + else{ + RCLCPP_INFO(this->get_logger(), "NDT input covariance values will be used "); + } + + return ndt_covariance; +} void NDTScanMatcher::publish_pose( const rclcpp::Time & sensor_ros_time, const geometry_msgs::msg::Pose & result_pose_msg, const std::array & ndt_covariance, const bool is_converged) @@ -547,10 +617,17 @@ void NDTScanMatcher::publish_pose( result_pose_with_cov_msg.pose.pose = result_pose_msg; result_pose_with_cov_msg.pose.covariance = ndt_covariance; - if (is_converged) { - ndt_pose_pub_->publish(result_pose_stamped_msg); - ndt_pose_with_covariance_pub_->publish(result_pose_with_cov_msg); + if(activate_pose_covariance_modifier_ && close_ndt_pose_source_){ + ndt_pose_pub_->publish(result_pose_stamped_msg); + RCLCPP_INFO(this->get_logger(),"NDT pose with covariance topic closed."); } + else{ + if (is_converged) { + ndt_pose_pub_->publish(result_pose_stamped_msg); + ndt_pose_with_covariance_pub_->publish(result_pose_with_cov_msg); + } + } + } void NDTScanMatcher::publish_point_cloud(