feat(planning_validator): add planning validator package

launch/tier4_planning_launch/launch/planning.launch.xml

@@ -50,6 +50,8 @@
   <!-- motion velocity smoother -->
   <arg name="motion_velocity_smoother_param_path"/>
   <arg name="smoother_type_param_path"/>
+  <!-- planning validator -->
+  <arg name="planning_validator_param_path"/>
 
   <group>
     <push-ros-namespace namespace="planning"/>
@@ -72,10 +74,13 @@
       </include>
     </group>
 
-    <!-- planning error monitor -->
+    <!-- planning validator -->
     <group>
-      <push-ros-namespace namespace="planning_diagnostics"/>
-      <include file="$(find-pkg-share tier4_planning_launch)/launch/planning_diagnostics/planning_error_monitor.launch.xml"/>
+      <include file="$(find-pkg-share planning_validator)/launch/planning_validator.launch.xml">
+        <arg name="input_trajectory" value="/planning/scenario_planning/motion_velocity_smoother/trajectory"/>
+        <arg name="output_trajectory" value="/planning/scenario_planning/trajectory"/>
+        <arg name="planning_validator_param_path" value="$(var planning_validator_param_path)"/>
+      </include>
     </group>
   </group>
 </launch>

launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml

@@ -25,7 +25,7 @@
     <!-- motion velocity smoother -->
     <group>
       <set_remap from="~/input/trajectory" to="/planning/scenario_planning/scenario_selector/trajectory"/>
-      <set_remap from="~/output/trajectory" to="/planning/scenario_planning/trajectory"/>
+      <set_remap from="~/output/trajectory" to="/planning/scenario_planning/motion_velocity_smoother/trajectory"/>
       <include file="$(find-pkg-share motion_velocity_smoother)/launch/motion_velocity_smoother.launch.xml">
         <arg name="smoother_type" value="$(var smoother_type)"/>
         <arg name="common_param_path" value="$(var common_param_path)"/>

launch/tier4_planning_launch/package.xml

@@ -66,7 +66,7 @@
   <exec_depend>obstacle_avoidance_planner</exec_depend>
   <exec_depend>obstacle_cruise_planner</exec_depend>
   <exec_depend>obstacle_stop_planner</exec_depend>
-  <exec_depend>planning_error_monitor</exec_depend>
+  <exec_depend>planning_validator</exec_depend>
   <exec_depend>rtc_auto_mode_manager</exec_depend>
   <exec_depend>scenario_selector</exec_depend>
   <exec_depend>surround_obstacle_checker</exec_depend>

planning/planning_validator/CMakeLists.txt

@@ -0,0 +1,68 @@
+cmake_minimum_required(VERSION 3.14)
+project(planning_validator)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+ament_auto_add_library(planning_validator_helpers SHARED
+  src/utils.cpp
+  src/debug_marker.cpp
+)
+
+# planning validator
+ament_auto_add_library(planning_validator_component SHARED
+  include/planning_validator/planning_validator.hpp
+  src/planning_validator.cpp
+)
+target_link_libraries(planning_validator_component planning_validator_helpers)
+rclcpp_components_register_node(planning_validator_component
+  PLUGIN "planning_validator::PlanningValidator"
+  EXECUTABLE planning_validator_node
+)
+
+# invalid trajectory publisher (for debug)
+ament_auto_add_library(invalid_trajectory_publisher_node SHARED
+  src/invalid_trajectory_publisher/invalid_trajectory_publisher.cpp
+)
+rclcpp_components_register_node(invalid_trajectory_publisher_node
+  PLUGIN "planning_validator::InvalidTrajectoryPublisherNode"
+  EXECUTABLE invalid_trajectory_publisher
+)
+
+rosidl_generate_interfaces(
+  ${PROJECT_NAME}
+  "msg/PlanningValidatorStatus.msg"
+  DEPENDENCIES builtin_interfaces
+)
+
+# to use a message defined in the same package
+if(${rosidl_cmake_VERSION} VERSION_LESS 2.5.0)
+    rosidl_target_interfaces(planning_validator_component
+    ${PROJECT_NAME} "rosidl_typesupport_cpp")
+else()
+    rosidl_get_typesupport_target(
+            cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp")
+    target_link_libraries(planning_validator_component "${cpp_typesupport_target}")
+endif()
+
+if(BUILD_TESTING)
+  ament_add_ros_isolated_gtest(test_planning_validator
+    test/src/test_main.cpp
+    test/src/test_planning_validator_functions.cpp
+    test/src/test_planning_validator_helper.cpp
+    test/src/test_planning_validator_pubsub.cpp
+  )
+  ament_target_dependencies(test_planning_validator
+    rclcpp
+    autoware_auto_planning_msgs
+  )
+  target_link_libraries(test_planning_validator
+  planning_validator_component
+  )
+endif()
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+    config
+    launch
+)

planning/planning_validator/README.md

