feat(metrics_calculation): add kinematic and localization evaluators …

…with metrics (tier4#928)

* initial skeleton of localization evaluator

Signed-off-by: djargot <dominik.jargot@robotec.ai>

* Add simple localization evaliuation framework

Signed-off-by: djargot <dominik.jargot@robotec.ai>

* Clean and add tests

Signed-off-by: djargot <dominik.jargot@robotec.ai>

* ci(pre-commit): autofix

Signed-off-by: djargot <dominik.jargot@robotec.ai>

* Clean localization evaluator

Signed-off-by: djargot <dominik.jargot@robotec.ai>

* Update kinematic evaluator

Signed-off-by: djargot <dominik.jargot@robotec.ai>

* ci(pre-commit): autofix

Signed-off-by: djargot <dominik.jargot@robotec.ai>

* dependency fix

Signed-off-by: djargot <dominik.jargot@robotec.ai>

* Fix localization evaluator tests

Signed-off-by: djargot <dominik.jargot@robotec.ai>

* ci(pre-commit): autofix

Signed-off-by: djargot <dominik.jargot@robotec.ai>

* Add missing includes and remove unnecessary quotes

Signed-off-by: djargot <dominik.jargot@robotec.ai>

Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Co-authored-by: Wojciech Jaworski <wojciech.jaworski@robotec.ai>

evaluator/kinematic_evaluator/CMakeLists.txt

@@ -0,0 +1,45 @@
+cmake_minimum_required(VERSION 3.14)
+project(kinematic_evaluator)
+
+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()
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+ament_auto_add_library(${PROJECT_NAME}_node SHARED
+  src/metrics_calculator.cpp
+  src/${PROJECT_NAME}_node.cpp
+  src/kinematic_evaluator_node.cpp
+  src/metrics/kinematic_metrics.cpp
+)
+
+rclcpp_components_register_node(${PROJECT_NAME}_node
+  PLUGIN "kinematic_diagnostics::KinematicEvaluatorNode"
+  EXECUTABLE ${PROJECT_NAME}
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+  ament_add_gtest(test_${PROJECT_NAME}
+    test/test_kinematic_evaluator_node.cpp
+  )
+  target_link_libraries(test_${PROJECT_NAME}
+    ${PROJECT_NAME}_node
+  )
+endif()
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+    param
+    launch
+)

evaluator/kinematic_evaluator/README.md

@@ -0,0 +1,3 @@
+# Kinematic Evaluator
+
+TBD

evaluator/kinematic_evaluator/include/kinematic_evaluator/kinematic_evaluator_node.hpp

@@ -0,0 +1,82 @@
+// Copyright 2021 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 KINEMATIC_EVALUATOR__KINEMATIC_EVALUATOR_NODE_HPP_
+#define KINEMATIC_EVALUATOR__KINEMATIC_EVALUATOR_NODE_HPP_
+
+#include "kinematic_evaluator/metrics_calculator.hpp"
+#include "kinematic_evaluator/stat.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "tf2_ros/buffer.h"
+#include "tf2_ros/transform_listener.h"
+
+#include "diagnostic_msgs/msg/diagnostic_array.hpp"
+#include "geometry_msgs/msg/transform_stamped.hpp"
+#include <nav_msgs/msg/odometry.hpp>
+
+#include <deque>
+#include <memory>
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+namespace kinematic_diagnostics
+{
+using diagnostic_msgs::msg::DiagnosticArray;
+using diagnostic_msgs::msg::DiagnosticStatus;
+
+/**
+ * @brief Node for kinematic evaluation
+ */
+class KinematicEvaluatorNode : public rclcpp::Node
+{
+public:
+  explicit KinematicEvaluatorNode(const rclcpp::NodeOptions & node_options);
+  ~KinematicEvaluatorNode();
+
+  /**
+   * @brief callback on vehicle twist message
+   * @param [in] twist_msg twist message
+   */
+  void onOdom(const nav_msgs::msg::Odometry::SharedPtr msg);
+
+  /**
+   * @brief publish the given metric statistic
+   */
+  DiagnosticStatus generateDiagnosticStatus(
+    const Metric & metric, const Stat<double> & metric_stat) const;
+
+private:
+  geometry_msgs::msg::Pose getCurrentEgoPose() const;
+
+  // ROS
+  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
+  rclcpp::Publisher<DiagnosticArray>::SharedPtr metrics_pub_;
+  std::unique_ptr<tf2_ros::Buffer> tf_buffer_ptr_;
+  std::unique_ptr<tf2_ros::TransformListener> tf_listener_ptr_;
+
+  // Parameters
+  std::string output_file_str_;
+
+  // Calculator
+  MetricsCalculator metrics_calculator_;
+  // Metrics
+  std::vector<Metric> metrics_;
+  std::deque<rclcpp::Time> stamps_;
+  std::array<std::deque<Stat<double>>, static_cast<size_t>(Metric::SIZE)> metric_stats_;
+  std::unordered_map<Metric, Stat<double>> metrics_dict_;
+};
+}  // namespace kinematic_diagnostics
+
+#endif  // KINEMATIC_EVALUATOR__KINEMATIC_EVALUATOR_NODE_HPP_

evaluator/kinematic_evaluator/include/kinematic_evaluator/metrics/kinematic_metrics.hpp

@@ -0,0 +1,39 @@
+// Copyright 2021 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 KINEMATIC_EVALUATOR__METRICS__KINEMATIC_METRICS_HPP_
+#define KINEMATIC_EVALUATOR__METRICS__KINEMATIC_METRICS_HPP_
+
+#include "kinematic_evaluator/stat.hpp"
+
+#include <nav_msgs/msg/odometry.hpp>
+
+namespace kinematic_diagnostics
+{
+namespace metrics
+{
+using nav_msgs::msg::Odometry;
+
+/**
+ * @brief calculate velocity deviation of the given trajectory from the reference trajectory
+ * @param [in] value new value
+ * @param [in] stat_prev input trajectory
+ * @return calculated statistics
+ */
+Stat<double> updateVelocityStats(const double & value, const Stat<double> stat_prev);
+
+}  // namespace metrics
+}  // namespace kinematic_diagnostics
+
+#endif  // KINEMATIC_EVALUATOR__METRICS__KINEMATIC_METRICS_HPP_

evaluator/kinematic_evaluator/include/kinematic_evaluator/metrics/metric.hpp

@@ -0,0 +1,62 @@
+// Copyright 2021 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 KINEMATIC_EVALUATOR__METRICS__METRIC_HPP_
+#define KINEMATIC_EVALUATOR__METRICS__METRIC_HPP_
+
+#include <iostream>
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+namespace kinematic_diagnostics
+{
+/**
+ * @brief Enumeration of velocity metrics
+ */
+enum class Metric {
+  velocity_stats,
+  SIZE,
+};
+
+static const std::unordered_map<std::string, Metric> str_to_metric = {
+  {"velocity_stats", Metric::velocity_stats}};
+static const std::unordered_map<Metric, std::string> metric_to_str = {
+  {Metric::velocity_stats, "velocity_stats"}};
+
+// Metrics descriptions
+static const std::unordered_map<Metric, std::string> metric_descriptions = {
+  {Metric::velocity_stats, "velocity_stats[m/s]"}};
+
+namespace details
+{
+static struct CheckCorrectMaps
+{
+  CheckCorrectMaps()
+  {
+    if (
+      str_to_metric.size() != static_cast<size_t>(Metric::SIZE) ||
+      metric_to_str.size() != static_cast<size_t>(Metric::SIZE) ||
+      metric_descriptions.size() != static_cast<size_t>(Metric::SIZE)) {
+      std::cerr << "[metrics/metrics.hpp] Maps are not defined for all metrics: ";
+      std::cerr << str_to_metric.size() << " " << metric_to_str.size() << " "
+                << metric_descriptions.size() << std::endl;
+    }
+  }
+} check;
+
+}  // namespace details
+}  // namespace kinematic_diagnostics
+
+#endif  // KINEMATIC_EVALUATOR__METRICS__METRIC_HPP_

evaluator/kinematic_evaluator/include/kinematic_evaluator/metrics_calculator.hpp

@@ -0,0 +1,56 @@
+// Copyright 2021 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 KINEMATIC_EVALUATOR__METRICS_CALCULATOR_HPP_
+#define KINEMATIC_EVALUATOR__METRICS_CALCULATOR_HPP_
+
+#include "kinematic_evaluator/metrics/metric.hpp"
+#include "kinematic_evaluator/parameters.hpp"
+#include "kinematic_evaluator/stat.hpp"
+
+#include "geometry_msgs/msg/pose.hpp"
+#include <nav_msgs/msg/odometry.hpp>
+
+namespace kinematic_diagnostics
+{
+using nav_msgs::msg::Odometry;
+
+class MetricsCalculator
+{
+public:
+  Parameters parameters;
+
+  MetricsCalculator() = default;
+
+  /**
+   * @brief calculate
+   * @param [in] metric Metric enum value
+   * @param [in] odom Odometry
+   * @return string describing the requested metric
+   */
+  Stat<double> calculate(const Metric metric, const Odometry & odom) const;
+
+  /**
+   * @brief update Metrics
+   * @param [in] metric Previous metric enum value
+   * @param [in] odom Odometry
+   * @return string describing the requested metric
+   */
+  Stat<double> updateStat(
+    const Metric metric, const Odometry & odom, const Stat<double> stat_prev) const;
+};  // class MetricsCalculator
+
+}  // namespace kinematic_diagnostics
+
+#endif  // KINEMATIC_EVALUATOR__METRICS_CALCULATOR_HPP_

evaluator/kinematic_evaluator/include/kinematic_evaluator/parameters.hpp

@@ -0,0 +1,34 @@
+// Copyright 2021 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 KINEMATIC_EVALUATOR__PARAMETERS_HPP_
+#define KINEMATIC_EVALUATOR__PARAMETERS_HPP_
+
+#include "kinematic_evaluator/metrics/metric.hpp"
+
+#include <array>
+
+namespace kinematic_diagnostics
+{
+/**
+ * @brief Enumeration of trajectory metrics
+ */
+struct Parameters
+{
+  std::array<bool, static_cast<size_t>(Metric::SIZE)> metrics{};  // default values to false
+};                                                                // struct Parameters
+
+}  // namespace kinematic_diagnostics
+
+#endif  // KINEMATIC_EVALUATOR__PARAMETERS_HPP_