From bb816a60179d064a30f13efb9b793c17be5de7cd Mon Sep 17 00:00:00 2001 From: Takeshi Miura <57553950+1222-takeshi@users.noreply.github.com> Date: Fri, 3 Dec 2021 12:31:11 +0900 Subject: [PATCH] feat: add localization diagnostics package (#54) * Ros2 v0.8.0 localization monitor (#314) * add use_sim-time option (#454) * Remove use_sim_time for set_parameter (#1260) Signed-off-by: wep21 * added projected-ellipse length for localization error monitor (#1512) * added projected-ellipse length for localization error monitor * now use tf2::getYaw, add some const, and use M_PI from cmath * format code * format code using cpplint * format code using uncrustify * format code * format code (including files order) * format code (including files order) * removed M_PI/2, added parameters in launch, added scale_, and renamed projected names * debugged * renamed some anbiguous variables * debugged * debugged * format code * renamed variables in launch file * added readme & debugged launch file * pre-commit fixes * fix typo Co-authored-by: pre-commit Co-authored-by: YamatoAndo * Fix -Wunused-parameter (#1836) * Fix -Wunused-parameter Signed-off-by: Kenji Miyake * Fix mistake Signed-off-by: Kenji Miyake * fix spell * Fix lint issues Signed-off-by: Kenji Miyake * Ignore flake8 warnings Signed-off-by: Kenji Miyake Co-authored-by: Hiroki OTA * Feature/expand footprint (#1757) * add sort-package-xml hook in pre-commit (#1881) * add sort xml hook in pre-commit * change retval to exit_status * rename * add prettier plugin-xml * use early return * add license note * add tier4 license * restore prettier * change license order * move local hooks to public repo * move prettier-xml to pre-commit-hooks-ros * update version for bug-fix * apply pre-commit * Change formatter to clang-format and black (#2332) * Revert "Temporarily comment out pre-commit hooks" This reverts commit 748e9cdb145ce12f8b520bcbd97f5ff899fc28a3. * Replace ament_lint_common with autoware_lint_common Signed-off-by: Kenji Miyake * Remove ament_cmake_uncrustify and ament_clang_format Signed-off-by: Kenji Miyake * Apply Black Signed-off-by: Kenji Miyake * Apply clang-format Signed-off-by: Kenji Miyake * Fix build errors Signed-off-by: Kenji Miyake * Fix for cpplint * Fix include double quotes to angle brackets Signed-off-by: Kenji Miyake * Apply clang-format Signed-off-by: Kenji Miyake * Fix build errors Signed-off-by: Kenji Miyake * Add COLCON_IGNORE (#500) Signed-off-by: Kenji Miyake * Remove COLCON_IGNORE under localization (#553) Co-authored-by: Shinnosuke Hirakawa <8327162+0x126@users.noreply.github.com> Co-authored-by: tkimura4 Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Co-authored-by: kminoda <44218668+kminoda@users.noreply.github.com> Co-authored-by: pre-commit Co-authored-by: YamatoAndo Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Co-authored-by: Hiroki OTA Co-authored-by: Kosuke Murakami Co-authored-by: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com> Co-authored-by: Takeshi Ishita --- .../localization_error_monitor/CMakeLists.txt | 33 ++++ .../localization_error_monitor/README.md | 34 ++++ .../localization_error_monitor/node.hpp | 65 +++++++ .../localization_error_monitor.launch.xml | 17 ++ .../localization_error_monitor/package.xml | 28 +++ .../localization_error_monitor/src/main.cpp | 28 +++ .../localization_error_monitor/src/node.cpp | 172 ++++++++++++++++++ 7 files changed, 377 insertions(+) create mode 100644 localization/localization_error_monitor/CMakeLists.txt create mode 100644 localization/localization_error_monitor/README.md create mode 100644 localization/localization_error_monitor/include/localization_error_monitor/node.hpp create mode 100644 localization/localization_error_monitor/launch/localization_error_monitor.launch.xml create mode 100644 localization/localization_error_monitor/package.xml create mode 100644 localization/localization_error_monitor/src/main.cpp create mode 100644 localization/localization_error_monitor/src/node.cpp diff --git a/localization/localization_error_monitor/CMakeLists.txt b/localization/localization_error_monitor/CMakeLists.txt new file mode 100644 index 0000000000000..dc526a365c71d --- /dev/null +++ b/localization/localization_error_monitor/CMakeLists.txt @@ -0,0 +1,33 @@ +cmake_minimum_required(VERSION 3.5) +project(localization_error_monitor) + +### Compile options +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +### Dependencies +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + + +# Target +## Add executables +ament_auto_add_executable(localization_error_monitor + src/main.cpp + src/node.cpp +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package(INSTALL_TO_SHARE + launch +) diff --git a/localization/localization_error_monitor/README.md b/localization/localization_error_monitor/README.md new file mode 100644 index 0000000000000..bf005d6a00bbb --- /dev/null +++ b/localization/localization_error_monitor/README.md @@ -0,0 +1,34 @@ +# localization_error_monitor + +## Purpose + +localization_error_monitor is a package for diagnosing localization errors by monitoring uncertainty of the localization results. +The package monitors the following two values: + +- size of long radius of confidence ellipse +- size of confidence ellipse along lateral direction (body-frame) + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| --------------------- | ----------------------------------------------- | ------------------- | +| `input/pose_with_cov` | `geometry_msgs::msg::PoseWithCovarianceStamped` | localization result | + +### Output + +| Name | Type | Description | +| ---------------------- | --------------------------------------- | ------------------- | +| `debug/ellipse_marker` | `visualization_msgs::msg::Marker` | ellipse marker | +| `diagnostics` | `diagnostic_msgs::msg::DiagnosticArray` | diagnostics outputs | + +## Parameters + +| Name | Type | Description | +| -------------------------------------- | ------ | ------------------------------------------------------------------------------------------- | +| `scale` | double | scale factor for monitored values (default: 3.0) | +| `error_ellipse_size` | double | error threshold for long radius of confidence ellipse [m] (default: 1.0) | +| `warn_ellipse_size` | double | warning threshold for long radius of confidence ellipse [m] (default: 0.8) | +| `error_ellipse_size_lateral_direction` | double | error threshold for size of confidence ellipse along lateral direction [m] (default: 0.3) | +| `warn_ellipse_size_lateral_direction` | double | warning threshold for size of confidence ellipse along lateral direction [m] (default: 0.2) | diff --git a/localization/localization_error_monitor/include/localization_error_monitor/node.hpp b/localization/localization_error_monitor/include/localization_error_monitor/node.hpp new file mode 100644 index 0000000000000..272a6ad5dab70 --- /dev/null +++ b/localization/localization_error_monitor/include/localization_error_monitor/node.hpp @@ -0,0 +1,65 @@ +// Copyright 2020 Tier IV, Inc. +// +// 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 LOCALIZATION_ERROR_MONITOR__NODE_HPP_ +#define LOCALIZATION_ERROR_MONITOR__NODE_HPP_ + +#include +#include +#include + +#include +#include +#include + +struct Ellipse +{ + double long_radius; + double short_radius; + double yaw; + Eigen::Matrix2d P; + double size_lateral_direction; +}; + +class LocalizationErrorMonitor : public rclcpp::Node +{ +private: + rclcpp::Subscription::SharedPtr pose_with_cov_sub_; + rclcpp::Publisher::SharedPtr ellipse_marker_pub_; + + rclcpp::TimerBase::SharedPtr timer_; + double scale_; + double error_ellipse_size_; + double warn_ellipse_size_; + double error_ellipse_size_lateral_direction_; + double warn_ellipse_size_lateral_direction_; + Ellipse ellipse_; + diagnostic_updater::Updater updater_; + + void checkLocalizationAccuracy(diagnostic_updater::DiagnosticStatusWrapper & stat); + void checkLocalizationAccuracyLateralDirection( + diagnostic_updater::DiagnosticStatusWrapper & stat); + void onPoseWithCovariance( + geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr input_msg); + visualization_msgs::msg::Marker createEllipseMarker( + const Ellipse & ellipse, + geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr pose_with_cov); + double measureSizeEllipseAlongBodyFrame(const Eigen::Matrix2d & Pinv, double theta); + void onTimer(); + +public: + LocalizationErrorMonitor(); + ~LocalizationErrorMonitor() = default; +}; +#endif // LOCALIZATION_ERROR_MONITOR__NODE_HPP_ diff --git a/localization/localization_error_monitor/launch/localization_error_monitor.launch.xml b/localization/localization_error_monitor/launch/localization_error_monitor.launch.xml new file mode 100644 index 0000000000000..e544d6ae17958 --- /dev/null +++ b/localization/localization_error_monitor/launch/localization_error_monitor.launch.xml @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/localization/localization_error_monitor/package.xml b/localization/localization_error_monitor/package.xml new file mode 100644 index 0000000000000..67d2825d9ba6e --- /dev/null +++ b/localization/localization_error_monitor/package.xml @@ -0,0 +1,28 @@ + + + + localization_error_monitor + 0.1.0 + ros node for monitoring localization error + taichiH + Apache License 2.0 + + taichiH + + ament_cmake_auto + + diagnostic_msgs + diagnostic_updater + eigen + geometry_msgs + tf2 + tf2_geometry_msgs + visualization_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/localization/localization_error_monitor/src/main.cpp b/localization/localization_error_monitor/src/main.cpp new file mode 100644 index 0000000000000..f4fa5ab664005 --- /dev/null +++ b/localization/localization_error_monitor/src/main.cpp @@ -0,0 +1,28 @@ +// Copyright 2020 Tier IV, Inc. +// +// 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 "localization_error_monitor/node.hpp" + +#include + +#include + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/localization/localization_error_monitor/src/node.cpp b/localization/localization_error_monitor/src/node.cpp new file mode 100644 index 0000000000000..cb42896164b9e --- /dev/null +++ b/localization/localization_error_monitor/src/node.cpp @@ -0,0 +1,172 @@ +// Copyright 2020 Tier IV, Inc. +// +// 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 "localization_error_monitor/node.hpp" + +#include + +#include +#include + +#include +#include +#include +#include +#include +#include + +LocalizationErrorMonitor::LocalizationErrorMonitor() +: Node("localization_error_monitor"), updater_(this) +{ + scale_ = this->declare_parameter("scale", 3.0); + error_ellipse_size_ = this->declare_parameter("error_ellipse_size", 1.0); + warn_ellipse_size_ = this->declare_parameter("warn_ellipse_size", 0.8); + + error_ellipse_size_lateral_direction_ = + this->declare_parameter("error_ellipse_size_lateral_direction", 0.3); + warn_ellipse_size_lateral_direction_ = + this->declare_parameter("warn_ellipse_size_lateral_direction", 0.2); + + pose_with_cov_sub_ = this->create_subscription( + "input/pose_with_cov", 1, + std::bind(&LocalizationErrorMonitor::onPoseWithCovariance, this, std::placeholders::_1)); + + // QoS setup + rclcpp::QoS durable_qos(1); + durable_qos.transient_local(); // option for latching + ellipse_marker_pub_ = + this->create_publisher("debug/ellipse_marker", durable_qos); + + updater_.setHardwareID("localization_error_monitor"); + updater_.add("localization_accuracy", this, &LocalizationErrorMonitor::checkLocalizationAccuracy); + updater_.add( + "localization_accuracy_lateral_direction", this, + &LocalizationErrorMonitor::checkLocalizationAccuracyLateralDirection); + + // Set timer + auto timer_callback = std::bind(&LocalizationErrorMonitor::onTimer, this); + const auto period_ns = + std::chrono::duration_cast(std::chrono::duration(0.1)); + timer_ = std::make_shared>( + this->get_clock(), period_ns, std::move(timer_callback), + this->get_node_base_interface()->get_context()); + this->get_node_timers_interface()->add_timer(timer_, nullptr); +} + +void LocalizationErrorMonitor::onTimer() { updater_.force_update(); } + +void LocalizationErrorMonitor::checkLocalizationAccuracy( + diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + stat.add("localization_accuracy", ellipse_.long_radius); + int8_t diag_level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::string diag_message = "ellipse size is within the expected range"; + if (warn_ellipse_size_ <= ellipse_.long_radius) { + diag_level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + diag_message = "ellipse size is too large"; + } + if (error_ellipse_size_ <= ellipse_.long_radius) { + diag_level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + diag_message = "ellipse size is over the expected range"; + } + stat.summary(diag_level, diag_message); +} + +void LocalizationErrorMonitor::checkLocalizationAccuracyLateralDirection( + diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + stat.add("localization_accuracy_lateral_direction", ellipse_.size_lateral_direction); + int8_t diag_level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::string diag_message = "ellipse size along lateral direction is within the expected range"; + if (warn_ellipse_size_lateral_direction_ <= ellipse_.size_lateral_direction) { + diag_level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + diag_message = "ellipse size along lateral direction is too large"; + } + if (error_ellipse_size_lateral_direction_ <= ellipse_.size_lateral_direction) { + diag_level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + diag_message = "ellipse size along lateral direction is over the expected range"; + } + stat.summary(diag_level, diag_message); +} + +visualization_msgs::msg::Marker LocalizationErrorMonitor::createEllipseMarker( + const Ellipse & ellipse, + geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr pose_with_cov) +{ + tf2::Quaternion quat; + quat.setEuler(0, 0, ellipse.yaw); + + const double ellipse_long_radius = std::min(ellipse.long_radius, 30.0); + const double ellipse_short_radius = std::min(ellipse.short_radius, 30.0); + visualization_msgs::msg::Marker marker; + marker.header = pose_with_cov->header; + marker.header.stamp = this->now(); + marker.ns = "error_ellipse"; + marker.id = 0; + marker.type = visualization_msgs::msg::Marker::SPHERE; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.pose = pose_with_cov->pose.pose; + marker.pose.orientation = tf2::toMsg(quat); + marker.scale.x = ellipse_long_radius * 2; + marker.scale.y = ellipse_short_radius * 2; + marker.scale.z = 0.01; + marker.color.a = 0.1; + marker.color.r = 0.0; + marker.color.g = 0.0; + marker.color.b = 1.0; + return marker; +} + +void LocalizationErrorMonitor::onPoseWithCovariance( + geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr input_msg) +{ + // create xy covariance (2x2 matrix) + // input geometry_msgs::PoseWithCovariance contain 6x6 matrix + Eigen::Matrix2d xy_covariance; + const auto cov = input_msg->pose.covariance; + xy_covariance(0, 0) = cov[0 * 6 + 0]; + xy_covariance(0, 1) = cov[0 * 6 + 1]; + xy_covariance(1, 0) = cov[1 * 6 + 0]; + xy_covariance(1, 1) = cov[1 * 6 + 1]; + + Eigen::SelfAdjointEigenSolver eigensolver(xy_covariance); + + // eigen values and vectors are sorted in ascending order + ellipse_.long_radius = scale_ * std::sqrt(eigensolver.eigenvalues()(1)); + ellipse_.short_radius = scale_ * std::sqrt(eigensolver.eigenvalues()(0)); + + // principal component vector + const Eigen::Vector2d pc_vector = eigensolver.eigenvectors().col(1); + ellipse_.yaw = std::atan2(pc_vector.y(), pc_vector.x()); + + // ellipse size along lateral direction (body-frame) + ellipse_.P = xy_covariance; + const double yaw_vehicle = tf2::getYaw(input_msg->pose.pose.orientation); + ellipse_.size_lateral_direction = + scale_ * measureSizeEllipseAlongBodyFrame(ellipse_.P.inverse(), yaw_vehicle); + + const auto ellipse_marker = createEllipseMarker(ellipse_, input_msg); + ellipse_marker_pub_->publish(ellipse_marker); +} + +double LocalizationErrorMonitor::measureSizeEllipseAlongBodyFrame( + const Eigen::Matrix2d & Pinv, const double theta) +{ + Eigen::MatrixXd e(2, 1); + e(0, 0) = std::cos(theta); + e(1, 0) = std::sin(theta); + + double d = std::sqrt((e.transpose() * Pinv * e)(0, 0) / Pinv.determinant()); + return d; +}