feat: add planning error monitor package (autowarefoundation#34)

* port-planning-diagnostics-from-develop (autowarefoundation#1966)

* [planning diagnostics] Fix porting mistake (autowarefoundation#2021)

* add invalid trajectory publisher (autowarefoundation#2067)

* Add missing dependencies of diagnostic_updater (autowarefoundation#2242)

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* add debug marker of planning error monitor (autowarefoundation#2251)

* add debug marker of planning error monitor

* fix format

* fix debug marker

* add color for error marker order

* update readme

* add debug topics

Co-authored-by: taikitanaka <ttatcoder@outlook.jp>

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

* port planning error monitor package (autowarefoundation#491)

* add planning error monitor package

* remove colcon ignore

* change test msgs

* fix format

* ci(pre-commit): autofix

* feat: add launchj option

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com>
Co-authored-by: Takeshi Miura <57553950+1222-takeshi@users.noreply.github.com>
Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com>
Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp>
Co-authored-by: purewater0901 <43805014+purewater0901@users.noreply.github.com>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

planning/planning_error_monitor/CMakeLists.txt

@@ -0,0 +1,54 @@
+cmake_minimum_required(VERSION 3.5)
+project(planning_error_monitor)
+
+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(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+ament_auto_add_library(planning_error_monitor_node SHARED
+  src/planning_error_monitor_node.cpp
+  src/debug_marker.cpp
+)
+rclcpp_components_register_node(planning_error_monitor_node
+  PLUGIN "planning_diagnostics::PlanningErrorMonitorNode"
+  EXECUTABLE planning_error_monitor
+)
+
+ament_auto_add_library(invalid_trajectory_publisher_node SHARED
+  src/invalid_trajectory_publisher.cpp
+)
+rclcpp_components_register_node(invalid_trajectory_publisher_node
+  PLUGIN "planning_diagnostics::InvalidTrajectoryPublisherNode"
+  EXECUTABLE invalid_trajectory_publisher
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+  ament_add_gtest(test_planning_error_monitor
+    test/src/test_main.cpp
+    test/src/test_planning_error_monitor_functions.cpp
+    test/src/test_planning_error_monitor_helper.hpp
+    test/src/test_planning_error_monitor_pubsub.cpp
+  )
+  ament_target_dependencies(test_planning_error_monitor
+    rclcpp
+    autoware_auto_planning_msgs
+  )
+  target_link_libraries(test_planning_error_monitor
+    planning_error_monitor_node
+  )
+endif()
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+    launch
+)

planning/planning_error_monitor/README.md

@@ -0,0 +1,80 @@
+# Planning Error Monitor
+
+## Purpose
+
+`planning_error_monitor` checks a trajectory that if it has any invalid numerical values in its positions, twist and accel values. In addition, it also checks the distance between any two consecutive points and curvature value at a certain point. This package basically monitors if a trajectory, which is generated by planning module, has any unexpected errors.
+
+## Inner-workings / Algorithms
+
+![flow_chart_image](./media/flowchart.png)
+
+### Point Value Checker (onTrajectoryPointValueChecker)
+
+This function checks position, twist and accel values of all points on a trajectory. If they have `Nan` or `Infinity`, this function outputs error status.
+
+### Interval Checker (onTrajectoryIntervalChecker)
+
+This function computes interval distance between two consecutive points, and will output error messages if the distance is over the `interval_threshold`.
+
+### Curvature Checker (onTrajectoryCurvatureChecker)
+
+This function checks if the curvature at each point on a trajectory has an appropriate value. Calculation details are described in the following picture. First, we choose one point(green point in the picture) that are 1.0[m] behind the current point. Then we get a point(blue point in the picture) 1.0[m] ahead of the current point. Using these three points, we calculate the curvature by [this method](https://en.wikipedia.org/wiki/Menger_curvature).
+
+### Relative Angle Checker (onTrajectoryRelativeAngleChecker)
+
+This function checks if the relative angle at point1 generated from point2 and 3 on a trajectory has an appropriate value.
+
+![curvature_calculation_diagram](./media/curvature_calculation_diagram.svg)
+
+## Inputs / Outputs
+
+### Input
+
+| Name                 | Type                                     | Description                            |
+| -------------------- | ---------------------------------------- | -------------------------------------- |
+| `~/input/trajectory` | `autoware_auto_planning_msgs/Trajectory` | Planned Trajectory by planning modules |
+
+### Output
+
+| Name             | Type                              | Description           |
+| ---------------- | --------------------------------- | --------------------- |
+| `/diagnostics`   | `diagnostic_msgs/DiagnosticArray` | diagnostics outputs   |
+| `~/debug/marker` | `visualization_msgs/MarkerArray`  | visualization markers |
+
+## Parameters
+
+| Name                      | Type     | Description                           | Default value |
+| :------------------------ | :------- | :------------------------------------ | :------------ |
+| `error_interval`          | `double` | Error Interval Distance Threshold [m] | 100.0         |
+| `error_curvature`         | `double` | Error Curvature Threshold             | 1.0           |
+| `error_sharp_angle`       | `double` | Error Sharp Angle Threshold           | $\pi$/4       |
+| `ignore_too_close_points` | `double` | Ignore Too Close Distance Threshold   | 0.005         |
+
+## Visualization
+
+When the trajectory error occurs, markers for visualization are published at the topic `~/debug/marker`.
+
+- trajectory_interval: An error occurs when the distance between two points exceeds a certain large value. The two points where the error occurred will be visualized.
+- trajectory_curvature: An error occurs when the curvature exceeds a certain large value. The three points used to calculate the curvature will be visualized.
+- trajectory_relative_angle: An error occurs when the angle in the direction of the path point changes significantly. The three points used to calculate the relative angle will be visualized.
+
+## Assumptions / Known limits
+
+- It cannot compute curvature values at start and end points of the trajectory.
+- If trajectory points are too close, curvature calculation might output incorrect values.
+
+## Future extensions / Unimplemented parts
+
+- Collision checker with obstacles may be implemented in the future.
+
+## Error detection and handling
+
+For the onsite validation, you can use the `invalid_trajectory_publisher` node. Please launch the node with the following command when the target trajectory is being published.
+
+```bash
+ros2 launch planning_error_monitor invalid_trajectory_publisher.launch.xml
+```
+
+This node subscribes the target trajectory, inserts the invalid point, and publishes it with the same name. The invalid trajectory is supposed to be detected by the `planning_error_monitor`.
+
+Limitation: Once the `invalid_trajectory_publisher` receives the trajectory, it will turn off the subscriber. This is to prevent the trajectory from looping in the same node, therefore, only the one pattern of invalid trajectory is generated.

planning/planning_error_monitor/include/planning_error_monitor/debug_marker.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 PLANNING_ERROR_MONITOR__DEBUG_MARKER_HPP_
+#define PLANNING_ERROR_MONITOR__DEBUG_MARKER_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <visualization_msgs/msg/marker.hpp>
+#include <visualization_msgs/msg/marker_array.hpp>
+
+#include <map>
+#include <memory>
+#include <string>
+#include <vector>
+
+class PlanningErrorMonitorDebugNode
+{
+public:
+  PlanningErrorMonitorDebugNode();
+
+  void initialize(rclcpp::Node * node);
+  void pushPoseMarker(const geometry_msgs::msg::Pose & pose, const std::string & ns, int id = 0);
+  void clearPoseMarker(const std::string & ns);
+  void publish();
+
+private:
+  rclcpp::Node * node_;
+  visualization_msgs::msg::MarkerArray marker_array_;
+  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr debug_viz_pub_;
+  std::map<std::string, int> marker_id_;
+  bool initialized = false;
+
+  int getMarkerId(const std::string & ns)
+  {
+    if (marker_id_.count(ns) == 0) {
+      marker_id_[ns] = 0.0;
+    }
+    return marker_id_[ns]++;
+  }
+
+  void clearMarkerId(const std::string & ns) { marker_id_[ns] = 0; }
+};
+
+#endif  // PLANNING_ERROR_MONITOR__DEBUG_MARKER_HPP_

planning/planning_error_monitor/include/planning_error_monitor/invalid_trajectory_publisher.hpp

@@ -0,0 +1,46 @@
+// 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 PLANNING_ERROR_MONITOR__INVALID_TRAJECTORY_PUBLISHER_HPP_
+#define PLANNING_ERROR_MONITOR__INVALID_TRAJECTORY_PUBLISHER_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <autoware_auto_planning_msgs/msg/trajectory.hpp>
+
+#include <string>
+
+namespace planning_diagnostics
+{
+using autoware_auto_planning_msgs::msg::Trajectory;
+using autoware_auto_planning_msgs::msg::TrajectoryPoint;
+
+class InvalidTrajectoryPublisherNode : public rclcpp::Node
+{
+public:
+  explicit InvalidTrajectoryPublisherNode(const rclcpp::NodeOptions & node_options);
+  void onCurrentTrajectory(const Trajectory::ConstSharedPtr msg);
+  void onTimer();
+
+private:
+  // ROS
+  rclcpp::Subscription<Trajectory>::SharedPtr traj_sub_;
+  rclcpp::Publisher<Trajectory>::SharedPtr traj_pub_;
+  rclcpp::TimerBase::SharedPtr timer_;
+
+  Trajectory::ConstSharedPtr current_trajectory_ = nullptr;
+};
+}  // namespace planning_diagnostics
+
+#endif  // PLANNING_ERROR_MONITOR__INVALID_TRAJECTORY_PUBLISHER_HPP_

planning/planning_error_monitor/include/planning_error_monitor/planning_error_monitor_node.hpp

@@ -0,0 +1,81 @@
+// 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 PLANNING_ERROR_MONITOR__PLANNING_ERROR_MONITOR_NODE_HPP_
+#define PLANNING_ERROR_MONITOR__PLANNING_ERROR_MONITOR_NODE_HPP_
+
+#include "planning_error_monitor/debug_marker.hpp"
+
+#include <diagnostic_updater/diagnostic_updater.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <autoware_auto_planning_msgs/msg/trajectory.hpp>
+#include <diagnostic_msgs/msg/diagnostic_array.hpp>
+
+#include <string>
+
+namespace planning_diagnostics
+{
+using autoware_auto_planning_msgs::msg::Trajectory;
+using autoware_auto_planning_msgs::msg::TrajectoryPoint;
+using diagnostic_updater::DiagnosticStatusWrapper;
+using diagnostic_updater::Updater;
+
+class PlanningErrorMonitorNode : public rclcpp::Node
+{
+public:
+  explicit PlanningErrorMonitorNode(const rclcpp::NodeOptions & node_options);
+
+  void onCurrentTrajectory(const Trajectory::ConstSharedPtr msg);
+  void onTimer();
+
+  void onTrajectoryPointValueChecker(DiagnosticStatusWrapper & stat);
+  void onTrajectoryIntervalChecker(DiagnosticStatusWrapper & stat);
+  void onTrajectoryCurvatureChecker(DiagnosticStatusWrapper & stat);
+  void onTrajectoryRelativeAngleChecker(DiagnosticStatusWrapper & stat);
+  static bool checkTrajectoryRelativeAngle(
+    const Trajectory & traj, const double relative_angle_threshold, const double min_dist_threshold,
+    std::string & error_msg, PlanningErrorMonitorDebugNode & debug_marker);
+
+  static bool checkTrajectoryPointValue(const Trajectory & traj, std::string & error_msg);
+  static bool checkTrajectoryInterval(
+    const Trajectory & traj, const double & interval_threshold, std::string & error_msg,
+    PlanningErrorMonitorDebugNode & debug_marker);
+  static bool checkTrajectoryCurvature(
+    const Trajectory & traj, const double & curvature_threshold, std::string & error_msg,
+    PlanningErrorMonitorDebugNode & debug_marker);
+
+private:
+  static bool checkFinite(const TrajectoryPoint & p);
+  static size_t getIndexAfterDistance(
+    const Trajectory & traj, const size_t curr_id, const double distance);
+
+  // ROS
+  rclcpp::Subscription<Trajectory>::SharedPtr traj_sub_;
+  rclcpp::TimerBase::SharedPtr timer_;
+  Updater updater_{this};
+
+  Trajectory::ConstSharedPtr current_trajectory_;
+
+  // Parameter
+  double error_interval_;
+  double error_curvature_;
+  double error_sharp_angle_;
+  double ignore_too_close_points_;
+
+  PlanningErrorMonitorDebugNode debug_marker_;
+};
+}  // namespace planning_diagnostics
+
+#endif  // PLANNING_ERROR_MONITOR__PLANNING_ERROR_MONITOR_NODE_HPP_

planning/planning_error_monitor/launch/invalid_trajectory_publisher.launch.xml

@@ -0,0 +1,9 @@
+<launch>
+  <arg name="input/trajectory" default="/planning/scenario_planning/trajectory" />
+  <arg name="output/trajectory" default="/planning/scenario_planning/trajectory" />
+
+  <node name="invalid_trajectory_publisher" exec="invalid_trajectory_publisher" pkg="planning_error_monitor" output="screen">
+    <remap from="~/input/trajectory" to="$(var input/trajectory)" />
+    <remap from="~/output/trajectory" to="$(var output/trajectory)" />
+  </node>
+</launch>

planning/planning_error_monitor/launch/planning_error_monitor.launch.xml

@@ -0,0 +1,15 @@
+<launch>
+    <!--select run mode for analyze trajectory-->
+  <arg name="run_mode" default="final_output" description="options: final_output, avoidance,surround_obstacle,obstacle_stop"/>
+  <arg name="input/trajectory" default="/planning/scenario_planning/trajectory" />
+  <let name="input/trajectory" value="/planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/trajectory" if="$(eval &quot;'$(var run_mode)'=='avoidance'&quot;)"/>
+  <let name="input/trajectory" value="/planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/trajectory" if="$(eval &quot;'$(var run_mode)'=='surround_obstacle'&quot;)"/>
+  <let name="input/trajectory" value="/planning/scenario_planning/lane_driving/trajectory" if="$(eval &quot;'$(var run_mode)'=='obstacle_stop'&quot;)"/>
+  <node name="planning_error_monitor" exec="planning_error_monitor" pkg="planning_error_monitor" output="screen">
+    <remap from="~/input/trajectory" to="$(var input/trajectory)" />
+    <param name="error_interval" value="100.0" />
+    <param name="error_curvature" value="2.0" />
+    <param name="error_sharp_angle" value="0.785398"/>
+    <param name="ignore_too_close_points" value="0.01" />
+  </node>
+</launch>

planning/planning_error_monitor/package.xml

@@ -0,0 +1,30 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>planning_error_monitor</name>
+  <version>0.1.0</version>
+  <description>ros node for monitoring planning error </description>
+
+  <maintainer email="yutaka.shimizu@tier4.jp">Yutaka Shimizu</maintainer>
+
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <depend>autoware_auto_planning_msgs</depend>
+  <depend>autoware_utils</depend>
+  <depend>diagnostic_updater</depend>
+  <depend>geometry_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>tf2</depend>
+  <depend>tf2_ros</depend>
+  <depend>visualization_msgs</depend>
+
+  <test_depend>ament_cmake_gtest</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>