From 50ffeef592440957ed01ecda2f9af290ac93e762 Mon Sep 17 00:00:00 2001 From: xtk8532704 <1041084556@qq.com> Date: Mon, 25 Nov 2024 18:08:02 +0900 Subject: [PATCH 1/9] add Accumulator class to autoware_universe_utils Signed-off-by: xtk8532704 <1041084556@qq.com> --- .../universe_utils/math/accumulator.hpp | 93 +++++++++++++++++++ .../test/src/math/test_accumulator.cpp | 29 ++++++ 2 files changed, 122 insertions(+) create mode 100644 common/autoware_universe_utils/include/autoware/universe_utils/math/accumulator.hpp create mode 100644 common/autoware_universe_utils/test/src/math/test_accumulator.cpp diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/math/accumulator.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/math/accumulator.hpp new file mode 100644 index 0000000000000..790c27f348532 --- /dev/null +++ b/common/autoware_universe_utils/include/autoware/universe_utils/math/accumulator.hpp @@ -0,0 +1,93 @@ +// Copyright 2024 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 AUTOWARE__UNIVERSE_UTILS__MATH__CONSTANTS_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__MATH__CONSTANTS_HPP_ + +namespace autoware::universe_utils +{ +/** + * @brief class to accumulate statistical data, supporting min, max and mean. + * @typedef T type of the values (default to double) + */ +template +class Accumulator +{ +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 Accumulator & accumulator); + +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 Accumulator & accumulator) +{ + if (accumulator.count() == 0) { + os << "None None None"; + } else { + os << accumulator.min() << " " << accumulator.max() << " " << accumulator.mean(); + } + return os; +} + +} // namespace autoware::universe_utils + +#endif // AUTOWARE__UNIVERSE_UTILS__MATH__CONSTANTS_HPP_ diff --git a/common/autoware_universe_utils/test/src/math/test_accumulator.cpp b/common/autoware_universe_utils/test/src/math/test_accumulator.cpp new file mode 100644 index 0000000000000..3e0963d9c8ad6 --- /dev/null +++ b/common/autoware_universe_utils/test/src/math/test_accumulator.cpp @@ -0,0 +1,29 @@ +// Copyright 2024 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 "autoware/universe_utils/math/accumulator.hpp" + +#include + +TEST(accumulator, statistic) +{ + autoware::universe_utils::Accumulator acc; + acc.add(1.0); + acc.add(2.0); + acc.add(3.0); + + EXPECT_DOUBLE_EQ(acc.mean(), 2.0); + EXPECT_DOUBLE_EQ(acc.min(), 1.0); + EXPECT_DOUBLE_EQ(acc.max(), 3.0); +} From e9df0812f30445550375a5b374851f03bfd453a3 Mon Sep 17 00:00:00 2001 From: xtk8532704 <1041084556@qq.com> Date: Mon, 25 Nov 2024 19:36:46 +0900 Subject: [PATCH 2/9] use Accumulator on all evaluators. Signed-off-by: xtk8532704 <1041084556@qq.com> --- .../universe_utils/math/accumulator.hpp | 6 +- .../autoware_planning_evaluator/README.md | 4 +- .../metrics/deviation_metrics.hpp | 15 +-- .../metrics/metrics_utils.hpp | 2 - .../metrics/obstacle_metrics.hpp | 7 +- .../metrics/stability_metrics.hpp | 7 +- .../metrics/trajectory_metrics.hpp | 19 ++-- .../planning_evaluator/metrics_calculator.hpp | 7 +- .../motion_evaluator_node.hpp | 3 +- .../planning_evaluator_node.hpp | 7 +- .../autoware/planning_evaluator/stat.hpp | 93 ------------------- .../src/metrics/deviation_metrics.cpp | 24 ++--- .../src/metrics/obstacle_metrics.cpp | 8 +- .../src/metrics/stability_metrics.cpp | 8 +- .../src/metrics/trajectory_metrics.cpp | 32 +++---- .../src/metrics_calculator.cpp | 4 +- .../src/planning_evaluator_node.cpp | 2 +- .../kinematic_evaluator_node.hpp | 9 +- .../metrics/kinematic_metrics.hpp | 5 +- .../metrics_calculator.hpp | 9 +- .../include/kinematic_evaluator/stat.hpp | 93 ------------------- .../src/kinematic_evaluator_node.cpp | 4 +- .../src/metrics/kinematic_metrics.cpp | 4 +- .../src/metrics_calculator.cpp | 4 +- .../localization_evaluator_node.hpp | 9 +- .../metrics/localization_metrics.hpp | 11 ++- .../metrics_calculator.hpp | 7 +- .../include/localization_evaluator/stat.hpp | 93 ------------------- .../src/localization_evaluator_node.cpp | 4 +- .../src/metrics/localization_metrics.cpp | 12 +-- .../src/metrics_calculator.cpp | 4 +- .../metrics/detection_count.hpp | 1 - .../metrics/deviation_metrics.hpp | 2 - .../metrics/metric.hpp | 9 +- .../metrics_calculator.hpp | 1 - .../perception_online_evaluator_node.hpp | 5 +- .../perception_online_evaluator/stat.hpp | 93 ------------------- .../src/metrics_calculator.cpp | 6 +- .../src/perception_online_evaluator_node.cpp | 2 +- 39 files changed, 136 insertions(+), 499 deletions(-) delete mode 100644 evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/stat.hpp delete mode 100644 evaluator/kinematic_evaluator/include/kinematic_evaluator/stat.hpp delete mode 100644 evaluator/localization_evaluator/include/localization_evaluator/stat.hpp delete mode 100644 evaluator/perception_online_evaluator/include/perception_online_evaluator/stat.hpp diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/math/accumulator.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/math/accumulator.hpp index 790c27f348532..cbb565b23ea98 100644 --- a/common/autoware_universe_utils/include/autoware/universe_utils/math/accumulator.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/math/accumulator.hpp @@ -15,8 +15,8 @@ #include #include -#ifndef AUTOWARE__UNIVERSE_UTILS__MATH__CONSTANTS_HPP_ -#define AUTOWARE__UNIVERSE_UTILS__MATH__CONSTANTS_HPP_ +#ifndef AUTOWARE__UNIVERSE_UTILS__MATH__ACCUMULATOR_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__MATH__ACCUMULATOR_HPP_ namespace autoware::universe_utils { @@ -90,4 +90,4 @@ std::ostream & operator<<(std::ostream & os, const Accumulator & accumulator) } // namespace autoware::universe_utils -#endif // AUTOWARE__UNIVERSE_UTILS__MATH__CONSTANTS_HPP_ +#endif // AUTOWARE__UNIVERSE_UTILS__MATH__ACCUMULATOR_HPP_ diff --git a/evaluator/autoware_planning_evaluator/README.md b/evaluator/autoware_planning_evaluator/README.md index d09949bd69b0e..30ae1559ca9da 100644 --- a/evaluator/autoware_planning_evaluator/README.md +++ b/evaluator/autoware_planning_evaluator/README.md @@ -20,7 +20,7 @@ which is also responsible for calculating metrics. ### Stat -Each metric is calculated using a `Stat` instance which contains +Each metric is calculated using a `autoware::universe_utils::Accumulator` instance which contains the minimum, maximum, and mean values calculated for the metric as well as the number of values measured. @@ -35,7 +35,7 @@ The `MetricsCalculator` is responsible for calculating metric statistics through calls to function: ```C++ -Stat MetricsCalculator::calculate(const Metric metric, const Trajectory & traj) const; +Accumulator MetricsCalculator::calculate(const Metric metric, const Trajectory & traj) const; ``` Adding a new metric `M` requires the following steps: diff --git a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/deviation_metrics.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/deviation_metrics.hpp index 8d7cfbd30d604..59866c96ad702 100644 --- a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/deviation_metrics.hpp +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/deviation_metrics.hpp @@ -15,7 +15,7 @@ #ifndef AUTOWARE__PLANNING_EVALUATOR__METRICS__DEVIATION_METRICS_HPP_ #define AUTOWARE__PLANNING_EVALUATOR__METRICS__DEVIATION_METRICS_HPP_ -#include "autoware/planning_evaluator/stat.hpp" +#include "autoware/universe_utils/math/accumulator.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" #include "autoware_planning_msgs/msg/trajectory_point.hpp" @@ -24,6 +24,7 @@ namespace planning_diagnostics { namespace metrics { +using autoware::universe_utils::Accumulator; using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; using geometry_msgs::msg::Point; @@ -35,7 +36,7 @@ using geometry_msgs::msg::Pose; * @param [in] traj input trajectory * @return calculated statistics */ -Stat calcLateralDeviation(const Trajectory & ref, const Trajectory & traj); +Accumulator calcLateralDeviation(const Trajectory & ref, const Trajectory & traj); /** * @brief calculate yaw deviation of the given trajectory from the reference trajectory @@ -43,7 +44,7 @@ Stat calcLateralDeviation(const Trajectory & ref, const Trajectory & tra * @param [in] traj input trajectory * @return calculated statistics */ -Stat calcYawDeviation(const Trajectory & ref, const Trajectory & traj); +Accumulator calcYawDeviation(const Trajectory & ref, const Trajectory & traj); /** * @brief calculate velocity deviation of the given trajectory from the reference trajectory @@ -51,7 +52,7 @@ Stat calcYawDeviation(const Trajectory & ref, const Trajectory & traj); * @param [in] traj input trajectory * @return calculated statistics */ -Stat calcVelocityDeviation(const Trajectory & ref, const Trajectory & traj); +Accumulator calcVelocityDeviation(const Trajectory & ref, const Trajectory & traj); /** * @brief calculate longitudinal deviation of the given ego pose from the modified goal pose @@ -59,7 +60,7 @@ Stat calcVelocityDeviation(const Trajectory & ref, const Trajectory & tr * @param [in] target_point target point * @return calculated statistics */ -Stat calcLongitudinalDeviation(const Pose & base_pose, const Point & target_point); +Accumulator calcLongitudinalDeviation(const Pose & base_pose, const Point & target_point); /** * @brief calculate lateral deviation of the given ego pose from the modified goal pose @@ -67,7 +68,7 @@ Stat calcLongitudinalDeviation(const Pose & base_pose, const Point & tar * @param [in] target_point target point * @return calculated statistics */ -Stat calcLateralDeviation(const Pose & base_pose, const Point & target_point); +Accumulator calcLateralDeviation(const Pose & base_pose, const Point & target_point); /** * @brief calculate yaw deviation of the given ego pose from the modified goal pose @@ -75,7 +76,7 @@ Stat calcLateralDeviation(const Pose & base_pose, const Point & target_p * @param [in] target_pose target pose * @return calculated statistics */ -Stat calcYawDeviation(const Pose & base_pose, const Pose & target_pose); +Accumulator calcYawDeviation(const Pose & base_pose, const Pose & target_pose); } // namespace metrics } // namespace planning_diagnostics diff --git a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/metrics_utils.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/metrics_utils.hpp index 2ade316ba7ad5..0006e49c3338a 100644 --- a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/metrics_utils.hpp +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/metrics_utils.hpp @@ -15,8 +15,6 @@ #ifndef AUTOWARE__PLANNING_EVALUATOR__METRICS__METRICS_UTILS_HPP_ #define AUTOWARE__PLANNING_EVALUATOR__METRICS__METRICS_UTILS_HPP_ -#include "autoware/planning_evaluator/stat.hpp" - #include "autoware_planning_msgs/msg/trajectory.hpp" #include "autoware_planning_msgs/msg/trajectory_point.hpp" diff --git a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/obstacle_metrics.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/obstacle_metrics.hpp index c4d468b4dacd5..79688f6ca9078 100644 --- a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/obstacle_metrics.hpp +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/obstacle_metrics.hpp @@ -15,7 +15,7 @@ #ifndef AUTOWARE__PLANNING_EVALUATOR__METRICS__OBSTACLE_METRICS_HPP_ #define AUTOWARE__PLANNING_EVALUATOR__METRICS__OBSTACLE_METRICS_HPP_ -#include "autoware/planning_evaluator/stat.hpp" +#include "autoware/universe_utils/math/accumulator.hpp" #include "autoware_perception_msgs/msg/predicted_objects.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" @@ -24,6 +24,7 @@ namespace planning_diagnostics { namespace metrics { +using autoware::universe_utils::Accumulator; using autoware_perception_msgs::msg::PredictedObjects; using autoware_planning_msgs::msg::Trajectory; @@ -33,7 +34,7 @@ using autoware_planning_msgs::msg::Trajectory; * @param [in] traj trajectory * @return calculated statistics */ -Stat calcDistanceToObstacle(const PredictedObjects & obstacles, const Trajectory & traj); +Accumulator calcDistanceToObstacle(const PredictedObjects & obstacles, const Trajectory & traj); /** * @brief calculate the time to collision of the trajectory with the given obstacles @@ -42,7 +43,7 @@ Stat calcDistanceToObstacle(const PredictedObjects & obstacles, const Tr * @param [in] traj trajectory * @return calculated statistics */ -Stat calcTimeToCollision( +Accumulator calcTimeToCollision( const PredictedObjects & obstacles, const Trajectory & traj, const double distance_threshold); } // namespace metrics diff --git a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/stability_metrics.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/stability_metrics.hpp index f1e93380d34d6..38f388feeadda 100644 --- a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/stability_metrics.hpp +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/stability_metrics.hpp @@ -15,7 +15,7 @@ #ifndef AUTOWARE__PLANNING_EVALUATOR__METRICS__STABILITY_METRICS_HPP_ #define AUTOWARE__PLANNING_EVALUATOR__METRICS__STABILITY_METRICS_HPP_ -#include "autoware/planning_evaluator/stat.hpp" +#include "autoware/universe_utils/math/accumulator.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" @@ -23,6 +23,7 @@ namespace planning_diagnostics { namespace metrics { +using autoware::universe_utils::Accumulator; using autoware_planning_msgs::msg::Trajectory; /** @@ -31,7 +32,7 @@ using autoware_planning_msgs::msg::Trajectory; * @param [in] traj2 second trajectory * @return calculated statistics */ -Stat calcFrechetDistance(const Trajectory & traj1, const Trajectory & traj2); +Accumulator calcFrechetDistance(const Trajectory & traj1, const Trajectory & traj2); /** * @brief calculate the lateral distance between two trajectories @@ -39,7 +40,7 @@ Stat calcFrechetDistance(const Trajectory & traj1, const Trajectory & tr * @param [in] traj2 second trajectory * @return calculated statistics */ -Stat calcLateralDistance(const Trajectory & traj1, const Trajectory & traj2); +Accumulator calcLateralDistance(const Trajectory & traj1, const Trajectory & traj2); } // namespace metrics } // namespace planning_diagnostics diff --git a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/trajectory_metrics.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/trajectory_metrics.hpp index 14a6a5c451619..f26e61cc3ea5d 100644 --- a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/trajectory_metrics.hpp +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/trajectory_metrics.hpp @@ -15,7 +15,7 @@ #ifndef AUTOWARE__PLANNING_EVALUATOR__METRICS__TRAJECTORY_METRICS_HPP_ #define AUTOWARE__PLANNING_EVALUATOR__METRICS__TRAJECTORY_METRICS_HPP_ -#include "autoware/planning_evaluator/stat.hpp" +#include "autoware/universe_utils/math/accumulator.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" #include "autoware_planning_msgs/msg/trajectory_point.hpp" @@ -24,6 +24,7 @@ namespace planning_diagnostics { namespace metrics { +using autoware::universe_utils::Accumulator; using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; @@ -33,56 +34,56 @@ using autoware_planning_msgs::msg::TrajectoryPoint; * @param [in] min_dist_threshold minimum distance between successive points * @return calculated statistics */ -Stat calcTrajectoryRelativeAngle(const Trajectory & traj, const double min_dist_threshold); +Accumulator 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); +Accumulator calcTrajectoryInterval(const Trajectory & traj); /** * @brief calculate curvature metric * @param [in] traj input trajectory * @return calculated statistics */ -Stat calcTrajectoryCurvature(const Trajectory & traj); +Accumulator calcTrajectoryCurvature(const Trajectory & traj); /** * @brief calculate length of the trajectory [m] * @param [in] traj input trajectory * @return calculated statistics */ -Stat calcTrajectoryLength(const Trajectory & traj); +Accumulator calcTrajectoryLength(const Trajectory & traj); /** * @brief calculate duration of the trajectory [s] * @param [in] traj input trajectory * @return calculated statistics */ -Stat calcTrajectoryDuration(const Trajectory & traj); +Accumulator calcTrajectoryDuration(const Trajectory & traj); /** * @brief calculate velocity metrics for the trajectory * @param [in] traj input trajectory * @return calculated statistics */ -Stat calcTrajectoryVelocity(const Trajectory & traj); +Accumulator calcTrajectoryVelocity(const Trajectory & traj); /** * @brief calculate acceleration metrics for the trajectory * @param [in] traj input trajectory * @return calculated statistics */ -Stat calcTrajectoryAcceleration(const Trajectory & traj); +Accumulator calcTrajectoryAcceleration(const Trajectory & traj); /** * @brief calculate jerk metrics for the trajectory * @param [in] traj input trajectory * @return calculated statistics */ -Stat calcTrajectoryJerk(const Trajectory & traj); +Accumulator calcTrajectoryJerk(const Trajectory & traj); } // namespace metrics } // namespace planning_diagnostics diff --git a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics_calculator.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics_calculator.hpp index f500a435edba1..97d23cad10af2 100644 --- a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics_calculator.hpp +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics_calculator.hpp @@ -16,7 +16,7 @@ #define AUTOWARE__PLANNING_EVALUATOR__METRICS_CALCULATOR_HPP_ #include "autoware/planning_evaluator/metrics/metric.hpp" #include "autoware/planning_evaluator/parameters.hpp" -#include "autoware/planning_evaluator/stat.hpp" +#include "autoware/universe_utils/math/accumulator.hpp" #include "autoware_perception_msgs/msg/predicted_objects.hpp" #include "autoware_planning_msgs/msg/pose_with_uuid_stamped.hpp" @@ -28,6 +28,7 @@ namespace planning_diagnostics { +using autoware::universe_utils::Accumulator; using autoware_perception_msgs::msg::PredictedObjects; using autoware_planning_msgs::msg::PoseWithUuidStamped; using autoware_planning_msgs::msg::Trajectory; @@ -47,8 +48,8 @@ class MetricsCalculator * @param [in] metric Metric enum value * @return string describing the requested metric */ - std::optional> calculate(const Metric metric, const Trajectory & traj) const; - std::optional> calculate( + std::optional> calculate(const Metric metric, const Trajectory & traj) const; + std::optional> calculate( const Metric metric, const Pose & base_pose, const Pose & target_pose) const; /** diff --git a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/motion_evaluator_node.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/motion_evaluator_node.hpp index ed9c975436ba5..8193472c4dc39 100644 --- a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/motion_evaluator_node.hpp +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/motion_evaluator_node.hpp @@ -16,7 +16,7 @@ #define AUTOWARE__PLANNING_EVALUATOR__MOTION_EVALUATOR_NODE_HPP_ #include "autoware/planning_evaluator/metrics_calculator.hpp" -#include "autoware/planning_evaluator/stat.hpp" +#include "autoware/universe_utils/math/accumulator.hpp" #include "rclcpp/rclcpp.hpp" #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" @@ -33,6 +33,7 @@ namespace planning_diagnostics { +using autoware::universe_utils::Accumulator; using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; diff --git a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp index 065d168b02d38..0f4b8f7289a4f 100644 --- a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp @@ -16,7 +16,7 @@ #define AUTOWARE__PLANNING_EVALUATOR__PLANNING_EVALUATOR_NODE_HPP_ #include "autoware/planning_evaluator/metrics_calculator.hpp" -#include "autoware/planning_evaluator/stat.hpp" +#include "autoware/universe_utils/math/accumulator.hpp" #include "rclcpp/rclcpp.hpp" #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" @@ -43,6 +43,7 @@ #include namespace planning_diagnostics { +using autoware::universe_utils::Accumulator; using autoware_perception_msgs::msg::PredictedObjects; using autoware_planning_msgs::msg::PoseWithUuidStamped; using autoware_planning_msgs::msg::Trajectory; @@ -98,7 +99,7 @@ class PlanningEvaluatorNode : public rclcpp::Node /** * @brief publish the given metric statistic */ - void AddMetricMsg(const Metric & metric, const Stat & metric_stat); + void AddMetricMsg(const Metric & metric, const Accumulator & metric_stat); /** * @brief publish current ego lane info @@ -162,7 +163,7 @@ class PlanningEvaluatorNode : public rclcpp::Node // Metrics std::vector metrics_; std::deque stamps_; - std::array>, static_cast(Metric::SIZE)> metric_stats_; + std::array>, static_cast(Metric::SIZE)> metric_stats_; rclcpp::TimerBase::SharedPtr timer_; std::optional prev_acc_stamped_{std::nullopt}; diff --git a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/stat.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/stat.hpp deleted file mode 100644 index 5f63f29a96f26..0000000000000 --- a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/stat.hpp +++ /dev/null @@ -1,93 +0,0 @@ -// 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 AUTOWARE__PLANNING_EVALUATOR__STAT_HPP_ -#define AUTOWARE__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 // AUTOWARE__PLANNING_EVALUATOR__STAT_HPP_ diff --git a/evaluator/autoware_planning_evaluator/src/metrics/deviation_metrics.cpp b/evaluator/autoware_planning_evaluator/src/metrics/deviation_metrics.cpp index 8ce8a009652d8..7e2217a49ef87 100644 --- a/evaluator/autoware_planning_evaluator/src/metrics/deviation_metrics.cpp +++ b/evaluator/autoware_planning_evaluator/src/metrics/deviation_metrics.cpp @@ -25,9 +25,9 @@ namespace metrics using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; -Stat calcLateralDeviation(const Trajectory & ref, const Trajectory & traj) +Accumulator calcLateralDeviation(const Trajectory & ref, const Trajectory & traj) { - Stat stat; + Accumulator stat; if (ref.points.empty() || traj.points.empty()) { return stat; @@ -45,9 +45,9 @@ Stat calcLateralDeviation(const Trajectory & ref, const Trajectory & tra return stat; } -Stat calcYawDeviation(const Trajectory & ref, const Trajectory & traj) +Accumulator calcYawDeviation(const Trajectory & ref, const Trajectory & traj) { - Stat stat; + Accumulator stat; if (ref.points.empty() || traj.points.empty()) { return stat; @@ -64,9 +64,9 @@ Stat calcYawDeviation(const Trajectory & ref, const Trajectory & traj) return stat; } -Stat calcVelocityDeviation(const Trajectory & ref, const Trajectory & traj) +Accumulator calcVelocityDeviation(const Trajectory & ref, const Trajectory & traj) { - Stat stat; + Accumulator stat; if (ref.points.empty() || traj.points.empty()) { return stat; @@ -81,23 +81,23 @@ Stat calcVelocityDeviation(const Trajectory & ref, const Trajectory & tr return stat; } -Stat calcLongitudinalDeviation(const Pose & base_pose, const Point & target_point) +Accumulator calcLongitudinalDeviation(const Pose & base_pose, const Point & target_point) { - Stat stat; + Accumulator stat; stat.add(std::abs(autoware::universe_utils::calcLongitudinalDeviation(base_pose, target_point))); return stat; } -Stat calcLateralDeviation(const Pose & base_pose, const Point & target_point) +Accumulator calcLateralDeviation(const Pose & base_pose, const Point & target_point) { - Stat stat; + Accumulator stat; stat.add(std::abs(autoware::universe_utils::calcLateralDeviation(base_pose, target_point))); return stat; } -Stat calcYawDeviation(const Pose & base_pose, const Pose & target_pose) +Accumulator calcYawDeviation(const Pose & base_pose, const Pose & target_pose) { - Stat stat; + Accumulator stat; stat.add(std::abs(autoware::universe_utils::calcYawDeviation(base_pose, target_pose))); return stat; } diff --git a/evaluator/autoware_planning_evaluator/src/metrics/obstacle_metrics.cpp b/evaluator/autoware_planning_evaluator/src/metrics/obstacle_metrics.cpp index 3cdaf3d7eaaae..083d1cbb67d3e 100644 --- a/evaluator/autoware_planning_evaluator/src/metrics/obstacle_metrics.cpp +++ b/evaluator/autoware_planning_evaluator/src/metrics/obstacle_metrics.cpp @@ -30,9 +30,9 @@ namespace metrics using autoware::universe_utils::calcDistance2d; using autoware_planning_msgs::msg::TrajectoryPoint; -Stat calcDistanceToObstacle(const PredictedObjects & obstacles, const Trajectory & traj) +Accumulator calcDistanceToObstacle(const PredictedObjects & obstacles, const Trajectory & traj) { - Stat stat; + Accumulator stat; for (const TrajectoryPoint & p : traj.points) { double min_dist = std::numeric_limits::max(); for (const auto & object : obstacles.objects) { @@ -45,10 +45,10 @@ Stat calcDistanceToObstacle(const PredictedObjects & obstacles, const Tr return stat; } -Stat calcTimeToCollision( +Accumulator calcTimeToCollision( const PredictedObjects & obstacles, const Trajectory & traj, const double distance_threshold) { - Stat stat; + Accumulator stat; /** TODO(Maxime CLEMENT): * this implementation assumes static obstacles and does not work for dynamic obstacles */ diff --git a/evaluator/autoware_planning_evaluator/src/metrics/stability_metrics.cpp b/evaluator/autoware_planning_evaluator/src/metrics/stability_metrics.cpp index 4d1c02faa406f..e6ede0d07b9b3 100644 --- a/evaluator/autoware_planning_evaluator/src/metrics/stability_metrics.cpp +++ b/evaluator/autoware_planning_evaluator/src/metrics/stability_metrics.cpp @@ -29,9 +29,9 @@ namespace metrics { using autoware_planning_msgs::msg::TrajectoryPoint; -Stat calcFrechetDistance(const Trajectory & traj1, const Trajectory & traj2) +Accumulator calcFrechetDistance(const Trajectory & traj1, const Trajectory & traj2) { - Stat stat; + Accumulator stat; if (traj1.points.empty() || traj2.points.empty()) { return stat; @@ -58,9 +58,9 @@ Stat calcFrechetDistance(const Trajectory & traj1, const Trajectory & tr return stat; } -Stat calcLateralDistance(const Trajectory & traj1, const Trajectory & traj2) +Accumulator calcLateralDistance(const Trajectory & traj1, const Trajectory & traj2) { - Stat stat; + Accumulator stat; if (traj1.points.empty()) { return stat; } diff --git a/evaluator/autoware_planning_evaluator/src/metrics/trajectory_metrics.cpp b/evaluator/autoware_planning_evaluator/src/metrics/trajectory_metrics.cpp index 4526865ced97d..4d553bd9ba2d0 100644 --- a/evaluator/autoware_planning_evaluator/src/metrics/trajectory_metrics.cpp +++ b/evaluator/autoware_planning_evaluator/src/metrics/trajectory_metrics.cpp @@ -23,18 +23,18 @@ namespace metrics using autoware::universe_utils::calcCurvature; using autoware::universe_utils::calcDistance2d; -Stat calcTrajectoryInterval(const Trajectory & traj) +Accumulator calcTrajectoryInterval(const Trajectory & traj) { - Stat stat; + Accumulator 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) +Accumulator calcTrajectoryRelativeAngle(const Trajectory & traj, const double min_dist_threshold) { - Stat stat; + Accumulator 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) { @@ -79,9 +79,9 @@ Stat calcTrajectoryRelativeAngle(const Trajectory & traj, const double m return stat; } -Stat calcTrajectoryCurvature(const Trajectory & traj) +Accumulator calcTrajectoryCurvature(const Trajectory & traj) { - Stat stat; + Accumulator stat; // We need at least three points to compute curvature if (traj.points.size() < 3) { return stat; @@ -111,18 +111,18 @@ Stat calcTrajectoryCurvature(const Trajectory & traj) return stat; } -Stat calcTrajectoryLength(const Trajectory & traj) +Accumulator 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; + Accumulator stat; stat.add(length); return stat; } -Stat calcTrajectoryDuration(const Trajectory & traj) +Accumulator calcTrajectoryDuration(const Trajectory & traj) { double duration = 0.0; for (size_t i = 0; i + 1 < traj.points.size(); ++i) { @@ -132,32 +132,32 @@ Stat calcTrajectoryDuration(const Trajectory & traj) duration += length / std::abs(velocity); } } - Stat stat; + Accumulator stat; stat.add(duration); return stat; } -Stat calcTrajectoryVelocity(const Trajectory & traj) +Accumulator calcTrajectoryVelocity(const Trajectory & traj) { - Stat stat; + Accumulator stat; for (TrajectoryPoint p : traj.points) { stat.add(p.longitudinal_velocity_mps); } return stat; } -Stat calcTrajectoryAcceleration(const Trajectory & traj) +Accumulator calcTrajectoryAcceleration(const Trajectory & traj) { - Stat stat; + Accumulator stat; for (TrajectoryPoint p : traj.points) { stat.add(p.acceleration_mps2); } return stat; } -Stat calcTrajectoryJerk(const Trajectory & traj) +Accumulator calcTrajectoryJerk(const Trajectory & traj) { - Stat stat; + Accumulator stat; for (size_t i = 0; i + 1 < traj.points.size(); ++i) { const double vel = traj.points.at(i).longitudinal_velocity_mps; if (vel != 0) { diff --git a/evaluator/autoware_planning_evaluator/src/metrics_calculator.cpp b/evaluator/autoware_planning_evaluator/src/metrics_calculator.cpp index 8658a666b4976..6e057bcc9fc3d 100644 --- a/evaluator/autoware_planning_evaluator/src/metrics_calculator.cpp +++ b/evaluator/autoware_planning_evaluator/src/metrics_calculator.cpp @@ -22,7 +22,7 @@ #include "autoware/universe_utils/geometry/geometry.hpp" namespace planning_diagnostics { -std::optional> MetricsCalculator::calculate( +std::optional> MetricsCalculator::calculate( const Metric metric, const Trajectory & traj) const { // Functions to calculate trajectory metrics @@ -74,7 +74,7 @@ std::optional> MetricsCalculator::calculate( } } -std::optional> MetricsCalculator::calculate( +std::optional> MetricsCalculator::calculate( const Metric metric, const Pose & base_pose, const Pose & target_pose) const { // Functions to calculate pose metrics diff --git a/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp b/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp index e5e1c947a735c..a78400b0c32cb 100644 --- a/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp +++ b/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp @@ -198,7 +198,7 @@ void PlanningEvaluatorNode::AddKinematicStateMetricMsg( return; } -void PlanningEvaluatorNode::AddMetricMsg(const Metric & metric, const Stat & metric_stat) +void PlanningEvaluatorNode::AddMetricMsg(const Metric & metric, const Accumulator & metric_stat) { const std::string base_name = metric_to_str.at(metric) + "/"; MetricMsg metric_msg; diff --git a/evaluator/kinematic_evaluator/include/kinematic_evaluator/kinematic_evaluator_node.hpp b/evaluator/kinematic_evaluator/include/kinematic_evaluator/kinematic_evaluator_node.hpp index 16b4230fc4656..ebf7ddb3794f1 100644 --- a/evaluator/kinematic_evaluator/include/kinematic_evaluator/kinematic_evaluator_node.hpp +++ b/evaluator/kinematic_evaluator/include/kinematic_evaluator/kinematic_evaluator_node.hpp @@ -16,7 +16,7 @@ #define KINEMATIC_EVALUATOR__KINEMATIC_EVALUATOR_NODE_HPP_ #include "kinematic_evaluator/metrics_calculator.hpp" -#include "kinematic_evaluator/stat.hpp" +#include "autoware/universe_utils/math/accumulator.hpp" #include "rclcpp/rclcpp.hpp" #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" @@ -33,6 +33,7 @@ namespace kinematic_diagnostics { +using autoware::universe_utils::Accumulator; using diagnostic_msgs::msg::DiagnosticArray; using diagnostic_msgs::msg::DiagnosticStatus; @@ -55,7 +56,7 @@ class KinematicEvaluatorNode : public rclcpp::Node * @brief publish the given metric statistic */ DiagnosticStatus generateDiagnosticStatus( - const Metric & metric, const Stat & metric_stat) const; + const Metric & metric, const Accumulator & metric_stat) const; private: geometry_msgs::msg::Pose getCurrentEgoPose() const; @@ -74,8 +75,8 @@ class KinematicEvaluatorNode : public rclcpp::Node // Metrics std::vector metrics_; std::deque stamps_; - std::array>, static_cast(Metric::SIZE)> metric_stats_; - std::unordered_map> metrics_dict_; + std::array>, static_cast(Metric::SIZE)> metric_stats_; + std::unordered_map> metrics_dict_; }; } // namespace kinematic_diagnostics diff --git a/evaluator/kinematic_evaluator/include/kinematic_evaluator/metrics/kinematic_metrics.hpp b/evaluator/kinematic_evaluator/include/kinematic_evaluator/metrics/kinematic_metrics.hpp index d20bf079a363a..e7d18910e7c39 100644 --- a/evaluator/kinematic_evaluator/include/kinematic_evaluator/metrics/kinematic_metrics.hpp +++ b/evaluator/kinematic_evaluator/include/kinematic_evaluator/metrics/kinematic_metrics.hpp @@ -15,7 +15,7 @@ #ifndef KINEMATIC_EVALUATOR__METRICS__KINEMATIC_METRICS_HPP_ #define KINEMATIC_EVALUATOR__METRICS__KINEMATIC_METRICS_HPP_ -#include "kinematic_evaluator/stat.hpp" +#include "autoware/universe_utils/math/accumulator.hpp" #include @@ -23,6 +23,7 @@ namespace kinematic_diagnostics { namespace metrics { +using autoware::universe_utils::Accumulator; using nav_msgs::msg::Odometry; /** @@ -31,7 +32,7 @@ using nav_msgs::msg::Odometry; * @param [in] stat_prev input trajectory * @return calculated statistics */ -Stat updateVelocityStats(const double & value, const Stat stat_prev); +Accumulator updateVelocityStats(const double & value, const Accumulator stat_prev); } // namespace metrics } // namespace kinematic_diagnostics diff --git a/evaluator/kinematic_evaluator/include/kinematic_evaluator/metrics_calculator.hpp b/evaluator/kinematic_evaluator/include/kinematic_evaluator/metrics_calculator.hpp index 67f152e30c9eb..7132363fae7de 100644 --- a/evaluator/kinematic_evaluator/include/kinematic_evaluator/metrics_calculator.hpp +++ b/evaluator/kinematic_evaluator/include/kinematic_evaluator/metrics_calculator.hpp @@ -17,13 +17,14 @@ #include "kinematic_evaluator/metrics/metric.hpp" #include "kinematic_evaluator/parameters.hpp" -#include "kinematic_evaluator/stat.hpp" +#include "autoware/universe_utils/math/accumulator.hpp" #include "geometry_msgs/msg/pose.hpp" #include namespace kinematic_diagnostics { +using autoware::universe_utils::Accumulator; using nav_msgs::msg::Odometry; class MetricsCalculator @@ -39,7 +40,7 @@ class MetricsCalculator * @param [in] odom Odometry * @return string describing the requested metric */ - Stat calculate(const Metric metric, const Odometry & odom) const; + Accumulator calculate(const Metric metric, const Odometry & odom) const; /** * @brief update Metrics @@ -47,8 +48,8 @@ class MetricsCalculator * @param [in] odom Odometry * @return string describing the requested metric */ - Stat updateStat( - const Metric metric, const Odometry & odom, const Stat stat_prev) const; + Accumulator updateStat( + const Metric metric, const Odometry & odom, const Accumulator stat_prev) const; }; // class MetricsCalculator } // namespace kinematic_diagnostics diff --git a/evaluator/kinematic_evaluator/include/kinematic_evaluator/stat.hpp b/evaluator/kinematic_evaluator/include/kinematic_evaluator/stat.hpp deleted file mode 100644 index 096cb90b3dcf4..0000000000000 --- a/evaluator/kinematic_evaluator/include/kinematic_evaluator/stat.hpp +++ /dev/null @@ -1,93 +0,0 @@ -// 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 KINEMATIC_EVALUATOR__STAT_HPP_ -#define KINEMATIC_EVALUATOR__STAT_HPP_ - -namespace kinematic_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 kinematic_diagnostics - -#endif // KINEMATIC_EVALUATOR__STAT_HPP_ diff --git a/evaluator/kinematic_evaluator/src/kinematic_evaluator_node.cpp b/evaluator/kinematic_evaluator/src/kinematic_evaluator_node.cpp index 57631e5f861df..d34f9deb7f1ba 100644 --- a/evaluator/kinematic_evaluator/src/kinematic_evaluator_node.cpp +++ b/evaluator/kinematic_evaluator/src/kinematic_evaluator_node.cpp @@ -46,7 +46,7 @@ KinematicEvaluatorNode::KinematicEvaluatorNode(const rclcpp::NodeOptions & node_ for (const std::string & selected_metric : declare_parameter>("selected_metrics")) { Metric metric = str_to_metric.at(selected_metric); - metrics_dict_[metric] = Stat(); + metrics_dict_[metric] = Accumulator(); metrics_.push_back(metric); } } @@ -83,7 +83,7 @@ KinematicEvaluatorNode::~KinematicEvaluatorNode() } DiagnosticStatus KinematicEvaluatorNode::generateDiagnosticStatus( - const Metric & metric, const Stat & metric_stat) const + const Metric & metric, const Accumulator & metric_stat) const { DiagnosticStatus status; status.level = status.OK; diff --git a/evaluator/kinematic_evaluator/src/metrics/kinematic_metrics.cpp b/evaluator/kinematic_evaluator/src/metrics/kinematic_metrics.cpp index 2bb0dad86bf71..d410863a05010 100644 --- a/evaluator/kinematic_evaluator/src/metrics/kinematic_metrics.cpp +++ b/evaluator/kinematic_evaluator/src/metrics/kinematic_metrics.cpp @@ -19,9 +19,9 @@ namespace kinematic_diagnostics namespace metrics { -Stat updateVelocityStats(const double & value, const Stat stat_prev) +Accumulator updateVelocityStats(const double & value, const Accumulator stat_prev) { - Stat stat(stat_prev); + Accumulator stat(stat_prev); stat.add(value); return stat; } diff --git a/evaluator/kinematic_evaluator/src/metrics_calculator.cpp b/evaluator/kinematic_evaluator/src/metrics_calculator.cpp index af1e1114b9c43..8b0f581817b38 100644 --- a/evaluator/kinematic_evaluator/src/metrics_calculator.cpp +++ b/evaluator/kinematic_evaluator/src/metrics_calculator.cpp @@ -18,8 +18,8 @@ namespace kinematic_diagnostics { -Stat MetricsCalculator::updateStat( - const Metric metric, const Odometry & odom, const Stat stat_prev) const +Accumulator MetricsCalculator::updateStat( + const Metric metric, const Odometry & odom, const Accumulator stat_prev) const { switch (metric) { case Metric::velocity_stats: diff --git a/evaluator/localization_evaluator/include/localization_evaluator/localization_evaluator_node.hpp b/evaluator/localization_evaluator/include/localization_evaluator/localization_evaluator_node.hpp index 8c93ddd5fa0fc..c866b849e0598 100644 --- a/evaluator/localization_evaluator/include/localization_evaluator/localization_evaluator_node.hpp +++ b/evaluator/localization_evaluator/include/localization_evaluator/localization_evaluator_node.hpp @@ -16,7 +16,7 @@ #define LOCALIZATION_EVALUATOR__LOCALIZATION_EVALUATOR_NODE_HPP_ #include "localization_evaluator/metrics_calculator.hpp" -#include "localization_evaluator/stat.hpp" +#include "autoware/universe_utils/math/accumulator.hpp" #include "rclcpp/rclcpp.hpp" #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" @@ -38,6 +38,7 @@ namespace localization_diagnostics { +using autoware::universe_utils::Accumulator; using diagnostic_msgs::msg::DiagnosticArray; using diagnostic_msgs::msg::DiagnosticStatus; using geometry_msgs::msg::PoseWithCovarianceStamped; @@ -70,7 +71,7 @@ class LocalizationEvaluatorNode : public rclcpp::Node * @brief publish the given metric statistic */ DiagnosticStatus generateDiagnosticStatus( - const Metric & metric, const Stat & metric_stat) const; + const Metric & metric, const Accumulator & metric_stat) const; private: // ROS @@ -93,8 +94,8 @@ class LocalizationEvaluatorNode : public rclcpp::Node // Metrics std::vector metrics_; std::deque stamps_; - std::array>, static_cast(Metric::SIZE)> metric_stats_; - std::unordered_map> metrics_dict_; + std::array>, static_cast(Metric::SIZE)> metric_stats_; + std::unordered_map> metrics_dict_; }; } // namespace localization_diagnostics diff --git a/evaluator/localization_evaluator/include/localization_evaluator/metrics/localization_metrics.hpp b/evaluator/localization_evaluator/include/localization_evaluator/metrics/localization_metrics.hpp index c973d285fd503..82119efca6082 100644 --- a/evaluator/localization_evaluator/include/localization_evaluator/metrics/localization_metrics.hpp +++ b/evaluator/localization_evaluator/include/localization_evaluator/metrics/localization_metrics.hpp @@ -15,12 +15,13 @@ #ifndef LOCALIZATION_EVALUATOR__METRICS__LOCALIZATION_METRICS_HPP_ #define LOCALIZATION_EVALUATOR__METRICS__LOCALIZATION_METRICS_HPP_ -#include "localization_evaluator/stat.hpp" +#include "autoware/universe_utils/math/accumulator.hpp" #include namespace localization_diagnostics { +using autoware::universe_utils::Accumulator; namespace metrics { /** @@ -30,8 +31,8 @@ namespace metrics * @param [in] lateral_ref reference lateral position * @return calculated statistics */ -Stat updateLateralStats( - const Stat stat_prev, const double & lateral_pos, const double & lateral_ref); +Accumulator updateLateralStats( + const Accumulator stat_prev, const double & lateral_pos, const double & lateral_ref); /** * @brief calculate absolute localization error @@ -40,8 +41,8 @@ Stat updateLateralStats( * @param [in] pos_ref reference position of the vehicle * @return calculated statistics */ -Stat updateAbsoluteStats( - const Stat stat_prev, const geometry_msgs::msg::Point & pos, +Accumulator updateAbsoluteStats( + const Accumulator stat_prev, const geometry_msgs::msg::Point & pos, const geometry_msgs::msg::Point & pos_ref); } // namespace metrics diff --git a/evaluator/localization_evaluator/include/localization_evaluator/metrics_calculator.hpp b/evaluator/localization_evaluator/include/localization_evaluator/metrics_calculator.hpp index 88f8ea85d4267..1eebb94a083af 100644 --- a/evaluator/localization_evaluator/include/localization_evaluator/metrics_calculator.hpp +++ b/evaluator/localization_evaluator/include/localization_evaluator/metrics_calculator.hpp @@ -17,13 +17,14 @@ #include "localization_evaluator/metrics/metric.hpp" #include "localization_evaluator/parameters.hpp" -#include "localization_evaluator/stat.hpp" +#include "autoware/universe_utils/math/accumulator.hpp" #include "geometry_msgs/msg/pose.hpp" #include namespace localization_diagnostics { +using autoware::universe_utils::Accumulator; class MetricsCalculator { public: @@ -38,8 +39,8 @@ class MetricsCalculator * @param [in] pos_ref reference position * @return string describing the requested metric */ - Stat updateStat( - const Stat stat_prev, const Metric metric, const geometry_msgs::msg::Point & pos, + Accumulator updateStat( + const Accumulator stat_prev, const Metric metric, const geometry_msgs::msg::Point & pos, const geometry_msgs::msg::Point & pos_ref) const; }; // class MetricsCalculator diff --git a/evaluator/localization_evaluator/include/localization_evaluator/stat.hpp b/evaluator/localization_evaluator/include/localization_evaluator/stat.hpp deleted file mode 100644 index fbea5b61813fb..0000000000000 --- a/evaluator/localization_evaluator/include/localization_evaluator/stat.hpp +++ /dev/null @@ -1,93 +0,0 @@ -// 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 LOCALIZATION_EVALUATOR__STAT_HPP_ -#define LOCALIZATION_EVALUATOR__STAT_HPP_ - -namespace localization_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 localization_diagnostics - -#endif // LOCALIZATION_EVALUATOR__STAT_HPP_ diff --git a/evaluator/localization_evaluator/src/localization_evaluator_node.cpp b/evaluator/localization_evaluator/src/localization_evaluator_node.cpp index db0c04a698725..8a9352d2a73e8 100644 --- a/evaluator/localization_evaluator/src/localization_evaluator_node.cpp +++ b/evaluator/localization_evaluator/src/localization_evaluator_node.cpp @@ -49,7 +49,7 @@ LocalizationEvaluatorNode::LocalizationEvaluatorNode(const rclcpp::NodeOptions & for (const std::string & selected_metric : declare_parameter>("selected_metrics")) { Metric metric = str_to_metric.at(selected_metric); - metrics_dict_[metric] = Stat(); + metrics_dict_[metric] = Accumulator(); metrics_.push_back(metric); } } @@ -86,7 +86,7 @@ LocalizationEvaluatorNode::~LocalizationEvaluatorNode() } DiagnosticStatus LocalizationEvaluatorNode::generateDiagnosticStatus( - const Metric & metric, const Stat & metric_stat) const + const Metric & metric, const Accumulator & metric_stat) const { DiagnosticStatus status; status.level = status.OK; diff --git a/evaluator/localization_evaluator/src/metrics/localization_metrics.cpp b/evaluator/localization_evaluator/src/metrics/localization_metrics.cpp index fab3377913bd4..97fd8e326cca7 100644 --- a/evaluator/localization_evaluator/src/metrics/localization_metrics.cpp +++ b/evaluator/localization_evaluator/src/metrics/localization_metrics.cpp @@ -21,19 +21,19 @@ namespace localization_diagnostics namespace metrics { -Stat updateLateralStats( - const Stat stat_prev, const double & lateral_pos, const double & lateral_ref) +Accumulator updateLateralStats( + const Accumulator stat_prev, const double & lateral_pos, const double & lateral_ref) { - Stat stat(stat_prev); + Accumulator stat(stat_prev); stat.add(std::abs(lateral_ref - lateral_pos)); return stat; } -Stat updateAbsoluteStats( - const Stat stat_prev, const geometry_msgs::msg::Point & pos, +Accumulator updateAbsoluteStats( + const Accumulator stat_prev, const geometry_msgs::msg::Point & pos, const geometry_msgs::msg::Point & pos_ref) { - Stat stat(stat_prev); + Accumulator stat(stat_prev); double dist = std::sqrt( (pos_ref.x - pos.x) * (pos_ref.x - pos.x) + (pos_ref.y - pos.y) * (pos_ref.y - pos.y) + (pos_ref.z - pos.z) * (pos_ref.z - pos.z)); diff --git a/evaluator/localization_evaluator/src/metrics_calculator.cpp b/evaluator/localization_evaluator/src/metrics_calculator.cpp index 72184937e846b..864fb4a5ddbd2 100644 --- a/evaluator/localization_evaluator/src/metrics_calculator.cpp +++ b/evaluator/localization_evaluator/src/metrics_calculator.cpp @@ -18,8 +18,8 @@ namespace localization_diagnostics { -Stat MetricsCalculator::updateStat( - const Stat stat_prev, const Metric metric, const geometry_msgs::msg::Point & pos, +Accumulator MetricsCalculator::updateStat( + const Accumulator stat_prev, const Metric metric, const geometry_msgs::msg::Point & pos, const geometry_msgs::msg::Point & pos_ref) const { if ( diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/detection_count.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/detection_count.hpp index 010f1497da3da..c111a725a2ea0 100644 --- a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/detection_count.hpp +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/detection_count.hpp @@ -16,7 +16,6 @@ #define PERCEPTION_ONLINE_EVALUATOR__METRICS__DETECTION_COUNT_HPP_ #include "perception_online_evaluator/parameters.hpp" -#include "perception_online_evaluator/stat.hpp" #include "tf2_ros/buffer.h" #include diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/deviation_metrics.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/deviation_metrics.hpp index d67413c174c41..0f3379b3f055e 100644 --- a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/deviation_metrics.hpp +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/deviation_metrics.hpp @@ -15,8 +15,6 @@ #ifndef PERCEPTION_ONLINE_EVALUATOR__METRICS__DEVIATION_METRICS_HPP_ #define PERCEPTION_ONLINE_EVALUATOR__METRICS__DEVIATION_METRICS_HPP_ -#include "perception_online_evaluator/stat.hpp" - #include #include diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/metric.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/metric.hpp index eaa07f2317940..4caf932919e61 100644 --- a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/metric.hpp +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/metric.hpp @@ -15,7 +15,7 @@ #ifndef PERCEPTION_ONLINE_EVALUATOR__METRICS__METRIC_HPP_ #define PERCEPTION_ONLINE_EVALUATOR__METRICS__METRIC_HPP_ -#include "perception_online_evaluator/stat.hpp" +#include "autoware/universe_utils/math/accumulator.hpp" #include #include @@ -39,14 +39,15 @@ enum class Metric { // Each metric has a different return type. (statistic or just a one value etc). // To handle them all in the MetricsCalculator::calculate function, define MetricsMap as a variant -using MetricStatMap = std::unordered_map>; +using autoware::universe_utils::Accumulator; +using MetricStatMap = std::unordered_map>; using MetricValueMap = std::unordered_map; using MetricsMap = std::variant; struct PredictedPathDeviationMetrics { - Stat mean; - Stat variance; + Accumulator mean; + Accumulator variance; }; static const std::unordered_map str_to_metric = { diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp index a9b1388281ce8..c17051d2a30a7 100644 --- a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp @@ -19,7 +19,6 @@ #include "perception_online_evaluator/metrics/deviation_metrics.hpp" #include "perception_online_evaluator/metrics/metric.hpp" #include "perception_online_evaluator/parameters.hpp" -#include "perception_online_evaluator/stat.hpp" #include "perception_online_evaluator/utils/objects_filtering.hpp" #include "tf2_ros/buffer.h" diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/perception_online_evaluator_node.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/perception_online_evaluator_node.hpp index 1bc427e667a2a..bddd8c38e3303 100644 --- a/evaluator/perception_online_evaluator/include/perception_online_evaluator/perception_online_evaluator_node.hpp +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/perception_online_evaluator_node.hpp @@ -17,11 +17,11 @@ #include "perception_online_evaluator/metrics_calculator.hpp" #include "perception_online_evaluator/parameters.hpp" -#include "perception_online_evaluator/stat.hpp" #include "rclcpp/rclcpp.hpp" #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" +#include "autoware/universe_utils/math/accumulator.hpp" #include "autoware_perception_msgs/msg/predicted_objects.hpp" #include "diagnostic_msgs/msg/diagnostic_array.hpp" #include "nav_msgs/msg/odometry.hpp" @@ -37,6 +37,7 @@ namespace perception_diagnostics { using autoware_perception_msgs::msg::ObjectClassification; using autoware_perception_msgs::msg::PredictedObjects; +using autoware::universe_utils::Accumulator; using diagnostic_msgs::msg::DiagnosticArray; using diagnostic_msgs::msg::DiagnosticStatus; using nav_msgs::msg::Odometry; @@ -60,7 +61,7 @@ class PerceptionOnlineEvaluatorNode : public rclcpp::Node void onObjects(const PredictedObjects::ConstSharedPtr objects_msg); DiagnosticStatus generateDiagnosticStatus( - const std::string metric, const Stat & metric_stat) const; + const std::string metric, const Accumulator & metric_stat) const; DiagnosticStatus generateDiagnosticStatus( const std::string & metric, const double metric_value) const; diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/stat.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/stat.hpp deleted file mode 100644 index 63494f90d02ee..0000000000000 --- a/evaluator/perception_online_evaluator/include/perception_online_evaluator/stat.hpp +++ /dev/null @@ -1,93 +0,0 @@ -// Copyright 2024 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 PERCEPTION_ONLINE_EVALUATOR__STAT_HPP_ -#define PERCEPTION_ONLINE_EVALUATOR__STAT_HPP_ - -namespace perception_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 perception_diagnostics - -#endif // PERCEPTION_ONLINE_EVALUATOR__STAT_HPP_ diff --git a/evaluator/perception_online_evaluator/src/metrics_calculator.cpp b/evaluator/perception_online_evaluator/src/metrics_calculator.cpp index fea4316fa6820..0ced0b9d6c8a8 100644 --- a/evaluator/perception_online_evaluator/src/metrics_calculator.cpp +++ b/evaluator/perception_online_evaluator/src/metrics_calculator.cpp @@ -235,7 +235,7 @@ MetricStatMap MetricsCalculator::calcLateralDeviationMetrics( !parameters_->object_parameters.at(label).check_lateral_deviation) { continue; } - Stat stat{}; + Accumulator stat{}; const auto stamp = rclcpp::Time(objects.header.stamp); for (const auto & object : objects.objects) { const auto uuid = autoware::universe_utils::toHexString(object.object_id); @@ -264,7 +264,7 @@ MetricStatMap MetricsCalculator::calcYawDeviationMetrics( !parameters_->object_parameters.at(label).check_yaw_deviation) { continue; } - Stat stat{}; + Accumulator stat{}; const auto stamp = rclcpp::Time(objects.header.stamp); for (const auto & object : objects.objects) { const auto uuid = autoware::universe_utils::toHexString(object.object_id); @@ -420,7 +420,7 @@ MetricStatMap MetricsCalculator::calcYawRateMetrics(const ClassObjectsMap & clas MetricStatMap metric_stat_map{}; for (const auto & [label, objects] : class_objects_map) { - Stat stat{}; + Accumulator stat{}; const auto stamp = rclcpp::Time(objects.header.stamp); for (const auto & object : objects.objects) { diff --git a/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp b/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp index a577fd359563c..abbdbd382498a 100644 --- a/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp +++ b/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp @@ -99,7 +99,7 @@ void PerceptionOnlineEvaluatorNode::publishMetrics() } DiagnosticStatus PerceptionOnlineEvaluatorNode::generateDiagnosticStatus( - const std::string metric, const Stat & metric_stat) const + const std::string metric, const Accumulator & metric_stat) const { DiagnosticStatus status; From 833ebf24d263c8096f9fbdc6f9c499c07295ce37 Mon Sep 17 00:00:00 2001 From: xtk8532704 <1041084556@qq.com> Date: Mon, 25 Nov 2024 19:37:47 +0900 Subject: [PATCH 3/9] pre-commit Signed-off-by: xtk8532704 <1041084556@qq.com> --- .../autoware/planning_evaluator/metrics/obstacle_metrics.hpp | 3 ++- .../planning_evaluator/metrics/trajectory_metrics.hpp | 3 ++- .../src/metrics/obstacle_metrics.cpp | 3 ++- .../src/metrics/trajectory_metrics.cpp | 3 ++- .../src/planning_evaluator_node.cpp | 3 ++- .../include/kinematic_evaluator/kinematic_evaluator_node.hpp | 2 +- .../include/kinematic_evaluator/metrics_calculator.hpp | 2 +- .../localization_evaluator/localization_evaluator_node.hpp | 2 +- .../include/localization_evaluator/metrics_calculator.hpp | 2 +- .../perception_online_evaluator_node.hpp | 4 ++-- 10 files changed, 16 insertions(+), 11 deletions(-) diff --git a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/obstacle_metrics.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/obstacle_metrics.hpp index 79688f6ca9078..55e46bf710450 100644 --- a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/obstacle_metrics.hpp +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/obstacle_metrics.hpp @@ -34,7 +34,8 @@ using autoware_planning_msgs::msg::Trajectory; * @param [in] traj trajectory * @return calculated statistics */ -Accumulator calcDistanceToObstacle(const PredictedObjects & obstacles, const Trajectory & traj); +Accumulator calcDistanceToObstacle( + const PredictedObjects & obstacles, const Trajectory & traj); /** * @brief calculate the time to collision of the trajectory with the given obstacles diff --git a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/trajectory_metrics.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/trajectory_metrics.hpp index f26e61cc3ea5d..c530a8d6d8b05 100644 --- a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/trajectory_metrics.hpp +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/trajectory_metrics.hpp @@ -34,7 +34,8 @@ using autoware_planning_msgs::msg::TrajectoryPoint; * @param [in] min_dist_threshold minimum distance between successive points * @return calculated statistics */ -Accumulator calcTrajectoryRelativeAngle(const Trajectory & traj, const double min_dist_threshold); +Accumulator calcTrajectoryRelativeAngle( + const Trajectory & traj, const double min_dist_threshold); /** * @brief calculate metric for the distance between trajectory points diff --git a/evaluator/autoware_planning_evaluator/src/metrics/obstacle_metrics.cpp b/evaluator/autoware_planning_evaluator/src/metrics/obstacle_metrics.cpp index 083d1cbb67d3e..0afc8d982a7e9 100644 --- a/evaluator/autoware_planning_evaluator/src/metrics/obstacle_metrics.cpp +++ b/evaluator/autoware_planning_evaluator/src/metrics/obstacle_metrics.cpp @@ -30,7 +30,8 @@ namespace metrics using autoware::universe_utils::calcDistance2d; using autoware_planning_msgs::msg::TrajectoryPoint; -Accumulator calcDistanceToObstacle(const PredictedObjects & obstacles, const Trajectory & traj) +Accumulator calcDistanceToObstacle( + const PredictedObjects & obstacles, const Trajectory & traj) { Accumulator stat; for (const TrajectoryPoint & p : traj.points) { diff --git a/evaluator/autoware_planning_evaluator/src/metrics/trajectory_metrics.cpp b/evaluator/autoware_planning_evaluator/src/metrics/trajectory_metrics.cpp index 4d553bd9ba2d0..bd5eed06f96f8 100644 --- a/evaluator/autoware_planning_evaluator/src/metrics/trajectory_metrics.cpp +++ b/evaluator/autoware_planning_evaluator/src/metrics/trajectory_metrics.cpp @@ -32,7 +32,8 @@ Accumulator calcTrajectoryInterval(const Trajectory & traj) return stat; } -Accumulator calcTrajectoryRelativeAngle(const Trajectory & traj, const double min_dist_threshold) +Accumulator calcTrajectoryRelativeAngle( + const Trajectory & traj, const double min_dist_threshold) { Accumulator stat; // We need at least three points to compute relative angle diff --git a/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp b/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp index a78400b0c32cb..d770833bc40a8 100644 --- a/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp +++ b/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp @@ -198,7 +198,8 @@ void PlanningEvaluatorNode::AddKinematicStateMetricMsg( return; } -void PlanningEvaluatorNode::AddMetricMsg(const Metric & metric, const Accumulator & metric_stat) +void PlanningEvaluatorNode::AddMetricMsg( + const Metric & metric, const Accumulator & metric_stat) { const std::string base_name = metric_to_str.at(metric) + "/"; MetricMsg metric_msg; diff --git a/evaluator/kinematic_evaluator/include/kinematic_evaluator/kinematic_evaluator_node.hpp b/evaluator/kinematic_evaluator/include/kinematic_evaluator/kinematic_evaluator_node.hpp index ebf7ddb3794f1..d401f63649468 100644 --- a/evaluator/kinematic_evaluator/include/kinematic_evaluator/kinematic_evaluator_node.hpp +++ b/evaluator/kinematic_evaluator/include/kinematic_evaluator/kinematic_evaluator_node.hpp @@ -15,8 +15,8 @@ #ifndef KINEMATIC_EVALUATOR__KINEMATIC_EVALUATOR_NODE_HPP_ #define KINEMATIC_EVALUATOR__KINEMATIC_EVALUATOR_NODE_HPP_ -#include "kinematic_evaluator/metrics_calculator.hpp" #include "autoware/universe_utils/math/accumulator.hpp" +#include "kinematic_evaluator/metrics_calculator.hpp" #include "rclcpp/rclcpp.hpp" #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" diff --git a/evaluator/kinematic_evaluator/include/kinematic_evaluator/metrics_calculator.hpp b/evaluator/kinematic_evaluator/include/kinematic_evaluator/metrics_calculator.hpp index 7132363fae7de..372d4a34af62c 100644 --- a/evaluator/kinematic_evaluator/include/kinematic_evaluator/metrics_calculator.hpp +++ b/evaluator/kinematic_evaluator/include/kinematic_evaluator/metrics_calculator.hpp @@ -15,9 +15,9 @@ #ifndef KINEMATIC_EVALUATOR__METRICS_CALCULATOR_HPP_ #define KINEMATIC_EVALUATOR__METRICS_CALCULATOR_HPP_ +#include "autoware/universe_utils/math/accumulator.hpp" #include "kinematic_evaluator/metrics/metric.hpp" #include "kinematic_evaluator/parameters.hpp" -#include "autoware/universe_utils/math/accumulator.hpp" #include "geometry_msgs/msg/pose.hpp" #include diff --git a/evaluator/localization_evaluator/include/localization_evaluator/localization_evaluator_node.hpp b/evaluator/localization_evaluator/include/localization_evaluator/localization_evaluator_node.hpp index c866b849e0598..feb61dd3cacbe 100644 --- a/evaluator/localization_evaluator/include/localization_evaluator/localization_evaluator_node.hpp +++ b/evaluator/localization_evaluator/include/localization_evaluator/localization_evaluator_node.hpp @@ -15,8 +15,8 @@ #ifndef LOCALIZATION_EVALUATOR__LOCALIZATION_EVALUATOR_NODE_HPP_ #define LOCALIZATION_EVALUATOR__LOCALIZATION_EVALUATOR_NODE_HPP_ -#include "localization_evaluator/metrics_calculator.hpp" #include "autoware/universe_utils/math/accumulator.hpp" +#include "localization_evaluator/metrics_calculator.hpp" #include "rclcpp/rclcpp.hpp" #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" diff --git a/evaluator/localization_evaluator/include/localization_evaluator/metrics_calculator.hpp b/evaluator/localization_evaluator/include/localization_evaluator/metrics_calculator.hpp index 1eebb94a083af..310d1b25e92f1 100644 --- a/evaluator/localization_evaluator/include/localization_evaluator/metrics_calculator.hpp +++ b/evaluator/localization_evaluator/include/localization_evaluator/metrics_calculator.hpp @@ -15,9 +15,9 @@ #ifndef LOCALIZATION_EVALUATOR__METRICS_CALCULATOR_HPP_ #define LOCALIZATION_EVALUATOR__METRICS_CALCULATOR_HPP_ +#include "autoware/universe_utils/math/accumulator.hpp" #include "localization_evaluator/metrics/metric.hpp" #include "localization_evaluator/parameters.hpp" -#include "autoware/universe_utils/math/accumulator.hpp" #include "geometry_msgs/msg/pose.hpp" #include diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/perception_online_evaluator_node.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/perception_online_evaluator_node.hpp index bddd8c38e3303..ea560a48f928b 100644 --- a/evaluator/perception_online_evaluator/include/perception_online_evaluator/perception_online_evaluator_node.hpp +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/perception_online_evaluator_node.hpp @@ -15,13 +15,13 @@ #ifndef PERCEPTION_ONLINE_EVALUATOR__PERCEPTION_ONLINE_EVALUATOR_NODE_HPP_ #define PERCEPTION_ONLINE_EVALUATOR__PERCEPTION_ONLINE_EVALUATOR_NODE_HPP_ +#include "autoware/universe_utils/math/accumulator.hpp" #include "perception_online_evaluator/metrics_calculator.hpp" #include "perception_online_evaluator/parameters.hpp" #include "rclcpp/rclcpp.hpp" #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" -#include "autoware/universe_utils/math/accumulator.hpp" #include "autoware_perception_msgs/msg/predicted_objects.hpp" #include "diagnostic_msgs/msg/diagnostic_array.hpp" #include "nav_msgs/msg/odometry.hpp" @@ -35,9 +35,9 @@ namespace perception_diagnostics { +using autoware::universe_utils::Accumulator; using autoware_perception_msgs::msg::ObjectClassification; using autoware_perception_msgs::msg::PredictedObjects; -using autoware::universe_utils::Accumulator; using diagnostic_msgs::msg::DiagnosticArray; using diagnostic_msgs::msg::DiagnosticStatus; using nav_msgs::msg::Odometry; From 2fdee87b4f4dbae6061822d7f2e9235f8ff57a72 Mon Sep 17 00:00:00 2001 From: xtk8532704 <1041084556@qq.com> Date: Tue, 26 Nov 2024 16:18:22 +0900 Subject: [PATCH 4/9] found and fixed a bug. add more tests. Signed-off-by: xtk8532704 <1041084556@qq.com> --- .../universe_utils/math/accumulator.hpp | 2 +- .../test/src/math/test_accumulator.cpp | 51 ++++++++++++++++--- 2 files changed, 45 insertions(+), 8 deletions(-) diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/math/accumulator.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/math/accumulator.hpp index cbb565b23ea98..10127d01c0a3a 100644 --- a/common/autoware_universe_utils/include/autoware/universe_utils/math/accumulator.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/math/accumulator.hpp @@ -69,7 +69,7 @@ class Accumulator private: T min_ = std::numeric_limits::max(); - T max_ = std::numeric_limits::min(); + T max_ = std::numeric_limits::lowest(); long double mean_ = 0.0; unsigned int count_ = 0; }; diff --git a/common/autoware_universe_utils/test/src/math/test_accumulator.cpp b/common/autoware_universe_utils/test/src/math/test_accumulator.cpp index 3e0963d9c8ad6..e150e6c762c6c 100644 --- a/common/autoware_universe_utils/test/src/math/test_accumulator.cpp +++ b/common/autoware_universe_utils/test/src/math/test_accumulator.cpp @@ -16,14 +16,51 @@ #include -TEST(accumulator, statistic) +TEST(accumulator, empty) { autoware::universe_utils::Accumulator acc; - acc.add(1.0); - acc.add(2.0); - acc.add(3.0); - EXPECT_DOUBLE_EQ(acc.mean(), 2.0); - EXPECT_DOUBLE_EQ(acc.min(), 1.0); - EXPECT_DOUBLE_EQ(acc.max(), 3.0); + EXPECT_DOUBLE_EQ(acc.mean(), 0.0); + EXPECT_DOUBLE_EQ(acc.min(), std::numeric_limits::max()); + EXPECT_DOUBLE_EQ(acc.max(), std::numeric_limits::lowest()); + EXPECT_EQ(acc.count(), 0); } + + +TEST(accumulator, addValues) +{ + autoware::universe_utils::Accumulator acc; + acc.add(100.0); + + EXPECT_DOUBLE_EQ(acc.mean(), 100.0); + EXPECT_DOUBLE_EQ(acc.min(), 100.0); + EXPECT_DOUBLE_EQ(acc.max(), 100.0); + EXPECT_EQ(acc.count(), 1); +} + +TEST(accumulator, positiveValues) +{ + autoware::universe_utils::Accumulator acc; + acc.add(10.0); + acc.add(40.0); + acc.add(10.0); + + EXPECT_DOUBLE_EQ(acc.mean(), 20.0); + EXPECT_DOUBLE_EQ(acc.min(), 10.0); + EXPECT_DOUBLE_EQ(acc.max(), 40.0); + EXPECT_EQ(acc.count(), 3); +} + +TEST(accumulator, negativeValues) +{ + autoware::universe_utils::Accumulator acc; + acc.add(-10.0); + acc.add(-40.0); + acc.add(-10.0); + + EXPECT_DOUBLE_EQ(acc.mean(), -20.0); + EXPECT_DOUBLE_EQ(acc.min(), -40.0); + EXPECT_DOUBLE_EQ(acc.max(), -10.0); + EXPECT_EQ(acc.count(), 3); +} + From e50283c4e7e0ebaddff6786dbc9ea7e20f5337e2 Mon Sep 17 00:00:00 2001 From: xtk8532704 <1041084556@qq.com> Date: Tue, 26 Nov 2024 16:42:13 +0900 Subject: [PATCH 5/9] pre-commit Signed-off-by: xtk8532704 <1041084556@qq.com> --- .../autoware_universe_utils/test/src/math/test_accumulator.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/common/autoware_universe_utils/test/src/math/test_accumulator.cpp b/common/autoware_universe_utils/test/src/math/test_accumulator.cpp index e150e6c762c6c..89630454d5f19 100644 --- a/common/autoware_universe_utils/test/src/math/test_accumulator.cpp +++ b/common/autoware_universe_utils/test/src/math/test_accumulator.cpp @@ -26,7 +26,6 @@ TEST(accumulator, empty) EXPECT_EQ(acc.count(), 0); } - TEST(accumulator, addValues) { autoware::universe_utils::Accumulator acc; @@ -63,4 +62,3 @@ TEST(accumulator, negativeValues) EXPECT_DOUBLE_EQ(acc.max(), -10.0); EXPECT_EQ(acc.count(), 3); } - From f607d65d9717a2bc62676424bac51cd57a3f9ef2 Mon Sep 17 00:00:00 2001 From: "Kem (TiankuiXian)" <1041084556@qq.com> Date: Tue, 26 Nov 2024 20:36:16 +0900 Subject: [PATCH 6/9] Update common/autoware_universe_utils/include/autoware/universe_utils/math/accumulator.hpp Co-authored-by: Kosuke Takeuchi Signed-off-by: xtk8532704 <1041084556@qq.com> --- .../include/autoware/universe_utils/math/accumulator.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/math/accumulator.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/math/accumulator.hpp index 10127d01c0a3a..eb460cc685891 100644 --- a/common/autoware_universe_utils/include/autoware/universe_utils/math/accumulator.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/math/accumulator.hpp @@ -1,4 +1,4 @@ -// Copyright 2024 Tier IV, Inc. +// Copyright 2021-2024 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. From 618cbff97ca0f38b5e5460393885e06f2d9fc61a Mon Sep 17 00:00:00 2001 From: xtk8532704 <1041084556@qq.com> Date: Tue, 26 Nov 2024 23:03:57 +0900 Subject: [PATCH 7/9] refactor and add output_metrics. a bug existing when psim. Signed-off-by: xtk8532704 <1041084556@qq.com> --- .../autoware_control_evaluator/README.md | 6 +- .../control_evaluator_node.hpp | 17 ++- .../control_evaluator/metrics/metric.hpp | 82 +++++++++++++ .../autoware_control_evaluator/package.xml | 1 + .../param/control_evaluator.defaults.yaml | 1 + .../src/control_evaluator_node.cpp | 111 +++++++++++++----- .../test/test_control_evaluator_node.cpp | 2 + 7 files changed, 182 insertions(+), 38 deletions(-) create mode 100644 evaluator/autoware_control_evaluator/include/autoware/control_evaluator/metrics/metric.hpp diff --git a/evaluator/autoware_control_evaluator/README.md b/evaluator/autoware_control_evaluator/README.md index 59c09015bd7b5..b7da418764959 100644 --- a/evaluator/autoware_control_evaluator/README.md +++ b/evaluator/autoware_control_evaluator/README.md @@ -4,14 +4,14 @@ This package provides nodes that generate metrics to evaluate the quality of control. -It publishes diagnostic information about control modules' outputs as well as the ego vehicle's current kinematics and position. +It publishes metric information about control modules' outputs as well as the ego vehicle's current kinematics and position. ## Evaluated metrics -The control evaluator uses the metrics defined in `include/autoware/control_evaluator/metrics/deviation_metrics.hpp`to calculate deviations in yaw and lateral distance from the ego's set-point. The control_evaluator can also be customized to offer metrics/evaluation about other control modules. Currently, the control_evaluator offers a simple diagnostic output based on the autonomous_emergency_braking node's output, but this functionality can be extended to evaluate other control modules' performance. +The control evaluator uses the metrics defined in `include/autoware/control_evaluator/metrics/metric.hpp`to calculate deviations in yaw and lateral distance from the ego's set-point. The control_evaluator can also be customized to offer metrics/evaluation about other control modules. Currently, the control_evaluator offers a simple metric output based on the autonomous_emergency_braking node's output, but this functionality can be extended to evaluate other control modules' performance. ## Kinematics output -The control evaluator module also constantly publishes information regarding the ego vehicle's kinematics and position. It publishes the current ego lane id with the longitudinal `s` and lateral `t` arc coordinates. It also publishes the current ego speed, acceleration and jerk in its diagnostic messages. +The control evaluator module also constantly publishes information regarding the ego vehicle's kinematics and position. It publishes the current ego lane id with the longitudinal `s` and lateral `t` arc coordinates. It also publishes the current ego speed, acceleration and jerk in its metric messages. This information can be used by other nodes to establish automated evaluation using rosbags: by crosschecking the ego position and kinematics with the evaluated control module's output, it is possible to judge if the evaluated control modules reacted in a satisfactory way at certain interesting points of the rosbag reproduction. diff --git a/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp index 6e4a04964a72c..6c356d1ad7952 100644 --- a/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp +++ b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp @@ -16,6 +16,8 @@ #define AUTOWARE__CONTROL_EVALUATOR__CONTROL_EVALUATOR_NODE_HPP_ #include "autoware/control_evaluator/metrics/deviation_metrics.hpp" +#include "autoware/control_evaluator/metrics/metric.hpp" +#include "autoware/universe_utils/math/accumulator.hpp" #include #include @@ -32,10 +34,11 @@ #include #include #include +#include namespace control_diagnostics { - +using autoware::universe_utils::Accumulator; using autoware_planning_msgs::msg::Trajectory; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; @@ -53,6 +56,9 @@ class ControlEvaluatorNode : public rclcpp::Node { public: explicit ControlEvaluatorNode(const rclcpp::NodeOptions & node_options); + ~ControlEvaluatorNode() override; + + void AddMetricMsg(const Metric & metric, const double & metric_value); void AddLateralDeviationMetricMsg(const Trajectory & traj, const Point & ego_point); void AddYawDeviationMetricMsg(const Trajectory & traj, const Pose & ego_pose); void AddGoalLongitudinalDeviationMetricMsg(const Pose & ego_pose); @@ -85,9 +91,12 @@ class ControlEvaluatorNode : public rclcpp::Node // update Route Handler void getRouteData(); - // Calculator - // Metrics - std::deque stamps_; + // Parameters + bool output_metrics_; + + // Metric + std::vector metrics_; + std::array, static_cast(Metric::SIZE)> metric_accumulators_; // 3(min, max, mean) * metric_size MetricArrayMsg metrics_msg_; autoware::route_handler::RouteHandler route_handler_; diff --git a/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/metrics/metric.hpp b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/metrics/metric.hpp new file mode 100644 index 0000000000000..2da339d173216 --- /dev/null +++ b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/metrics/metric.hpp @@ -0,0 +1,82 @@ +// 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 AUTOWARE__CONTROL_EVALUATOR__METRICS_METRIC_HPP_ +#define AUTOWARE__CONTROL_EVALUATOR__METRICS_METRIC_HPP_ + +#include +#include +#include +#include + +namespace control_diagnostics +{ +/** + * @brief Enumeration of trajectory metrics + */ +enum class Metric { + lateral_deviation, + yaw_deviation, + goal_longitudinal_deviation, + goal_lateral_deviation, + goal_yaw_deviation, + SIZE, +}; + +static const std::unordered_map str_to_metric = { + {"lateral_deviation", Metric::lateral_deviation}, + {"yaw_deviation", Metric::yaw_deviation}, + {"goal_longitudinal_deviation", Metric::goal_longitudinal_deviation}, + {"goal_lateral_deviation", Metric::goal_lateral_deviation}, + {"goal_yaw_deviation", Metric::goal_yaw_deviation}, +}; + +static const std::unordered_map metric_to_str = { + {Metric::lateral_deviation, "lateral_deviation"}, + {Metric::yaw_deviation, "yaw_deviation"}, + {Metric::goal_longitudinal_deviation, "goal_longitudinal_deviation"}, + {Metric::goal_lateral_deviation, "goal_lateral_deviation"}, + {Metric::goal_yaw_deviation, "goal_yaw_deviation"}, +}; + +// Metrics descriptions +static const std::unordered_map metric_descriptions = { + {Metric::lateral_deviation, "Lateral deviation from the reference trajectory[m]"}, + {Metric::yaw_deviation, "Yaw deviation from the reference trajectory[rad]"}, + {Metric::goal_longitudinal_deviation, "Longitudinal deviation from the goal point[m]"}, + {Metric::goal_lateral_deviation, "Lateral deviation from the goal point[m]"}, + {Metric::goal_yaw_deviation, "Yaw deviation from the goal point[rad]"} +}; + +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 control_diagnostics + +#endif // AUTOWARE__CONTROL_EVALUATOR__METRICS__METRIC_HPP_ diff --git a/evaluator/autoware_control_evaluator/package.xml b/evaluator/autoware_control_evaluator/package.xml index 8ab2c73cb619a..1c724fc7bd0ec 100644 --- a/evaluator/autoware_control_evaluator/package.xml +++ b/evaluator/autoware_control_evaluator/package.xml @@ -22,6 +22,7 @@ autoware_test_utils autoware_universe_utils nav_msgs + nlohmann-json-dev pluginlib rclcpp rclcpp_components diff --git a/evaluator/autoware_control_evaluator/param/control_evaluator.defaults.yaml b/evaluator/autoware_control_evaluator/param/control_evaluator.defaults.yaml index a20dbd7ffd3d9..dcb1e6d3d7bdc 100644 --- a/evaluator/autoware_control_evaluator/param/control_evaluator.defaults.yaml +++ b/evaluator/autoware_control_evaluator/param/control_evaluator.defaults.yaml @@ -1,2 +1,3 @@ /**: ros__parameters: + output_metrics: false # if true, metrics are written to `/autoware_metrics/-.json`. \ No newline at end of file diff --git a/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp b/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp index c24dfee571852..efb0e2933b1ff 100644 --- a/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp +++ b/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp @@ -17,6 +17,11 @@ #include #include +#include + +#include +#include +#include #include #include #include @@ -28,6 +33,9 @@ ControlEvaluatorNode::ControlEvaluatorNode(const rclcpp::NodeOptions & node_opti { using std::placeholders::_1; + // Parameters + output_metrics_ = declare_parameter("output_metrics"); + // Publisher processing_time_pub_ = this->create_publisher("~/debug/processing_time_ms", 1); @@ -37,6 +45,50 @@ ControlEvaluatorNode::ControlEvaluatorNode(const rclcpp::NodeOptions & node_opti using namespace std::literals::chrono_literals; timer_ = rclcpp::create_timer(this, get_clock(), 100ms, std::bind(&ControlEvaluatorNode::onTimer, this)); + +} + +ControlEvaluatorNode::~ControlEvaluatorNode() +{ + if (!output_metrics_){ + return; + } + + // generate json data + using json = nlohmann::json; + json j; + for (Metric metric : metrics_) { + const std::string base_name = metric_to_str.at(metric) + "/"; + j[base_name + "min"] = metric_accumulators_[static_cast(metric)].min(); + j[base_name + "max"] = metric_accumulators_[static_cast(metric)].max(); + j[base_name + "mean"] = metric_accumulators_[static_cast(metric)].mean(); + } + + // get output folder + const std::string output_folder_str = rclcpp::get_logging_directory().string() + "/autoware_metrics"; + if (!std::filesystem::exists(output_folder_str)) { + if (!std::filesystem::create_directories(output_folder_str)) { + RCLCPP_ERROR(this->get_logger(), "Failed to create directories: %s", output_folder_str.c_str()); + return; + } + } + + // get time stamp + std::time_t now_time_t = std::chrono::system_clock::to_time_t(std::chrono::system_clock::now()); + std::tm* local_time = std::localtime(&now_time_t); + std::ostringstream oss; + oss << std::put_time(local_time, "%Y-%m-%d-%H-%M-%S"); + std::string cur_time_str = oss.str(); + + // Write metrics .json to file + const std::string output_file_str = output_folder_str + "/autoware_control_evaluator-"+cur_time_str+".json"; + std::ofstream f(output_file_str); + if (f.is_open()) { + f << j.dump(4); + f.close(); + } else { + RCLCPP_ERROR(this->get_logger(), "Failed to open file: %s", output_file_str.c_str()); + } } void ControlEvaluatorNode::getRouteData() @@ -62,6 +114,18 @@ void ControlEvaluatorNode::getRouteData() } } +void ControlEvaluatorNode::AddMetricMsg(const Metric & metric, const double & metric_value) +{ + MetricMsg metric_msg; + metric_msg.name = metric_to_str.at(metric); + metric_msg.value = std::to_string(metric_value); + metrics_msg_.metric_array.push_back(metric_msg); + + if (output_metrics_) { + metric_accumulators_[static_cast(metric)].add(metric_value); + } +} + void ControlEvaluatorNode::AddLaneletMetricMsg(const Pose & ego_pose) { const auto current_lanelets = [&]() { @@ -97,7 +161,6 @@ void ControlEvaluatorNode::AddLaneletMetricMsg(const Pose & ego_pose) metric_msg.value = std::to_string(arc_coordinates.distance); metrics_msg_.metric_array.push_back(metric_msg); } - return; } void ControlEvaluatorNode::AddKinematicStateMetricMsg( @@ -141,59 +204,45 @@ void ControlEvaluatorNode::AddKinematicStateMetricMsg( void ControlEvaluatorNode::AddLateralDeviationMetricMsg( const Trajectory & traj, const Point & ego_point) { - const double lateral_deviation = metrics::calcLateralDeviation(traj, ego_point); + const Metric metric = Metric::lateral_deviation; + const double metric_value = metrics::calcLateralDeviation(traj, ego_point); - MetricMsg metric_msg; - metric_msg.name = "lateral_deviation"; - metric_msg.value = std::to_string(lateral_deviation); - metrics_msg_.metric_array.push_back(metric_msg); - return; + AddMetricMsg(metric, metric_value); } void ControlEvaluatorNode::AddYawDeviationMetricMsg(const Trajectory & traj, const Pose & ego_pose) { - const double yaw_deviation = metrics::calcYawDeviation(traj, ego_pose); + const Metric metric = Metric::yaw_deviation; + const double metric_value = metrics::calcYawDeviation(traj, ego_pose); - MetricMsg metric_msg; - metric_msg.name = "yaw_deviation"; - metric_msg.value = std::to_string(yaw_deviation); - metrics_msg_.metric_array.push_back(metric_msg); - return; + AddMetricMsg(metric, metric_value); } void ControlEvaluatorNode::AddGoalLongitudinalDeviationMetricMsg(const Pose & ego_pose) { - const double longitudinal_deviation = + const Metric metric = Metric::goal_longitudinal_deviation; + const double metric_value = metrics::calcLongitudinalDeviation(route_handler_.getGoalPose(), ego_pose.position); - MetricMsg metric_msg; - metric_msg.name = "goal_longitudinal_deviation"; - metric_msg.value = std::to_string(longitudinal_deviation); - metrics_msg_.metric_array.push_back(metric_msg); - return; + AddMetricMsg(metric, metric_value); } void ControlEvaluatorNode::AddGoalLateralDeviationMetricMsg(const Pose & ego_pose) { - const double lateral_deviation = + const Metric metric = Metric::lateral_deviation; + const double metric_value = metrics::calcLateralDeviation(route_handler_.getGoalPose(), ego_pose.position); - MetricMsg metric_msg; - metric_msg.name = "goal_lateral_deviation"; - metric_msg.value = std::to_string(lateral_deviation); - metrics_msg_.metric_array.push_back(metric_msg); - return; + AddMetricMsg(metric, metric_value); } void ControlEvaluatorNode::AddGoalYawDeviationMetricMsg(const Pose & ego_pose) { - const double yaw_deviation = metrics::calcYawDeviation(route_handler_.getGoalPose(), ego_pose); - MetricMsg metric_msg; - metric_msg.name = "goal_yaw_deviation"; - metric_msg.value = std::to_string(yaw_deviation); - metrics_msg_.metric_array.push_back(metric_msg); - return; + const Metric metric = Metric::goal_yaw_deviation; + const double metric_value = metrics::calcYawDeviation(route_handler_.getGoalPose(), ego_pose); + + AddMetricMsg(metric, metric_value); } void ControlEvaluatorNode::onTimer() diff --git a/evaluator/autoware_control_evaluator/test/test_control_evaluator_node.cpp b/evaluator/autoware_control_evaluator/test/test_control_evaluator_node.cpp index 0a2f777e49b61..edf178949744e 100644 --- a/evaluator/autoware_control_evaluator/test/test_control_evaluator_node.cpp +++ b/evaluator/autoware_control_evaluator/test/test_control_evaluator_node.cpp @@ -52,6 +52,8 @@ class EvalTest : public ::testing::Test rclcpp::NodeOptions options; const auto share_dir = ament_index_cpp::get_package_share_directory("autoware_control_evaluator"); + options.arguments( + {"--ros-args", "--params-file", share_dir + "/param/control_evaluator.defaults.yaml"}); dummy_node = std::make_shared("control_evaluator_test_node"); eval_node = std::make_shared(options); From 9ecdb3a373b41fcd5659424b0c01b8287c01d2c1 Mon Sep 17 00:00:00 2001 From: xtk8532704 <1041084556@qq.com> Date: Wed, 27 Nov 2024 00:32:09 +0900 Subject: [PATCH 8/9] refactored launch file. Signed-off-by: xtk8532704 <1041084556@qq.com> --- .../control_evaluator_node.hpp | 12 +++++++--- .../control_evaluator/metrics/metric.hpp | 7 +++--- .../launch/control_evaluator.launch.xml | 11 ++++----- .../param/control_evaluator.defaults.yaml | 2 +- .../src/control_evaluator_node.cpp | 24 +++++++++---------- .../launch/control.launch.xml | 18 ++++---------- 6 files changed, 34 insertions(+), 40 deletions(-) diff --git a/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp index 6c356d1ad7952..50e8d65e813d0 100644 --- a/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp +++ b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp @@ -93,10 +93,16 @@ class ControlEvaluatorNode : public rclcpp::Node // Parameters bool output_metrics_; - + // Metric - std::vector metrics_; - std::array, static_cast(Metric::SIZE)> metric_accumulators_; // 3(min, max, mean) * metric_size + std::vector metrics_ = { + // collect all metrics + Metric::lateral_deviation, Metric::yaw_deviation, Metric::goal_longitudinal_deviation, + Metric::goal_lateral_deviation, Metric::goal_yaw_deviation, + }; + + std::array, static_cast(Metric::SIZE)> + metric_accumulators_; // 3(min, max, mean) * metric_size MetricArrayMsg metrics_msg_; autoware::route_handler::RouteHandler route_handler_; diff --git a/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/metrics/metric.hpp b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/metrics/metric.hpp index 2da339d173216..be2f3135d7f7e 100644 --- a/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/metrics/metric.hpp +++ b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/metrics/metric.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE__CONTROL_EVALUATOR__METRICS_METRIC_HPP_ -#define AUTOWARE__CONTROL_EVALUATOR__METRICS_METRIC_HPP_ +#ifndef AUTOWARE__CONTROL_EVALUATOR__METRICS__METRIC_HPP_ +#define AUTOWARE__CONTROL_EVALUATOR__METRICS__METRIC_HPP_ #include #include @@ -56,8 +56,7 @@ static const std::unordered_map metric_descriptions = { {Metric::yaw_deviation, "Yaw deviation from the reference trajectory[rad]"}, {Metric::goal_longitudinal_deviation, "Longitudinal deviation from the goal point[m]"}, {Metric::goal_lateral_deviation, "Lateral deviation from the goal point[m]"}, - {Metric::goal_yaw_deviation, "Yaw deviation from the goal point[rad]"} -}; + {Metric::goal_yaw_deviation, "Yaw deviation from the goal point[rad]"}}; namespace details { diff --git a/evaluator/autoware_control_evaluator/launch/control_evaluator.launch.xml b/evaluator/autoware_control_evaluator/launch/control_evaluator.launch.xml index e9fee7ffadbf2..e3fafb49e1187 100644 --- a/evaluator/autoware_control_evaluator/launch/control_evaluator.launch.xml +++ b/evaluator/autoware_control_evaluator/launch/control_evaluator.launch.xml @@ -1,20 +1,19 @@ - - - + + + - - - + + diff --git a/evaluator/autoware_control_evaluator/param/control_evaluator.defaults.yaml b/evaluator/autoware_control_evaluator/param/control_evaluator.defaults.yaml index dcb1e6d3d7bdc..ff1d8433d4ca8 100644 --- a/evaluator/autoware_control_evaluator/param/control_evaluator.defaults.yaml +++ b/evaluator/autoware_control_evaluator/param/control_evaluator.defaults.yaml @@ -1,3 +1,3 @@ /**: ros__parameters: - output_metrics: false # if true, metrics are written to `/autoware_metrics/-.json`. \ No newline at end of file + output_metrics: false # if true, metrics are written to `/autoware_metrics/-.json`. diff --git a/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp b/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp index efb0e2933b1ff..7576fdf662cc6 100644 --- a/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp +++ b/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp @@ -16,12 +16,11 @@ #include #include - #include -#include #include #include +#include #include #include #include @@ -33,24 +32,23 @@ ControlEvaluatorNode::ControlEvaluatorNode(const rclcpp::NodeOptions & node_opti { using std::placeholders::_1; - // Parameters - output_metrics_ = declare_parameter("output_metrics"); - // Publisher processing_time_pub_ = this->create_publisher("~/debug/processing_time_ms", 1); metrics_pub_ = create_publisher("~/metrics", 1); + // Parameters + output_metrics_ = declare_parameter("output_metrics"); + // Timer callback to publish evaluator diagnostics using namespace std::literals::chrono_literals; timer_ = rclcpp::create_timer(this, get_clock(), 100ms, std::bind(&ControlEvaluatorNode::onTimer, this)); - } ControlEvaluatorNode::~ControlEvaluatorNode() { - if (!output_metrics_){ + if (!output_metrics_) { return; } @@ -65,23 +63,26 @@ ControlEvaluatorNode::~ControlEvaluatorNode() } // get output folder - const std::string output_folder_str = rclcpp::get_logging_directory().string() + "/autoware_metrics"; + const std::string output_folder_str = + rclcpp::get_logging_directory().string() + "/autoware_metrics"; if (!std::filesystem::exists(output_folder_str)) { if (!std::filesystem::create_directories(output_folder_str)) { - RCLCPP_ERROR(this->get_logger(), "Failed to create directories: %s", output_folder_str.c_str()); + RCLCPP_ERROR( + this->get_logger(), "Failed to create directories: %s", output_folder_str.c_str()); return; } } // get time stamp std::time_t now_time_t = std::chrono::system_clock::to_time_t(std::chrono::system_clock::now()); - std::tm* local_time = std::localtime(&now_time_t); + std::tm * local_time = std::localtime(&now_time_t); std::ostringstream oss; oss << std::put_time(local_time, "%Y-%m-%d-%H-%M-%S"); std::string cur_time_str = oss.str(); // Write metrics .json to file - const std::string output_file_str = output_folder_str + "/autoware_control_evaluator-"+cur_time_str+".json"; + const std::string output_file_str = + output_folder_str + "/autoware_control_evaluator-" + cur_time_str + ".json"; std::ofstream f(output_file_str); if (f.is_open()) { f << j.dump(4); @@ -238,7 +239,6 @@ void ControlEvaluatorNode::AddGoalLateralDeviationMetricMsg(const Pose & ego_pos void ControlEvaluatorNode::AddGoalYawDeviationMetricMsg(const Pose & ego_pose) { - const Metric metric = Metric::goal_yaw_deviation; const double metric_value = metrics::calcYawDeviation(route_handler_.getGoalPose(), ego_pose); diff --git a/launch/tier4_control_launch/launch/control.launch.xml b/launch/tier4_control_launch/launch/control.launch.xml index 899db1448dbf8..387b82526f624 100644 --- a/launch/tier4_control_launch/launch/control.launch.xml +++ b/launch/tier4_control_launch/launch/control.launch.xml @@ -269,21 +269,11 @@ + - - - - - - - - - - - - - - + + + From 542230a7cfc27adf542492070eba9e08dfaf7e3f Mon Sep 17 00:00:00 2001 From: xtk8532704 <1041084556@qq.com> Date: Wed, 27 Nov 2024 00:38:12 +0900 Subject: [PATCH 9/9] output description Signed-off-by: xtk8532704 <1041084556@qq.com> --- .../autoware_control_evaluator/src/control_evaluator_node.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp b/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp index 7576fdf662cc6..2a2c07b2db319 100644 --- a/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp +++ b/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp @@ -60,6 +60,8 @@ ControlEvaluatorNode::~ControlEvaluatorNode() j[base_name + "min"] = metric_accumulators_[static_cast(metric)].min(); j[base_name + "max"] = metric_accumulators_[static_cast(metric)].max(); j[base_name + "mean"] = metric_accumulators_[static_cast(metric)].mean(); + j[base_name + "count"] = metric_accumulators_[static_cast(metric)].count(); + j[base_name + "description"] = metric_descriptions.at(metric); } // get output folder