@@ -0,0 +1,77 @@
+# Planning Validator
+
+The `planning_validator` is a module that checks the validity of a trajectory before it is published. The status of the validation can be viewed in the `/diagnostics` and `/validation_status` topics. When an invalid trajectory is detected, the `planning_validator` will process the trajectory following the selected option: "0. publish the trajectory as it is", "1. stop publishing the trajectory", "2. publish the last validated trajectory".
+
+![planning_validator](./image/planning_validator.drawio.svg)
+
+## Supported features
+
+The following features are supported for trajectory validation and can have thresholds set by parameters:
+
+- **Invalid field** : e.g. Inf, Nan
+- **Trajectory points interval** : invalid if any of the distance of trajectory points is too large
+- **Curvature** : invalid if the trajectory has too sharp turns that is not feasible for the given vehicle kinematics
+- **Relative angle** : invalid if the yaw angle changes too fast in the sequence of trajectory points
+- **Lateral acceleration** : invalid if the expected lateral acceleration/deceleration is too large
+- **Longitudinal acceleration/deceleration** : invalid if the acceleration/deceleration in the trajectory point is too large
+- **Steering angle** : invalid if the expected steering value is too large estimated from trajectory curvature
+- **Steering angle rate** : invalid if the expected steering rate value is too large
+- **Velocity deviation** : invalid if the planning velocity is too far from the ego velocity
+- **Distance deviation** : invalid if the ego is too far from the trajectory
+
+The following features are to be implemented.
+
+- **(TODO) TTC calculation** : invalid if the expected time-to-collision is too short on the trajectory
+
+## Inputs/Outputs
+
+### Inputs
+
+The `planning_validator` takes in the following inputs:
+
+| Name                 | Type                                   | Description                                    |
+| -------------------- | -------------------------------------- | ---------------------------------------------- |
+| `~/input/kinematics` | nav_msgs/Odometry                      | ego pose and twist                             |
+| `~/input/trajectory` | autoware_auto_planning_msgs/Trajectory | target trajectory to be validated in this node |
+
+### Outputs
+
+It outputs the following:
+
+| Name                         | Type                                       | Description                                                               |
+| ---------------------------- | ------------------------------------------ | ------------------------------------------------------------------------- |
+| `~/output/trajectory`        | autoware_auto_planning_msgs/Trajectory     | validated trajectory                                                      |
+| `~/output/validation_status` | planning_validator/PlanningValidatorStatus | validator status to inform the reason why the trajectory is valid/invalid |
+| `/diagnostics`               | diagnostic_msgs/DiagnosticStatus           | diagnostics to report errors                                              |
+
+## Parameters
+
+The following parameters can be set for the `planning_validator`:
+
+### System parameters
+
+| Name                               | Type | Description                                                                                                                                                                                                                                | Default value |
+| :--------------------------------- | :--- | :----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ |
+| `invalid_trajectory_handling_type` | int  | set the operation when the invalid trajectory is detected. <br>0: publish the trajectory even if it is invalid, <br>1: stop publishing the trajectory, <br>2: publish the last validated trajectory.                                       | 0             |
+| `publish_diag`                     | bool | the Diag will be set to ERROR when the number of consecutive invalid trajectory exceeds this threshold. (For example, threshold = 1 means, even if the trajectory is invalid, the Diag will not be ERROR if the next trajectory is valid.) | true          |
+| `diag_error_count_threshold`       | int  | if true, diagnostics msg is published.                                                                                                                                                                                                     | true          |
+| `display_on_terminal`              | bool | show error msg on terminal                                                                                                                                                                                                                 | true          |
+
+### Algorithm parameters
+
+#### Thresholds
+
+The input trajectory is detected as invalid if the index exceeds the following thresholds.
+
+| Name                              | Type   | Description                                                                                                   | Default value |
+| :-------------------------------- | :----- | :------------------------------------------------------------------------------------------------------------ | :------------ |
+| `thresholds.interval`             | double | invalid threshold of the distance of two neighboring trajectory points                                        | 100.0         |
+| `thresholds.relative_angle`       | double | invalid threshold of the relative angle of two neighboring trajectory points                                  | 2.0           |
+| `thresholds.curvature`            | double | invalid threshold of the curvature in each trajectory point                                                   | 1.0           |
+| `thresholds.lateral_acc`          | double | invalid threshold of the lateral acceleration in each trajectory point                                        | 9.8           |
+| `thresholds.longitudinal_max_acc` | double | invalid threshold of the maximum longitudinal acceleration in each trajectory point                           | 9.8           |
+| `thresholds.longitudinal_min_acc` | double | invalid threshold of the minimum longitudinal deceleration in each trajectory point                           | -9.8          |
+| `thresholds.steering`             | double | invalid threshold of the steering angle in each trajectory point                                              | 1.414         |
+| `thresholds.steering_rate`        | double | invalid threshold of the steering angle rate in each trajectory point                                         | 10.0          |
+| `thresholds.velocity_deviation`   | double | invalid threshold of the velocity deviation between the ego velocity and the trajectory point closest to ego. | 100.0         |
+| `thresholds.distance_deviation`   | double | invalid threshold of the distance deviation between the ego position and the trajectory point closest to ego. | 100.0         |

planning/planning_validator/config/planning_validator.param.yaml

@@ -0,0 +1,28 @@
+/**:
+  ros__parameters:
+    # Operation option when invalid trajectory is detected
+    #  0: publish the trajectory even if it is invalid
+    #  1: stop publishing the trajectory
+    #  2: publish the last validated trajectory
+    invalid_trajectory_handling_type: 0
+
+    publish_diag: true  # if true, diagnostic msg is published
+
+    # If the number of consecutive invalid trajectory exceeds this threshold, the Diag will be set to ERROR.
+    # (For example, threshold = 1 means, even if the trajectory is invalid, Diag will not be ERROR if
+    #  the next trajectory is valid.)
+    diag_error_count_threshold: 0
+
+    display_on_terminal: true # show error msg on terminal
+
+    thresholds:
+      interval: 100.0
+      relative_angle: 2.0  # (= 115 degree)
+      curvature: 1.0
+      lateral_acc: 9.8
+      longitudinal_max_acc: 9.8
+      longitudinal_min_acc: -9.8
+      steering: 1.414
+      steering_rate: 10.0
+      velocity_deviation: 100.0
+      distance_deviation: 100.0