feat: add localization diagnostics package (autowarefoundation#54)

* Ros2 v0.8.0 localization monitor (autowarefoundation#314) * add use_sim-time option (autowarefoundation#454) * Remove use_sim_time for set_parameter (autowarefoundation#1260) Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * added projected-ellipse length for localization error monitor (autowarefoundation#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 <pre-commit@example.com> Co-authored-by: YamatoAndo <yamato.ando@gmail.com> * Fix -Wunused-parameter (autowarefoundation#1836) * Fix -Wunused-parameter Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix mistake Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * fix spell * Fix lint issues Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Ignore flake8 warnings Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> Co-authored-by: Hiroki OTA <hiroki.ota@tier4.jp> * Feature/expand footprint (autowarefoundation#1757) * add sort-package-xml hook in pre-commit (autowarefoundation#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 (autowarefoundation#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 <kenji.miyake@tier4.jp> * Remove ament_cmake_uncrustify and ament_clang_format Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Apply Black Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Apply clang-format Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix build errors Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix for cpplint * Fix include double quotes to angle brackets Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Apply clang-format Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix build errors Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Add COLCON_IGNORE (autowarefoundation#500) Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Remove COLCON_IGNORE under localization (autowarefoundation#553) Co-authored-by: Shinnosuke Hirakawa <8327162+0x126@users.noreply.github.com> Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp> 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 <pre-commit@example.com> Co-authored-by: YamatoAndo <yamato.ando@gmail.com> Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Co-authored-by: Hiroki OTA <hiroki.ota@tier4.jp> Co-authored-by: Kosuke Murakami <kosuke.murakami@tier4.jp> Co-authored-by: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com> Co-authored-by: Takeshi Ishita <ishitah.takeshi@gmail.com>
mitsudome-r · Dec 3, 2021 · bb816a6 · bb816a6
1 parent 899f829
commit bb816a6
Show file tree

Hide file tree

Showing 7 changed files with 377 additions and 0 deletions.
diff --git a/localization/localization_error_monitor/CMakeLists.txt 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
@@ -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
@@ -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 <Eigen/Dense>
+#include <diagnostic_updater/diagnostic_updater.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <diagnostic_msgs/msg/diagnostic_array.hpp>
+#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
+#include <visualization_msgs/msg/marker_array.hpp>
+
+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<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_with_cov_sub_;
+  rclcpp::Publisher<visualization_msgs::msg::Marker>::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
@@ -0,0 +1,17 @@
+<launch>
+  <arg name="input/pose_with_cov" default="/localization/pose_with_covariance" />
+  <arg name="scale" default="3.0" />
+  <arg name="error_ellipse_size" default="1.0" />
+  <arg name="warn_ellipse_size" default="0.8" />
+  <arg name="error_ellipse_size_lateral_direction" default="0.3" />
+  <arg name="warn_ellipse_size_lateral_direction" default="0.2" />
+
+  <node name="localization_error_monitor" exec="localization_error_monitor" pkg="localization_error_monitor" output="screen">
+    <remap from="input/pose_with_cov" to="$(var input/pose_with_cov)" />
+    <param name="scale" value="$(var scale)" />
+    <param name="error_ellipse_size" value="$(var error_ellipse_size)" />
+    <param name="warn_ellipse_size" value="$(var warn_ellipse_size)" />
+    <param name="error_ellipse_size_lateral_direction" value="$(var error_ellipse_size_lateral_direction)" />
+    <param name="warn_ellipse_size_lateral_direction" value="$(var warn_ellipse_size_lateral_direction)" />
+  </node>
+</launch>
diff --git a/localization/localization_error_monitor/package.xml b/localization/localization_error_monitor/package.xml
@@ -0,0 +1,28 @@
+<?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>localization_error_monitor</name>
+  <version>0.1.0</version>
+  <description>ros node for monitoring localization error</description>
+  <maintainer email="taichi.higashide@tier4.jp">taichiH</maintainer>
+  <license>Apache License 2.0</license>
+
+  <author email="taichi.higashide@tier4.jp">taichiH</author>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <depend>diagnostic_msgs</depend>
+  <depend>diagnostic_updater</depend>
+  <depend>eigen</depend>
+  <depend>geometry_msgs</depend>
+  <depend>tf2</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>visualization_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
diff --git a/localization/localization_error_monitor/src/main.cpp 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 <rclcpp/rclcpp.hpp>
+
+#include <memory>
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+  auto node = std::make_shared<LocalizationErrorMonitor>();
+  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
@@ -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 <Eigen/Dense>
+
+#include <tf2/utils.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+
+#include <algorithm>
+#include <cmath>
+#include <functional>
+#include <memory>
+#include <string>
+#include <utility>
+
+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<geometry_msgs::msg::PoseWithCovarianceStamped>(
+    "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<visualization_msgs::msg::Marker>("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::nanoseconds>(std::chrono::duration<double>(0.1));
+  timer_ = std::make_shared<rclcpp::GenericTimer<decltype(timer_callback)>>(
+    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<Eigen::Matrix2d> 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;
+}