diff --git a/planning/planning_evaluator/CMakeLists.txt b/planning/planning_evaluator/CMakeLists.txt new file mode 100644 index 0000000000000..88998404cc506 --- /dev/null +++ b/planning/planning_evaluator/CMakeLists.txt @@ -0,0 +1,52 @@ +cmake_minimum_required(VERSION 3.5) +project(planning_evaluator) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +ament_auto_add_library(${PROJECT_NAME}_node SHARED + src/metrics_calculator.cpp + src/${PROJECT_NAME}_node.cpp + src/motion_evaluator_node.cpp + src/metrics/deviation_metrics.cpp + src/metrics/metrics_utils.cpp + src/metrics/obstacle_metrics.cpp + src/metrics/stability_metrics.cpp + src/metrics/trajectory_metrics.cpp +) + +rclcpp_components_register_node(${PROJECT_NAME}_node + PLUGIN "planning_diagnostics::PlanningEvaluatorNode" + EXECUTABLE ${PROJECT_NAME} +) + +rclcpp_components_register_node(${PROJECT_NAME}_node + PLUGIN "planning_diagnostics::MotionEvaluatorNode" + EXECUTABLE motion_evaluator +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + ament_add_gtest(test_${PROJECT_NAME} + test/test_planning_evaluator_node.cpp + ) + target_link_libraries(test_${PROJECT_NAME} + ${PROJECT_NAME}_node + ) +endif() + +ament_auto_package( + INSTALL_TO_SHARE + param + launch +) diff --git a/planning/planning_evaluator/README.md b/planning/planning_evaluator/README.md new file mode 100644 index 0000000000000..23c8101429c35 --- /dev/null +++ b/planning/planning_evaluator/README.md @@ -0,0 +1,96 @@ +# Planning Evaluator + +## Purpose + +This package provides nodes that generate metrics to evaluate the quality of planning and control. + +## Inner-workings / Algorithms + +The evaluation node calculates metrics each time it receives a trajectory `T(0)`. +Metrics are calculated using the following information: + +- the trajectory `T(0)` itself. +- the previous trajectory `T(-1)`. +- the _reference_ trajectory assumed to be used as the reference to plan `T(0)`. +- the current ego pose. +- the set of objects in the environment. + +These information are maintained by an instance of class `MetricsCalculator` +which is also responsible for calculating metrics. + +### Stat + +Each metric is calculated using a `Stat` instance which contains +the minimum, maximum, and mean values calculated for the metric +as well as the number of values measured. + +### Metric calculation and adding more metrics + +All possible metrics are defined in the `Metric` enumeration defined +`include/planning_evaluator/metrics/metric.hpp`. +This file also defines conversions from/to string as well as human readable descriptions +to be used as header of the output file. + +The `MetricsCalculator` is responsible for calculating metric statistics +through calls to function: + +```C++ +Stat MetricsCalculator::calculate(const Metric metric, const Trajectory & traj) const; +``` + +Adding a new metric `M` requires the following steps: + +- `metrics/metric.hpp`: add `M` to the `enum`, to the from/to string conversion maps, and to the description map. +- `metrics_calculator.cpp`: add `M` to the `switch/case` statement of the `calculate` function. +- Add `M` to the `selected_metrics` parameters. + +## Inputs / Outputs + +### Inputs + +| Name | Type | Description | +| ------------------------------ | ------------------------------------------------------ | ------------------------------------------------- | +| `~/input/trajectory` | `autoware_auto_planning_msgs::msg::Trajectory` | Main trajectory to evaluate | +| `~/input/reference_trajectory` | `autoware_auto_planning_msgs::msg::Trajectory` | Reference trajectory to use for deviation metrics | +| `~/input/objects` | `autoware_auto_perception_msgs::msg::PredictedObjects` | Obstacles | + +### Outputs + +Each metric is published on a topic named after the metric name. + +| Name | Type | Description | +| ----------- | --------------------------------------- | ------------------------------------------------------- | +| `~/metrics` | `diagnostic_msgs::msg::DiagnosticArray` | DiagnosticArray with a DiagnosticStatus for each metric | + +When shut down, the evaluation node writes the values of the metrics measured during its lifetime +to a file as specified by the `output_file` parameter. + +## Parameters + +| Name | Type | Description | +| :-------------------------------- | :------- | :-------------------------------------------------------------------------- | +| `output_file` | `string` | file used to write metrics | +| `ego_frame` | `string` | frame used for the ego pose | +| `selected_metrics` | List | metrics to measure and publish | +| `trajectory.min_point_dist_m` | `double` | minimum distance between two successive points to use for angle calculation | +| `trajectory.lookahead.max_dist_m` | `double` | maximum distance from ego along the trajectory to use for calculation | +| `trajectory.lookahead.max_time_m` | `double` | maximum time ahead of ego along the trajectory to use for calculation | +| `obstacle.dist_thr_m` | `double` | distance between ego and the obstacle below which a collision is considered | + +## Assumptions / Known limits + +There is a strong assumption that when receiving a trajectory `T(0)`, +it has been generated using the last received reference trajectory and objects. +This can be wrong if a new reference trajectory or objects are published while `T(0)` is being calculated. + +Precision is currently limited by the resolution of the trajectories. +It is possible to interpolate the trajectory and reference trajectory to increase precision but would make computation significantly more expensive. + +## Future extensions / Unimplemented parts + +- Use `Route` or `Path` messages as reference trajectory. +- RSS metrics (done in another node ). +- Add option to publish the `min` and `max` metric values. For now only the `mean` value is published. +- `motion_evaluator_node`. + - Node which constructs a trajectory over time from the real motion of ego. + - Only a proof of concept is currently implemented. diff --git a/planning/planning_evaluator/include/planning_evaluator/metrics/deviation_metrics.hpp b/planning/planning_evaluator/include/planning_evaluator/metrics/deviation_metrics.hpp new file mode 100644 index 0000000000000..e71b887ff3a10 --- /dev/null +++ b/planning/planning_evaluator/include/planning_evaluator/metrics/deviation_metrics.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_EVALUATOR__METRICS__DEVIATION_METRICS_HPP_ +#define PLANNING_EVALUATOR__METRICS__DEVIATION_METRICS_HPP_ + +#include "planning_evaluator/stat.hpp" + +#include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" + +namespace planning_diagnostics +{ +namespace metrics +{ +using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_auto_planning_msgs::msg::TrajectoryPoint; + +/** + * @brief calculate lateral deviation of the given trajectory from the reference trajectory + * @param [in] ref reference trajectory + * @param [in] traj input trajectory + * @return calculated statistics + */ +Stat calcLateralDeviation(const Trajectory & ref, const Trajectory & traj); + +/** + * @brief calculate yaw deviation of the given trajectory from the reference trajectory + * @param [in] ref reference trajectory + * @param [in] traj input trajectory + * @return calculated statistics + */ +Stat calcYawDeviation(const Trajectory & ref, const Trajectory & traj); + +/** + * @brief calculate velocity deviation of the given trajectory from the reference trajectory + * @param [in] ref reference trajectory + * @param [in] traj input trajectory + * @return calculated statistics + */ +Stat calcVelocityDeviation(const Trajectory & ref, const Trajectory & traj); + +} // namespace metrics +} // namespace planning_diagnostics + +#endif // PLANNING_EVALUATOR__METRICS__DEVIATION_METRICS_HPP_ diff --git a/planning/planning_evaluator/include/planning_evaluator/metrics/metric.hpp b/planning/planning_evaluator/include/planning_evaluator/metrics/metric.hpp new file mode 100644 index 0000000000000..16811f91b32b1 --- /dev/null +++ b/planning/planning_evaluator/include/planning_evaluator/metrics/metric.hpp @@ -0,0 +1,122 @@ +// 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_EVALUATOR__METRICS__METRIC_HPP_ +#define PLANNING_EVALUATOR__METRICS__METRIC_HPP_ + +#include +#include +#include +#include + +namespace planning_diagnostics +{ +/** + * @brief Enumeration of trajectory metrics + */ +enum class Metric { + curvature, + point_interval, + relative_angle, + length, + duration, + velocity, + acceleration, + jerk, + lateral_deviation, + yaw_deviation, + velocity_deviation, + stability, + stability_frechet, + obstacle_distance, + obstacle_ttc, + SIZE, +}; + +/** TODO(Maxime CLEMENT): + * make the addition of metrics simpler, e.g. with some macro ADD_METRIC(metric, metric_description) + */ +static const std::unordered_map str_to_metric = { + {"curvature", Metric::curvature}, + {"point_interval", Metric::point_interval}, + {"relative_angle", Metric::relative_angle}, + {"length", Metric::length}, + {"duration", Metric::duration}, + {"velocity", Metric::velocity}, + {"acceleration", Metric::acceleration}, + {"jerk", Metric::jerk}, + {"lateral_deviation", Metric::lateral_deviation}, + {"yaw_deviation", Metric::yaw_deviation}, + {"velocity_deviation", Metric::velocity_deviation}, + {"stability", Metric::stability}, + {"stability_frechet", Metric::stability_frechet}, + {"obstacle_distance", Metric::obstacle_distance}, + {"obstacle_ttc", Metric::obstacle_ttc}, +}; +static const std::unordered_map metric_to_str = { + {Metric::curvature, "curvature"}, + {Metric::point_interval, "point_interval"}, + {Metric::relative_angle, "relative_angle"}, + {Metric::length, "length"}, + {Metric::duration, "duration"}, + {Metric::velocity, "velocity"}, + {Metric::acceleration, "acceleration"}, + {Metric::jerk, "jerk"}, + {Metric::lateral_deviation, "lateral_deviation"}, + {Metric::yaw_deviation, "yaw_deviation"}, + {Metric::velocity_deviation, "velocity_deviation"}, + {Metric::stability, "stability"}, + {Metric::stability_frechet, "stability_frechet"}, + {Metric::obstacle_distance, "obstacle_distance"}, + {Metric::obstacle_ttc, "obstacle_ttc"}}; + +// Metrics descriptions +static const std::unordered_map metric_descriptions = { + {Metric::curvature, "Curvature[1/rad]"}, + {Metric::point_interval, "Interval_between_points[m]"}, + {Metric::relative_angle, "Relative_angle[rad]"}, + {Metric::length, "Trajectory_length[m]"}, + {Metric::duration, "Trajectory_duration[s]"}, + {Metric::velocity, "Trajectory_velocity[m/s]"}, + {Metric::acceleration, "Trajectory_acceleration[m/s²]"}, + {Metric::jerk, "Trajectory_jerk[m/s³]"}, + {Metric::lateral_deviation, "Lateral_deviation[m]"}, + {Metric::yaw_deviation, "Yaw_deviation[rad]"}, + {Metric::velocity_deviation, "Velocity_deviation[m/s]"}, + {Metric::stability, "Stability[m]"}, + {Metric::stability_frechet, "StabilityFrechet[m]"}, + {Metric::obstacle_distance, "Obstacle_distance[m]"}, + {Metric::obstacle_ttc, "Obstacle_time_to_collision[s]"}}; + +namespace details +{ +static struct CheckCorrectMaps +{ + CheckCorrectMaps() + { + if ( + str_to_metric.size() != static_cast(Metric::SIZE) || + metric_to_str.size() != static_cast(Metric::SIZE) || + metric_descriptions.size() != static_cast(Metric::SIZE)) { + std::cerr << "[metrics/metrics.hpp] Maps are not defined for all metrics: "; + std::cerr << str_to_metric.size() << " " << metric_to_str.size() << " " + << metric_descriptions.size() << std::endl; + } + } +} check; + +} // namespace details +} // namespace planning_diagnostics + +#endif // PLANNING_EVALUATOR__METRICS__METRIC_HPP_ diff --git a/planning/planning_evaluator/include/planning_evaluator/metrics/metrics_utils.hpp b/planning/planning_evaluator/include/planning_evaluator/metrics/metrics_utils.hpp new file mode 100644 index 0000000000000..4806446d4270f --- /dev/null +++ b/planning/planning_evaluator/include/planning_evaluator/metrics/metrics_utils.hpp @@ -0,0 +1,43 @@ +// 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_EVALUATOR__METRICS__METRICS_UTILS_HPP_ +#define PLANNING_EVALUATOR__METRICS__METRICS_UTILS_HPP_ + +#include "planning_evaluator/stat.hpp" + +#include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" + +namespace planning_diagnostics +{ +namespace metrics +{ +namespace utils +{ +using autoware_auto_planning_msgs::msg::Trajectory; + +/** + * @brief find the index in the trajectory at the given distance of the given index + * @param [in] traj input trajectory + * @param [in] curr_id index + * @param [in] distance distance + * @return index of the trajectory point at distance ahead of traj[curr_id] + */ +size_t getIndexAfterDistance(const Trajectory & traj, const size_t curr_id, const double distance); + +} // namespace utils +} // namespace metrics +} // namespace planning_diagnostics +#endif // PLANNING_EVALUATOR__METRICS__METRICS_UTILS_HPP_ diff --git a/planning/planning_evaluator/include/planning_evaluator/metrics/obstacle_metrics.hpp b/planning/planning_evaluator/include/planning_evaluator/metrics/obstacle_metrics.hpp new file mode 100644 index 0000000000000..848d92c91f13e --- /dev/null +++ b/planning/planning_evaluator/include/planning_evaluator/metrics/obstacle_metrics.hpp @@ -0,0 +1,51 @@ +// 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_EVALUATOR__METRICS__OBSTACLE_METRICS_HPP_ +#define PLANNING_EVALUATOR__METRICS__OBSTACLE_METRICS_HPP_ + +#include "planning_evaluator/stat.hpp" + +#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" +#include "autoware_auto_planning_msgs/msg/trajectory.hpp" + +namespace planning_diagnostics +{ +namespace metrics +{ +using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_auto_planning_msgs::msg::Trajectory; + +/** + * @brief calculate the distance to the closest obstacle at each point of the trajectory + * @param [in] obstacles obstacles + * @param [in] traj trajectory + * @return calculated statistics + */ +Stat calcDistanceToObstacle(const PredictedObjects & obstacles, const Trajectory & traj); + +/** + * @brief calculate the time to collision of the trajectory with the given obstacles + * Assume that "now" corresponds to the first trajectory point + * @param [in] obstacles obstacles + * @param [in] traj trajectory + * @return calculated statistics + */ +Stat calcTimeToCollision( + const PredictedObjects & obstacles, const Trajectory & traj, const double distance_threshold); + +} // namespace metrics +} // namespace planning_diagnostics + +#endif // PLANNING_EVALUATOR__METRICS__OBSTACLE_METRICS_HPP_ diff --git a/planning/planning_evaluator/include/planning_evaluator/metrics/stability_metrics.hpp b/planning/planning_evaluator/include/planning_evaluator/metrics/stability_metrics.hpp new file mode 100644 index 0000000000000..c4bf1fe901604 --- /dev/null +++ b/planning/planning_evaluator/include/planning_evaluator/metrics/stability_metrics.hpp @@ -0,0 +1,47 @@ +// 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_EVALUATOR__METRICS__STABILITY_METRICS_HPP_ +#define PLANNING_EVALUATOR__METRICS__STABILITY_METRICS_HPP_ + +#include "planning_evaluator/stat.hpp" + +#include "autoware_auto_planning_msgs/msg/trajectory.hpp" + +namespace planning_diagnostics +{ +namespace metrics +{ +using autoware_auto_planning_msgs::msg::Trajectory; + +/** + * @brief calculate the discrete Frechet distance between two trajectories + * @param [in] traj1 first trajectory + * @param [in] traj2 second trajectory + * @return calculated statistics + */ +Stat calcFrechetDistance(const Trajectory & traj1, const Trajectory & traj2); + +/** + * @brief calculate the lateral distance between two trajectories + * @param [in] traj1 first trajectory + * @param [in] traj2 second trajectory + * @return calculated statistics + */ +Stat calcLateralDistance(const Trajectory & traj1, const Trajectory & traj2); + +} // namespace metrics +} // namespace planning_diagnostics + +#endif // PLANNING_EVALUATOR__METRICS__STABILITY_METRICS_HPP_ diff --git a/planning/planning_evaluator/include/planning_evaluator/metrics/trajectory_metrics.hpp b/planning/planning_evaluator/include/planning_evaluator/metrics/trajectory_metrics.hpp new file mode 100644 index 0000000000000..b55ad245d8425 --- /dev/null +++ b/planning/planning_evaluator/include/planning_evaluator/metrics/trajectory_metrics.hpp @@ -0,0 +1,90 @@ +// 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_EVALUATOR__METRICS__TRAJECTORY_METRICS_HPP_ +#define PLANNING_EVALUATOR__METRICS__TRAJECTORY_METRICS_HPP_ + +#include "planning_evaluator/stat.hpp" + +#include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" + +namespace planning_diagnostics +{ +namespace metrics +{ +using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_auto_planning_msgs::msg::TrajectoryPoint; + +/** + * @brief calculate relative angle metric (angle between successive points) + * @param [in] traj input trajectory + * @param [in] min_dist_threshold minimum distance between successive points + * @return calculated statistics + */ +Stat calcTrajectoryRelativeAngle(const Trajectory & traj, const double min_dist_threshold); + +/** + * @brief calculate metric for the distance between trajectory points + * @param [in] traj input trajectory + * @return calculated statistics + */ +Stat calcTrajectoryInterval(const Trajectory & traj); + +/** + * @brief calculate curvature metric + * @param [in] traj input trajectory + * @return calculated statistics + */ +Stat calcTrajectoryCurvature(const Trajectory & traj); + +/** + * @brief calculate length of the trajectory [m] + * @param [in] traj input trajectory + * @return calculated statistics + */ +Stat calcTrajectoryLength(const Trajectory & traj); + +/** + * @brief calculate duration of the trajectory [s] + * @param [in] traj input trajectory + * @return calculated statistics + */ +Stat calcTrajectoryDuration(const Trajectory & traj); + +/** + * @brief calculate velocity metrics for the trajectory + * @param [in] traj input trajectory + * @return calculated statistics + */ +Stat calcTrajectoryVelocity(const Trajectory & traj); + +/** + * @brief calculate acceleration metrics for the trajectory + * @param [in] traj input trajectory + * @return calculated statistics + */ +Stat calcTrajectoryAcceleration(const Trajectory & traj); + +/** + * @brief calculate jerk metrics for the trajectory + * @param [in] traj input trajectory + * @return calculated statistics + */ +Stat calcTrajectoryJerk(const Trajectory & traj); + +} // namespace metrics +} // namespace planning_diagnostics + +#endif // PLANNING_EVALUATOR__METRICS__TRAJECTORY_METRICS_HPP_ diff --git a/planning/planning_evaluator/include/planning_evaluator/metrics_calculator.hpp b/planning/planning_evaluator/include/planning_evaluator/metrics_calculator.hpp new file mode 100644 index 0000000000000..0ed2af7b3862e --- /dev/null +++ b/planning/planning_evaluator/include/planning_evaluator/metrics_calculator.hpp @@ -0,0 +1,93 @@ +// 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_EVALUATOR__METRICS_CALCULATOR_HPP_ +#define PLANNING_EVALUATOR__METRICS_CALCULATOR_HPP_ + +#include "planning_evaluator/metrics/metric.hpp" +#include "planning_evaluator/parameters.hpp" +#include "planning_evaluator/stat.hpp" + +#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" +#include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "geometry_msgs/msg/pose.hpp" + +namespace planning_diagnostics +{ +using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_auto_planning_msgs::msg::TrajectoryPoint; + +class MetricsCalculator +{ +public: + Parameters parameters; + + MetricsCalculator() = default; + + /** + * @brief calculate + * @param [in] metric Metric enum value + * @return string describing the requested metric + */ + Stat calculate(const Metric metric, const Trajectory & traj) const; + + /** + * @brief set the reference trajectory used to calculate the deviation metrics + * @param [in] traj input reference trajectory + */ + void setReferenceTrajectory(const Trajectory & traj); + + /** + * @brief set the previous trajectory used to calculate the stability metrics + * @param [in] traj input previous trajectory + */ + void setPreviousTrajectory(const Trajectory & traj); + + /** + * @brief set the dynamic objects used to calculate obstacle metrics + * @param [in] traj input previous trajectory + */ + void setPredictedObjects(const PredictedObjects & objects); + + /** + * @brief set the ego pose + * @param [in] traj input previous trajectory + */ + void setEgoPose(const geometry_msgs::msg::Pose & pose); + +private: + /** + * @brief trim a trajectory from the current ego pose to some fixed time or distance + * @param [in] traj input trajectory to trim + * @param [in] max_dist_m [m] maximum distance ahead of the ego pose + * @param [in] max_time_s [s] maximum time ahead of the ego pose + * @return sub-trajectory starting from the ego pose and of maximum length max_dist_m, maximum + * duration max_time_s + */ + Trajectory getLookaheadTrajectory( + const Trajectory & traj, const double max_dist_m, const double max_time_s) const; + + Trajectory reference_trajectory_; + Trajectory reference_trajectory_lookahead_; + Trajectory previous_trajectory_; + Trajectory previous_trajectory_lookahead_; + PredictedObjects dynamic_objects_; + geometry_msgs::msg::Pose ego_pose_; +}; // class MetricsCalculator + +} // namespace planning_diagnostics + +#endif // PLANNING_EVALUATOR__METRICS_CALCULATOR_HPP_ diff --git a/planning/planning_evaluator/include/planning_evaluator/motion_evaluator_node.hpp b/planning/planning_evaluator/include/planning_evaluator/motion_evaluator_node.hpp new file mode 100644 index 0000000000000..28a4b2cba8365 --- /dev/null +++ b/planning/planning_evaluator/include/planning_evaluator/motion_evaluator_node.hpp @@ -0,0 +1,74 @@ +// 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_EVALUATOR__MOTION_EVALUATOR_NODE_HPP_ +#define PLANNING_EVALUATOR__MOTION_EVALUATOR_NODE_HPP_ + +#include "planning_evaluator/metrics_calculator.hpp" +#include "planning_evaluator/stat.hpp" +#include "rclcpp/rclcpp.hpp" +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" + +#include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include + +#include +#include +#include +#include + +namespace planning_diagnostics +{ +using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_auto_planning_msgs::msg::TrajectoryPoint; + +/** + * @brief Node for planning evaluation + */ +class MotionEvaluatorNode : public rclcpp::Node +{ +public: + explicit MotionEvaluatorNode(const rclcpp::NodeOptions & node_options); + ~MotionEvaluatorNode(); + + /** + * @brief callback on vehicle twist message + * @param [in] twist_msg twist message + */ + void onOdom(const nav_msgs::msg::Odometry::SharedPtr msg); + +private: + geometry_msgs::msg::Pose getCurrentEgoPose() const; + + // ROS + rclcpp::Subscription::SharedPtr twist_sub_; + std::unique_ptr tf_buffer_ptr_; + std::unique_ptr tf_listener_ptr_; + + // Parameters + std::string output_file_str_; + + // Calculator + MetricsCalculator metrics_calculator_; + // Metrics + std::vector metrics_; + std::deque stamps_; + Trajectory accumulated_trajectory_; +}; +} // namespace planning_diagnostics + +#endif // PLANNING_EVALUATOR__MOTION_EVALUATOR_NODE_HPP_ diff --git a/planning/planning_evaluator/include/planning_evaluator/parameters.hpp b/planning/planning_evaluator/include/planning_evaluator/parameters.hpp new file mode 100644 index 0000000000000..fb4acb53888f2 --- /dev/null +++ b/planning/planning_evaluator/include/planning_evaluator/parameters.hpp @@ -0,0 +1,49 @@ +// 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_EVALUATOR__PARAMETERS_HPP_ +#define PLANNING_EVALUATOR__PARAMETERS_HPP_ + +#include "planning_evaluator/metrics/metric.hpp" + +#include + +namespace planning_diagnostics +{ +/** + * @brief Enumeration of trajectory metrics + */ +struct Parameters +{ + std::array(Metric::SIZE)> metrics{}; // default values to false + + struct + { + double min_point_dist_m = 0.1; + struct + { + double max_dist_m = 5.0; + double max_time_s = 3.0; + } lookahead; + } trajectory; + + struct + { + double dist_thr_m = 1.0; + } obstacle; +}; // struct Parameters + +} // namespace planning_diagnostics + +#endif // PLANNING_EVALUATOR__PARAMETERS_HPP_ diff --git a/planning/planning_evaluator/include/planning_evaluator/planning_evaluator_node.hpp b/planning/planning_evaluator/include/planning_evaluator/planning_evaluator_node.hpp new file mode 100644 index 0000000000000..e9fd82c85cf7b --- /dev/null +++ b/planning/planning_evaluator/include/planning_evaluator/planning_evaluator_node.hpp @@ -0,0 +1,105 @@ +// 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_EVALUATOR__PLANNING_EVALUATOR_NODE_HPP_ +#define PLANNING_EVALUATOR__PLANNING_EVALUATOR_NODE_HPP_ + +#include "planning_evaluator/metrics_calculator.hpp" +#include "planning_evaluator/stat.hpp" +#include "rclcpp/rclcpp.hpp" +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" + +#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" +#include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "diagnostic_msgs/msg/diagnostic_array.hpp" + +#include +#include +#include +#include +#include + +namespace planning_diagnostics +{ +using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using diagnostic_msgs::msg::DiagnosticArray; +using diagnostic_msgs::msg::DiagnosticStatus; + +/** + * @brief Node for planning evaluation + */ +class PlanningEvaluatorNode : public rclcpp::Node +{ +public: + explicit PlanningEvaluatorNode(const rclcpp::NodeOptions & node_options); + ~PlanningEvaluatorNode(); + + /** + * @brief callback on receiving a trajectory + * @param [in] traj_msg received trajectory message + */ + void onTrajectory(const Trajectory::ConstSharedPtr traj_msg); + + /** + * @brief callback on receiving a reference trajectory + * @param [in] traj_msg received trajectory message + */ + void onReferenceTrajectory(const Trajectory::ConstSharedPtr traj_msg); + + /** + * @brief callback on receiving a dynamic objects array + * @param [in] objects_msg received dynamic object array message + */ + void onObjects(const PredictedObjects::ConstSharedPtr objects_msg); + + /** + * @brief update the ego pose stored in the MetricsCalculator + */ + void updateCalculatorEgoPose(const std::string & target_frame); + + /** + * @brief publish the given metric statistic + */ + DiagnosticStatus generateDiagnosticStatus( + const Metric & metric, const Stat & metric_stat) const; + +private: + static bool isFinite(const TrajectoryPoint & p); + + // ROS + rclcpp::Subscription::SharedPtr traj_sub_; + rclcpp::Subscription::SharedPtr ref_sub_; + rclcpp::Subscription::SharedPtr objects_sub_; + rclcpp::Publisher::SharedPtr metrics_pub_; + std::shared_ptr transform_listener_{nullptr}; + std::unique_ptr tf_buffer_; + + // Parameters + std::string output_file_str_; + std::string ego_frame_str_; + + // Calculator + MetricsCalculator metrics_calculator_; + // Metrics + std::vector metrics_; + std::deque stamps_; + std::array>, static_cast(Metric::SIZE)> metric_stats_; +}; +} // namespace planning_diagnostics + +#endif // PLANNING_EVALUATOR__PLANNING_EVALUATOR_NODE_HPP_ diff --git a/planning/planning_evaluator/include/planning_evaluator/stat.hpp b/planning/planning_evaluator/include/planning_evaluator/stat.hpp new file mode 100644 index 0000000000000..ce03aea7669b4 --- /dev/null +++ b/planning/planning_evaluator/include/planning_evaluator/stat.hpp @@ -0,0 +1,93 @@ +// 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 + +#ifndef PLANNING_EVALUATOR__STAT_HPP_ +#define PLANNING_EVALUATOR__STAT_HPP_ + +namespace planning_diagnostics +{ +/** + * @brief class to incrementally build statistics + * @typedef T type of the values (default to double) + */ +template +class Stat +{ +public: + /** + * @brief add a value + * @param value value to add + */ + void add(const T & value) + { + if (value < min_) { + min_ = value; + } + if (value > max_) { + max_ = value; + } + ++count_; + mean_ = mean_ + (value - mean_) / count_; + } + + /** + * @brief get the mean value + */ + long double mean() const { return mean_; } + + /** + * @brief get the minimum value + */ + T min() const { return min_; } + + /** + * @brief get the maximum value + */ + T max() const { return max_; } + + /** + * @brief get the number of values used to build this statistic + */ + unsigned int count() const { return count_; } + + template + friend std::ostream & operator<<(std::ostream & os, const Stat & stat); + +private: + T min_ = std::numeric_limits::max(); + T max_ = std::numeric_limits::min(); + long double mean_ = 0.0; + unsigned int count_ = 0; +}; + +/** + * @brief overload << operator for easy print to output stream + */ +template +std::ostream & operator<<(std::ostream & os, const Stat & stat) +{ + if (stat.count() == 0) { + os << "None None None"; + } else { + os << stat.min() << " " << stat.max() << " " << stat.mean(); + } + return os; +} + +} // namespace planning_diagnostics + +#endif // PLANNING_EVALUATOR__STAT_HPP_ diff --git a/planning/planning_evaluator/launch/motion_evaluator.launch.xml b/planning/planning_evaluator/launch/motion_evaluator.launch.xml new file mode 100644 index 0000000000000..93a38884c6d49 --- /dev/null +++ b/planning/planning_evaluator/launch/motion_evaluator.launch.xml @@ -0,0 +1,7 @@ + + + + + + + diff --git a/planning/planning_evaluator/launch/planning_evaluator.launch.xml b/planning/planning_evaluator/launch/planning_evaluator.launch.xml new file mode 100644 index 0000000000000..e33ac129f2510 --- /dev/null +++ b/planning/planning_evaluator/launch/planning_evaluator.launch.xml @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/planning/planning_evaluator/package.xml b/planning/planning_evaluator/package.xml new file mode 100644 index 0000000000000..14b424b594ab6 --- /dev/null +++ b/planning/planning_evaluator/package.xml @@ -0,0 +1,32 @@ + + + planning_evaluator + 0.1.0 + ROS2 node for evaluating planners + + Maxime CLEMENT + + Apache License 2.0 + + ament_cmake_auto + + autoware_auto_perception_msgs + autoware_auto_planning_msgs + diagnostic_msgs + eigen + geometry_msgs + nav_msgs + rclcpp + rclcpp_components + tf2 + tf2_ros + tier4_autoware_utils + + ament_cmake_gtest + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/planning_evaluator/param/planning_evaluator.defaults.yaml b/planning/planning_evaluator/param/planning_evaluator.defaults.yaml new file mode 100644 index 0000000000000..bd47deedb282f --- /dev/null +++ b/planning/planning_evaluator/param/planning_evaluator.defaults.yaml @@ -0,0 +1,30 @@ +/**: + ros__parameters: + output_file: "" # if empty, metrics are not written to file + ego_frame: base_link # reference frame of ego + + selected_metrics: + - curvature + - point_interval + - relative_angle + - length + - duration + - velocity + - acceleration + - jerk + - lateral_deviation + - yaw_deviation + - velocity_deviation + - stability + - stability_frechet + - obstacle_distance + - obstacle_ttc + + trajectory: + min_point_dist_m: 0.1 # [m] minimum distance between two successive points to use for angle calculation + lookahead: + max_dist_m: 5.0 # [m] maximum distance from ego along the trajectory to use for calculation + max_time_s: 3.0 # [s] maximum time ahead of ego along the trajectory to use for calculation + + obstacle: + dist_thr_m: 1.0 # [m] distance between ego and the obstacle below which a collision is considered diff --git a/planning/planning_evaluator/src/metrics/deviation_metrics.cpp b/planning/planning_evaluator/src/metrics/deviation_metrics.cpp new file mode 100644 index 0000000000000..669008ff86c9e --- /dev/null +++ b/planning/planning_evaluator/src/metrics/deviation_metrics.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_evaluator/metrics/deviation_metrics.hpp" + +#include "tier4_autoware_utils/tier4_autoware_utils.hpp" + +namespace planning_diagnostics +{ +namespace metrics +{ +using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_auto_planning_msgs::msg::TrajectoryPoint; + +Stat calcLateralDeviation(const Trajectory & ref, const Trajectory & traj) +{ + Stat stat; + + if (ref.points.empty() || traj.points.empty()) { + return stat; + } + + /** TODO(Maxime CLEMENT): + * need more precise calculation, e.g., lateral distance from spline of the reference traj + */ + for (TrajectoryPoint p : traj.points) { + const size_t nearest_index = + tier4_autoware_utils::findNearestIndex(ref.points, p.pose.position); + stat.add( + tier4_autoware_utils::calcLateralDeviation(ref.points[nearest_index].pose, p.pose.position)); + } + return stat; +} + +Stat calcYawDeviation(const Trajectory & ref, const Trajectory & traj) +{ + Stat stat; + + if (ref.points.empty() || traj.points.empty()) { + return stat; + } + + /** TODO(Maxime CLEMENT): + * need more precise calculation, e.g., yaw distance from spline of the reference traj + */ + for (TrajectoryPoint p : traj.points) { + const size_t nearest_index = + tier4_autoware_utils::findNearestIndex(ref.points, p.pose.position); + stat.add(tier4_autoware_utils::calcYawDeviation(ref.points[nearest_index].pose, p.pose)); + } + return stat; +} + +Stat calcVelocityDeviation(const Trajectory & ref, const Trajectory & traj) +{ + Stat stat; + + if (ref.points.empty() || traj.points.empty()) { + return stat; + } + + // TODO(Maxime CLEMENT) need more precise calculation + for (TrajectoryPoint p : traj.points) { + const size_t nearest_index = + tier4_autoware_utils::findNearestIndex(ref.points, p.pose.position); + stat.add(p.longitudinal_velocity_mps - ref.points[nearest_index].longitudinal_velocity_mps); + } + return stat; +} + +} // namespace metrics +} // namespace planning_diagnostics diff --git a/planning/planning_evaluator/src/metrics/metrics_utils.cpp b/planning/planning_evaluator/src/metrics/metrics_utils.cpp new file mode 100644 index 0000000000000..e2fd04a1d7a6e --- /dev/null +++ b/planning/planning_evaluator/src/metrics/metrics_utils.cpp @@ -0,0 +1,48 @@ +// 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_evaluator/metrics/trajectory_metrics.hpp" +#include "tier4_autoware_utils/tier4_autoware_utils.hpp" + +namespace planning_diagnostics +{ +namespace metrics +{ +namespace utils +{ +using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using tier4_autoware_utils::calcDistance2d; + +size_t getIndexAfterDistance(const Trajectory & traj, const size_t curr_id, const double distance) +{ + // Get Current Trajectory Point + const TrajectoryPoint & curr_p = traj.points.at(curr_id); + + size_t target_id = curr_id; + double current_distance = 0.0; + for (size_t traj_id = curr_id + 1; traj_id < traj.points.size(); ++traj_id) { + current_distance = calcDistance2d(traj.points.at(traj_id), curr_p); + if (current_distance >= distance) { + target_id = traj_id; + break; + } + } + + return target_id; +} + +} // namespace utils +} // namespace metrics +} // namespace planning_diagnostics diff --git a/planning/planning_evaluator/src/metrics/obstacle_metrics.cpp b/planning/planning_evaluator/src/metrics/obstacle_metrics.cpp new file mode 100644 index 0000000000000..46ad5c79b8b4f --- /dev/null +++ b/planning/planning_evaluator/src/metrics/obstacle_metrics.cpp @@ -0,0 +1,84 @@ +// 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_evaluator/metrics/obstacle_metrics.hpp" + +#include "eigen3/Eigen/Core" +#include "tier4_autoware_utils/tier4_autoware_utils.hpp" + +#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" + +#include +#include + +namespace planning_diagnostics +{ +namespace metrics +{ +using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using tier4_autoware_utils::calcDistance2d; + +Stat calcDistanceToObstacle(const PredictedObjects & obstacles, const Trajectory & traj) +{ + Stat stat; + for (const TrajectoryPoint & p : traj.points) { + double min_dist = std::numeric_limits::max(); + for (const auto & object : obstacles.objects) { + // TODO(Maxime CLEMENT): take into account the shape, not only the centroid + const auto dist = calcDistance2d(object.kinematics.initial_pose_with_covariance.pose, p); + min_dist = std::min(min_dist, dist); + } + stat.add(min_dist); + } + return stat; +} + +Stat calcTimeToCollision( + const PredictedObjects & obstacles, const Trajectory & traj, const double distance_threshold) +{ + Stat stat; + /** TODO(Maxime CLEMENT): + * this implementation assumes static obstacles and does not work for dynamic obstacles + */ + TrajectoryPoint p0; + if (!traj.points.empty()) { + p0 = traj.points.front(); + } + + double t = 0.0; // [s] time from start of trajectory + for (const TrajectoryPoint & p : traj.points) { + const double traj_dist = calcDistance2d(p0, p); + if (p0.longitudinal_velocity_mps != 0) { + const double dt = traj_dist / std::abs(p0.longitudinal_velocity_mps); + t += dt; + for (auto obstacle : obstacles.objects) { + const double obst_dist = + calcDistance2d(p, obstacle.kinematics.initial_pose_with_covariance.pose); + // TODO(Maxime CLEMENT): take shape into consideration + if (obst_dist <= distance_threshold) { + stat.add(t); + break; + } + } + } + if (stat.count() > 0) { + break; + } + p0 = p; + } + return stat; +} + +} // namespace metrics +} // namespace planning_diagnostics diff --git a/planning/planning_evaluator/src/metrics/stability_metrics.cpp b/planning/planning_evaluator/src/metrics/stability_metrics.cpp new file mode 100644 index 0000000000000..f4b50d5d59d20 --- /dev/null +++ b/planning/planning_evaluator/src/metrics/stability_metrics.cpp @@ -0,0 +1,96 @@ +// 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_evaluator/metrics/stability_metrics.hpp" + +#include "eigen3/Eigen/Core" +#include "tier4_autoware_utils/tier4_autoware_utils.hpp" + +#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" + +#include + +namespace planning_diagnostics +{ +namespace metrics +{ +using autoware_auto_planning_msgs::msg::TrajectoryPoint; + +Stat calcFrechetDistance(const Trajectory & traj1, const Trajectory & traj2) +{ + Stat stat; + + if (traj1.points.empty() || traj2.points.empty()) { + return stat; + } + + Eigen::MatrixXd ca = Eigen::MatrixXd::Zero(traj1.points.size(), traj2.points.size()); + + for (size_t i = 0; i < traj1.points.size(); ++i) { + for (size_t j = 0; j < traj2.points.size(); ++j) { + const double dist = tier4_autoware_utils::calcDistance2d(traj1.points[i], traj2.points[j]); + if (i > 0 && j > 0) { + ca(i, j) = std::max(std::min(ca(i - 1, j), std::min(ca(i - 1, j - 1), ca(i, j - 1))), dist); + } else if (i > 0 /*&& j == 0*/) { + ca(i, j) = std::max(ca(i - 1, 0), dist); + } else if (j > 0 /*&& i == 0*/) { + ca(i, j) = std::max(ca(0, j - 1), dist); + } else { /* i == j == 0 */ + ca(i, j) = dist; + } + } + } + stat.add(ca(traj1.points.size() - 1, traj2.points.size() - 1)); + return stat; +} + +Stat calcLateralDistance(const Trajectory & traj1, const Trajectory & traj2) +{ + Stat stat; + if (traj1.points.empty()) { + return stat; + } + for (const auto point : traj2.points) { + const auto p0 = tier4_autoware_utils::getPoint(point); + // find nearest segment + const size_t nearest_segment_idx = + tier4_autoware_utils::findNearestSegmentIndex(traj1.points, p0); + double dist; + // distance to segment + if ( + nearest_segment_idx == traj1.points.size() - 2 && + tier4_autoware_utils::calcLongitudinalOffsetToSegment(traj1.points, nearest_segment_idx, p0) > + tier4_autoware_utils::calcDistance2d( + traj1.points[nearest_segment_idx], traj1.points[nearest_segment_idx + 1])) { + // distance to last point + dist = tier4_autoware_utils::calcDistance2d(traj1.points.back(), p0); + } else if ( // NOLINT + nearest_segment_idx == 0 && tier4_autoware_utils::calcLongitudinalOffsetToSegment( + traj1.points, nearest_segment_idx, p0) <= 0) { + // distance to first point + dist = tier4_autoware_utils::calcDistance2d(traj1.points.front(), p0); + } else { + // orthogonal distance + const auto p1 = tier4_autoware_utils::getPoint(traj1.points[nearest_segment_idx]); + const auto p2 = tier4_autoware_utils::getPoint(traj1.points[nearest_segment_idx + 1]); + dist = std::abs((p2.x - p1.x) * (p1.y - p0.y) - (p1.x - p0.x) * (p2.y - p1.y)) / + std::sqrt((p2.x - p1.x) * (p2.x - p1.x) + (p2.y - p1.y) * (p2.y - p1.y)); + } + stat.add(dist); + } + return stat; +} + +} // namespace metrics +} // namespace planning_diagnostics diff --git a/planning/planning_evaluator/src/metrics/trajectory_metrics.cpp b/planning/planning_evaluator/src/metrics/trajectory_metrics.cpp new file mode 100644 index 0000000000000..cba03e8b2d90e --- /dev/null +++ b/planning/planning_evaluator/src/metrics/trajectory_metrics.cpp @@ -0,0 +1,177 @@ +// 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_evaluator/metrics/trajectory_metrics.hpp" + +#include "planning_evaluator/metrics/metrics_utils.hpp" +#include "tier4_autoware_utils/tier4_autoware_utils.hpp" + +namespace planning_diagnostics +{ +namespace metrics +{ +using tier4_autoware_utils::calcCurvature; +using tier4_autoware_utils::calcDistance2d; + +Stat calcTrajectoryInterval(const Trajectory & traj) +{ + Stat stat; + for (size_t i = 1; i < traj.points.size(); ++i) { + stat.add(calcDistance2d(traj.points.at(i), traj.points.at(i - 1))); + } + return stat; +} + +Stat calcTrajectoryRelativeAngle(const Trajectory & traj, const double min_dist_threshold) +{ + Stat stat; + // We need at least three points to compute relative angle + const size_t relative_angle_points_num = 3; + if (traj.points.size() < relative_angle_points_num) { + return stat; + } + + for (size_t p1_id = 0; p1_id <= traj.points.size() - relative_angle_points_num; ++p1_id) { + // Get Point1 + const auto & p1 = traj.points.at(p1_id).pose.position; + + // Get Point2 + const auto & p2 = traj.points.at(p1_id + 1).pose.position; + + // Get Point3 + const auto & p3 = traj.points.at(p1_id + 2).pose.position; + + // ignore invert driving direction + if ( + traj.points.at(p1_id).longitudinal_velocity_mps < 0 || + traj.points.at(p1_id + 1).longitudinal_velocity_mps < 0 || + traj.points.at(p1_id + 2).longitudinal_velocity_mps < 0) { + continue; + } + + // convert to p1 coordinate + const double x3 = p3.x - p1.x; + const double x2 = p2.x - p1.x; + const double y3 = p3.y - p1.y; + const double y2 = p2.y - p1.y; + + // skip too close points case + if (std::hypot(x3, y3) < min_dist_threshold || std::hypot(x2, y2) < min_dist_threshold) { + continue; + } + + // calculate relative angle of vector p3 based on p1p2 vector + const double th = std::atan2(y2, x2); + const double th2 = + std::atan2(-x3 * std::sin(th) + y3 * std::cos(th), x3 * std::cos(th) + y3 * std::sin(th)); + stat.add(th2); + } + return stat; +} + +Stat calcTrajectoryCurvature(const Trajectory & traj) +{ + Stat stat; + // We need at least three points to compute curvature + if (traj.points.size() < 3) { + return stat; + } + + constexpr double points_distance = 1.0; + + for (size_t p1_id = 0; p1_id < traj.points.size() - 2; ++p1_id) { + // Get Point1 + const auto p1 = traj.points.at(p1_id).pose.position; + + // Get Point2 + const auto p2_id = utils::getIndexAfterDistance(traj, p1_id, points_distance); + const auto p2 = traj.points.at(p2_id).pose.position; + + // Get Point3 + const auto p3_id = utils::getIndexAfterDistance(traj, p2_id, points_distance); + const auto p3 = traj.points.at(p3_id).pose.position; + + // no need to check for pi, since there is no point with "points_distance" from p1. + if (p1_id == p2_id || p1_id == p3_id || p2_id == p3_id) { + break; + } + + stat.add(calcCurvature(p1, p2, p3)); + } + return stat; +} + +Stat calcTrajectoryLength(const Trajectory & traj) +{ + double length = 0.0; + for (size_t i = 1; i < traj.points.size(); ++i) { + length += calcDistance2d(traj.points.at(i), traj.points.at(i - 1)); + } + Stat stat; + stat.add(length); + return stat; +} + +Stat calcTrajectoryDuration(const Trajectory & traj) +{ + double duration = 0.0; + for (size_t i = 0; i < traj.points.size() - 1; ++i) { + const double length = calcDistance2d(traj.points.at(i), traj.points.at(i + 1)); + const double velocity = traj.points.at(i).longitudinal_velocity_mps; + if (velocity != 0) { + duration += length / std::abs(velocity); + } + } + Stat stat; + stat.add(duration); + return stat; +} + +Stat calcTrajectoryVelocity(const Trajectory & traj) +{ + Stat stat; + for (TrajectoryPoint p : traj.points) { + stat.add(p.longitudinal_velocity_mps); + } + return stat; +} + +Stat calcTrajectoryAcceleration(const Trajectory & traj) +{ + Stat stat; + for (TrajectoryPoint p : traj.points) { + stat.add(p.acceleration_mps2); + } + return stat; +} + +Stat calcTrajectoryJerk(const Trajectory & traj) +{ + Stat stat; + for (size_t i = 0; i < traj.points.size() - 1; ++i) { + const double vel = traj.points.at(i).longitudinal_velocity_mps; + if (vel != 0) { + const double duration = + calcDistance2d(traj.points.at(i), traj.points.at(i + 1)) / std::abs(vel); + if (duration != 0) { + const double start_accel = traj.points.at(i).acceleration_mps2; + const double end_accel = traj.points.at(i + 1).acceleration_mps2; + stat.add(end_accel - start_accel / duration); + } + } + } + return stat; +} +} // namespace metrics +} // namespace planning_diagnostics diff --git a/planning/planning_evaluator/src/metrics_calculator.cpp b/planning/planning_evaluator/src/metrics_calculator.cpp new file mode 100644 index 0000000000000..045331d1ee778 --- /dev/null +++ b/planning/planning_evaluator/src/metrics_calculator.cpp @@ -0,0 +1,124 @@ +// 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_evaluator/metrics_calculator.hpp" + +#include "planning_evaluator/metrics/deviation_metrics.hpp" +#include "planning_evaluator/metrics/obstacle_metrics.hpp" +#include "planning_evaluator/metrics/stability_metrics.hpp" +#include "planning_evaluator/metrics/trajectory_metrics.hpp" +#include "tier4_autoware_utils/tier4_autoware_utils.hpp" + +namespace planning_diagnostics +{ +Stat MetricsCalculator::calculate(const Metric metric, const Trajectory & traj) const +{ + // Functions to calculate metrics + switch (metric) { + case Metric::curvature: + return metrics::calcTrajectoryCurvature(traj); + case Metric::point_interval: + return metrics::calcTrajectoryInterval(traj); + case Metric::relative_angle: + return metrics::calcTrajectoryRelativeAngle(traj, parameters.trajectory.min_point_dist_m); + case Metric::length: + return metrics::calcTrajectoryLength(traj); + case Metric::duration: + return metrics::calcTrajectoryDuration(traj); + case Metric::velocity: + return metrics::calcTrajectoryVelocity(traj); + case Metric::acceleration: + return metrics::calcTrajectoryAcceleration(traj); + case Metric::jerk: + return metrics::calcTrajectoryJerk(traj); + case Metric::lateral_deviation: + return metrics::calcLateralDeviation(reference_trajectory_, traj); + case Metric::yaw_deviation: + return metrics::calcYawDeviation(reference_trajectory_, traj); + case Metric::velocity_deviation: + return metrics::calcVelocityDeviation(reference_trajectory_, traj); + case Metric::stability_frechet: + return metrics::calcFrechetDistance( + getLookaheadTrajectory( + previous_trajectory_, parameters.trajectory.lookahead.max_dist_m, + parameters.trajectory.lookahead.max_time_s), + getLookaheadTrajectory( + traj, parameters.trajectory.lookahead.max_dist_m, + parameters.trajectory.lookahead.max_time_s)); + case Metric::stability: + return metrics::calcLateralDistance( + getLookaheadTrajectory( + previous_trajectory_, parameters.trajectory.lookahead.max_dist_m, + parameters.trajectory.lookahead.max_time_s), + getLookaheadTrajectory( + traj, parameters.trajectory.lookahead.max_dist_m, + parameters.trajectory.lookahead.max_time_s)); + case Metric::obstacle_distance: + return metrics::calcDistanceToObstacle(dynamic_objects_, traj); + case Metric::obstacle_ttc: + return metrics::calcTimeToCollision(dynamic_objects_, traj, parameters.obstacle.dist_thr_m); + default: + throw std::runtime_error( + "[MetricsCalculator][calculate()] unknown Metric " + + std::to_string(static_cast(metric))); + } +} + +void MetricsCalculator::setReferenceTrajectory(const Trajectory & traj) +{ + reference_trajectory_ = traj; +} + +void MetricsCalculator::setPreviousTrajectory(const Trajectory & traj) +{ + previous_trajectory_ = traj; +} + +void MetricsCalculator::setPredictedObjects(const PredictedObjects & objects) +{ + dynamic_objects_ = objects; +} + +void MetricsCalculator::setEgoPose(const geometry_msgs::msg::Pose & pose) { ego_pose_ = pose; } + +Trajectory MetricsCalculator::getLookaheadTrajectory( + const Trajectory & traj, const double max_dist_m, const double max_time_s) const +{ + if (traj.points.empty()) { + return traj; + } + + const auto ego_index = + tier4_autoware_utils::findNearestSegmentIndex(traj.points, ego_pose_.position); + Trajectory lookahead_traj; + lookahead_traj.header = traj.header; + double dist = 0.0; + double time = 0.0; + auto curr_point_it = std::next(traj.points.begin(), ego_index); + auto prev_point_it = curr_point_it; + while (curr_point_it != traj.points.end() && dist <= max_dist_m && time <= max_time_s) { + lookahead_traj.points.push_back(*curr_point_it); + const auto d = tier4_autoware_utils::calcDistance2d( + prev_point_it->pose.position, curr_point_it->pose.position); + dist += d; + if (prev_point_it->longitudinal_velocity_mps != 0.0) { + time += d / std::abs(prev_point_it->longitudinal_velocity_mps); + } + prev_point_it = curr_point_it; + ++curr_point_it; + } + return lookahead_traj; +} + +} // namespace planning_diagnostics diff --git a/planning/planning_evaluator/src/motion_evaluator_node.cpp b/planning/planning_evaluator/src/motion_evaluator_node.cpp new file mode 100644 index 0000000000000..56b08fca81f9f --- /dev/null +++ b/planning/planning_evaluator/src/motion_evaluator_node.cpp @@ -0,0 +1,103 @@ +// 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_evaluator/motion_evaluator_node.hpp" + +#include +#include +#include +#include +#include + +namespace planning_diagnostics +{ +MotionEvaluatorNode::MotionEvaluatorNode(const rclcpp::NodeOptions & node_options) +: Node("motion_evaluator", node_options) +{ + tf_buffer_ptr_ = std::make_unique(this->get_clock()); + tf_listener_ptr_ = std::make_unique(*tf_buffer_ptr_); + + twist_sub_ = create_subscription( + "~/input/twist", rclcpp::QoS{1}, + std::bind(&MotionEvaluatorNode::onOdom, this, std::placeholders::_1)); + + output_file_str_ = declare_parameter("output_file"); + + // List of metrics to calculate + for (const std::string & selected_metric : + declare_parameter>("selected_metrics")) { + Metric metric = str_to_metric.at(selected_metric); + metrics_.push_back(metric); + } +} + +MotionEvaluatorNode::~MotionEvaluatorNode() +{ + // column width is the maximum size we might print + 1 for the space between columns + const auto column_width = 20; // std::to_string(std::numeric_limits::max()).size() + 1; + // Write data using format + std::ofstream f(output_file_str_); + f << std::fixed << std::left; + for (Metric metric : metrics_) { + f << std::setw(3 * column_width) << metric_descriptions.at(metric); + } + f << std::endl; + for (Metric metric : metrics_) { + const auto & stat = metrics_calculator_.calculate(metric, accumulated_trajectory_); + f /* << std::setw(3 * column_width) */ << stat << " "; + } + f.close(); +} + +void MotionEvaluatorNode::onOdom(const nav_msgs::msg::Odometry::SharedPtr msg) +{ + // TODO(Maxime CLEMENT): set some desired minimum time/distance between two points + TrajectoryPoint current_point; + current_point.pose = getCurrentEgoPose(); + current_point.longitudinal_velocity_mps = msg->twist.twist.linear.x; + const rclcpp::Time now = this->get_clock()->now(); + if (!accumulated_trajectory_.points.empty()) { + current_point.acceleration_mps2 = + (msg->twist.twist.linear.x - + accumulated_trajectory_.points.back().longitudinal_velocity_mps) / + (now - stamps_.back()).seconds(); + } + accumulated_trajectory_.points.push_back(current_point); + stamps_.push_back(now); +} + +geometry_msgs::msg::Pose MotionEvaluatorNode::getCurrentEgoPose() const +{ + geometry_msgs::msg::TransformStamped tf_current_pose; + + geometry_msgs::msg::Pose p; + try { + tf_current_pose = tf_buffer_ptr_->lookupTransform( + "map", "base_link", rclcpp::Time(0), rclcpp::Duration::from_seconds(1.0)); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR(get_logger(), "%s", ex.what()); + return p; + } + + p.orientation = tf_current_pose.transform.rotation; + p.position.x = tf_current_pose.transform.translation.x; + p.position.y = tf_current_pose.transform.translation.y; + p.position.z = tf_current_pose.transform.translation.z; + return p; +} + +} // namespace planning_diagnostics + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(planning_diagnostics::MotionEvaluatorNode) diff --git a/planning/planning_evaluator/src/planning_evaluator_node.cpp b/planning/planning_evaluator/src/planning_evaluator_node.cpp new file mode 100644 index 0000000000000..cca9bbf7c62e3 --- /dev/null +++ b/planning/planning_evaluator/src/planning_evaluator_node.cpp @@ -0,0 +1,191 @@ +// 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_evaluator/planning_evaluator_node.hpp" + +#include "boost/lexical_cast.hpp" + +#include +#include +#include +#include +#include +#include +#include + +namespace planning_diagnostics +{ +PlanningEvaluatorNode::PlanningEvaluatorNode(const rclcpp::NodeOptions & node_options) +: Node("planning_evaluator", node_options) +{ + using std::placeholders::_1; + + traj_sub_ = create_subscription( + "~/input/trajectory", 1, std::bind(&PlanningEvaluatorNode::onTrajectory, this, _1)); + + ref_sub_ = create_subscription( + "~/input/reference_trajectory", 1, + std::bind(&PlanningEvaluatorNode::onReferenceTrajectory, this, _1)); + + objects_sub_ = create_subscription( + "~/input/objects", 1, std::bind(&PlanningEvaluatorNode::onObjects, this, _1)); + tf_buffer_ = std::make_unique(this->get_clock()); + transform_listener_ = std::make_shared(*tf_buffer_); + + // Parameters + metrics_calculator_.parameters.trajectory.min_point_dist_m = + declare_parameter("trajectory.min_point_dist_m"); + metrics_calculator_.parameters.trajectory.lookahead.max_dist_m = + declare_parameter("trajectory.lookahead.max_dist_m"); + metrics_calculator_.parameters.trajectory.lookahead.max_time_s = + declare_parameter("trajectory.lookahead.max_time_s"); + metrics_calculator_.parameters.obstacle.dist_thr_m = + declare_parameter("obstacle.dist_thr_m"); + + output_file_str_ = declare_parameter("output_file"); + ego_frame_str_ = declare_parameter("ego_frame"); + + // List of metrics to calculate and publish + metrics_pub_ = create_publisher("~/metrics", 1); + for (const std::string & selected_metric : + declare_parameter>("selected_metrics")) { + Metric metric = str_to_metric.at(selected_metric); + metrics_.push_back(metric); + } +} + +PlanningEvaluatorNode::~PlanningEvaluatorNode() +{ + if (!output_file_str_.empty()) { + // column width is the maximum size we might print + 1 for the space between columns + // Write data using format + std::ofstream f(output_file_str_); + f << std::fixed << std::left; + // header + f << "#Stamp(ns)"; + for (Metric metric : metrics_) { + f << " " << metric_descriptions.at(metric); + f << " . ."; // extra "columns" to align columns headers + } + f << std::endl; + f << "#."; + for (Metric metric : metrics_) { + (void)metric; + f << " min max mean"; + } + f << std::endl; + // data + for (size_t i = 0; i < stamps_.size(); ++i) { + f << stamps_[i].nanoseconds(); + for (Metric metric : metrics_) { + const auto & stat = metric_stats_[static_cast(metric)][i]; + f << " " << stat; + } + f << std::endl; + } + f.close(); + } +} + +void PlanningEvaluatorNode::updateCalculatorEgoPose(const std::string & target_frame) +{ + try { + const geometry_msgs::msg::TransformStamped transform = + tf_buffer_->lookupTransform(target_frame, ego_frame_str_, tf2::TimePointZero); + geometry_msgs::msg::Pose ego_pose; + ego_pose.position.x = transform.transform.translation.x; + ego_pose.position.y = transform.transform.translation.y; + ego_pose.position.z = transform.transform.translation.z; + ego_pose.orientation = transform.transform.rotation; + metrics_calculator_.setEgoPose(ego_pose); + } catch (tf2::TransformException & ex) { + RCLCPP_INFO( + this->get_logger(), "Cannot set ego pose: could not transform %s to %s: %s", + target_frame.c_str(), ego_frame_str_.c_str(), ex.what()); + } +} + +DiagnosticStatus PlanningEvaluatorNode::generateDiagnosticStatus( + const Metric & metric, const Stat & metric_stat) const +{ + DiagnosticStatus status; + status.level = status.OK; + status.name = metric_to_str.at(metric); + diagnostic_msgs::msg::KeyValue key_value; + key_value.key = "min"; + key_value.value = boost::lexical_cast(metric_stat.min()); + status.values.push_back(key_value); + key_value.key = "max"; + key_value.value = boost::lexical_cast(metric_stat.max()); + status.values.push_back(key_value); + key_value.key = "mean"; + key_value.value = boost::lexical_cast(metric_stat.mean()); + status.values.push_back(key_value); + return status; +} + +void PlanningEvaluatorNode::onTrajectory(const Trajectory::ConstSharedPtr traj_msg) +{ + auto start = now(); + stamps_.push_back(traj_msg->header.stamp); + + updateCalculatorEgoPose(traj_msg->header.frame_id); + + DiagnosticArray metrics_msg; + metrics_msg.header.stamp = now(); + for (Metric metric : metrics_) { + const Stat metric_stat = metrics_calculator_.calculate(Metric(metric), *traj_msg); + metric_stats_[static_cast(metric)].push_back(metric_stat); + if (metric_stat.count() > 0) { + metrics_msg.status.push_back(generateDiagnosticStatus(metric, metric_stat)); + } + } + if (!metrics_msg.status.empty()) { + metrics_pub_->publish(metrics_msg); + } + metrics_calculator_.setPreviousTrajectory(*traj_msg); + auto runtime = (now() - start).seconds(); + RCLCPP_INFO(get_logger(), "Calculation time: %2.2f ms", runtime * 1e3); +} + +void PlanningEvaluatorNode::onReferenceTrajectory(const Trajectory::ConstSharedPtr traj_msg) +{ + metrics_calculator_.setReferenceTrajectory(*traj_msg); +} + +void PlanningEvaluatorNode::onObjects(const PredictedObjects::ConstSharedPtr objects_msg) +{ + metrics_calculator_.setPredictedObjects(*objects_msg); +} + +bool PlanningEvaluatorNode::isFinite(const TrajectoryPoint & point) +{ + const auto & o = point.pose.orientation; + const auto & p = point.pose.position; + const auto & v = point.longitudinal_velocity_mps; + const auto & w = point.lateral_velocity_mps; + const auto & a = point.acceleration_mps2; + const auto & z = point.heading_rate_rps; + const auto & f = point.front_wheel_angle_rad; + const auto & r = point.rear_wheel_angle_rad; + + return std::isfinite(o.x) && std::isfinite(o.y) && std::isfinite(o.z) && std::isfinite(o.w) && + std::isfinite(p.x) && std::isfinite(p.y) && std::isfinite(p.z) && std::isfinite(v) && + std::isfinite(w) && std::isfinite(a) && std::isfinite(z) && std::isfinite(f) && + std::isfinite(r); +} +} // namespace planning_diagnostics + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(planning_diagnostics::PlanningEvaluatorNode) diff --git a/planning/planning_evaluator/test/test_planning_evaluator_node.cpp b/planning/planning_evaluator/test/test_planning_evaluator_node.cpp new file mode 100644 index 0000000000000..8e9bcdd734222 --- /dev/null +++ b/planning/planning_evaluator/test/test_planning_evaluator_node.cpp @@ -0,0 +1,409 @@ +// 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 "ament_index_cpp/get_package_share_directory.hpp" +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/time.hpp" +#include "tf2_ros/transform_broadcaster.h" + +#include + +#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" +#include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "diagnostic_msgs/msg/diagnostic_array.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" + +#include "boost/lexical_cast.hpp" + +#include +#include +#include +#include + +using EvalNode = planning_diagnostics::PlanningEvaluatorNode; +using Trajectory = autoware_auto_planning_msgs::msg::Trajectory; +using TrajectoryPoint = autoware_auto_planning_msgs::msg::TrajectoryPoint; +using Objects = autoware_auto_perception_msgs::msg::PredictedObjects; +using diagnostic_msgs::msg::DiagnosticArray; + +class EvalTest : public ::testing::Test +{ +protected: + void SetUp() override + { + rclcpp::init(0, nullptr); + + rclcpp::NodeOptions options; + const auto share_dir = ament_index_cpp::get_package_share_directory("planning_evaluator"); + options.arguments( + {"--ros-args", "--params-file", share_dir + "/param/planning_evaluator.defaults.yaml"}); + + dummy_node = std::make_shared("planning_evaluator_test_node"); + eval_node = std::make_shared(options); + // Enable all logging in the node + auto ret = rcutils_logging_set_logger_level( + dummy_node->get_logger().get_name(), RCUTILS_LOG_SEVERITY_DEBUG); + if (ret != RCUTILS_RET_OK) { + std::cerr << "Failed to set logging severity to DEBUG\n"; + } + ret = rcutils_logging_set_logger_level( + eval_node->get_logger().get_name(), RCUTILS_LOG_SEVERITY_DEBUG); + if (ret != RCUTILS_RET_OK) { + std::cerr << "Failed to set logging severity to DEBUG\n"; + } + + traj_pub_ = + rclcpp::create_publisher(dummy_node, "/planning_evaluator/input/trajectory", 1); + ref_traj_pub_ = rclcpp::create_publisher( + dummy_node, "/planning_evaluator/input/reference_trajectory", 1); + objects_pub_ = + rclcpp::create_publisher(dummy_node, "/planning_evaluator/input/objects", 1); + + tf_broadcaster_ = std::make_unique(dummy_node); + publishEgoPose(0.0, 0.0, 0.0); + } + + ~EvalTest() override { rclcpp::shutdown(); } + + void setTargetMetric(planning_diagnostics::Metric metric) + { + const auto metric_str = planning_diagnostics::metric_to_str.at(metric); + const auto is_target_metric = [metric_str](const auto & status) { + return status.name == metric_str; + }; + metric_sub_ = rclcpp::create_subscription( + dummy_node, "/planning_evaluator/metrics", 1, [=](const DiagnosticArray::ConstSharedPtr msg) { + const auto it = std::find_if(msg->status.begin(), msg->status.end(), is_target_metric); + if (it != msg->status.end()) { + metric_value_ = boost::lexical_cast(it->values[2].value); + metric_updated_ = true; + } + }); + } + + Trajectory makeTrajectory(const std::vector> & traj) + { + Trajectory t; + t.header.frame_id = "map"; + TrajectoryPoint p; + for (const std::pair & point : traj) { + p.pose.position.x = point.first; + p.pose.position.y = point.second; + t.points.push_back(p); + } + return t; + } + + void publishTrajectory(const Trajectory & traj) + { + traj_pub_->publish(traj); + rclcpp::spin_some(eval_node); + rclcpp::spin_some(dummy_node); + rclcpp::sleep_for(std::chrono::milliseconds(100)); + } + void publishReferenceTrajectory(const Trajectory & traj) + { + ref_traj_pub_->publish(traj); + rclcpp::spin_some(eval_node); + rclcpp::spin_some(dummy_node); + rclcpp::sleep_for(std::chrono::milliseconds(100)); + } + void publishObjects(const Objects & obj) + { + objects_pub_->publish(obj); + rclcpp::spin_some(eval_node); + rclcpp::spin_some(dummy_node); + rclcpp::sleep_for(std::chrono::milliseconds(100)); + } + double publishTrajectoryAndGetMetric(const Trajectory & traj) + { + metric_updated_ = false; + traj_pub_->publish(traj); + while (!metric_updated_) { + rclcpp::spin_some(eval_node); + rclcpp::spin_some(dummy_node); + rclcpp::sleep_for(std::chrono::milliseconds(100)); + } + return metric_value_; + } + + void publishEgoPose(const double x, const double y, const double yaw) + { + geometry_msgs::msg::TransformStamped t; + + // Read message content and assign it to + // corresponding tf variables + t.header.stamp = dummy_node->now(); + t.header.frame_id = "map"; + t.child_frame_id = "base_link"; + + t.transform.translation.x = x; + t.transform.translation.y = y; + t.transform.translation.z = 0.0; + + tf2::Quaternion q; + q.setRPY(0.0, 0.0, yaw); + t.transform.rotation.x = q.x(); + t.transform.rotation.y = q.y(); + t.transform.rotation.z = q.z(); + t.transform.rotation.w = q.w(); + + tf_broadcaster_->sendTransform(t); + } + + // Latest metric value + bool metric_updated_ = false; + double metric_value_; + // Node + rclcpp::Node::SharedPtr dummy_node; + EvalNode::SharedPtr eval_node; + // Trajectory publishers + rclcpp::Publisher::SharedPtr traj_pub_; + rclcpp::Publisher::SharedPtr ref_traj_pub_; + rclcpp::Publisher::SharedPtr objects_pub_; + rclcpp::Subscription::SharedPtr metric_sub_; + // TF broadcaster + std::unique_ptr tf_broadcaster_; +}; + +TEST_F(EvalTest, TestCurvature) +{ + setTargetMetric(planning_diagnostics::Metric::curvature); + Trajectory t = makeTrajectory({{0.0, 0.0}, {1.0, 1.0}, {2.0, 0.0}}); + EXPECT_DOUBLE_EQ(publishTrajectoryAndGetMetric(t), -1.0); + t = makeTrajectory({{0.0, 0.0}, {2.0, -2.0}, {4.0, 0.0}}); + EXPECT_DOUBLE_EQ(publishTrajectoryAndGetMetric(t), 0.5); +} + +TEST_F(EvalTest, TestPointInterval) +{ + setTargetMetric(planning_diagnostics::Metric::point_interval); + Trajectory t = makeTrajectory({{0.0, 0.0}, {0.0, 1.0}, {0.0, 2.0}}); + EXPECT_DOUBLE_EQ(publishTrajectoryAndGetMetric(t), 1.0); + // double the average interval + TrajectoryPoint p; + p.pose.position.x = 0.0; + p.pose.position.y = 6.0; + t.points.push_back(p); + EXPECT_DOUBLE_EQ(publishTrajectoryAndGetMetric(t), 2.0); +} + +TEST_F(EvalTest, TestRelativeAngle) +{ + setTargetMetric(planning_diagnostics::Metric::relative_angle); + Trajectory t = makeTrajectory({{0.0, 0.0}, {0.0, 1.0}, {1.0, 1.0}}); + EXPECT_DOUBLE_EQ(publishTrajectoryAndGetMetric(t), -M_PI_4); + // add an angle of PI/4 to bring the average to 0 + TrajectoryPoint p; + p.pose.position.x = 1.0; + p.pose.position.y = 2.0; + t.points.push_back(p); + EXPECT_DOUBLE_EQ(publishTrajectoryAndGetMetric(t), 0.0); +} + +TEST_F(EvalTest, TestLength) +{ + setTargetMetric(planning_diagnostics::Metric::length); + Trajectory t = makeTrajectory({{0.0, 0.0}, {0.0, 1.0}, {0.0, 2.0}, {0.0, 3.0}}); + EXPECT_DOUBLE_EQ(publishTrajectoryAndGetMetric(t), 3.0); + TrajectoryPoint p; + p.pose.position.x = 3.0; + p.pose.position.y = 3.0; + t.points.push_back(p); + EXPECT_DOUBLE_EQ(publishTrajectoryAndGetMetric(t), 6.0); +} + +TEST_F(EvalTest, TestVelocity) +{ + setTargetMetric(planning_diagnostics::Metric::velocity); + Trajectory t = makeTrajectory({{0.0, 0.0}, {0.0, 1.0}, {0.0, 2.0}, {0.0, 3.0}}); + for (TrajectoryPoint & p : t.points) { + p.longitudinal_velocity_mps = 1.0; + } +} + +TEST_F(EvalTest, TestDuration) +{ + setTargetMetric(planning_diagnostics::Metric::duration); + Trajectory t = makeTrajectory({{0.0, 0.0}, {0.0, 1.0}, {0.0, 2.0}, {0.0, 3.0}}); + for (TrajectoryPoint & p : t.points) { + p.longitudinal_velocity_mps = 1.0; + } + EXPECT_DOUBLE_EQ(publishTrajectoryAndGetMetric(t), 3.0); + for (TrajectoryPoint & p : t.points) { + p.longitudinal_velocity_mps = 3.0; + } + EXPECT_DOUBLE_EQ(publishTrajectoryAndGetMetric(t), 1.0); +} + +TEST_F(EvalTest, TestAcceleration) +{ + setTargetMetric(planning_diagnostics::Metric::acceleration); + Trajectory t = makeTrajectory({{0.0, 0.0}, {0.0, 1.0}}); + t.points[0].acceleration_mps2 = 1.0; + t.points[1].acceleration_mps2 = 1.0; + EXPECT_DOUBLE_EQ(publishTrajectoryAndGetMetric(t), 1.0); + t.points[0].acceleration_mps2 = -1.0; + t.points[1].acceleration_mps2 = -1.0; + EXPECT_DOUBLE_EQ(publishTrajectoryAndGetMetric(t), -1.0); + t.points[0].acceleration_mps2 = 0.0; + t.points[1].acceleration_mps2 = 1.0; + EXPECT_DOUBLE_EQ(publishTrajectoryAndGetMetric(t), 0.5); +} + +TEST_F(EvalTest, TestJerk) +{ + setTargetMetric(planning_diagnostics::Metric::jerk); + Trajectory t = makeTrajectory({{0.0, 0.0}, {0.0, 1.0}}); + t.points[0].longitudinal_velocity_mps = 1.0; + t.points[0].acceleration_mps2 = 1.0; + t.points[1].longitudinal_velocity_mps = 2.0; + t.points[1].acceleration_mps2 = 1.0; + EXPECT_DOUBLE_EQ(publishTrajectoryAndGetMetric(t), 0.0); + t.points[0].longitudinal_velocity_mps = 1.0; + t.points[0].acceleration_mps2 = 1.0; + t.points[1].longitudinal_velocity_mps = 1.0; + t.points[1].acceleration_mps2 = 0.0; + EXPECT_DOUBLE_EQ(publishTrajectoryAndGetMetric(t), -1.0); +} + +TEST_F(EvalTest, TestLateralDeviation) +{ + setTargetMetric(planning_diagnostics::Metric::lateral_deviation); + Trajectory t = makeTrajectory({{0.0, 0.0}, {1.0, 0.0}}); + publishReferenceTrajectory(t); + EXPECT_DOUBLE_EQ(publishTrajectoryAndGetMetric(t), 0.0); + Trajectory t2 = makeTrajectory({{0.0, 1.0}, {1.0, 1.0}}); + EXPECT_DOUBLE_EQ(publishTrajectoryAndGetMetric(t2), 1.0); +} + +TEST_F(EvalTest, TestYawDeviation) +{ + auto setYaw = [](geometry_msgs::msg::Quaternion & msg, const double yaw_rad) { + tf2::Quaternion q; + q.setRPY(0.0, 0.0, yaw_rad); + msg.x = q.x(); + msg.y = q.y(); + msg.z = q.z(); + msg.w = q.w(); + }; + setTargetMetric(planning_diagnostics::Metric::yaw_deviation); + Trajectory t = makeTrajectory({{0.0, 0.0}, {1.0, 0.0}}); + for (auto & p : t.points) { + setYaw(p.pose.orientation, M_PI); + } + publishReferenceTrajectory(t); + EXPECT_DOUBLE_EQ(publishTrajectoryAndGetMetric(t), 0.0); + Trajectory t2 = t; + for (auto & p : t2.points) { + setYaw(p.pose.orientation, 0.0); + } + EXPECT_DOUBLE_EQ(publishTrajectoryAndGetMetric(t2), -M_PI); + for (auto & p : t2.points) { + setYaw(p.pose.orientation, -M_PI); + } + EXPECT_DOUBLE_EQ(publishTrajectoryAndGetMetric(t2), 0.0); +} + +TEST_F(EvalTest, TestVelocityDeviation) +{ + setTargetMetric(planning_diagnostics::Metric::velocity_deviation); + Trajectory t = makeTrajectory({{0.0, 0.0}, {1.0, 1.0}, {2.0, 2.0}}); + for (auto & p : t.points) { + p.longitudinal_velocity_mps = 0.0; + } + publishReferenceTrajectory(t); + EXPECT_DOUBLE_EQ(publishTrajectoryAndGetMetric(t), 0.0); + for (auto & p : t.points) { + p.longitudinal_velocity_mps = 1.0; + } + EXPECT_DOUBLE_EQ(publishTrajectoryAndGetMetric(t), 1.0); +} + +TEST_F(EvalTest, TestStability) +{ + setTargetMetric(planning_diagnostics::Metric::stability); + Trajectory t = makeTrajectory({{0.0, 0.0}, {1.0, 1.0}, {2.0, 2.0}, {3.0, 3.0}}); + publishTrajectory(t); + EXPECT_DOUBLE_EQ(publishTrajectoryAndGetMetric(t), 0.0); + t.points.back().pose.position.x = 0.0; + t.points.back().pose.position.y = 0.0; + EXPECT_DOUBLE_EQ(publishTrajectoryAndGetMetric(t), 0.0); + + Trajectory t2 = makeTrajectory({{0.0, 0.0}, {1.0, 1.0}, {2.0, 2.0}, {3.0, 3.0}}); + publishTrajectory(t2); + t2.points.back().pose.position.x = 4.0; + t2.points.back().pose.position.y = 3.0; + EXPECT_DOUBLE_EQ(publishTrajectoryAndGetMetric(t2), 1.0 / 4); +} + +TEST_F(EvalTest, TestFrechet) +{ + setTargetMetric(planning_diagnostics::Metric::stability_frechet); + Trajectory t = makeTrajectory({{0.0, 0.0}, {1.0, 1.0}, {2.0, 2.0}, {3.0, 3.0}}); + publishTrajectory(t); + EXPECT_DOUBLE_EQ(publishTrajectoryAndGetMetric(t), 0.0); + + // variation in the last point: simple distance from previous last point + t.points.back().pose.position.x = 0.0; + t.points.back().pose.position.y = 0.0; + EXPECT_DOUBLE_EQ(publishTrajectoryAndGetMetric(t), std::sqrt(18.0)); + Trajectory t2 = makeTrajectory({{0.0, 0.0}, {1.0, 1.0}, {2.0, 2.0}, {3.0, 3.0}}); + publishTrajectory(t2); + t2.points.back().pose.position.x = 4.0; + t2.points.back().pose.position.y = 3.0; + EXPECT_DOUBLE_EQ(publishTrajectoryAndGetMetric(t2), 1.0); + + // variations in the middle points: cannot go back to previous points that minimize the distance + t2.points[2].pose.position.x = 0.5; + t2.points[2].pose.position.y = 0.5; + EXPECT_DOUBLE_EQ(publishTrajectoryAndGetMetric(t2), std::sqrt(2 * (1.5 * 1.5))); +} + +TEST_F(EvalTest, TestObstacleDistance) +{ + setTargetMetric(planning_diagnostics::Metric::obstacle_distance); + Objects objs; + autoware_auto_perception_msgs::msg::PredictedObject obj; + obj.kinematics.initial_pose_with_covariance.pose.position.x = 0.0; + obj.kinematics.initial_pose_with_covariance.pose.position.y = 0.0; + objs.objects.push_back(obj); + publishObjects(objs); + + Trajectory t = makeTrajectory({{0.0, 0.0}, {1.0, 0.0}}); + EXPECT_DOUBLE_EQ(publishTrajectoryAndGetMetric(t), 0.5); + Trajectory t2 = makeTrajectory({{0.0, 0.0}, {1.0, 0.0}, {2.0, 0.0}}); + EXPECT_DOUBLE_EQ(publishTrajectoryAndGetMetric(t2), 1.0); // (0.0 + 1.0 + 2.0) / 3 +} + +TEST_F(EvalTest, TestObstacleTTC) +{ + setTargetMetric(planning_diagnostics::Metric::obstacle_ttc); + Objects objs; + autoware_auto_perception_msgs::msg::PredictedObject obj; + obj.kinematics.initial_pose_with_covariance.pose.position.x = 0.0; + obj.kinematics.initial_pose_with_covariance.pose.position.y = 0.0; + objs.objects.push_back(obj); + publishObjects(objs); + + Trajectory t = makeTrajectory({{3.0, 0.0}, {0.0, 0.0}, {-1.0, 0.0}}); + for (TrajectoryPoint & p : t.points) { + p.longitudinal_velocity_mps = 1.0; + } + EXPECT_DOUBLE_EQ(publishTrajectoryAndGetMetric(t), 3.0); + // if no exact collision point, last point before collision is used + t.points[1].pose.position.x = 1.0; + EXPECT_DOUBLE_EQ(publishTrajectoryAndGetMetric(t), 2.0); +}