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..cd6af3afde172 --- /dev/null +++ b/planning/planning_validator/README.md @@ -0,0 +1 @@ +# Planning Validator 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..16be56c5b182d --- /dev/null +++ b/planning/planning_validator/config/planning_validator.param.yaml @@ -0,0 +1,16 @@ +/**: + ros__parameters: + publish_diag: true # if true, diagnostic msg is published + use_previous_trajectory_on_invalid: true # if true, invalid trajectory is not published, previous valid trajectory is published instead. + 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..bd971d55b3985 --- /dev/null +++ b/planning/planning_validator/config/planning_validator_plotjugler_config.xml @@ -0,0 +1,135 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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..1c3a8bab851ca --- /dev/null +++ b/planning/planning_validator/include/planning_validator/debug_marker.hpp @@ -0,0 +1,58 @@ +// 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 +#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..9a5831cbefa34 --- /dev/null +++ b/planning/planning_validator/include/planning_validator/planning_validator.hpp @@ -0,0 +1,115 @@ +// 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__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); + + 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); + +private: + void setupDiag(); + + void setupParameters(); + + bool isDataReady(); + + void validate(const Trajectory & trajectory); + + void publishTrajectory(); + void publishDebugInfo(); + void displayStatus(); + + rclcpp::Subscription::SharedPtr sub_kinematics_; + rclcpp::Subscription::SharedPtr sub_traj_; + rclcpp::Publisher::SharedPtr pub_traj_; + rclcpp::Publisher::SharedPtr pub_status_; + + // system parameters + bool use_previous_trajectory_on_invalid_ = true; + bool publish_diag_ = true; + bool display_on_terminal_ = true; + + 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 debug_pose_publisher_; +}; +} // 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/invalid_trajectory_publisher.launch.xml b/planning/planning_validator/launch/invalid_trajectory_publisher.launch.xml new file mode 100644 index 0000000000000..fb64d2e0841bd --- /dev/null +++ b/planning/planning_validator/launch/invalid_trajectory_publisher.launch.xml @@ -0,0 +1,9 @@ + + + + + + + + + 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..a6b34edeec68c --- /dev/null +++ b/planning/planning_validator/launch/planning_validator.launch.xml @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + 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..a7df6f07731d3 --- /dev/null +++ b/planning/planning_validator/package.xml @@ -0,0 +1,39 @@ + + + + planning_validator + 0.1.0 + ros node for planning_validator + Takamasa Horibe + Yutaka Shimizu + Apache License 2.0 + + Takamasa Horibe + Yutaka Shimizu + + 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..2e0798db0bc08 --- /dev/null +++ b/planning/planning_validator/src/debug_marker.cpp @@ -0,0 +1,83 @@ +// 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/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/invalid_trajectory_publisher/invalid_trajectory_publisher.cpp b/planning/planning_validator/src/invalid_trajectory_publisher/invalid_trajectory_publisher.cpp new file mode 100644 index 0000000000000..6cca99cd9f14c --- /dev/null +++ b/planning/planning_validator/src/invalid_trajectory_publisher/invalid_trajectory_publisher.cpp @@ -0,0 +1,78 @@ +// 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 "invalid_trajectory_publisher.hpp" + +#include +#include +#include + +namespace planning_validator +{ +InvalidTrajectoryPublisherNode::InvalidTrajectoryPublisherNode( + const rclcpp::NodeOptions & node_options) +: Node("invalid_trajectory_publisher", node_options) +{ + using std::placeholders::_1; + using std::chrono_literals::operator""ms; + + traj_sub_ = create_subscription( + "~/input/trajectory", 1, + std::bind(&InvalidTrajectoryPublisherNode::onCurrentTrajectory, this, _1)); + + traj_pub_ = create_publisher("~/output/trajectory", 1); + + timer_ = rclcpp::create_timer( + this, get_clock(), 100ms, std::bind(&InvalidTrajectoryPublisherNode::onTimer, this)); +} + +void InvalidTrajectoryPublisherNode::onTimer() +{ + if (!current_trajectory_) { + RCLCPP_INFO(this->get_logger(), "waiting trajectory"); + return; + } + if (current_trajectory_->points.empty()) { + RCLCPP_INFO(this->get_logger(), "waiting trajectory"); + return; + } + + constexpr auto ADDED_VALUE = 1.0e3; + + auto output = *current_trajectory_; + auto & p = output.points.back().pose.position; + p.x += ADDED_VALUE; + p.y += ADDED_VALUE; + p.z += ADDED_VALUE; + + traj_pub_->publish(output); + + RCLCPP_INFO(this->get_logger(), "invalid trajectory is published."); + + bool EXIT_AFTER_PUBLISH = false; + if (EXIT_AFTER_PUBLISH) { + exit(0); + } +} + +void InvalidTrajectoryPublisherNode::onCurrentTrajectory(const Trajectory::ConstSharedPtr msg) +{ + current_trajectory_ = msg; + traj_sub_.reset(); +} + +} // namespace planning_validator + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(planning_validator::InvalidTrajectoryPublisherNode) diff --git a/planning/planning_validator/src/invalid_trajectory_publisher/invalid_trajectory_publisher.hpp b/planning/planning_validator/src/invalid_trajectory_publisher/invalid_trajectory_publisher.hpp new file mode 100644 index 0000000000000..c04969b700ea4 --- /dev/null +++ b/planning/planning_validator/src/invalid_trajectory_publisher/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_VALIDATOR__INVALID_TRAJECTORY_PUBLISHER_HPP_ +#define PLANNING_VALIDATOR__INVALID_TRAJECTORY_PUBLISHER_HPP_ + +#include + +#include + +#include + +namespace planning_validator +{ +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::SharedPtr traj_sub_; + rclcpp::Publisher::SharedPtr traj_pub_; + rclcpp::TimerBase::SharedPtr timer_; + + Trajectory::ConstSharedPtr current_trajectory_ = nullptr; +}; +} // namespace planning_validator + +#endif // PLANNING_VALIDATOR__INVALID_TRAJECTORY_PUBLISHER_HPP_ diff --git a/planning/planning_validator/src/planning_validator.cpp b/planning/planning_validator/src/planning_validator.cpp new file mode 100644 index 0000000000000..5488863f34225 --- /dev/null +++ b/planning/planning_validator/src/planning_validator.cpp @@ -0,0 +1,406 @@ +// 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/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); + + debug_pose_publisher_ = std::make_shared(this); + + setupParameters(); + + if (publish_diag_) { + setupDiag(); + } +} + +void PlanningValidator::setupParameters() +{ + use_previous_trajectory_on_invalid_ = + declare_parameter("use_previous_trajectory_on_invalid"); + publish_diag_ = declare_parameter("publish_diag"); + display_on_terminal_ = declare_parameter("display_on_terminal"); + + { + auto & p = validation_params_; + const std::string t = "thresholds."; + p.interval_threshold = declare_parameter(t + "interval"); + p.relative_angle_threshold = declare_parameter(t + "relative_angle"); + p.curvature_threshold = declare_parameter(t + "curvature"); + p.lateral_acc_threshold = declare_parameter(t + "lateral_acc"); + p.longitudinal_max_acc_threshold = declare_parameter(t + "longitudinal_max_acc"); + p.longitudinal_min_acc_threshold = declare_parameter(t + "longitudinal_min_acc"); + p.steering_threshold = declare_parameter(t + "steering"); + p.steering_rate_threshold = declare_parameter(t + "steering_rate"); + p.velocity_deviation_threshold = declare_parameter(t + "velocity_deviation"); + p.distance_deviation_threshold = declare_parameter(t + "distance_deviation"); + } + + 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() +{ + const auto checkFlag = + [](DiagnosticStatusWrapper & stat, const bool & is_ok, const std::string & msg) { + is_ok ? stat.summary(DiagnosticStatus::OK, "ok") : stat.summary(DiagnosticStatus::ERROR, msg); + }; + + auto & d = diag_updater_; + d.setHardwareID("planning_validator"); + + std::string ns = "trajectory_validation_"; + d.add(ns + "finite", [&](auto & stat) { + checkFlag(stat, validation_status_.is_valid_finite_value, "infinite value is found"); + }); + d.add(ns + "interval", [&](auto & stat) { + checkFlag(stat, validation_status_.is_valid_interval, "points interval is too long"); + }); + d.add(ns + "relative_angle", [&](auto & stat) { + checkFlag(stat, validation_status_.is_valid_relative_angle, "relative angle is too large"); + }); + d.add(ns + "curvature", [&](auto & stat) { + checkFlag(stat, validation_status_.is_valid_curvature, "curvature is too large"); + }); + d.add(ns + "lateral_acceleration", [&](auto & stat) { + checkFlag(stat, validation_status_.is_valid_lateral_acc, "lateral acceleration is too large"); + }); + d.add(ns + "acceleration", [&](auto & stat) { + checkFlag(stat, validation_status_.is_valid_longitudinal_max_acc, "acceleration is too large"); + }); + d.add(ns + "deceleration", [&](auto & stat) { + checkFlag(stat, validation_status_.is_valid_longitudinal_min_acc, "deceleration is too large"); + }); + d.add(ns + "steering", [&](auto & stat) { + checkFlag(stat, validation_status_.is_valid_steering, "expected steering is too large"); + }); + d.add(ns + "steering_rate", [&](auto & stat) { + checkFlag( + stat, validation_status_.is_valid_steering_rate, "expected steering rate is too large"); + }); + d.add(ns + "velocity_deviation", [&](auto & stat) { + checkFlag( + stat, validation_status_.is_valid_velocity_deviation, "velocity deviation is too large"); + }); +} + +bool PlanningValidator::isDataReady() +{ + const auto waiting = [this](const auto s) { + RCLCPP_INFO_SKIPFIRST_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(); + + displayStatus(); +} + +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 (use_previous_trajectory_on_invalid_ && 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_); + debug_pose_publisher_->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; +// std::cerr << "max_interval_distance = " << max_interval_distance << ", index = " << i << ", threshold = " << validation_params_.interval_threshold << std::endl; + + 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; +} + +void PlanningValidator::displayStatus() +{ + if (!display_on_terminal_) return; + + const auto warn = [this](const bool status, const std::string & msg) { + if (!status) { + RCLCPP_WARN(get_logger(), "%s", msg.c_str()); + } + }; + + const auto & s = validation_status_; + + warn(s.is_valid_curvature, "planning trajectory curvature is too large!!"); + warn(s.is_valid_distance_deviation, "planning trajectory is too far from ego!!"); + warn(s.is_valid_finite_value, "planning trajectory has invalid value!!"); + warn(s.is_valid_interval, "planning trajectory interval is too long!!"); + warn(s.is_valid_lateral_acc, "planning trajectory lateral acceleration is too high!!"); + warn(s.is_valid_longitudinal_max_acc, "planning trajectory acceleration is too high!!"); + warn(s.is_valid_longitudinal_min_acc, "planning trajectory deceleration is too high!!"); + warn(s.is_valid_relative_angle, "planning trajectory yaw angle varies too fast!!"); + warn(s.is_valid_steering, "planning trajectory expected steering angle is too high!!"); + warn(s.is_valid_steering_rate, "planning trajectory expected steering angle rate is too high!!"); + warn(s.is_valid_velocity_deviation, "planning trajectory velocity deviation is too high!!"); +} + +} // 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 diff --git a/planning/planning_validator/test/src/test_main.cpp b/planning/planning_validator/test/src/test_main.cpp new file mode 100644 index 0000000000000..399bcff8f918b --- /dev/null +++ b/planning/planning_validator/test/src/test_main.cpp @@ -0,0 +1,26 @@ +// 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 + +#include + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + bool result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/planning/planning_validator/test/src/test_planning_validator_functions.cpp b/planning/planning_validator/test/src/test_planning_validator_functions.cpp new file mode 100644 index 0000000000000..a65be49e8e56a --- /dev/null +++ b/planning/planning_validator/test/src/test_planning_validator_functions.cpp @@ -0,0 +1,161 @@ +// 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/debug_marker.hpp" +#include "test_planning_validator_helper.hpp" + +#include + +#include + +using autoware_auto_planning_msgs::msg::Trajectory; +using planning_validator::PlanningValidator; + +TEST(PlanningValidatorTestSuite, checkValidFiniteValueFunction) +{ + auto validator = std::make_shared(getNodeOptionsWithDefaultParams()); + + // Valid Trajectory + { + Trajectory valid_traj = generateTrajectory(NOMINAL_INTERVAL); + ASSERT_TRUE(validator->checkValidFiniteValue(valid_traj)); + } + + // Nan Trajectory + { + Trajectory nan_traj = generateNanTrajectory(); + ASSERT_FALSE(validator->checkValidFiniteValue(nan_traj)); + } + + // Inf Trajectory + { + Trajectory inf_traj = generateInfTrajectory(); + ASSERT_FALSE(validator->checkValidFiniteValue(inf_traj)); + } +} + +TEST(PlanningValidatorTestSuite, checkValidIntervalFunction) +{ + auto validator = std::make_shared(getNodeOptionsWithDefaultParams()); + + // Normal Trajectory + { + Trajectory valid_traj = generateTrajectory(NOMINAL_INTERVAL); + ASSERT_TRUE(validator->checkValidInterval(valid_traj)); + } + + // Boundary Trajectory + { + // Note: too small value is not supported like numerical_limits::epsilon + const auto ep = 1.0e-5; + + Trajectory ok_bound_traj = generateTrajectory(ERROR_INTERVAL - ep); + ASSERT_TRUE(validator->checkValidInterval(ok_bound_traj)); + + Trajectory ng_bound_traj = generateTrajectory(ERROR_INTERVAL + ep); + ASSERT_FALSE(validator->checkValidInterval(ng_bound_traj)); + } + + // Long Interval Trajectory + { + Trajectory long_interval_traj = generateTrajectory(ERROR_INTERVAL * 2.0); + ASSERT_FALSE(validator->checkValidInterval(long_interval_traj)); + } +} + +TEST(PlanningValidatorTestSuite, checkValidCurvatureFunction) +{ + auto validator = std::make_shared(getNodeOptionsWithDefaultParams()); + + // Normal Trajectory + { + Trajectory valid_traj = generateTrajectory(NOMINAL_INTERVAL); + ASSERT_TRUE(validator->checkValidCurvature(valid_traj)); + } + + // Invalid curvature trajectory + { + // TODO(Horibe): write me + } +} + +TEST(PlanningValidatorTestSuite, checkValidRelativeAngleFunction) +{ + auto validator = std::make_shared(getNodeOptionsWithDefaultParams()); + + // valid case + { + /** + * x: 0 1 2 3 4 5 6 7 8 9 10 + * y: 0 0.1 0 -0.1 0 0.2 0 0 0 0 0 + * max relative angle is about 0.197 radian (= 11 degree) + **/ + constexpr auto interval = 1.0; + Trajectory valid_traj = generateTrajectory(interval); + valid_traj.points[1].pose.position.y = 0.1; + valid_traj.points[3].pose.position.y = -0.1; + valid_traj.points[5].pose.position.y = 0.2; + ASSERT_TRUE(validator->checkValidRelativeAngle(valid_traj)); + } + + // invalid case + { + /** + * x: 0 1 2 3 4 5 6 7 8 9 10 + * y: 0 0 0 0 10 0 0 0 0 0 0 + * the relative angle around index [4] is about 1.4 radian (= 84 degree) + **/ + constexpr auto interval = 1.0; + Trajectory invalid_traj = generateTrajectory(interval); + invalid_traj.points[4].pose.position.x = 3; + invalid_traj.points[4].pose.position.y = 10; + // for (auto t : invalid_traj.points) { + // std::cout << "p: (x , y) = " << "( "<checkValidRelativeAngle(invalid_traj)); + } + + { + /** <---inverted pattern----- + * x: 0 -1 -2 -3 -4 -5 -6 -7 -8 -9 -10 + * y: 0 0 0 0 10 0 0 0 0 0 0 + **/ + constexpr auto interval = 1.0; + Trajectory invalid_traj = generateTrajectory(interval); + invalid_traj.points[4].pose.position.y = 10; + for (auto t : invalid_traj.points) { + t.pose.position.x *= -1; + } + ASSERT_FALSE(validator->checkValidRelativeAngle(invalid_traj)); + } + + { + /** vertical pattern + * x: 0 0 0 0 10 0 0 0 0 0 0 + * y: 0 1 2 3 4 5 6 7 8 9 10 + **/ + constexpr auto interval = 1.0; + Trajectory invalid_traj = generateTrajectory(interval); + for (size_t i = 0; i < invalid_traj.points.size(); i++) { + auto & p = invalid_traj.points[i].pose.position; + p.x = 0; + p.y = i; + } + invalid_traj.points[4].pose.position.x = 10; + std::string valid_error_msg; + ASSERT_FALSE(validator->checkValidRelativeAngle(invalid_traj)); + } +} diff --git a/planning/planning_validator/test/src/test_planning_validator_helper.cpp b/planning/planning_validator/test/src/test_planning_validator_helper.cpp new file mode 100644 index 0000000000000..d823e465c3143 --- /dev/null +++ b/planning/planning_validator/test/src/test_planning_validator_helper.cpp @@ -0,0 +1,95 @@ +// 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 "test_planning_validator_helper.hpp" +#include + +using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_auto_planning_msgs::msg::TrajectoryPoint; + +Trajectory generateTrajectory(double interval_distance) +{ + Trajectory traj; + for (double s = 0.0; s <= 10.0 * interval_distance; s += interval_distance) { + TrajectoryPoint p; + p.pose.position.x = s; + p.longitudinal_velocity_mps = 1.0; + traj.points.push_back(p); + } + return traj; +} + +Trajectory generateNanTrajectory() +{ + Trajectory traj = generateTrajectory(1.0); + traj.points.front().pose.position.x = NAN; + return traj; +} + +Trajectory generateInfTrajectory() +{ + Trajectory traj = generateTrajectory(1.0); + traj.points.front().pose.position.x = INFINITY; + return traj; +} + +Trajectory generateBadCurvatureTrajectory() +{ + Trajectory traj; + + double y = 1.5; + for (double s = 0.0; s <= 10.0; s += 1.0) { + TrajectoryPoint p; + p.longitudinal_velocity_mps = 1.0; + p.pose.position.x = s; + p.pose.position.y = y; + y *= -1.0; // invert sign + traj.points.push_back(p); + } + + return traj; +} + +rclcpp::NodeOptions getNodeOptionsWithDefaultParams() +{ + rclcpp::NodeOptions node_options; + + // for planing validator + node_options.append_parameter_override("publish_diag", true); + node_options.append_parameter_override("use_previous_trajectory_on_invalid", true); + node_options.append_parameter_override("interval_threshold", ERROR_INTERVAL); + node_options.append_parameter_override("relative_angle_threshold", 1.0); + node_options.append_parameter_override("curvature_threshold", ERROR_CURVATURE); + node_options.append_parameter_override("lateral_acc_threshold", 100.0); + node_options.append_parameter_override("longitudinal_max_acc_threshold", 100.0); + node_options.append_parameter_override("longitudinal_min_acc_threshold", -100.0); + node_options.append_parameter_override("steering_threshold", 100.0); + node_options.append_parameter_override("steering_rate_threshold", 100.0); + node_options.append_parameter_override("velocity_deviation_threshold", 100.0); + node_options.append_parameter_override("distance_deviation_threshold", 100.0); + + // for vehicle info + node_options.append_parameter_override("wheel_radius", 0.5); + node_options.append_parameter_override("wheel_width", 0.2); + node_options.append_parameter_override("wheel_base", 3.0); + node_options.append_parameter_override("wheel_tread", 2.0); + node_options.append_parameter_override("front_overhang", 1.0); + node_options.append_parameter_override("rear_overhang", 1.0); + node_options.append_parameter_override("left_overhang", 0.5); + node_options.append_parameter_override("right_overhang", 0.5); + node_options.append_parameter_override("vehicle_height", 1.5); + node_options.append_parameter_override("max_steer_angle", 0.7); + + return node_options; +} diff --git a/planning/planning_validator/test/src/test_planning_validator_helper.hpp b/planning/planning_validator/test/src/test_planning_validator_helper.hpp new file mode 100644 index 0000000000000..46267dc76b1e0 --- /dev/null +++ b/planning/planning_validator/test/src/test_planning_validator_helper.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 TEST_PLANNING_VALIDATOR_HELPER_HPP_ +#define TEST_PLANNING_VALIDATOR_HELPER_HPP_ + +#include +#include +#include + +constexpr double NOMINAL_INTERVAL = 1.0; +constexpr double ERROR_INTERVAL = 1000.0; +constexpr double ERROR_CURVATURE = 2.0; + +using autoware_auto_planning_msgs::msg::Trajectory; + +Trajectory generateTrajectory(double interval_distance); + +Trajectory generateNanTrajectory(); + +Trajectory generateInfTrajectory(); + +Trajectory generateBadCurvatureTrajectory(); + +rclcpp::NodeOptions getNodeOptionsWithDefaultParams(); + + +#endif // TEST_PLANNING_VALIDATOR_HELPER_HPP_ diff --git a/planning/planning_validator/test/src/test_planning_validator_pubsub.cpp b/planning/planning_validator/test/src/test_planning_validator_pubsub.cpp new file mode 100644 index 0000000000000..6cdb01981e168 --- /dev/null +++ b/planning/planning_validator/test/src/test_planning_validator_pubsub.cpp @@ -0,0 +1,129 @@ +// 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 "test_planning_validator_helper.hpp" + +#include + +#include + +#include +#include + +/* + * This test checks the diagnostics message published from the planning_validator node + */ + +using autoware_auto_planning_msgs::msg::Trajectory; +using diagnostic_msgs::msg::DiagnosticArray; +using diagnostic_msgs::msg::DiagnosticStatus; +using planning_validator::PlanningValidator; + +class PubSubManager : public rclcpp::Node +{ +public: + PubSubManager() : Node("test_pub_sub") + { + traj_pub_ = create_publisher("/planning_validator/input/trajectory", 1); + diag_sub_ = create_subscription( + "/diagnostics", 1, + [this](const DiagnosticArray::ConstSharedPtr msg) { received_diags_.push_back(msg); }); + } + + rclcpp::Publisher::SharedPtr traj_pub_; + rclcpp::Subscription::SharedPtr diag_sub_; + + std::vector received_diags_; +}; + +void spinSome(rclcpp::Node::SharedPtr node_ptr) +{ + for (int i = 0; i < 50; ++i) { + rclcpp::spin_some(node_ptr); + rclcpp::WallRate(100).sleep(); + } +} + +bool isAllOK(const std::vector & diags) +{ + for (const auto & diag : diags) { + for (const auto & status : diag->status) { + if (status.level != DiagnosticStatus::OK) { + return false; + } + } + } + return true; +} + +bool hasError(const std::vector & diags) +{ + for (const auto & diag : diags) { + for (const auto & status : diag->status) { + if (status.level == DiagnosticStatus::ERROR) { + return true; + } + } + } + return false; +} + +void runWithOKTrajectory(const Trajectory & trajectory) +{ + auto validator = std::make_shared(getNodeOptionsWithDefaultParams()); + auto manager = std::make_shared(); + EXPECT_GE(manager->traj_pub_->get_subscription_count(), 1U) << "topic is not connected."; + + manager->traj_pub_->publish(trajectory); + spinSome(validator); + spinSome(manager); + + EXPECT_GE(manager->received_diags_.size(), 1U) << "diag has not received!"; + EXPECT_TRUE(isAllOK(manager->received_diags_)); +} + +void runWithBadTrajectory(const Trajectory & trajectory) +{ + auto validator = std::make_shared(getNodeOptionsWithDefaultParams()); + auto manager = std::make_shared(); + EXPECT_GE(manager->traj_pub_->get_subscription_count(), 1U) << "topic is not connected."; + + manager->traj_pub_->publish(trajectory); + spinSome(validator); + spinSome(manager); + + EXPECT_GE(manager->received_diags_.size(), 1U) << "diag has not received!"; + EXPECT_TRUE(hasError(manager->received_diags_)); +} + +// OK cases +TEST(PlanningValidator, DiagCheckForNominalTrajectory) +{ + runWithOKTrajectory(generateTrajectory(NOMINAL_INTERVAL)); +} + +// Bad cases +TEST(PlanningValidator, DiagCheckForNaNTrajectory) +{ + runWithBadTrajectory(generateNanTrajectory()); +} +TEST(PlanningValidator, DiagCheckForInfTrajectory) +{ + runWithBadTrajectory(generateInfTrajectory()); +} +TEST(PlanningValidator, DiagCheckForTooLongIntervalTrajectory) +{ + runWithBadTrajectory(generateTrajectory(ERROR_INTERVAL)); +} diff --git a/system/system_error_monitor/config/diagnostic_aggregator/planning.param.yaml b/system/system_error_monitor/config/diagnostic_aggregator/planning.param.yaml index 151c6247dbbb4..a13ab3568a5d7 100644 --- a/system/system_error_monitor/config/diagnostic_aggregator/planning.param.yaml +++ b/system/system_error_monitor/config/diagnostic_aggregator/planning.param.yaml @@ -24,24 +24,66 @@ analyzers: trajectory_finite_value: type: diagnostic_aggregator/GenericAnalyzer - path: trajectory_point_validation - contains: [": trajectory_point_validation"] + path: trajectory_validation_finite + contains: [": trajectory_validation_finite"] timeout: 1.0 trajectory_interval: type: diagnostic_aggregator/GenericAnalyzer - path: trajectory_interval_validation - contains: [": trajectory_interval_validation"] + path: trajectory_validation_interval + contains: [": trajectory_validation_interval"] timeout: 1.0 trajectory_curvature: type: diagnostic_aggregator/GenericAnalyzer - path: trajectory_curvature_validation - contains: [": trajectory_curvature_validation"] + path: trajectory_validation_curvature + contains: [": trajectory_validation_curvature"] timeout: 1.0 trajectory_sharp_angle: type: diagnostic_aggregator/GenericAnalyzer - path: trajectory_relative_angle_validation - contains: [": trajectory_relative_angle_validation"] + path: trajectory_validation_relative_angle + contains: [": trajectory_validation_relative_angle"] timeout: 1.0 + + trajectory_relative_angle: + type: diagnostic_aggregator/GenericAnalyzer + path: trajectory_validation_relative_angle + contains: [": trajectory_validation_relative_angle"] + timeout: 1.0 + + trajectory_lateral_acceleration: + type: diagnostic_aggregator/GenericAnalyzer + path: trajectory_validation_lateral_acceleration + contains: [": trajectory_validation_lateral_acceleration"] + timeout: 1.0 + + trajectory_acceleration: + type: diagnostic_aggregator/GenericAnalyzer + path: trajectory_validation_acceleration + contains: [": trajectory_validation_acceleration"] + timeout: 1.0 + + trajectory_deceleration: + type: diagnostic_aggregator/GenericAnalyzer + path: trajectory_validation_deceleration + contains: [": trajectory_validation_deceleration"] + timeout: 1.0 + + trajectory_steering: + type: diagnostic_aggregator/GenericAnalyzer + path: trajectory_validation_steering + contains: [": trajectory_validation_steering"] + timeout: 1.0 + + trajectory_steering_rate: + type: diagnostic_aggregator/GenericAnalyzer + path: trajectory_validation_steering_rate + contains: [": trajectory_validation_steering_rate"] + timeout: 1.0 + + trajectory_velocity_deviation: + type: diagnostic_aggregator/GenericAnalyzer + path: trajectory_validation_velocity_deviation + contains: [": trajectory_validation_velocity_deviation"] + timeout: 1.0 \ No newline at end of file