diff --git a/planning/planning_validator/CMakeLists.txt b/planning/planning_validator/CMakeLists.txt new file mode 100644 index 0000000000000..1aa7d7f8cd0b4 --- /dev/null +++ b/planning/planning_validator/CMakeLists.txt @@ -0,0 +1,46 @@ +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 +) + +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 +) + +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) +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..0890b32da6330 --- /dev/null +++ b/planning/planning_validator/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. 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..edd76ac4901a3 --- /dev/null +++ b/planning/planning_validator/config/planning_validator.param.yaml @@ -0,0 +1,12 @@ +/**: + ros__parameters: + interval_threshold: 100.0 + relative_angle_threshold: 10.0 + curvature_threshold: 100.0 + lateral_acc_threshold: 100.0 + longitudinal_max_acc_threshold: 100.0 + longitudinal_min_acc_threshold: -100.0 + steering_threshold: 100.0 + steering_rate_threshold: 100.0 + velocity_deviation_threshold: 100.0 + distance_deviation_threshold: 100.0 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..dfbe053099b52 --- /dev/null +++ b/planning/planning_validator/include/planning_validator/debug_marker.hpp @@ -0,0 +1,58 @@ +// 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_VALIDATOR__DEBUG_MARKER_HPP_ +#define PLANNING_VALIDATOR__DEBUG_MARKER_HPP_ + +#include + +#include +#include +#include + +#include +#include +#include +#include + +class PlanningValidatorDebugPosePublisher +{ +public: + explicit PlanningValidatorDebugPosePublisher(rclcpp::Node * node); + + void pushPoseMarker( + const autoware_auto_planning_msgs::msg::TrajectoryPoint & p, const std::string & ns, + int id = 0); + 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::SharedPtr debug_viz_pub_; + std::map marker_id_; + + 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_VALIDATOR__DEBUG_MARKER_HPP_ diff --git a/planning/planning_validator/include/planning_validator/planning_validator.hpp b/planning/planning_validator/include/planning_validator/planning_validator.hpp new file mode 100644 index 0000000000000..bad4cc8832c8e --- /dev/null +++ b/planning/planning_validator/include/planning_validator/planning_validator.hpp @@ -0,0 +1,109 @@ +// 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_VALIDATOR__PLANNING_VALIDATOR_HPP_ +#define PLANNING_VALIDATOR__PLANNING_VALIDATOR_HPP_ + +#include "planning_validator/debug_marker.hpp" +#include "planning_validator/msg/planning_validator_status.hpp" +#include "vehicle_info_util/vehicle_info_util.hpp" + +#include +#include + +#include +#include +#include + +#include +#include + +namespace planning_validator +{ +using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using diagnostic_updater::DiagnosticStatusWrapper; +using diagnostic_updater::Updater; +using nav_msgs::msg::Odometry; +using planning_validator::msg::PlanningValidatorStatus; + +struct ValidationParams +{ + double interval_threshold; + double relative_angle_threshold; + double curvature_threshold; + double lateral_acc_threshold; + double longitudinal_max_acc_threshold; + double longitudinal_min_acc_threshold; + double steering_threshold; + double steering_rate_threshold; + double velocity_deviation_threshold; + double distance_deviation_threshold; +}; + +class PlanningValidator : public rclcpp::Node +{ +public: + explicit PlanningValidator(const rclcpp::NodeOptions & options); + + void onTrajectory(const Trajectory::ConstSharedPtr msg); + +private: + void setupDiag(); + void setDiagStatus( + const std::string & name, const bool ok_condition, const std::string & error_msg); + void setupParameters(); + + bool isDataReady(); + + void validate(const Trajectory & trajectory); + bool checkValidFiniteValue(const Trajectory & trajectory); + bool checkValidInterval(const Trajectory & trajectory); + bool checkValidRelativeAngle(const Trajectory & trajectory); + bool checkValidCurvature(const Trajectory & trajectory); + bool checkValidLateralAcceleration(const Trajectory & trajectory); + bool checkValidMaxLongitudinalAcceleration(const Trajectory & trajectory); + bool checkValidMinLongitudinalAcceleration(const Trajectory & trajectory); + bool checkValidSteering(const Trajectory & trajectory); + bool checkValidSteeringRate(const Trajectory & trajectory); + bool checkValidVelocityDeviation(const Trajectory & trajectory); + bool checkValidDistanceDeviation(const Trajectory & trajectory); + + void publishTrajectory(); + void publishDebugInfo(); + + rclcpp::Subscription::SharedPtr sub_kinematics_; + rclcpp::Subscription::SharedPtr sub_traj_; + rclcpp::Publisher::SharedPtr pub_traj_; + rclcpp::Publisher::SharedPtr pub_status_; + + Updater diag_updater_{this}; + + PlanningValidatorStatus validation_status_; + ValidationParams validation_params_; // for thresholds + + vehicle_info_util::VehicleInfo vehicle_info_; + + bool isAllValid(const PlanningValidatorStatus & status); + + Trajectory::ConstSharedPtr current_trajectory_; + Trajectory::ConstSharedPtr previous_published_trajectory_; + + Odometry::ConstSharedPtr current_kinematics_; + + std::shared_ptr debugger_; +}; +} // namespace planning_validator + +#endif // PLANNING_VALIDATOR__PLANNING_VALIDATOR_HPP_ diff --git a/planning/planning_validator/include/planning_validator/utils.hpp b/planning/planning_validator/include/planning_validator/utils.hpp new file mode 100644 index 0000000000000..320672c55a57a --- /dev/null +++ b/planning/planning_validator/include/planning_validator/utils.hpp @@ -0,0 +1,57 @@ +// 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_VALIDATOR__UTILS_HPP_ +#define PLANNING_VALIDATOR__UTILS_HPP_ + +#include + +#include + +#include +#include +#include + +namespace planning_validator +{ +using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_auto_planning_msgs::msg::TrajectoryPoint; + +std::pair getMaxValAndIdx(const std::vector & v); + +std::pair getMinValAndIdx(const std::vector & v); + +std::pair getAbsMaxValAndIdx(const std::vector & v); + +Trajectory resampleTrajectory(const Trajectory & trajectory, const double min_interval); + +std::vector calcCurvature(const Trajectory & trajectory); + +std::vector calcIntervalDistance(const Trajectory & trajectory); + +std::vector calcLateralAcceleration(const Trajectory & trajectory); + +std::vector getLongitudinalAccArray(const Trajectory & trajectory); + +std::vector calcRelativeAngles(const Trajectory & trajectory); + +std::vector calcSteeringAngles(const Trajectory & trajectory, const double wheelbase); + +std::vector calcSteeringRates(const Trajectory & trajectory, const double wheelbase); + +bool checkFinite(const TrajectoryPoint & point); + +} // namespace planning_validator + +#endif // PLANNING_VALIDATOR__UTILS_HPP_ diff --git a/planning/planning_validator/launch/planning_validator.launch.xml b/planning/planning_validator/launch/planning_validator.launch.xml new file mode 100644 index 0000000000000..42a47b898c5f8 --- /dev/null +++ b/planning/planning_validator/launch/planning_validator.launch.xml @@ -0,0 +1,13 @@ + + + + + + + + + + + + + diff --git a/planning/planning_validator/msg/PlanningValidatorStatus.msg b/planning/planning_validator/msg/PlanningValidatorStatus.msg new file mode 100644 index 0000000000000..95389b1db0c2b --- /dev/null +++ b/planning/planning_validator/msg/PlanningValidatorStatus.msg @@ -0,0 +1,26 @@ +builtin_interfaces/Time stamp + +# states +bool is_valid_finite_value +bool is_valid_interval +bool is_valid_relative_angle +bool is_valid_curvature +bool is_valid_lateral_acc +bool is_valid_longitudinal_max_acc +bool is_valid_longitudinal_min_acc +bool is_valid_steering +bool is_valid_steering_rate +bool is_valid_velocity_deviation +bool is_valid_distance_deviation + +# values +float64 max_interval_distance +float64 max_relative_angle +float64 max_curvature +float64 max_lateral_acc +float64 max_longitudinal_acc +float64 min_longitudinal_acc +float64 max_steering +float64 max_steering_rate +float64 velocity_deviation +float64 distance_deviation diff --git a/planning/planning_validator/package.xml b/planning/planning_validator/package.xml new file mode 100644 index 0000000000000..c988fc8a33c7a --- /dev/null +++ b/planning/planning_validator/package.xml @@ -0,0 +1,37 @@ + + + + planning_validator + 0.1.0 + ros node for planning_validator + Takamasa Horibe + Apache License 2.0 + + Takamasa Horibe + + ament_cmake_auto + autoware_cmake + rosidl_default_generators + + autoware_auto_planning_msgs + diagnostic_updater + geometry_msgs + motion_utils + nav_msgs + rclcpp + rclcpp_components + tier4_autoware_utils + vehicle_info_util + visualization_msgs + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + rosidl_default_runtime + rosidl_interface_packages + + + ament_cmake + + diff --git a/planning/planning_validator/src/debug_marker.cpp b/planning/planning_validator/src/debug_marker.cpp new file mode 100644 index 0000000000000..73a1c5162c915 --- /dev/null +++ b/planning/planning_validator/src/debug_marker.cpp @@ -0,0 +1,83 @@ +// 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. + +#include "planning_validator/debug_marker.hpp" + +#include + +#include +#include +#include + +using visualization_msgs::msg::Marker; + +PlanningValidatorDebugPosePublisher::PlanningValidatorDebugPosePublisher(rclcpp::Node * node) +: node_(node) +{ + debug_viz_pub_ = + node_->create_publisher("~/debug/marker", 1); +} + +void PlanningValidatorDebugPosePublisher::pushPoseMarker( + const autoware_auto_planning_msgs::msg::TrajectoryPoint & p, const std::string & ns, int id) +{ + pushPoseMarker(p.pose, ns, id); +} + +void PlanningValidatorDebugPosePublisher::pushPoseMarker( + const geometry_msgs::msg::Pose & pose, const std::string & ns, int id) +{ + // append arrow marker + Marker marker{}; + marker.header.frame_id = "map"; + marker.header.stamp = node_->get_clock()->now(); + marker.ns = ns; + marker.id = getMarkerId(ns); + marker.lifetime = rclcpp::Duration::from_seconds(0.2); + marker.type = Marker::ARROW; + marker.action = Marker::ADD; + marker.pose = pose; + marker.scale = tier4_autoware_utils::createMarkerScale(0.2, 0.1, 0.3); + if (id == 0) // Red + { + marker.color = tier4_autoware_utils::createMarkerColor(1.0, 0.0, 0.0, 0.999); + } + if (id == 1) // Green + { + marker.color = tier4_autoware_utils::createMarkerColor(0.0, 1.0, 0.0, 0.999); + } + if (id == 2) // Blue + { + marker.color = tier4_autoware_utils::createMarkerColor(0.0, 0.0, 1.0, 0.999); + } + marker_array_.markers.push_back(marker); +} + +void PlanningValidatorDebugPosePublisher::clearPoseMarker(const std::string & ns) +{ + clearMarkerId(ns); + + if (marker_array_.markers.empty()) { + return; + } + + for (int i = static_cast(marker_array_.markers.size()) - 1; i >= 0; i--) { + if (marker_array_.markers.at(i).ns == ns) { + // clear markers with the namespace "ns" + marker_array_.markers.erase(marker_array_.markers.begin() + i); + } + } +} + +void PlanningValidatorDebugPosePublisher::publish() { debug_viz_pub_->publish(marker_array_); } diff --git a/planning/planning_validator/src/planning_validator.cpp b/planning/planning_validator/src/planning_validator.cpp new file mode 100644 index 0000000000000..db5b1f4753965 --- /dev/null +++ b/planning/planning_validator/src/planning_validator.cpp @@ -0,0 +1,359 @@ +// 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. + +#include "planning_validator/planning_validator.hpp" + +#include "planning_validator/utils.hpp" + +#include +#include + +#include +#include +#include + +namespace planning_validator +{ +using diagnostic_msgs::msg::DiagnosticStatus; + +PlanningValidator::PlanningValidator(const rclcpp::NodeOptions & options) +: Node("planning_validator", options) +{ + using std::placeholders::_1; + + sub_kinematics_ = create_subscription( + "~/input/kinematics", 1, + [this](const Odometry::ConstSharedPtr msg) { current_kinematics_ = msg; }); + sub_traj_ = create_subscription( + "~/input/trajectory", 1, std::bind(&PlanningValidator::onTrajectory, this, _1)); + + pub_traj_ = create_publisher("~/output/trajectory", 1); + pub_status_ = create_publisher("~/output/validation_status", 1); + + debugger_ = std::make_shared(this); + + setupParameters(); +} + +void PlanningValidator::setupParameters() +{ + const auto dp = [this](const auto & s) { + try { + return declare_parameter(s); + } catch (const std::exception & e) { + RCLCPP_FATAL(get_logger(), "failed to declare param: %s. msg: %s", s, e.what()); + std::exit(1); + } + }; + auto & p = validation_params_; + p.interval_threshold = dp("interval_threshold"); + p.relative_angle_threshold = dp("relative_angle_threshold"); + p.curvature_threshold = dp("curvature_threshold"); + p.lateral_acc_threshold = dp("lateral_acc_threshold"); + p.longitudinal_max_acc_threshold = dp("longitudinal_max_acc_threshold"); + p.longitudinal_min_acc_threshold = dp("longitudinal_min_acc_threshold"); + p.steering_threshold = dp("steering_threshold"); + p.steering_rate_threshold = dp("steering_rate_threshold"); + p.velocity_deviation_threshold = dp("velocity_deviation_threshold"); + p.distance_deviation_threshold = dp("distance_deviation_threshold"); + + try { + vehicle_info_ = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); + } catch (...) { + RCLCPP_ERROR(get_logger(), "failed to get vehicle info. use default value."); + vehicle_info_.wheel_base_m = 4.0; + } +} + +void PlanningValidator::setupDiag() +{ + diag_updater_.setHardwareID("planning_validator"); + + const auto & s = validation_status_; + setDiagStatus("finite", s.is_valid_finite_value, "infinite value is found"); + setDiagStatus("interval", s.is_valid_interval, "points interval is too long"); + setDiagStatus("relative_angle", s.is_valid_relative_angle, "relative angle is too large"); + setDiagStatus("curvature", s.is_valid_curvature, "curvature is too large"); + setDiagStatus( + "lateral_acceleration", s.is_valid_lateral_acc, "lateral acceleration is too large"); + setDiagStatus("acceleration", s.is_valid_longitudinal_max_acc, "acceleration is too large"); + setDiagStatus("deceleration", s.is_valid_longitudinal_min_acc, "deceleration is too large"); + setDiagStatus("steering", s.is_valid_steering, "expected steering is too large"); + setDiagStatus("steering_rate", s.is_valid_steering_rate, "expected steering rate is too large"); + setDiagStatus( + "velocity_deviation", s.is_valid_velocity_deviation, + "velocity deviation between planning and actual is too large"); +} + +void PlanningValidator::setDiagStatus( + const std::string & name, const bool is_ok, const std::string & error_msg) +{ + diag_updater_.add( + "trajectory_validation_" + name, [this, is_ok, &error_msg](DiagnosticStatusWrapper & stat) { + is_ok ? stat.summary(DiagnosticStatus::OK, "ok") + : stat.summary(DiagnosticStatus::ERROR, error_msg); + }); +} + +bool PlanningValidator::isDataReady() +{ + const auto waiting = [this](const auto s) { + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 5000, "waiting for %s", s); + return false; + }; + + if (!current_kinematics_) { + return waiting("current_kinematics_"); + } + if (!current_trajectory_) { + return waiting("current_trajectory_"); + } + return true; +} + +void PlanningValidator::onTrajectory(const Trajectory::ConstSharedPtr msg) +{ + current_trajectory_ = msg; + + if (!isDataReady()) return; + + validate(*current_trajectory_); + + diag_updater_.force_update(); + + publishTrajectory(); + + publishDebugInfo(); +} + +void PlanningValidator::publishTrajectory() +{ + // Validation check is all green. Publish the trajectory. + if (isAllValid(validation_status_)) { + pub_traj_->publish(*current_trajectory_); + previous_published_trajectory_ = current_trajectory_; + return; + } + + // invalid factor is found. Publish previous trajectory. + if (previous_published_trajectory_) { + pub_traj_->publish(*previous_published_trajectory_); + RCLCPP_ERROR(get_logger(), "Invalid Trajectory is detected. Use previous trajectory."); + return; + } + + // current_path is invalid and no previous trajectory available. + // trajectory is not published. + RCLCPP_ERROR(get_logger(), "Invalid Trajectory is detected. Trajectory is not published."); + return; +} + +void PlanningValidator::publishDebugInfo() +{ + validation_status_.stamp = get_clock()->now(); + pub_status_->publish(validation_status_); + debugger_->publish(); +} + +void PlanningValidator::validate(const Trajectory & trajectory) +{ + auto & s = validation_status_; + + s.is_valid_finite_value = checkValidFiniteValue(trajectory); + s.is_valid_interval = checkValidInterval(trajectory); + s.is_valid_lateral_acc = checkValidLateralAcceleration(trajectory); + s.is_valid_longitudinal_max_acc = checkValidMaxLongitudinalAcceleration(trajectory); + s.is_valid_longitudinal_min_acc = checkValidMinLongitudinalAcceleration(trajectory); + s.is_valid_velocity_deviation = checkValidVelocityDeviation(trajectory); + s.is_valid_distance_deviation = checkValidDistanceDeviation(trajectory); + + // use resampled trajectory because the following metrics can not be evaluated for closed points. + // Note: do not interpolate to keep original trajectory shape. + constexpr auto min_interval = 1.0; + const auto resampled = resampleTrajectory(trajectory, min_interval); + + s.is_valid_relative_angle = checkValidRelativeAngle(resampled); + s.is_valid_curvature = checkValidCurvature(resampled); + s.is_valid_steering = checkValidSteering(resampled); + s.is_valid_steering_rate = checkValidSteeringRate(resampled); +} + +bool PlanningValidator::checkValidFiniteValue(const Trajectory & trajectory) +{ + for (const auto & p : trajectory.points) { + if (!checkFinite(p)) return false; + } + return true; +} + +bool PlanningValidator::checkValidInterval(const Trajectory & trajectory) +{ + // // debug_marker_.clearPoseMarker("trajectory_interval"); + + const auto interval_distances = calcIntervalDistance(trajectory); + const auto [max_interval_distance, i] = getAbsMaxValAndIdx(interval_distances); + validation_status_.max_interval_distance = max_interval_distance; + + if (max_interval_distance > validation_params_.interval_threshold) { + // const auto &p = trajectory.points; + // debug_marker_.pushPoseMarker(p.at(i - 1), "trajectory_interval"); + // debug_marker_.pushPoseMarker(p.at(i), "trajectory_interval"); + return false; + } + + return true; +} + +bool PlanningValidator::checkValidRelativeAngle(const Trajectory & trajectory) +{ + // debug_marker_.clearPoseMarker("trajectory_relative_angle"); + + const auto relative_angles = calcRelativeAngles(trajectory); + const auto [max_relative_angle, i] = getAbsMaxValAndIdx(relative_angles); + validation_status_.max_relative_angle = max_relative_angle; + + if (max_relative_angle > validation_params_.relative_angle_threshold) { + // const auto &p = trajectory.points; + // debug_marker_.pushPoseMarker(p.at(i), "trajectory_relative_angle", 0); + // debug_marker_.pushPoseMarker(p.at(i + 1), "trajectory_relative_angle", 1); + // debug_marker_.pushPoseMarker(p.at(i + 2), "trajectory_relative_angle", 2); + return false; + } + return true; +} + +bool PlanningValidator::checkValidCurvature(const Trajectory & trajectory) +{ + // // debug_marker.clearPoseMarker("trajectory_curvature"); + + const auto curvatures = calcCurvature(trajectory); + const auto [max_curvature, i] = getAbsMaxValAndIdx(curvatures); + validation_status_.max_curvature = max_curvature; + if (max_curvature > validation_params_.curvature_threshold) { + // const auto &p = trajectory.points; + // debug_marker_.pushPoseMarker(p.at(i - 1), "trajectory_curvature"); + // debug_marker_.pushPoseMarker(p.at(i), "trajectory_curvature"); + // debug_marker_.pushPoseMarker(p.at(i + 1), "trajectory_curvature"); + return false; + } + return true; +} + +bool PlanningValidator::checkValidLateralAcceleration(const Trajectory & trajectory) +{ + const auto lateral_acc_arr = calcLateralAcceleration(trajectory); + const auto [max_lateral_acc, i] = getAbsMaxValAndIdx(lateral_acc_arr); + validation_status_.max_lateral_acc = max_lateral_acc; + if (max_lateral_acc > validation_params_.lateral_acc_threshold) { + // debug_marker_.pushPoseMarker(trajectory.points.at(i), "lateral_acceleration"); + return false; + } + return true; +} + +bool PlanningValidator::checkValidMinLongitudinalAcceleration(const Trajectory & trajectory) +{ + const auto acc_arr = getLongitudinalAccArray(trajectory); + const auto [min_longitudinal_acc, i_min] = getMinValAndIdx(acc_arr); + validation_status_.min_longitudinal_acc = min_longitudinal_acc; + + if (min_longitudinal_acc < validation_params_.longitudinal_min_acc_threshold) { + // debug_marker_.pushPoseMarker(trajectory.points.at(i).pose, "min_longitudinal_acc"); + return false; + } + return true; +} + +bool PlanningValidator::checkValidMaxLongitudinalAcceleration(const Trajectory & trajectory) +{ + const auto acc_arr = getLongitudinalAccArray(trajectory); + const auto [max_longitudinal_acc, i_max] = getAbsMaxValAndIdx(acc_arr); + validation_status_.max_longitudinal_acc = max_longitudinal_acc; + + if (max_longitudinal_acc > validation_params_.longitudinal_max_acc_threshold) { + // debug_marker_.pushPoseMarker(trajectory.points.at(i).pose, "max_longitudinal_acc"); + return false; + } + return true; +} + +bool PlanningValidator::checkValidSteering(const Trajectory & trajectory) +{ + const auto steerings = calcSteeringAngles(trajectory, vehicle_info_.wheel_base_m); + const auto [max_steering, i_max] = getAbsMaxValAndIdx(steerings); + validation_status_.max_steering = max_steering; + + if (max_steering > validation_params_.steering_threshold) { + // debug_marker_.pushPoseMarker(trajectory.points.at(i_max).pose, "max_steering"); + return false; + } + return true; +} + +bool PlanningValidator::checkValidSteeringRate(const Trajectory & trajectory) +{ + const auto steering_rates = calcSteeringRates(trajectory, vehicle_info_.wheel_base_m); + const auto [max_steering_rate, i_max] = getAbsMaxValAndIdx(steering_rates); + validation_status_.max_steering_rate = max_steering_rate; + + if (max_steering_rate > validation_params_.steering_rate_threshold) { + // debug_marker_.pushPoseMarker(trajectory.points.at(i_max).pose, "max_steering_rate"); + return false; + } + return true; +} + +bool PlanningValidator::checkValidVelocityDeviation(const Trajectory & trajectory) +{ + // TODO(horibe): set appropriate thresholds for index search + const auto idx = motion_utils::findFirstNearestIndexWithSoftConstraints( + trajectory.points, current_kinematics_->pose.pose); + + validation_status_.velocity_deviation = std::abs( + trajectory.points.at(idx).longitudinal_velocity_mps - + current_kinematics_->twist.twist.linear.x); + + if (validation_status_.velocity_deviation > validation_params_.velocity_deviation_threshold) { + return false; + } + return true; +} + +bool PlanningValidator::checkValidDistanceDeviation(const Trajectory & trajectory) +{ + // TODO(horibe): set appropriate thresholds for index search + const auto idx = motion_utils::findFirstNearestIndexWithSoftConstraints( + trajectory.points, current_kinematics_->pose.pose); + + validation_status_.distance_deviation = + tier4_autoware_utils::calcDistance2d(trajectory.points.at(idx), current_kinematics_->pose.pose); + + if (validation_status_.distance_deviation > validation_params_.distance_deviation_threshold) { + return false; + } + return true; +} + +bool PlanningValidator::isAllValid(const PlanningValidatorStatus & s) +{ + return s.is_valid_finite_value && s.is_valid_interval && s.is_valid_relative_angle && + s.is_valid_curvature && s.is_valid_lateral_acc && s.is_valid_longitudinal_max_acc && + s.is_valid_longitudinal_min_acc && s.is_valid_steering && s.is_valid_steering_rate && + s.is_valid_velocity_deviation && s.is_valid_distance_deviation; +} + +} // namespace planning_validator + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(planning_validator::PlanningValidator) diff --git a/planning/planning_validator/src/utils.cpp b/planning/planning_validator/src/utils.cpp new file mode 100644 index 0000000000000..ffbc00478a25d --- /dev/null +++ b/planning/planning_validator/src/utils.cpp @@ -0,0 +1,216 @@ +// 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. + +#include "planning_validator/utils.hpp" + +#include +#include + +#include +#include +#include +#include + +namespace planning_validator +{ +using tier4_autoware_utils::calcCurvature; +using tier4_autoware_utils::calcDistance2d; +using tier4_autoware_utils::getPoint; + +std::pair getMaxValAndIdx(const std::vector & v) +{ + const auto iter = std::max_element(v.begin(), v.end()); + const auto idx = std::distance(v.begin(), iter); + return {*iter, idx}; +} + +std::pair getMinValAndIdx(const std::vector & v) +{ + const auto iter = std::min_element(v.begin(), v.end()); + const auto idx = std::distance(v.begin(), iter); + return {*iter, idx}; +} + +std::pair getAbsMaxValAndIdx(const std::vector & v) +{ + const auto iter = std::max_element( + v.begin(), v.end(), [](const auto & a, const auto & b) { return std::abs(a) < std::abs(b); }); + const auto idx = std::distance(v.begin(), iter); + return {std::abs(*iter), idx}; +} + +// Do not interpolate. +Trajectory resampleTrajectory(const Trajectory & trajectory, const double min_interval) +{ + Trajectory resampled; + resampled.header = trajectory.header; + + if (trajectory.points.empty()) { + return resampled; + } + + resampled.points.push_back(trajectory.points.front()); + for (size_t i = 1; i < trajectory.points.size(); ++i) { + const auto prev = resampled.points.back(); + const auto curr = trajectory.points.at(i); + if (calcDistance2d(prev, curr) > min_interval) { + resampled.points.push_back(curr); + } + } + return resampled; +} + +std::vector calcCurvature(const Trajectory & trajectory) +{ + if (trajectory.points.size() < 3) { + return std::vector(trajectory.points.size(), 0.0); + } + + std::vector curvature_arr; + curvature_arr.push_back(0.0); + for (size_t i = 1; i < trajectory.points.size() - 1; ++i) { + const auto p1 = getPoint(trajectory.points.at(i - 1)); + const auto p2 = getPoint(trajectory.points.at(i)); + const auto p3 = getPoint(trajectory.points.at(i + 1)); + try { + curvature_arr.push_back(tier4_autoware_utils::calcCurvature(p1, p2, p3)); + } catch (...) { + // maybe distance is too close + curvature_arr.push_back(0.0); + } + } + curvature_arr.push_back(0.0); + return curvature_arr; +} + +std::vector calcIntervalDistance(const Trajectory & trajectory) +{ + if (trajectory.points.size() < 2) { + return std::vector(trajectory.points.size(), 0.0); + } + + std::vector interval_distances; + for (size_t i = 1; i < trajectory.points.size(); ++i) { + interval_distances.push_back( + calcDistance2d(trajectory.points.at(i), trajectory.points.at(i - 1))); + } + return interval_distances; +} + +std::vector calcLateralAcceleration(const Trajectory & trajectory) +{ + const auto curvatures = calcCurvature(trajectory); + + std::vector lat_acc_arr; + for (size_t i = 0; i < curvatures.size(); ++i) { + const auto v = trajectory.points.at(i).longitudinal_velocity_mps; + const auto lat_acc = v * v * curvatures.at(i); + lat_acc_arr.push_back(lat_acc); + } + return lat_acc_arr; +} + +std::vector getLongitudinalAccArray(const Trajectory & trajectory) +{ + std::vector acc_arr; + for (const auto & p : trajectory.points) { + acc_arr.push_back(p.acceleration_mps2); + } + return acc_arr; +} + +std::vector calcRelativeAngles(const Trajectory & trajectory) +{ + // We need at least three points to compute relative angle + const size_t relative_angle_points_num = 3; + if (trajectory.points.size() < relative_angle_points_num) { + return {}; + } + + std::vector relative_angles; + + for (size_t i = 0; i <= trajectory.points.size() - relative_angle_points_num; ++i) { + const auto & p1 = trajectory.points.at(i).pose.position; + const auto & p2 = trajectory.points.at(i + 1).pose.position; + const auto & p3 = trajectory.points.at(i + 2).pose.position; + + const auto angle_a = tier4_autoware_utils::calcAzimuthAngle(p1, p2); + const auto angle_b = tier4_autoware_utils::calcAzimuthAngle(p2, p3); + + // convert relative angle to [-pi ~ pi] + const auto relative_angle = std::abs(tier4_autoware_utils::normalizeRadian(angle_b - angle_a)); + + relative_angles.push_back(relative_angle); + } + + return relative_angles; +} + +std::vector calcSteeringAngles(const Trajectory & trajectory, const double wheelbase) +{ + const auto curvature_to_steering = [](const auto k, const auto wheelbase) { + return std::atan(k * wheelbase); + }; + + const auto curvatures = calcCurvature(trajectory); + + std::vector steerings; + for (const auto & k : curvatures) { + steerings.push_back(curvature_to_steering(k, wheelbase)); + } + return steerings; +} + +std::vector calcSteeringRates(const Trajectory & trajectory, const double wheelbase) +{ + const auto steerings = calcSteeringAngles(trajectory, wheelbase); + + std::vector steering_rates; + for (size_t i = 0; i < trajectory.points.size() - 1; ++i) { + const auto & p_prev = trajectory.points.at(i); + const auto & p_next = trajectory.points.at(i + 1); + const auto delta_s = calcDistance2d(p_prev, p_next); + const auto v = 0.5 * (p_next.longitudinal_velocity_mps + p_prev.longitudinal_velocity_mps); + const auto dt = delta_s / std::max(v, 1.0e-5); + + const auto steer_prev = steerings.at(i); + const auto steer_next = steerings.at(i + 1); + + const auto steer_rate = (steer_next - steer_prev) / dt; + steering_rates.push_back(steer_rate); + } + + if (!steering_rates.empty()) { + steering_rates.push_back(steering_rates.back()); + } + + return steering_rates; +} + +bool checkFinite(const TrajectoryPoint & point) +{ + const auto & p = point.pose.position; + const auto & o = point.pose.orientation; + + using std::isfinite; + const bool p_result = isfinite(p.x) && isfinite(p.y) && isfinite(p.z); + const bool quat_result = isfinite(o.x) && isfinite(o.y) && isfinite(o.z) && isfinite(o.w); + const bool v_result = isfinite(point.longitudinal_velocity_mps); + const bool w_result = isfinite(point.heading_rate_rps); + const bool a_result = isfinite(point.acceleration_mps2); + + return quat_result && p_result && v_result && w_result && a_result; +} + +} // namespace planning_validator