diff --git a/launch/tier4_planning_launch/launch/planning.launch.xml b/launch/tier4_planning_launch/launch/planning.launch.xml
index 81d4b89a99e3f..62f34565deff1 100644
--- a/launch/tier4_planning_launch/launch/planning.launch.xml
+++ b/launch/tier4_planning_launch/launch/planning.launch.xml
@@ -50,6 +50,8 @@
+
+
@@ -72,10 +74,13 @@
-
+
-
-
+
+
+
+
+
diff --git a/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml
index 657792f7db01f..dbeaae2df622b 100644
--- a/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml
+++ b/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml
@@ -25,7 +25,7 @@
-
+
diff --git a/launch/tier4_planning_launch/package.xml b/launch/tier4_planning_launch/package.xml
index 45e2e186336a1..c0c7ac47145fe 100644
--- a/launch/tier4_planning_launch/package.xml
+++ b/launch/tier4_planning_launch/package.xml
@@ -66,7 +66,7 @@
obstacle_avoidance_plannerobstacle_cruise_plannerobstacle_stop_planner
- planning_error_monitor
+ planning_validatorrtc_auto_mode_managerscenario_selectorsurround_obstacle_checker
diff --git a/planning/planning_validator/CMakeLists.txt b/planning/planning_validator/CMakeLists.txt
new file mode 100644
index 0000000000000..34bdb695ae2c8
--- /dev/null
+++ b/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
+)
diff --git a/planning/planning_validator/README.md b/planning/planning_validator/README.md
new file mode 100644
index 0000000000000..08ddee246317f
--- /dev/null
+++ b/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. 0: publish the trajectory even if it is invalid, 1: stop publishing the trajectory, 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 |
diff --git a/planning/planning_validator/config/planning_validator.param.yaml b/planning/planning_validator/config/planning_validator.param.yaml
new file mode 100644
index 0000000000000..658a968906187
--- /dev/null
+++ b/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
diff --git a/planning/planning_validator/config/planning_validator_plotjugler_config.xml b/planning/planning_validator/config/planning_validator_plotjugler_config.xml
new file mode 100644
index 0000000000000..d331ed9985dfc
--- /dev/null
+++ b/planning/planning_validator/config/planning_validator_plotjugler_config.xml
@@ -0,0 +1,156 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/planning/planning_validator/image/planning_validator.drawio.svg b/planning/planning_validator/image/planning_validator.drawio.svg
new file mode 100644
index 0000000000000..070c1a6d68b81
--- /dev/null
+++ b/planning/planning_validator/image/planning_validator.drawio.svg
@@ -0,0 +1,215 @@
+
diff --git a/planning/planning_validator/include/planning_validator/debug_marker.hpp b/planning/planning_validator/include/planning_validator/debug_marker.hpp
new file mode 100644
index 0000000000000..4c7f6907a97b2
--- /dev/null
+++ b/planning/planning_validator/include/planning_validator/debug_marker.hpp
@@ -0,0 +1,61 @@
+// Copyright 2022 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_VALIDATOR__DEBUG_MARKER_HPP_
+#define PLANNING_VALIDATOR__DEBUG_MARKER_HPP_
+
+#include
+
+#include
+#include
+#include
+
+#include