feat: add localization_error_monitor package

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
+)

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) |

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_

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>

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>

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;
+}

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;
+}