From 3b1235c787e97bb87a209955e1f5ab8cd3aa8182 Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Tue, 20 Feb 2024 23:20:16 +0900 Subject: [PATCH 1/8] feat(perception_evaluator): add perception_evaluator Signed-off-by: kosuke55 tmp Signed-off-by: kosuke55 update Signed-off-by: kosuke55 add Signed-off-by: kosuke55 add add update clean up Signed-off-by: kosuke55 change time horizon Signed-off-by: kosuke55 --- evaluator/perception_evaluator/CMakeLists.txt | 44 ++ evaluator/perception_evaluator/README.md | 43 ++ .../metrics/deviation_metrics.hpp | 53 ++ .../perception_evaluator/metrics/metric.hpp | 75 +++ .../metrics_calculator.hpp | 141 ++++++ .../perception_evaluator/parameters.hpp | 58 +++ .../perception_evaluator_node.hpp | 95 ++++ .../include/perception_evaluator/stat.hpp | 93 ++++ .../utils/marker_utils.hpp | 82 ++++ .../utils/objects_filtering.hpp | 128 +++++ .../launch/motion_evaluator.launch.xml | 7 + .../launch/perception_evaluator.launch.xml | 12 + evaluator/perception_evaluator/package.xml | 42 ++ .../param/perception_evaluator.defaults.yaml | 39 ++ .../src/metrics/deviation_metrics.cpp | 65 +++ .../src/metrics_calculator.cpp | 458 ++++++++++++++++++ .../src/perception_evaluator_node.cpp | 348 +++++++++++++ .../src/utils/marker_utils.cpp | 198 ++++++++ .../src/utils/objects_filtering.cpp | 103 ++++ .../launch/perception.launch.xml | 5 + .../launch/simulator.launch.xml | 5 + 21 files changed, 2094 insertions(+) create mode 100644 evaluator/perception_evaluator/CMakeLists.txt create mode 100644 evaluator/perception_evaluator/README.md create mode 100644 evaluator/perception_evaluator/include/perception_evaluator/metrics/deviation_metrics.hpp create mode 100644 evaluator/perception_evaluator/include/perception_evaluator/metrics/metric.hpp create mode 100644 evaluator/perception_evaluator/include/perception_evaluator/metrics_calculator.hpp create mode 100644 evaluator/perception_evaluator/include/perception_evaluator/parameters.hpp create mode 100644 evaluator/perception_evaluator/include/perception_evaluator/perception_evaluator_node.hpp create mode 100644 evaluator/perception_evaluator/include/perception_evaluator/stat.hpp create mode 100644 evaluator/perception_evaluator/include/perception_evaluator/utils/marker_utils.hpp create mode 100644 evaluator/perception_evaluator/include/perception_evaluator/utils/objects_filtering.hpp create mode 100644 evaluator/perception_evaluator/launch/motion_evaluator.launch.xml create mode 100644 evaluator/perception_evaluator/launch/perception_evaluator.launch.xml create mode 100644 evaluator/perception_evaluator/package.xml create mode 100644 evaluator/perception_evaluator/param/perception_evaluator.defaults.yaml create mode 100644 evaluator/perception_evaluator/src/metrics/deviation_metrics.cpp create mode 100644 evaluator/perception_evaluator/src/metrics_calculator.cpp create mode 100644 evaluator/perception_evaluator/src/perception_evaluator_node.cpp create mode 100644 evaluator/perception_evaluator/src/utils/marker_utils.cpp create mode 100644 evaluator/perception_evaluator/src/utils/objects_filtering.cpp diff --git a/evaluator/perception_evaluator/CMakeLists.txt b/evaluator/perception_evaluator/CMakeLists.txt new file mode 100644 index 0000000000000..6a787058a9df3 --- /dev/null +++ b/evaluator/perception_evaluator/CMakeLists.txt @@ -0,0 +1,44 @@ +cmake_minimum_required(VERSION 3.14) +project(perception_evaluator) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(pluginlib REQUIRED) + +find_package(glog REQUIRED) + +ament_auto_add_library(${PROJECT_NAME}_node SHARED + src/metrics_calculator.cpp + src/${PROJECT_NAME}_node.cpp + src/metrics/deviation_metrics.cpp + src/utils/marker_utils.cpp + src/utils/objects_filtering.cpp +) + +rclcpp_components_register_node(${PROJECT_NAME}_node + PLUGIN "perception_diagnostics::PerceptionEvaluatorNode" + EXECUTABLE ${PROJECT_NAME} +) + +rclcpp_components_register_node(${PROJECT_NAME}_node + PLUGIN "perception_diagnostics::MotionEvaluatorNode" + EXECUTABLE motion_evaluator +) + +target_link_libraries(${PROJECT_NAME}_node glog::glog) + +# if(BUILD_TESTING) +# ament_add_ros_isolated_gtest(test_${PROJECT_NAME} +# test/test_perception_evaluator_node.cpp +# ) +# target_link_libraries(test_${PROJECT_NAME} +# ${PROJECT_NAME}_node +# ) +# endif() + +ament_auto_package( + INSTALL_TO_SHARE + param + launch +) diff --git a/evaluator/perception_evaluator/README.md b/evaluator/perception_evaluator/README.md new file mode 100644 index 0000000000000..b801e5f418cef --- /dev/null +++ b/evaluator/perception_evaluator/README.md @@ -0,0 +1,43 @@ +# Perception Evaluator + +A node for evaluating the output of perception systems. + +## Purpose + +This module allows for the evaluation of how accurately perception results are generated without the need for annotations. It is capable of confirming performance and can evaluate results from a few seconds prior, enabling online execution. + +## Inner-workings / Algorithms + +- Calculates lateral deviation between the predicted path and the actual traveled trajectory. +- Calculates lateral deviation between the smoothed traveled trajectory and the perceived position to evaluate the stability of lateral position recognition. +- Calculates yaw deviation between the smoothed traveled trajectory and the perceived position to evaluate the stability of yaw recognition. + +## Inputs / Outputs + +| Name | Type | Description | +| ----------------- | ------------------------------------------------------ | ------------------------------------------------- | +| `~/input/objects` | `autoware_auto_perception_msgs::msg::PredictedObjects` | The predicted objects to evaluate. | +| `~/metrics` | `diagnostic_msgs::msg::DiagnosticArray` | Diagnostic information about perception accuracy. | +| `~/markers` | `visualization_msgs::msg::MarkerArray` | Visual markers for debugging and visualization. | + +## Parameters + +| Name | Type | Description | +| --------------------------------- | ------------ | ------------------------------------------------------------------------------------------------ | +| `selected_metrics` | List | Metrics to be evaluated, such as lateral deviation, yaw deviation, and predicted path deviation. | +| `smoothing_window_size` | Integer | Determines the window size for smoothing path, should be an odd number. | +| `prediction_time_horizons` | list[double] | Time horizons for prediction evaluation in seconds. | +| `target_object.*.check_deviation` | bool | Whether to check deviation for specific object types (car, truck, etc.). | +| `debug_marker.*` | bool | Debugging parameters for marker visualization (history path, predicted path, etc.). | + +## Assumptions / Known limits + +It is assumed that the current positions of PredictedObjects are reasonably accurate. + +## Future extensions / Unimplemented parts + +- Increase rate in recognition per class +- Metrics for objects with strange physical behavior (e.g., going through a fence) +- Metrics for splitting objects +- Metrics for problems with objects that are normally stationary but move +- Disappearing object metrics diff --git a/evaluator/perception_evaluator/include/perception_evaluator/metrics/deviation_metrics.hpp b/evaluator/perception_evaluator/include/perception_evaluator/metrics/deviation_metrics.hpp new file mode 100644 index 0000000000000..00255835ba547 --- /dev/null +++ b/evaluator/perception_evaluator/include/perception_evaluator/metrics/deviation_metrics.hpp @@ -0,0 +1,53 @@ +// 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. + +#ifndef PERCEPTION_EVALUATOR__METRICS__DEVIATION_METRICS_HPP_ +#define PERCEPTION_EVALUATOR__METRICS__DEVIATION_METRICS_HPP_ + +#include "perception_evaluator/stat.hpp" + +#include +#include + +#include + +namespace perception_diagnostics +{ +namespace metrics +{ +using autoware_auto_perception_msgs::msg::PredictedPath; +using geometry_msgs::msg::Pose; + +/** + * @brief calculate lateral deviation of the given path from the reference path + * @param [in] ref_path reference path + * @param [in] pred_path predicted path + * @return calculated statistics + */ +double calcLateralDeviation(const std::vector & ref_path, const Pose & target_pose); + +/** + * @brief calculate yaw deviation of the given path from the reference path + * @param [in] ref_path reference path + * @param [in] pred_path predicted path + * @return calculated statistics + */ +double calcYawDeviation(const std::vector & ref_path, const Pose & target_pose); + +std::vector calcPredictedPathDeviation( + const std::vector & ref_path, const PredictedPath & pred_path); +} // namespace metrics +} // namespace perception_diagnostics + +#endif // PERCEPTION_EVALUATOR__METRICS__DEVIATION_METRICS_HPP_ diff --git a/evaluator/perception_evaluator/include/perception_evaluator/metrics/metric.hpp b/evaluator/perception_evaluator/include/perception_evaluator/metrics/metric.hpp new file mode 100644 index 0000000000000..94f9fd32e9828 --- /dev/null +++ b/evaluator/perception_evaluator/include/perception_evaluator/metrics/metric.hpp @@ -0,0 +1,75 @@ +// 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. + +#ifndef PERCEPTION_EVALUATOR__METRICS__METRIC_HPP_ +#define PERCEPTION_EVALUATOR__METRICS__METRIC_HPP_ + +#include "perception_evaluator/stat.hpp" + +#include +#include +#include +#include + +namespace perception_diagnostics +{ +/** + * @brief Enumeration of trajectory metrics + */ +enum class Metric { + lateral_deviation, + yaw_deviation, + predicted_path_deviation, + SIZE, +}; + +using MetricStatMap = std::unordered_map>; + +static const std::unordered_map str_to_metric = { + {"lateral_deviation", Metric::lateral_deviation}, + {"yaw_deviation", Metric::yaw_deviation}, + {"predicted_path_deviation", Metric::predicted_path_deviation}}; + +static const std::unordered_map metric_to_str = { + {Metric::lateral_deviation, "lateral_deviation"}, + {Metric::yaw_deviation, "yaw_deviation"}, + {Metric::predicted_path_deviation, "predicted_path_deviation"}}; + +// Metrics descriptions +static const std::unordered_map metric_descriptions = { + {Metric::lateral_deviation, "Lateral_deviation[m]"}, + {Metric::yaw_deviation, "Yaw_deviation[rad]"}, + {Metric::predicted_path_deviation, "Predicted_path_deviation[m]"}}; + +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 perception_diagnostics + +#endif // PERCEPTION_EVALUATOR__METRICS__METRIC_HPP_ diff --git a/evaluator/perception_evaluator/include/perception_evaluator/metrics_calculator.hpp b/evaluator/perception_evaluator/include/perception_evaluator/metrics_calculator.hpp new file mode 100644 index 0000000000000..a2796d54e45fd --- /dev/null +++ b/evaluator/perception_evaluator/include/perception_evaluator/metrics_calculator.hpp @@ -0,0 +1,141 @@ +// 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. + +#ifndef PERCEPTION_EVALUATOR__METRICS_CALCULATOR_HPP_ +#define PERCEPTION_EVALUATOR__METRICS_CALCULATOR_HPP_ + +#include "perception_evaluator/metrics/deviation_metrics.hpp" +#include "perception_evaluator/metrics/metric.hpp" +#include "perception_evaluator/parameters.hpp" +#include "perception_evaluator/stat.hpp" + +#include + +#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" +#include "geometry_msgs/msg/pose.hpp" +#include + +#include +#include +#include +#include +#include + +namespace perception_diagnostics +{ +using autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_auto_perception_msgs::msg::PredictedObjects; +using geometry_msgs::msg::Point; +using geometry_msgs::msg::Pose; +using unique_identifier_msgs::msg::UUID; + +struct ObjectData +{ + PredictedObject object; + std::vector> path_pairs; + + std::vector getPredictedPath() const + { + std::vector path; + path.resize(path_pairs.size()); + std::transform( + path_pairs.begin(), path_pairs.end(), path.begin(), + [](const std::pair & pair) -> Pose { return pair.first; }); + return path; + } + + std::vector getHistoryPath() const + { + std::vector path; + path.resize(path_pairs.size()); + std::transform( + path_pairs.begin(), path_pairs.end(), path.begin(), + [](const std::pair & pair) -> Pose { return pair.second; }); + return path; + } +}; +using ObjectDataMap = std::unordered_map; + +// pair of history_path and smoothed_history_path for each object id +using HistoryPathMap = + std::unordered_map, std::vector>>; + +class MetricsCalculator +{ +public: + explicit MetricsCalculator(const std::shared_ptr & parameters) + : parameters_(parameters){}; + + /** + * @brief calculate + * @param [in] metric Metric enum value + * @return map of string describing the requested metric and the calculated value + */ + std::optional calculate(const Metric & metric) const; + + /** + * @brief set the dynamic objects used to calculate obstacle metrics + * @param [in] objects predicted objects + */ + void setPredictedObjects(const PredictedObjects & objects); + + HistoryPathMap getHistoryPathMap() const { return history_path_map_; } + ObjectDataMap getDebugObjectData() const { return debug_target_object_; } + +private: + std::shared_ptr parameters_; + + // Store predicted objects information and calculation results + std::unordered_map> object_map_; + HistoryPathMap history_path_map_; + + rclcpp::Time current_stamp_; + + // debug + mutable ObjectDataMap debug_target_object_; + + // Functions to calculate history path + void updateHistoryPath(); + std::vector averageFilterPath( + const std::vector & path, const size_t window_size) const; + std::vector generateHistoryPathWithPrev( + const std::vector & prev_history_path, const Pose & new_pose, const size_t window_size); + + // Update object data + void updateObjects( + const std::string uuid, const rclcpp::Time stamp, const PredictedObject & object); + void deleteOldObjects(const rclcpp::Time stamp); + + // Calculate metrics + MetricStatMap calcLateralDeviationMetrics(const PredictedObjects & objects) const; + MetricStatMap calcYawDeviationMetrics(const PredictedObjects & objects) const; + MetricStatMap calcPredictedPathDeviationMetrics(const PredictedObjects & objects) const; + Stat calcPredictedPathDeviationMetrics( + const PredictedObjects & objects, const double time_horizon) const; + + bool hasPassedTime(const rclcpp::Time stamp) const; + bool hasPassedTime(const std::string uuid, const rclcpp::Time stamp) const; + double getTimeDelay() const; + + // Extract object + rclcpp::Time getClosestStamp(const rclcpp::Time stamp) const; + std::optional getObjectByStamp( + const std::string uuid, const rclcpp::Time stamp) const; + PredictedObjects getObjectsByStamp(const rclcpp::Time stamp) const; + +}; // class MetricsCalculator + +} // namespace perception_diagnostics + +#endif // PERCEPTION_EVALUATOR__METRICS_CALCULATOR_HPP_ diff --git a/evaluator/perception_evaluator/include/perception_evaluator/parameters.hpp b/evaluator/perception_evaluator/include/perception_evaluator/parameters.hpp new file mode 100644 index 0000000000000..7ab29ea53fe85 --- /dev/null +++ b/evaluator/perception_evaluator/include/perception_evaluator/parameters.hpp @@ -0,0 +1,58 @@ +// 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. + +#ifndef PERCEPTION_EVALUATOR__PARAMETERS_HPP_ +#define PERCEPTION_EVALUATOR__PARAMETERS_HPP_ + +#include "perception_evaluator/metrics/metric.hpp" + +#include +#include + +namespace perception_diagnostics +{ +/** + * @brief Enumeration of perception metrics + */ + +struct ObjectParameter +{ + bool check_deviation{false}; +}; + +struct DebugMarkerParameter +{ + bool show_history_path{false}; + bool show_history_path_arrows{false}; + bool show_smoothed_history_path{true}; + bool show_smoothed_history_path_arrows{false}; + bool show_predicted_path{true}; + bool show_predicted_path_gt{true}; + bool show_deviation_lines{true}; + bool show_object_polygon{true}; +}; + +struct Parameters +{ + std::vector metrics; + size_t smoothing_window_size{0}; + std::vector prediction_time_horizons; + DebugMarkerParameter debug_marker_parameters; + // parameters depend on object class + std::unordered_map object_parameters; +}; + +} // namespace perception_diagnostics + +#endif // PERCEPTION_EVALUATOR__PARAMETERS_HPP_ diff --git a/evaluator/perception_evaluator/include/perception_evaluator/perception_evaluator_node.hpp b/evaluator/perception_evaluator/include/perception_evaluator/perception_evaluator_node.hpp new file mode 100644 index 0000000000000..8994c404f21b9 --- /dev/null +++ b/evaluator/perception_evaluator/include/perception_evaluator/perception_evaluator_node.hpp @@ -0,0 +1,95 @@ +// 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. + +#ifndef PERCEPTION_EVALUATOR__PERCEPTION_EVALUATOR_NODE_HPP_ +#define PERCEPTION_EVALUATOR__PERCEPTION_EVALUATOR_NODE_HPP_ + +#include "perception_evaluator/metrics_calculator.hpp" +#include "perception_evaluator/parameters.hpp" +#include "perception_evaluator/stat.hpp" +#include "rclcpp/rclcpp.hpp" +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" + +#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" +#include "diagnostic_msgs/msg/diagnostic_array.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "visualization_msgs/msg/marker_array.hpp" + +#include +#include +#include +#include +#include + +namespace perception_diagnostics +{ +using autoware_auto_perception_msgs::msg::ObjectClassification; +using autoware_auto_perception_msgs::msg::PredictedObjects; +using diagnostic_msgs::msg::DiagnosticArray; +using diagnostic_msgs::msg::DiagnosticStatus; +using nav_msgs::msg::Odometry; + +using MarkerArray = visualization_msgs::msg::MarkerArray; + +/** + * @brief Node for perception evaluation + */ +class PerceptionEvaluatorNode : public rclcpp::Node +{ +public: + explicit PerceptionEvaluatorNode(const rclcpp::NodeOptions & node_options); + ~PerceptionEvaluatorNode(); + + /** + * @brief callback on receiving a dynamic objects array + * @param [in] objects_msg received dynamic object array message + */ + void onObjects(const PredictedObjects::ConstSharedPtr objects_msg); + + DiagnosticStatus generateDiagnosticStatus( + const std::string metric, const Stat & metric_stat) const; + +private: + // Timer + rclcpp::TimerBase::SharedPtr timer_; + void initTimer(double period_s); + void onTimer(); + + // Subscribers and publishers + rclcpp::Subscription::SharedPtr objects_sub_; + rclcpp::Publisher::SharedPtr metrics_pub_; + rclcpp::Publisher::SharedPtr pub_marker_; + + // TF + std::shared_ptr transform_listener_{nullptr}; + std::unique_ptr tf_buffer_; + + // Parameters + std::shared_ptr parameters_; + void initParameter(); + rcl_interfaces::msg::SetParametersResult onParameter( + const std::vector & parameters); + OnSetParametersCallbackHandle::SharedPtr set_param_res_; + + // Metrics Calculator + MetricsCalculator metrics_calculator_; + std::deque stamps_; + + // Debug + void publishDebugMarker(); +}; +} // namespace perception_diagnostics + +#endif // PERCEPTION_EVALUATOR__PERCEPTION_EVALUATOR_NODE_HPP_ diff --git a/evaluator/perception_evaluator/include/perception_evaluator/stat.hpp b/evaluator/perception_evaluator/include/perception_evaluator/stat.hpp new file mode 100644 index 0000000000000..8ee9ac96ac9a8 --- /dev/null +++ b/evaluator/perception_evaluator/include/perception_evaluator/stat.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 PERCEPTION_EVALUATOR__STAT_HPP_ +#define PERCEPTION_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_EVALUATOR__STAT_HPP_ diff --git a/evaluator/perception_evaluator/include/perception_evaluator/utils/marker_utils.hpp b/evaluator/perception_evaluator/include/perception_evaluator/utils/marker_utils.hpp new file mode 100644 index 0000000000000..8fe3e3d690044 --- /dev/null +++ b/evaluator/perception_evaluator/include/perception_evaluator/utils/marker_utils.hpp @@ -0,0 +1,82 @@ +// 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. + +#ifndef PERCEPTION_EVALUATOR__UTILS__MARKER_UTILS_HPP_ +#define PERCEPTION_EVALUATOR__UTILS__MARKER_UTILS_HPP_ + +#include + +#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" +#include +#include + +#include + +#include +#include +#include + +namespace marker_utils +{ + +using autoware_auto_perception_msgs::msg::PredictedObject; +using geometry_msgs::msg::Point; +using geometry_msgs::msg::Pose; +using std_msgs::msg::ColorRGBA; +using tier4_autoware_utils::Polygon2d; +using visualization_msgs::msg::Marker; +using visualization_msgs::msg::MarkerArray; + +inline int64_t bitShift(int64_t original_id) +{ + return original_id << (sizeof(int32_t) * 8 / 2); +} + +void addFootprintMarker( + visualization_msgs::msg::Marker & marker, const geometry_msgs::msg::Pose & pose, + const vehicle_info_util::VehicleInfo & vehicle_info); + +MarkerArray createFootprintMarkerArray( + const Pose & base_link_pose, const vehicle_info_util::VehicleInfo vehicle_info, + const std::string && ns, const int32_t & id, const float & r, const float & g, const float & b); + +MarkerArray createPointsMarkerArray( + const std::vector points, const std::string & ns, const int32_t id, const float r, + const float g, const float b); +MarkerArray createPointsMarkerArray( + const std::vector poses, const std::string & ns, const int32_t id, const float r, + const float g, const float b); + +MarkerArray createPoseMarkerArray( + const Pose & pose, std::string && ns, const int32_t & id, const float & r, const float & g, + const float & b); + +MarkerArray createPosesMarkerArray( + const std::vector poses, std::string && ns, const int32_t & id, const float & r, + const float & g, const float & b, const float & x_scale = 0.5, const float & y_scale = 0.2, + const float & z_scale = 0.2); + +std_msgs::msg::ColorRGBA createColorFromString(const std::string & str); + +MarkerArray createObjectPolygonMarkerArray( + const PredictedObject & object, std::string && ns, const int32_t & id, const float & r, + const float & g, const float & b); + +MarkerArray createDeviationLines( + const std::vector poses1, const std::vector poses2, const std::string & ns, + const int32_t id, const float r, const float g, const float b); + +} // namespace marker_utils + +#endif // PERCEPTION_EVALUATOR__UTILS__MARKER_UTILS_HPP_ diff --git a/evaluator/perception_evaluator/include/perception_evaluator/utils/objects_filtering.hpp b/evaluator/perception_evaluator/include/perception_evaluator/utils/objects_filtering.hpp new file mode 100644 index 0000000000000..c31dea28ef639 --- /dev/null +++ b/evaluator/perception_evaluator/include/perception_evaluator/utils/objects_filtering.hpp @@ -0,0 +1,128 @@ +// 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. + +#ifndef PERCEPTION_EVALUATOR__UTILS__OBJECTS_FILTERING_HPP_ +#define PERCEPTION_EVALUATOR__UTILS__OBJECTS_FILTERING_HPP_ + +#include "perception_evaluator/parameters.hpp" + +#include +#include +#include + +#include +#include +#include +#include + +/** + * most of this file is copied from objects_filtering.hpp in safety_check of behavior_path_planner + */ + +namespace perception_diagnostics +{ + +using autoware_auto_perception_msgs::msg::ObjectClassification; +using autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_auto_perception_msgs::msg::PredictedObjects; + +std::uint8_t getHighestProbLabel(const std::vector & classification); + +/** + * @brief Specifies which object class should be checked. + */ +struct ObjectTypesToCheck +{ + bool check_car{true}; ///< Check for cars. + bool check_truck{true}; ///< Check for trucks. + bool check_bus{true}; ///< Check for buses. + bool check_trailer{true}; ///< Check for trailers. + bool check_unknown{true}; ///< Check for unknown object types. + bool check_bicycle{true}; ///< Check for bicycles. + bool check_motorcycle{true}; ///< Check for motorcycles. + bool check_pedestrian{true}; ///< Check for pedestrians. +}; + +/** + * @brief Determines whether the predicted object type matches any of the target object types + * specified by the user. + * + * @param object The predicted object whose type is to be checked. + * @param target_object_types A structure containing boolean flags for each object type that the + * user is interested in checking. + * + * @return Returns true if the predicted object's highest probability label matches any of the + * specified target object types. + */ +bool isTargetObjectType( + const PredictedObject & object, const ObjectTypesToCheck & target_object_types); + +/** + * @brief Filters objects in the 'selected' container based on the provided filter function. + * + * This function partitions the 'selected' container based on the 'filter' function + * and moves objects that satisfy the filter condition to the 'removed' container. + * + * @tparam Func The type of the filter function. + * @param selected [in,out] The container of objects to be filtered. + * @param removed [out] The container where objects not satisfying the filter condition are moved. + * @param filter The filter function that determines whether an object should be removed. + */ +template +void filterObjects(PredictedObjects & selected, PredictedObjects & removed, Func filter) +{ + auto partitioned = std::partition(selected.objects.begin(), selected.objects.end(), filter); + removed.objects.insert(removed.objects.end(), partitioned, selected.objects.end()); + selected.objects.erase(partitioned, selected.objects.end()); +} + +/** + * @brief Filters objects in the 'objects' container based on the provided filter function. + * + * This function is an overload that simplifies filtering when you don't need to specify a + * separate 'removed' container. It internally creates a 'removed_objects' container and calls the + * main 'filterObjects' function. + * + * @tparam Func The type of the filter function. + * @param objects [in,out] The container of objects to be filtered. + * @param filter The filter function that determines whether an object should be removed. + */ +template +void filterObjects(PredictedObjects & objects, Func filter) +{ + [[maybe_unused]] PredictedObjects removed_objects{}; + filterObjects(objects, removed_objects, filter); +} + +/** + * @brief Filters the provided objects based on their classification. + * + * @param objects The predicted objects to be filtered. + * @param target_object_types The types of objects to retain after filtering. + */ +void filterObjectsByClass( + PredictedObjects & objects, const ObjectTypesToCheck & target_object_types); + +/** + * @brief Filters the provided objects based on their classification. + * + * @param objects The predicted objects to be filtered. + * @param params The parameters for each object class. + */ +void filterDeviationCheckObjects( + PredictedObjects & objects, const std::unordered_map & params); + +} // namespace perception_diagnostics + +#endif // PERCEPTION_EVALUATOR__UTILS__OBJECTS_FILTERING_HPP_ diff --git a/evaluator/perception_evaluator/launch/motion_evaluator.launch.xml b/evaluator/perception_evaluator/launch/motion_evaluator.launch.xml new file mode 100644 index 0000000000000..3e499638f9480 --- /dev/null +++ b/evaluator/perception_evaluator/launch/motion_evaluator.launch.xml @@ -0,0 +1,7 @@ + + + + + + + diff --git a/evaluator/perception_evaluator/launch/perception_evaluator.launch.xml b/evaluator/perception_evaluator/launch/perception_evaluator.launch.xml new file mode 100644 index 0000000000000..eaa882b346485 --- /dev/null +++ b/evaluator/perception_evaluator/launch/perception_evaluator.launch.xml @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/evaluator/perception_evaluator/package.xml b/evaluator/perception_evaluator/package.xml new file mode 100644 index 0000000000000..7a173caed1947 --- /dev/null +++ b/evaluator/perception_evaluator/package.xml @@ -0,0 +1,42 @@ + + + + perception_evaluator + 0.1.0 + ROS 2 node for evaluating perception + Fumiya Watanabe + Kosuke Takeuchi + Kotaro Uetake + Kyoichi Sugahara + Shunsuke Miura + Yoshi Ri + Apache License 2.0 + + Kosuke Takeuchi + + ament_cmake_auto + autoware_cmake + + autoware_perception_msgs + diagnostic_msgs + eigen + geometry_msgs + libgoogle-glog-dev + motion_utils + nav_msgs + pluginlib + rclcpp + rclcpp_components + tf2 + tf2_ros + tier4_autoware_utils + vehicle_info_util + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/evaluator/perception_evaluator/param/perception_evaluator.defaults.yaml b/evaluator/perception_evaluator/param/perception_evaluator.defaults.yaml new file mode 100644 index 0000000000000..6749bac81aa35 --- /dev/null +++ b/evaluator/perception_evaluator/param/perception_evaluator.defaults.yaml @@ -0,0 +1,39 @@ +/**: + ros__parameters: + selected_metrics: + - lateral_deviation + - yaw_deviation + - predicted_path_deviation + + # this should be an odd number, because it includes the target point + smoothing_window_size: 11 + + prediction_time_horizons: [1.0, 2.0, 3.0, 5.0] + + target_object: + car: + check_deviation: true + truck: + check_deviation: true + bus: + check_deviation: true + trailer: + check_deviation: true + bicycle: + check_deviation: true + motorcycle: + check_deviation: true + pedestrian: + check_deviation: true + unknown: + check_deviation: false + + debug_marker: + history_path: false + history_path_arrows: false + smoothed_history_path: true + smoothed_history_path_arrows: false + predicted_path: true + predicted_path_gt: true + deviation_lines: true + object_polygon: true diff --git a/evaluator/perception_evaluator/src/metrics/deviation_metrics.cpp b/evaluator/perception_evaluator/src/metrics/deviation_metrics.cpp new file mode 100644 index 0000000000000..fd5fe0bccb134 --- /dev/null +++ b/evaluator/perception_evaluator/src/metrics/deviation_metrics.cpp @@ -0,0 +1,65 @@ +// 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 "perception_evaluator/metrics/deviation_metrics.hpp" + +#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "tier4_autoware_utils/geometry/pose_deviation.hpp" + +#include + +namespace perception_diagnostics +{ +namespace metrics +{ + +double calcLateralDeviation(const std::vector & ref_path, const Pose & target_pose) +{ + if (ref_path.empty()) { + return 0.0; + } + + const size_t nearest_index = motion_utils::findNearestIndex(ref_path, target_pose.position); + return std::abs( + tier4_autoware_utils::calcLateralDeviation(ref_path[nearest_index], target_pose.position)); +} + +double calcYawDeviation(const std::vector & ref_path, const Pose & target_pose) +{ + if (ref_path.empty()) { + return 0.0; + } + + const size_t nearest_index = motion_utils::findNearestIndex(ref_path, target_pose.position); + return std::abs(tier4_autoware_utils::calcYawDeviation(ref_path[nearest_index], target_pose)); +} + +std::vector calcPredictedPathDeviation( + const std::vector & ref_path, const PredictedPath & pred_path) +{ + std::vector deviations; + + if (ref_path.empty() || pred_path.path.empty()) { + return {}; + } + for (const Pose & p : pred_path.path) { + const size_t nearest_index = motion_utils::findNearestIndex(ref_path, p.position); + deviations.push_back( + tier4_autoware_utils::calcDistance2d(ref_path[nearest_index].position, p.position)); + } + + return deviations; +} +} // namespace metrics +} // namespace perception_diagnostics diff --git a/evaluator/perception_evaluator/src/metrics_calculator.cpp b/evaluator/perception_evaluator/src/metrics_calculator.cpp new file mode 100644 index 0000000000000..739c455d3ff2f --- /dev/null +++ b/evaluator/perception_evaluator/src/metrics_calculator.cpp @@ -0,0 +1,458 @@ +// 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 "perception_evaluator/metrics_calculator.hpp" + +#include "motion_utils/trajectory/trajectory.hpp" +#include "perception_evaluator/utils/objects_filtering.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" + +#include + +namespace perception_diagnostics +{ +std::optional MetricsCalculator::calculate(const Metric & metric) const +{ + if (object_map_.empty()) { + return {}; + } + + // time delay is max element of parameters_->prediction_time_horizons + const double time_delay = getTimeDelay(); + const auto target_stamp = current_stamp_ - rclcpp::Duration::from_seconds(time_delay); + if (!hasPassedTime(target_stamp)) { + return {}; + } + const auto target_objects = getObjectsByStamp(target_stamp); + + switch (metric) { + case Metric::lateral_deviation: + return calcLateralDeviationMetrics(target_objects); + case Metric::yaw_deviation: + return calcYawDeviationMetrics(target_objects); + case Metric::predicted_path_deviation: + return calcPredictedPathDeviationMetrics(target_objects); + default: + return {}; + } +} + +double MetricsCalculator::getTimeDelay() const +{ + const auto max_element_it = std::max_element( + parameters_->prediction_time_horizons.begin(), parameters_->prediction_time_horizons.end()); + if (max_element_it != parameters_->prediction_time_horizons.end()) { + return *max_element_it; + } + throw std::runtime_error("prediction_time_horizons is empty"); +} + +bool MetricsCalculator::hasPassedTime(const std::string uuid, const rclcpp::Time stamp) const +{ + if (object_map_.find(uuid) == object_map_.end()) { + return false; + } + const auto oldest_stamp = object_map_.at(uuid).begin()->first; + return oldest_stamp <= stamp; +} + +bool MetricsCalculator::hasPassedTime(const rclcpp::Time stamp) const +{ + std::vector timestamps; + for (const auto & [uuid, stamp_and_objects] : object_map_) { + if (!stamp_and_objects.empty()) { + timestamps.push_back(stamp_and_objects.begin()->first); + } + } + + auto it = std::min_element(timestamps.begin(), timestamps.end()); + if (it != timestamps.end()) { + rclcpp::Time oldest_stamp = *it; + if (oldest_stamp > stamp) { + return false; + } + } + return true; +} + +rclcpp::Time MetricsCalculator::getClosestStamp(const rclcpp::Time stamp) const +{ + rclcpp::Time closest_stamp; + rclcpp::Duration min_duration = + rclcpp::Duration::from_nanoseconds(std::numeric_limits::max()); + + for (const auto & [uuid, stamp_and_objects] : object_map_) { + if (!stamp_and_objects.empty()) { + auto it = std::lower_bound( + stamp_and_objects.begin(), stamp_and_objects.end(), stamp, + [](const auto & pair, const rclcpp::Time & val) { return pair.first < val; }); + + // check the upper bound + if (it != stamp_and_objects.end()) { + const auto duration = it->first - stamp; + if (std::abs(duration.nanoseconds()) < min_duration.nanoseconds()) { + min_duration = duration; + closest_stamp = it->first; + } + } + + // check the lower bound (if it is not the first element) + if (it != stamp_and_objects.begin()) { + const auto prev_it = std::prev(it); + const auto duration = stamp - prev_it->first; + if (std::abs(duration.nanoseconds()) < min_duration.nanoseconds()) { + min_duration = duration; + closest_stamp = prev_it->first; + } + } + } + } + + return closest_stamp; +} + +std::optional MetricsCalculator::getObjectByStamp( + const std::string uuid, const rclcpp::Time stamp) const +{ + const auto closest_stamp = getClosestStamp(stamp); + auto it = std::lower_bound( + object_map_.at(uuid).begin(), object_map_.at(uuid).end(), closest_stamp, + [](const auto & pair, const rclcpp::Time & val) { return pair.first < val; }); + + if (it != object_map_.at(uuid).end() && it->first == closest_stamp) { + return it->second; + } + return std::nullopt; +} + +PredictedObjects MetricsCalculator::getObjectsByStamp(const rclcpp::Time stamp) const +{ + const auto closest_stamp = getClosestStamp(stamp); + + PredictedObjects objects; + objects.header.stamp = stamp; + for (const auto & [uuid, stamp_and_objects] : object_map_) { + auto it = std::lower_bound( + stamp_and_objects.begin(), stamp_and_objects.end(), closest_stamp, + [](const auto & pair, const rclcpp::Time & val) { return pair.first < val; }); + + // add the object only if the element pointed to by lower_bound is equal to stamp + if (it != stamp_and_objects.end() && it->first == closest_stamp) { + objects.objects.push_back(it->second); + } + } + return objects; +} + +MetricStatMap MetricsCalculator::calcLateralDeviationMetrics(const PredictedObjects & objects) const +{ + Stat stat{}; + const auto stamp = rclcpp::Time(objects.header.stamp); + + for (const auto & object : objects.objects) { + const auto uuid = tier4_autoware_utils::toHexString(object.object_id); + if (!hasPassedTime(uuid, stamp)) { + continue; + } + const auto object_pose = object.kinematics.initial_pose_with_covariance.pose; + const auto history_path = history_path_map_.at(uuid).second; + if (history_path.empty()) { + continue; + } + stat.add(metrics::calcLateralDeviation(history_path, object_pose)); + } + + MetricStatMap metric_stat_map; + metric_stat_map["lateral_deviation"] = stat; + return metric_stat_map; +} + +MetricStatMap MetricsCalculator::calcYawDeviationMetrics(const PredictedObjects & objects) const +{ + Stat stat{}; + const auto stamp = rclcpp::Time(objects.header.stamp); + for (const auto & object : objects.objects) { + const auto uuid = tier4_autoware_utils::toHexString(object.object_id); + if (!hasPassedTime(uuid, stamp)) { + continue; + } + const auto object_pose = object.kinematics.initial_pose_with_covariance.pose; + const auto history_path = history_path_map_.at(uuid).second; + if (history_path.empty()) { + continue; + } + stat.add(metrics::calcYawDeviation(history_path, object_pose)); + } + + MetricStatMap metric_stat_map; + metric_stat_map["yaw_deviation"] = stat; + return metric_stat_map; +} + +MetricStatMap MetricsCalculator::calcPredictedPathDeviationMetrics( + const PredictedObjects & objects) const +{ + const auto time_horizons = parameters_->prediction_time_horizons; + + MetricStatMap metric_stat_map; + for (const double time_horizon : time_horizons) { + const auto stat = calcPredictedPathDeviationMetrics(objects, time_horizon); + std::ostringstream stream; + stream << std::fixed << std::setprecision(2) << time_horizon; + std::string time_horizon_str = stream.str(); + metric_stat_map["predicted_path_deviation_" + time_horizon_str] = stat; + } + + return metric_stat_map; +} + +Stat MetricsCalculator::calcPredictedPathDeviationMetrics( + const PredictedObjects & objects, const double time_horizon) const +{ + // For each object, select the predicted path that is closest to the history path and store the + // distance to the history path + std::unordered_map>> + deviation_map_for_paths; + // For debugging. Save the pairs of predicted path pose and history path pose. + // Visualize the correspondence in rviz from the node. + std::unordered_map>> + debug_predicted_path_pairs_map; + + // Find the corresponding pose in the history path for each pose of the predicted path of each + // object, and record the distances + const auto stamp = objects.header.stamp; + for (const auto & object : objects.objects) { + const auto uuid = tier4_autoware_utils::toHexString(object.object_id); + const auto predicted_paths = object.kinematics.predicted_paths; + for (size_t i = 0; i < predicted_paths.size(); i++) { + const auto predicted_path = predicted_paths[i]; + const std::string path_id = uuid + "_" + std::to_string(i); + for (size_t j = 0; j < predicted_path.path.size(); j++) { + const double time_duration = + rclcpp::Duration(predicted_path.time_step).seconds() * static_cast(j); + if (time_duration > time_horizon) { + break; + } + const rclcpp::Time target_stamp = + rclcpp::Time(stamp) + rclcpp::Duration::from_seconds(time_duration); + if (!hasPassedTime(uuid, target_stamp)) { + continue; + } + const auto history_object_opt = getObjectByStamp(uuid, target_stamp); + if (!history_object_opt.has_value()) { + continue; + } + const auto history_object = history_object_opt.value(); + const auto history_pose = history_object.kinematics.initial_pose_with_covariance.pose; + const Pose & p = predicted_path.path[j]; + const double distance = + tier4_autoware_utils::calcDistance2d(p.position, history_pose.position); + deviation_map_for_paths[uuid][i].push_back(distance); + // debug + debug_predicted_path_pairs_map[path_id].push_back(std::make_pair(p, history_pose)); + } + } + } + + // Select the predicted path with the smallest deviation for each object + std::unordered_map> deviation_map_for_objects; + for (const auto & [uuid, deviation_map] : deviation_map_for_paths) { + size_t min_deviation_index = 0; + double min_sum_deviation = std::numeric_limits::max(); + for (const auto & [i, deviations] : deviation_map) { + if (deviations.empty()) { + continue; + } + const double sum = std::accumulate(deviations.begin(), deviations.end(), 0.0); + if (sum < min_sum_deviation) { + min_sum_deviation = sum; + min_deviation_index = i; + } + } + deviation_map_for_objects[uuid] = deviation_map.at(min_deviation_index); + + // debug: save the delayed target object and the corresponding predicted path + const auto path_id = uuid + "_" + std::to_string(min_deviation_index); + const auto target_stamp_object = getObjectByStamp(uuid, stamp); + if (target_stamp_object.has_value()) { + ObjectData object_data; + object_data.object = target_stamp_object.value(); + object_data.path_pairs = debug_predicted_path_pairs_map[path_id]; + debug_target_object_[uuid] = object_data; + } + } + + // Store the deviation as a metric + Stat stat; + for (const auto & [uuid, deviations] : deviation_map_for_objects) { + if (deviations.empty()) { + continue; + } + for (const auto & deviation : deviations) { + stat.add(deviation); + } + } + return stat; +} + +void MetricsCalculator::setPredictedObjects(const PredictedObjects & objects) +{ + // using TimeStamp = builtin_interfaces::msg::Time; + current_stamp_ = objects.header.stamp; + + // store objects to check deviation + { + auto deviation_check_objects = objects; + filterDeviationCheckObjects(deviation_check_objects, parameters_->object_parameters); + using tier4_autoware_utils::toHexString; + for (const auto & object : deviation_check_objects.objects) { + std::string uuid = toHexString(object.object_id); + updateObjects(uuid, current_stamp_, object); + } + deleteOldObjects(current_stamp_); + updateHistoryPath(); + } +} + +void MetricsCalculator::deleteOldObjects(const rclcpp::Time stamp) +{ + // delete the data older than 2*time_delay_ + const double time_delay = getTimeDelay(); + for (auto & [uuid, stamp_and_objects] : object_map_) { + for (auto it = stamp_and_objects.begin(); it != stamp_and_objects.end();) { + if (it->first < stamp - rclcpp::Duration::from_seconds(time_delay * 2)) { + it = stamp_and_objects.erase(it); + } else { + break; + } + } + } + + const auto object_map = object_map_; + for (const auto & [uuid, stamp_and_objects] : object_map) { + if (stamp_and_objects.empty()) { + object_map_.erase(uuid); + history_path_map_.erase(uuid); + debug_target_object_.erase(uuid); // debug + } + } +} + +void MetricsCalculator::updateObjects( + const std::string uuid, const rclcpp::Time stamp, const PredictedObject & object) +{ + object_map_[uuid][stamp] = object; +} + +void MetricsCalculator::updateHistoryPath() +{ + const double window_size = parameters_->smoothing_window_size; + + for (const auto & [uuid, stamp_and_objects] : object_map_) { + std::vector history_path; + for (const auto & [stamp, object] : stamp_and_objects) { + history_path.push_back(object.kinematics.initial_pose_with_covariance.pose); + } + + // pair of history_path(raw) and smoothed_history_path + // history_path(raw) is just for debugging + history_path_map_[uuid] = + std::make_pair(history_path, averageFilterPath(history_path, window_size)); + } +} + +std::vector MetricsCalculator::generateHistoryPathWithPrev( + const std::vector & prev_history_path, const Pose & new_pose, const size_t window_size) +{ + std::vector history_path; + const size_t half_window_size = static_cast(window_size / 2); + history_path.insert( + history_path.end(), prev_history_path.begin(), prev_history_path.end() - half_window_size); + + std::vector updated_poses; + updated_poses.insert( + updated_poses.end(), prev_history_path.end() - half_window_size * 2, prev_history_path.end()); + updated_poses.push_back(new_pose); + + updated_poses = averageFilterPath(updated_poses, window_size); + history_path.insert( + history_path.end(), updated_poses.begin() + half_window_size, updated_poses.end()); + return history_path; +} + +std::vector MetricsCalculator::averageFilterPath( + const std::vector & path, const size_t window_size) const +{ + using tier4_autoware_utils::calcAzimuthAngle; + using tier4_autoware_utils::calcDistance2d; + using tier4_autoware_utils::createQuaternionFromYaw; + + std::vector filtered_path; + filtered_path.reserve(path.size()); // Reserve space to avoid reallocations + + const size_t half_window = static_cast(window_size / 2); + + // Calculate the moving average for positions + for (size_t i = 0; i < path.size(); ++i) { + double sum_x = 0.0, sum_y = 0.0, sum_z = 0.0; + size_t valid_points = 0; // Correctly initialize and use as counter + + for (size_t j = std::max(i - half_window, static_cast(0)); + j <= std::min(i + half_window, path.size() - 1); ++j) { + sum_x += path[j].position.x; + sum_y += path[j].position.y; + sum_z += path[j].position.z; + ++valid_points; + } + + Pose average_pose; + if (valid_points > 0) { // Prevent division by zero + average_pose.position.x = sum_x / valid_points; + average_pose.position.y = sum_y / valid_points; + average_pose.position.z = sum_z / valid_points; + } else { + average_pose.position = path.at(i).position; + } + + // Placeholder for orientation to ensure structure integrity + average_pose.orientation = path.at(i).orientation; + filtered_path.push_back(average_pose); + } + + // Calculate yaw and convert to quaternion after averaging positions + for (size_t i = 0; i < filtered_path.size(); ++i) { + Pose & p = filtered_path[i]; + + // if the current pose is too close to the previous one, use the previous orientation + if (i > 0) { + const Pose & p_prev = filtered_path[i - 1]; + if (calcDistance2d(p_prev.position, p.position) < 0.1) { + p.orientation = p_prev.orientation; + continue; + } + } + + if (i < filtered_path.size() - 1) { + const double yaw = calcAzimuthAngle(p.position, filtered_path[i + 1].position); + filtered_path[i].orientation = createQuaternionFromYaw(yaw); + } else if (filtered_path.size() > 1) { + // For the last point, use the orientation of the second-to-last point + p.orientation = filtered_path[i - 1].orientation; + } + } + + return filtered_path; +} + +} // namespace perception_diagnostics diff --git a/evaluator/perception_evaluator/src/perception_evaluator_node.cpp b/evaluator/perception_evaluator/src/perception_evaluator_node.cpp new file mode 100644 index 0000000000000..514f5caab3792 --- /dev/null +++ b/evaluator/perception_evaluator/src/perception_evaluator_node.cpp @@ -0,0 +1,348 @@ +// 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 "perception_evaluator/perception_evaluator_node.hpp" + +#include "perception_evaluator/utils/marker_utils.hpp" +#include "tier4_autoware_utils/ros/marker_helper.hpp" +#include "tier4_autoware_utils/ros/parameter.hpp" +#include "tier4_autoware_utils/ros/update_param.hpp" + +#include + +#include "boost/lexical_cast.hpp" + +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace perception_diagnostics +{ +PerceptionEvaluatorNode::PerceptionEvaluatorNode(const rclcpp::NodeOptions & node_options) +: Node("perception_evaluator", node_options), + parameters_(std::make_shared()), + metrics_calculator_(parameters_) +{ + using std::placeholders::_1; + + google::InitGoogleLogging("map_based_prediction_node"); + google::InstallFailureSignalHandler(); + + objects_sub_ = create_subscription( + "~/input/objects", 1, std::bind(&PerceptionEvaluatorNode::onObjects, this, _1)); + metrics_pub_ = create_publisher("~/metrics", 1); + pub_marker_ = create_publisher("~/markers", 10); + + tf_buffer_ = std::make_unique(this->get_clock()); + transform_listener_ = std::make_shared(*tf_buffer_); + + // Parameters + initParameter(); + + set_param_res_ = this->add_on_set_parameters_callback( + std::bind(&PerceptionEvaluatorNode::onParameter, this, std::placeholders::_1)); + + // Timer + initTimer(/*period_s=*/0.1); +} + +PerceptionEvaluatorNode::~PerceptionEvaluatorNode() +{ +} + +void PerceptionEvaluatorNode::initTimer(double period_s) +{ + const auto period_ns = + std::chrono::duration_cast(std::chrono::duration(period_s)); + timer_ = rclcpp::create_timer( + this, get_clock(), period_ns, std::bind(&PerceptionEvaluatorNode::onTimer, this)); +} + +void PerceptionEvaluatorNode::onTimer() +{ + DiagnosticArray metrics_msg; + for (const Metric & metric : parameters_->metrics) { + const auto metric_stat_map = metrics_calculator_.calculate(Metric(metric)); + if (!metric_stat_map.has_value()) { + continue; + } + + for (const auto & [metric, stat] : metric_stat_map.value()) { + if (stat.count() > 0) { + metrics_msg.status.push_back(generateDiagnosticStatus(metric, stat)); + } + } + } + + if (!metrics_msg.status.empty()) { + metrics_msg.header.stamp = now(); + metrics_pub_->publish(metrics_msg); + } + publishDebugMarker(); +} + +DiagnosticStatus PerceptionEvaluatorNode::generateDiagnosticStatus( + const std::string metric, const Stat & metric_stat) const +{ + DiagnosticStatus status; + + status.level = status.OK; + status.name = metric; + + diagnostic_msgs::msg::KeyValue key_value; + key_value.key = "min"; + key_value.value = std::to_string(metric_stat.min()); + status.values.push_back(key_value); + key_value.key = "max"; + key_value.value = std::to_string(metric_stat.max()); + status.values.push_back(key_value); + key_value.key = "mean"; + key_value.value = std::to_string(metric_stat.mean()); + status.values.push_back(key_value); + + return status; +} + +void PerceptionEvaluatorNode::onObjects(const PredictedObjects::ConstSharedPtr objects_msg) +{ + metrics_calculator_.setPredictedObjects(*objects_msg); +} + +void PerceptionEvaluatorNode::publishDebugMarker() +{ + using marker_utils::createColorFromString; + using marker_utils::createDeviationLines; + using marker_utils::createObjectPolygonMarkerArray; + using marker_utils::createPointsMarkerArray; + using marker_utils::createPosesMarkerArray; + + MarkerArray marker; + + const auto add = [&marker](MarkerArray added) { + for (auto & marker : added.markers) { + marker.lifetime = rclcpp::Duration::from_seconds(1.5); + } + tier4_autoware_utils::appendMarkerArray(added, &marker); + }; + + const auto history_path_map = metrics_calculator_.getHistoryPathMap(); + const auto & p = parameters_->debug_marker_parameters; + + for (const auto & [uuid, history_path] : history_path_map) { + { + const auto c = createColorFromString(uuid + "_raw"); + if (p.show_history_path) { + add(createPointsMarkerArray(history_path.first, "history_path_" + uuid, 0, c.r, c.g, c.b)); + } + if (p.show_history_path_arrows) { + add(createPosesMarkerArray( + history_path.first, "history_path_arrows_" + uuid, 0, c.r, c.g, c.b, 0.1, 0.05, 0.05)); + } + } + { + const auto c = createColorFromString(uuid); + if (p.show_smoothed_history_path) { + add(createPointsMarkerArray( + history_path.second, "smoothed_history_path_" + uuid, 0, c.r, c.g, c.b)); + } + if (p.show_smoothed_history_path_arrows) { + add(createPosesMarkerArray( + history_path.second, "smoothed_history_path_arrows_" + uuid, c.r, c.g, c.b, 0, 0.1, 0.05, + 0.05)); + } + } + } + const auto object_data_map = metrics_calculator_.getDebugObjectData(); + for (const auto & [uuid, object_data] : object_data_map) { + const auto c = createColorFromString(uuid); + const auto predicted_path = object_data.getPredictedPath(); + const auto history_path = object_data.getHistoryPath(); + if (p.show_predicted_path) { + add(createPosesMarkerArray(predicted_path, "predicted_path_" + uuid, 0, 0, 0, 1)); + } + if (p.show_predicted_path_gt) { + add(createPosesMarkerArray(history_path, "predicted_path_gt_" + uuid, 0, 1, 0, 0)); + } + if (p.show_deviation_lines) { + add( + createDeviationLines(predicted_path, history_path, "deviation_lines_" + uuid, 0, 1, 1, 1)); + } + if (p.show_object_polygon) { + add(createObjectPolygonMarkerArray( + object_data.object, "object_polygon_" + uuid, 0, c.r, c.g, c.b)); + } + } + + pub_marker_->publish(marker); +} + +rcl_interfaces::msg::SetParametersResult PerceptionEvaluatorNode::onParameter( + const std::vector & parameters) +{ + using tier4_autoware_utils::updateParam; + + auto & p = parameters_; + + updateParam(parameters, "smoothing_window_size", p->smoothing_window_size); + + // update metrics + { + std::vector metrics_str; + updateParam>(parameters, "selected_metrics", metrics_str); + std::vector metrics; + for (const std::string & selected_metric : metrics_str) { + const Metric metric = str_to_metric.at(selected_metric); + metrics.push_back(metric); + } + p->metrics = metrics; + } + + // update parameters for each object class + { + const auto get_object_param = [&](std::string && ns) -> std::optional { + ObjectParameter param{}; + if (updateParam(parameters, ns + "check_deviation", param.check_deviation)) { + return param; + } + return std::nullopt; + }; + + const std::string ns = "target_object."; + if (const auto new_param = get_object_param(ns + "car.")) { + p->object_parameters.at(ObjectClassification::CAR) = *new_param; + } + if (const auto new_param = get_object_param(ns + "truck.")) { + p->object_parameters.at(ObjectClassification::TRUCK) = *new_param; + } + if (const auto new_param = get_object_param(ns + "bus.")) { + p->object_parameters.at(ObjectClassification::BUS) = *new_param; + } + if (const auto new_param = get_object_param(ns + "trailer.")) { + p->object_parameters.at(ObjectClassification::TRAILER) = *new_param; + } + if (const auto new_param = get_object_param(ns + "bicycle.")) { + p->object_parameters.at(ObjectClassification::BICYCLE) = *new_param; + } + if (const auto new_param = get_object_param(ns + "motorcycle.")) { + p->object_parameters.at(ObjectClassification::MOTORCYCLE) = *new_param; + } + if (const auto new_param = get_object_param(ns + "pedestrian.")) { + p->object_parameters.at(ObjectClassification::PEDESTRIAN) = *new_param; + } + if (const auto new_param = get_object_param(ns + "unknown.")) { + p->object_parameters.at(ObjectClassification::UNKNOWN) = *new_param; + } + } + + // update debug marker parameters + { + const std::string ns = "debug_marker."; + updateParam( + parameters, ns + "history_path", p->debug_marker_parameters.show_history_path); + updateParam( + parameters, ns + "history_path_arrows", p->debug_marker_parameters.show_history_path_arrows); + updateParam( + parameters, ns + "smoothed_history_path", + p->debug_marker_parameters.show_smoothed_history_path); + updateParam( + parameters, ns + "smoothed_history_path_arrows", + p->debug_marker_parameters.show_smoothed_history_path_arrows); + updateParam( + parameters, ns + "predicted_path", p->debug_marker_parameters.show_predicted_path); + updateParam( + parameters, ns + "predicted_path_gt", p->debug_marker_parameters.show_predicted_path_gt); + updateParam( + parameters, ns + "deviation_lines", p->debug_marker_parameters.show_deviation_lines); + updateParam( + parameters, ns + "object_polygon", p->debug_marker_parameters.show_object_polygon); + } + + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + result.reason = "success"; + + return result; +} + +void PerceptionEvaluatorNode::initParameter() +{ + using tier4_autoware_utils::getOrDeclareParameter; + using tier4_autoware_utils::updateParam; + + auto & p = parameters_; + + p->smoothing_window_size = getOrDeclareParameter(*this, "smoothing_window_size"); + p->prediction_time_horizons = + getOrDeclareParameter>(*this, "prediction_time_horizons"); + + // set metrics + const auto selected_metrics = + getOrDeclareParameter>(*this, "selected_metrics"); + for (const std::string & selected_metric : selected_metrics) { + const Metric metric = str_to_metric.at(selected_metric); + parameters_->metrics.push_back(metric); + } + + // set parameters for each object class + { + const auto get_object_param = [&](std::string && ns) -> ObjectParameter { + ObjectParameter param{}; + param.check_deviation = getOrDeclareParameter(*this, ns + "check_deviation"); + return param; + }; + + const std::string ns = "target_object."; + p->object_parameters.emplace(ObjectClassification::CAR, get_object_param(ns + "car.")); + p->object_parameters.emplace(ObjectClassification::TRUCK, get_object_param(ns + "truck.")); + p->object_parameters.emplace(ObjectClassification::BUS, get_object_param(ns + "bus.")); + p->object_parameters.emplace(ObjectClassification::TRAILER, get_object_param(ns + "trailer.")); + p->object_parameters.emplace(ObjectClassification::BICYCLE, get_object_param(ns + "bicycle.")); + p->object_parameters.emplace( + ObjectClassification::MOTORCYCLE, get_object_param(ns + "motorcycle.")); + p->object_parameters.emplace( + ObjectClassification::PEDESTRIAN, get_object_param(ns + "pedestrian.")); + p->object_parameters.emplace(ObjectClassification::UNKNOWN, get_object_param(ns + "unknown.")); + } + + // set debug marker parameters + { + const std::string ns = "debug_marker."; + p->debug_marker_parameters.show_history_path = + getOrDeclareParameter(*this, ns + "history_path"); + p->debug_marker_parameters.show_history_path_arrows = + getOrDeclareParameter(*this, ns + "history_path_arrows"); + p->debug_marker_parameters.show_smoothed_history_path = + getOrDeclareParameter(*this, ns + "smoothed_history_path"); + p->debug_marker_parameters.show_smoothed_history_path_arrows = + getOrDeclareParameter(*this, ns + "smoothed_history_path_arrows"); + p->debug_marker_parameters.show_predicted_path = + getOrDeclareParameter(*this, ns + "predicted_path"); + p->debug_marker_parameters.show_predicted_path_gt = + getOrDeclareParameter(*this, ns + "predicted_path_gt"); + p->debug_marker_parameters.show_deviation_lines = + getOrDeclareParameter(*this, ns + "deviation_lines"); + p->debug_marker_parameters.show_object_polygon = + getOrDeclareParameter(*this, ns + "object_polygon"); + } +} +} // namespace perception_diagnostics + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(perception_diagnostics::PerceptionEvaluatorNode) diff --git a/evaluator/perception_evaluator/src/utils/marker_utils.cpp b/evaluator/perception_evaluator/src/utils/marker_utils.cpp new file mode 100644 index 0000000000000..b05792b474a55 --- /dev/null +++ b/evaluator/perception_evaluator/src/utils/marker_utils.cpp @@ -0,0 +1,198 @@ +// 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 "perception_evaluator/utils/marker_utils.hpp" + +#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" + +#include +#include +#include + +#include +#include +#include + +#include + +namespace marker_utils +{ +using std_msgs::msg::ColorRGBA; +using tier4_autoware_utils::calcOffsetPose; +using tier4_autoware_utils::createDefaultMarker; +using tier4_autoware_utils::createMarkerColor; +using tier4_autoware_utils::createMarkerOrientation; +using tier4_autoware_utils::createMarkerScale; +using tier4_autoware_utils::createPoint; +using visualization_msgs::msg::Marker; + +void addFootprintMarker( + visualization_msgs::msg::Marker & marker, const geometry_msgs::msg::Pose & pose, + const vehicle_info_util::VehicleInfo & vehicle_info) +{ + const double half_width = vehicle_info.vehicle_width_m / 2.0; + const double base_to_front = vehicle_info.vehicle_length_m - vehicle_info.rear_overhang_m; + const double base_to_rear = vehicle_info.rear_overhang_m; + + marker.points.push_back( + tier4_autoware_utils::calcOffsetPose(pose, base_to_front, -half_width, 0.0).position); + marker.points.push_back( + tier4_autoware_utils::calcOffsetPose(pose, base_to_front, half_width, 0.0).position); + marker.points.push_back( + tier4_autoware_utils::calcOffsetPose(pose, -base_to_rear, half_width, 0.0).position); + marker.points.push_back( + tier4_autoware_utils::calcOffsetPose(pose, -base_to_rear, -half_width, 0.0).position); + marker.points.push_back(marker.points.front()); +} + +MarkerArray createFootprintMarkerArray( + const Pose & base_link_pose, const vehicle_info_util::VehicleInfo vehicle_info, + const std::string && ns, const int32_t & id, const float & r, const float & g, const float & b) +{ + const auto current_time = rclcpp::Clock{RCL_ROS_TIME}.now(); + MarkerArray msg; + + Marker marker = createDefaultMarker( + "map", current_time, ns, id, Marker::LINE_STRIP, createMarkerScale(0.2, 0.2, 0.2), + createMarkerColor(r, g, b, 0.999)); + marker.lifetime = rclcpp::Duration::from_seconds(1.5); + + MarkerArray marker_array; + + addFootprintMarker(marker, base_link_pose, vehicle_info); + msg.markers.push_back(marker); + return msg; +} + +MarkerArray createPointsMarkerArray( + const std::vector points, const std::string & ns, const int32_t id, const float r, + const float g, const float b) +{ + auto marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, id, Marker::LINE_STRIP, + createMarkerScale(0.05, 0.0, 0.0), createMarkerColor(r, g, b, 0.999)); + + for (const auto & point : points) { + marker.points.push_back(point); + } + + MarkerArray msg; + msg.markers.push_back(marker); + return msg; +} + +MarkerArray createPointsMarkerArray( + const std::vector poses, const std::string & ns, const int32_t id, const float r, + const float g, const float b) +{ + auto marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, id, Marker::LINE_STRIP, + createMarkerScale(0.05, 0.0, 0.0), createMarkerColor(r, g, b, 0.999)); + + for (const auto & pose : poses) { + marker.points.push_back(pose.position); + } + + MarkerArray msg; + msg.markers.push_back(marker); + return msg; +} + +MarkerArray createDeviationLines( + const std::vector poses1, const std::vector poses2, const std::string & ns, + const int32_t id, const float r, const float g, const float b) +{ + MarkerArray msg; + + const size_t max_idx = std::min(poses1.size(), poses2.size()); + for (size_t i = 0; i < max_idx; ++i) { + auto marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, i, Marker::LINE_STRIP, + createMarkerScale(0.005, 0.0, 0.0), createMarkerColor(r, g, b, 0.5)); + marker.points.push_back(poses1.at(i).position); + marker.points.push_back(poses2.at(i).position); + msg.markers.push_back(marker); + } + + return msg; +} + +MarkerArray createPoseMarkerArray( + const Pose & pose, std::string && ns, const int32_t & id, const float & r, const float & g, + const float & b) +{ + MarkerArray msg; + + Marker marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, id, Marker::ARROW, + createMarkerScale(0.7, 0.3, 0.3), createMarkerColor(r, g, b, 0.999)); + marker.pose = pose; + msg.markers.push_back(marker); + + return msg; +} + +MarkerArray createPosesMarkerArray( + const std::vector poses, std::string && ns, const int32_t & id, const float & r, + const float & g, const float & b, const float & x_scale, const float & y_scale, + const float & z_scale) +{ + MarkerArray msg; + + for (size_t i = 0; i < poses.size(); ++i) { + Marker marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, id + i, Marker::ARROW, + createMarkerScale(x_scale, y_scale, z_scale), createMarkerColor(r, g, b, 0.5)); + marker.pose = poses.at(i); + msg.markers.push_back(marker); + } + + return msg; +} + +std_msgs::msg::ColorRGBA createColorFromString(const std::string & str) +{ + const auto hash = std::hash{}(str); + const auto r = (hash & 0xFF) / 255.0; + const auto g = ((hash >> 8) & 0xFF) / 255.0; + const auto b = ((hash >> 16) & 0xFF) / 255.0; + return tier4_autoware_utils::createMarkerColor(r, g, b, 0.5); +} + +MarkerArray createObjectPolygonMarkerArray( + const PredictedObject & object, std::string && ns, const int32_t & id, const float & r, + const float & g, const float & b) +{ + MarkerArray msg; + + auto marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, id, Marker::LINE_STRIP, + createMarkerScale(0.05, 0.0, 0.0), createMarkerColor(r, g, b, 0.999)); + + const double z = object.kinematics.initial_pose_with_covariance.pose.position.z; + const double height = object.shape.dimensions.z; + const auto polygon = tier4_autoware_utils::toPolygon2d( + object.kinematics.initial_pose_with_covariance.pose, object.shape); + for (const auto & p : polygon.outer()) { + marker.points.push_back(createPoint(p.x(), p.y(), z - height / 2)); + marker.points.push_back(createPoint(p.x(), p.y(), z + height / 2)); + } + marker.id = id; + msg.markers.push_back(marker); + + return msg; +} + +} // namespace marker_utils diff --git a/evaluator/perception_evaluator/src/utils/objects_filtering.cpp b/evaluator/perception_evaluator/src/utils/objects_filtering.cpp new file mode 100644 index 0000000000000..c2479ca5c28ef --- /dev/null +++ b/evaluator/perception_evaluator/src/utils/objects_filtering.cpp @@ -0,0 +1,103 @@ +// 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 "perception_evaluator/utils/objects_filtering.hpp" + +namespace perception_diagnostics +{ +ObjectTypesToCheck getDeviationCheckObjectTypes( + const std::unordered_map & params) +{ + ObjectTypesToCheck object_types_to_check; + for (const auto & [object_class, object_param] : params) { + switch (object_class) { + case ObjectClassification::CAR: + object_types_to_check.check_car = object_param.check_deviation; + break; + case ObjectClassification::TRUCK: + object_types_to_check.check_truck = object_param.check_deviation; + break; + case ObjectClassification::BUS: + object_types_to_check.check_bus = object_param.check_deviation; + break; + case ObjectClassification::TRAILER: + object_types_to_check.check_trailer = object_param.check_deviation; + break; + case ObjectClassification::BICYCLE: + object_types_to_check.check_bicycle = object_param.check_deviation; + break; + case ObjectClassification::MOTORCYCLE: + object_types_to_check.check_motorcycle = object_param.check_deviation; + break; + case ObjectClassification::PEDESTRIAN: + object_types_to_check.check_pedestrian = object_param.check_deviation; + break; + case ObjectClassification::UNKNOWN: + object_types_to_check.check_unknown = object_param.check_deviation; + break; + default: + break; + } + } + return object_types_to_check; +} + +std::uint8_t getHighestProbLabel(const std::vector & classification) +{ + std::uint8_t label = ObjectClassification::UNKNOWN; + float highest_prob = 0.0; + for (const auto & _class : classification) { + if (highest_prob < _class.probability) { + highest_prob = _class.probability; + label = _class.label; + } + } + + return label; +} + +bool isTargetObjectType( + const PredictedObject & object, const ObjectTypesToCheck & target_object_types) +{ + const auto t = getHighestProbLabel(object.classification); + + return ( + (t == ObjectClassification::CAR && target_object_types.check_car) || + (t == ObjectClassification::TRUCK && target_object_types.check_truck) || + (t == ObjectClassification::BUS && target_object_types.check_bus) || + (t == ObjectClassification::TRAILER && target_object_types.check_trailer) || + (t == ObjectClassification::UNKNOWN && target_object_types.check_unknown) || + (t == ObjectClassification::BICYCLE && target_object_types.check_bicycle) || + (t == ObjectClassification::MOTORCYCLE && target_object_types.check_motorcycle) || + (t == ObjectClassification::PEDESTRIAN && target_object_types.check_pedestrian)); +} + +void filterObjectsByClass( + PredictedObjects & objects, const ObjectTypesToCheck & target_object_types) +{ + const auto filter = [&](const auto & object) { + return isTargetObjectType(object, target_object_types); + }; + + filterObjects(objects, filter); +} + +void filterDeviationCheckObjects( + PredictedObjects & objects, const std::unordered_map & params) +{ + const auto object_types = getDeviationCheckObjectTypes(params); + filterObjectsByClass(objects, object_types); +} + +} // namespace perception_diagnostics diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index dbb74335f79d2..1be996e747039 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -247,4 +247,9 @@ + + + + + diff --git a/launch/tier4_simulator_launch/launch/simulator.launch.xml b/launch/tier4_simulator_launch/launch/simulator.launch.xml index 4da6720b0af47..679d350535c50 100644 --- a/launch/tier4_simulator_launch/launch/simulator.launch.xml +++ b/launch/tier4_simulator_launch/launch/simulator.launch.xml @@ -85,6 +85,11 @@ + + + + + From fd3682abc24e32690b7136fa32b60e79e1e4e4a7 Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Tue, 27 Feb 2024 13:44:07 +0900 Subject: [PATCH 2/8] fix build werror Signed-off-by: kosuke55 --- .../perception_evaluator/utils/marker_utils.hpp | 6 +++--- .../src/perception_evaluator_node.cpp | 11 +++++------ .../perception_evaluator/src/utils/marker_utils.cpp | 10 ++++------ 3 files changed, 12 insertions(+), 15 deletions(-) diff --git a/evaluator/perception_evaluator/include/perception_evaluator/utils/marker_utils.hpp b/evaluator/perception_evaluator/include/perception_evaluator/utils/marker_utils.hpp index 8fe3e3d690044..e128c380b7f36 100644 --- a/evaluator/perception_evaluator/include/perception_evaluator/utils/marker_utils.hpp +++ b/evaluator/perception_evaluator/include/perception_evaluator/utils/marker_utils.hpp @@ -63,8 +63,8 @@ MarkerArray createPoseMarkerArray( const float & b); MarkerArray createPosesMarkerArray( - const std::vector poses, std::string && ns, const int32_t & id, const float & r, - const float & g, const float & b, const float & x_scale = 0.5, const float & y_scale = 0.2, + const std::vector poses, std::string && ns, const float & r, const float & g, + const float & b, const float & x_scale = 0.5, const float & y_scale = 0.2, const float & z_scale = 0.2); std_msgs::msg::ColorRGBA createColorFromString(const std::string & str); @@ -75,7 +75,7 @@ MarkerArray createObjectPolygonMarkerArray( MarkerArray createDeviationLines( const std::vector poses1, const std::vector poses2, const std::string & ns, - const int32_t id, const float r, const float g, const float b); + const float r, const float g, const float b); } // namespace marker_utils diff --git a/evaluator/perception_evaluator/src/perception_evaluator_node.cpp b/evaluator/perception_evaluator/src/perception_evaluator_node.cpp index 514f5caab3792..a9bfbdc0ff004 100644 --- a/evaluator/perception_evaluator/src/perception_evaluator_node.cpp +++ b/evaluator/perception_evaluator/src/perception_evaluator_node.cpp @@ -153,7 +153,7 @@ void PerceptionEvaluatorNode::publishDebugMarker() } if (p.show_history_path_arrows) { add(createPosesMarkerArray( - history_path.first, "history_path_arrows_" + uuid, 0, c.r, c.g, c.b, 0.1, 0.05, 0.05)); + history_path.first, "history_path_arrows_" + uuid, c.r, c.g, c.b, 0.1, 0.05, 0.05)); } } { @@ -164,7 +164,7 @@ void PerceptionEvaluatorNode::publishDebugMarker() } if (p.show_smoothed_history_path_arrows) { add(createPosesMarkerArray( - history_path.second, "smoothed_history_path_arrows_" + uuid, c.r, c.g, c.b, 0, 0.1, 0.05, + history_path.second, "smoothed_history_path_arrows_" + uuid, c.r, c.g, c.b, 0.1, 0.05, 0.05)); } } @@ -175,14 +175,13 @@ void PerceptionEvaluatorNode::publishDebugMarker() const auto predicted_path = object_data.getPredictedPath(); const auto history_path = object_data.getHistoryPath(); if (p.show_predicted_path) { - add(createPosesMarkerArray(predicted_path, "predicted_path_" + uuid, 0, 0, 0, 1)); + add(createPosesMarkerArray(predicted_path, "predicted_path_" + uuid, 0, 0, 1)); } if (p.show_predicted_path_gt) { - add(createPosesMarkerArray(history_path, "predicted_path_gt_" + uuid, 0, 1, 0, 0)); + add(createPosesMarkerArray(history_path, "predicted_path_gt_" + uuid, 1, 0, 0)); } if (p.show_deviation_lines) { - add( - createDeviationLines(predicted_path, history_path, "deviation_lines_" + uuid, 0, 1, 1, 1)); + add(createDeviationLines(predicted_path, history_path, "deviation_lines_" + uuid, 1, 1, 1)); } if (p.show_object_polygon) { add(createObjectPolygonMarkerArray( diff --git a/evaluator/perception_evaluator/src/utils/marker_utils.cpp b/evaluator/perception_evaluator/src/utils/marker_utils.cpp index b05792b474a55..5d89425d1c864 100644 --- a/evaluator/perception_evaluator/src/utils/marker_utils.cpp +++ b/evaluator/perception_evaluator/src/utils/marker_utils.cpp @@ -17,7 +17,6 @@ #include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" -#include #include #include @@ -112,7 +111,7 @@ MarkerArray createPointsMarkerArray( MarkerArray createDeviationLines( const std::vector poses1, const std::vector poses2, const std::string & ns, - const int32_t id, const float r, const float g, const float b) + const float r, const float g, const float b) { MarkerArray msg; @@ -145,15 +144,14 @@ MarkerArray createPoseMarkerArray( } MarkerArray createPosesMarkerArray( - const std::vector poses, std::string && ns, const int32_t & id, const float & r, - const float & g, const float & b, const float & x_scale, const float & y_scale, - const float & z_scale) + const std::vector poses, std::string && ns, const float & r, const float & g, + const float & b, const float & x_scale, const float & y_scale, const float & z_scale) { MarkerArray msg; for (size_t i = 0; i < poses.size(); ++i) { Marker marker = createDefaultMarker( - "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, id + i, Marker::ARROW, + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, i, Marker::ARROW, createMarkerScale(x_scale, y_scale, z_scale), createMarkerColor(r, g, b, 0.5)); marker.pose = poses.at(i); msg.markers.push_back(marker); From 551f05520dacf7ba7601a8b129e9be9becade9fe Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Tue, 27 Feb 2024 13:55:05 +0900 Subject: [PATCH 3/8] fix topic name Signed-off-by: kosuke55 --- .../perception_evaluator/launch/perception_evaluator.launch.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/evaluator/perception_evaluator/launch/perception_evaluator.launch.xml b/evaluator/perception_evaluator/launch/perception_evaluator.launch.xml index eaa882b346485..7c05050a20b81 100644 --- a/evaluator/perception_evaluator/launch/perception_evaluator.launch.xml +++ b/evaluator/perception_evaluator/launch/perception_evaluator.launch.xml @@ -7,6 +7,7 @@ + From 5ec788f5a8f8a2150e1510a6b7c9c46ea3f97b49 Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Tue, 27 Feb 2024 18:22:38 +0900 Subject: [PATCH 4/8] clean up Signed-off-by: kosuke55 --- evaluator/perception_evaluator/CMakeLists.txt | 9 --------- 1 file changed, 9 deletions(-) diff --git a/evaluator/perception_evaluator/CMakeLists.txt b/evaluator/perception_evaluator/CMakeLists.txt index 6a787058a9df3..61043c88adcc9 100644 --- a/evaluator/perception_evaluator/CMakeLists.txt +++ b/evaluator/perception_evaluator/CMakeLists.txt @@ -28,15 +28,6 @@ rclcpp_components_register_node(${PROJECT_NAME}_node target_link_libraries(${PROJECT_NAME}_node glog::glog) -# if(BUILD_TESTING) -# ament_add_ros_isolated_gtest(test_${PROJECT_NAME} -# test/test_perception_evaluator_node.cpp -# ) -# target_link_libraries(test_${PROJECT_NAME} -# ${PROJECT_NAME}_node -# ) -# endif() - ament_auto_package( INSTALL_TO_SHARE param From ae84790fd448e4c594457a8880bd18044ee6dd99 Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Tue, 27 Feb 2024 20:08:03 +0900 Subject: [PATCH 5/8] rename to perception_online_evaluator Signed-off-by: kosuke55 --- .../launch/perception_evaluator.launch.xml | 13 -------- .../CMakeLists.txt | 4 +-- .../README.md | 0 .../metrics/deviation_metrics.hpp | 8 ++--- .../metrics/metric.hpp | 8 ++--- .../metrics_calculator.hpp | 14 ++++---- .../parameters.hpp | 8 ++--- .../perception_online_evaluator_node.hpp} | 18 +++++------ .../perception_online_evaluator}/stat.hpp | 6 ++-- .../utils/marker_utils.hpp | 6 ++-- .../utils/objects_filtering.hpp | 8 ++--- .../launch/motion_evaluator.launch.xml | 2 +- .../perception_online_evaluator.launch.xml | 13 ++++++++ .../package.xml | 2 +- ...perception_online_evaluator.defaults.yaml} | 0 .../src/metrics/deviation_metrics.cpp | 2 +- .../src/metrics_calculator.cpp | 4 +-- .../src/perception_online_evaluator_node.cpp} | 32 +++++++++---------- .../src/utils/marker_utils.cpp | 2 +- .../src/utils/objects_filtering.cpp | 2 +- .../launch/perception.launch.xml | 2 +- .../launch/simulator.launch.xml | 2 +- 22 files changed, 78 insertions(+), 78 deletions(-) delete mode 100644 evaluator/perception_evaluator/launch/perception_evaluator.launch.xml rename evaluator/{perception_evaluator => perception_online_evaluator}/CMakeLists.txt (87%) rename evaluator/{perception_evaluator => perception_online_evaluator}/README.md (100%) rename evaluator/{perception_evaluator/include/perception_evaluator => perception_online_evaluator/include/perception_online_evaluator}/metrics/deviation_metrics.hpp (86%) rename evaluator/{perception_evaluator/include/perception_evaluator => perception_online_evaluator/include/perception_online_evaluator}/metrics/metric.hpp (91%) rename evaluator/{perception_evaluator/include/perception_evaluator => perception_online_evaluator/include/perception_online_evaluator}/metrics_calculator.hpp (91%) rename evaluator/{perception_evaluator/include/perception_evaluator => perception_online_evaluator/include/perception_online_evaluator}/parameters.hpp (86%) rename evaluator/{perception_evaluator/include/perception_evaluator/perception_evaluator_node.hpp => perception_online_evaluator/include/perception_online_evaluator/perception_online_evaluator_node.hpp} (82%) rename evaluator/{perception_evaluator/include/perception_evaluator => perception_online_evaluator/include/perception_online_evaluator}/stat.hpp (93%) rename evaluator/{perception_evaluator/include/perception_evaluator => perception_online_evaluator/include/perception_online_evaluator}/utils/marker_utils.hpp (93%) rename evaluator/{perception_evaluator/include/perception_evaluator => perception_online_evaluator/include/perception_online_evaluator}/utils/objects_filtering.hpp (94%) rename evaluator/{perception_evaluator => perception_online_evaluator}/launch/motion_evaluator.launch.xml (83%) create mode 100644 evaluator/perception_online_evaluator/launch/perception_online_evaluator.launch.xml rename evaluator/{perception_evaluator => perception_online_evaluator}/package.xml (97%) rename evaluator/{perception_evaluator/param/perception_evaluator.defaults.yaml => perception_online_evaluator/param/perception_online_evaluator.defaults.yaml} (100%) rename evaluator/{perception_evaluator => perception_online_evaluator}/src/metrics/deviation_metrics.cpp (96%) rename evaluator/{perception_evaluator => perception_online_evaluator}/src/metrics_calculator.cpp (99%) rename evaluator/{perception_evaluator/src/perception_evaluator_node.cpp => perception_online_evaluator/src/perception_online_evaluator_node.cpp} (91%) rename evaluator/{perception_evaluator => perception_online_evaluator}/src/utils/marker_utils.cpp (99%) rename evaluator/{perception_evaluator => perception_online_evaluator}/src/utils/objects_filtering.cpp (98%) diff --git a/evaluator/perception_evaluator/launch/perception_evaluator.launch.xml b/evaluator/perception_evaluator/launch/perception_evaluator.launch.xml deleted file mode 100644 index 7c05050a20b81..0000000000000 --- a/evaluator/perception_evaluator/launch/perception_evaluator.launch.xml +++ /dev/null @@ -1,13 +0,0 @@ - - - - - - - - - - - - - diff --git a/evaluator/perception_evaluator/CMakeLists.txt b/evaluator/perception_online_evaluator/CMakeLists.txt similarity index 87% rename from evaluator/perception_evaluator/CMakeLists.txt rename to evaluator/perception_online_evaluator/CMakeLists.txt index 61043c88adcc9..8106f1e0b28b6 100644 --- a/evaluator/perception_evaluator/CMakeLists.txt +++ b/evaluator/perception_online_evaluator/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(perception_evaluator) +project(perception_online_evaluator) find_package(autoware_cmake REQUIRED) autoware_package() @@ -17,7 +17,7 @@ ament_auto_add_library(${PROJECT_NAME}_node SHARED ) rclcpp_components_register_node(${PROJECT_NAME}_node - PLUGIN "perception_diagnostics::PerceptionEvaluatorNode" + PLUGIN "perception_diagnostics::PerceptionOnlineEvaluatorNode" EXECUTABLE ${PROJECT_NAME} ) diff --git a/evaluator/perception_evaluator/README.md b/evaluator/perception_online_evaluator/README.md similarity index 100% rename from evaluator/perception_evaluator/README.md rename to evaluator/perception_online_evaluator/README.md diff --git a/evaluator/perception_evaluator/include/perception_evaluator/metrics/deviation_metrics.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/deviation_metrics.hpp similarity index 86% rename from evaluator/perception_evaluator/include/perception_evaluator/metrics/deviation_metrics.hpp rename to evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/deviation_metrics.hpp index 00255835ba547..da7a23b6980b6 100644 --- a/evaluator/perception_evaluator/include/perception_evaluator/metrics/deviation_metrics.hpp +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/deviation_metrics.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PERCEPTION_EVALUATOR__METRICS__DEVIATION_METRICS_HPP_ -#define PERCEPTION_EVALUATOR__METRICS__DEVIATION_METRICS_HPP_ +#ifndef PERCEPTION_ONLINE_EVALUATOR__METRICS__DEVIATION_METRICS_HPP_ +#define PERCEPTION_ONLINE_EVALUATOR__METRICS__DEVIATION_METRICS_HPP_ -#include "perception_evaluator/stat.hpp" +#include "perception_online_evaluator/stat.hpp" #include #include @@ -50,4 +50,4 @@ std::vector calcPredictedPathDeviation( } // namespace metrics } // namespace perception_diagnostics -#endif // PERCEPTION_EVALUATOR__METRICS__DEVIATION_METRICS_HPP_ +#endif // PERCEPTION_ONLINE_EVALUATOR__METRICS__DEVIATION_METRICS_HPP_ diff --git a/evaluator/perception_evaluator/include/perception_evaluator/metrics/metric.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/metric.hpp similarity index 91% rename from evaluator/perception_evaluator/include/perception_evaluator/metrics/metric.hpp rename to evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/metric.hpp index 94f9fd32e9828..8a2cddca476d4 100644 --- a/evaluator/perception_evaluator/include/perception_evaluator/metrics/metric.hpp +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/metric.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PERCEPTION_EVALUATOR__METRICS__METRIC_HPP_ -#define PERCEPTION_EVALUATOR__METRICS__METRIC_HPP_ +#ifndef PERCEPTION_ONLINE_EVALUATOR__METRICS__METRIC_HPP_ +#define PERCEPTION_ONLINE_EVALUATOR__METRICS__METRIC_HPP_ -#include "perception_evaluator/stat.hpp" +#include "perception_online_evaluator/stat.hpp" #include #include @@ -72,4 +72,4 @@ static struct CheckCorrectMaps } // namespace details } // namespace perception_diagnostics -#endif // PERCEPTION_EVALUATOR__METRICS__METRIC_HPP_ +#endif // PERCEPTION_ONLINE_EVALUATOR__METRICS__METRIC_HPP_ diff --git a/evaluator/perception_evaluator/include/perception_evaluator/metrics_calculator.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp similarity index 91% rename from evaluator/perception_evaluator/include/perception_evaluator/metrics_calculator.hpp rename to evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp index a2796d54e45fd..dd6756a17f194 100644 --- a/evaluator/perception_evaluator/include/perception_evaluator/metrics_calculator.hpp +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PERCEPTION_EVALUATOR__METRICS_CALCULATOR_HPP_ -#define PERCEPTION_EVALUATOR__METRICS_CALCULATOR_HPP_ +#ifndef PERCEPTION_ONLINE_EVALUATOR__METRICS_CALCULATOR_HPP_ +#define PERCEPTION_ONLINE_EVALUATOR__METRICS_CALCULATOR_HPP_ -#include "perception_evaluator/metrics/deviation_metrics.hpp" -#include "perception_evaluator/metrics/metric.hpp" -#include "perception_evaluator/parameters.hpp" -#include "perception_evaluator/stat.hpp" +#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 @@ -138,4 +138,4 @@ class MetricsCalculator } // namespace perception_diagnostics -#endif // PERCEPTION_EVALUATOR__METRICS_CALCULATOR_HPP_ +#endif // PERCEPTION_ONLINE_EVALUATOR__METRICS_CALCULATOR_HPP_ diff --git a/evaluator/perception_evaluator/include/perception_evaluator/parameters.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/parameters.hpp similarity index 86% rename from evaluator/perception_evaluator/include/perception_evaluator/parameters.hpp rename to evaluator/perception_online_evaluator/include/perception_online_evaluator/parameters.hpp index 7ab29ea53fe85..98cd3c25b71a3 100644 --- a/evaluator/perception_evaluator/include/perception_evaluator/parameters.hpp +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/parameters.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PERCEPTION_EVALUATOR__PARAMETERS_HPP_ -#define PERCEPTION_EVALUATOR__PARAMETERS_HPP_ +#ifndef PERCEPTION_ONLINE_EVALUATOR__PARAMETERS_HPP_ +#define PERCEPTION_ONLINE_EVALUATOR__PARAMETERS_HPP_ -#include "perception_evaluator/metrics/metric.hpp" +#include "perception_online_evaluator/metrics/metric.hpp" #include #include @@ -55,4 +55,4 @@ struct Parameters } // namespace perception_diagnostics -#endif // PERCEPTION_EVALUATOR__PARAMETERS_HPP_ +#endif // PERCEPTION_ONLINE_EVALUATOR__PARAMETERS_HPP_ diff --git a/evaluator/perception_evaluator/include/perception_evaluator/perception_evaluator_node.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/perception_online_evaluator_node.hpp similarity index 82% rename from evaluator/perception_evaluator/include/perception_evaluator/perception_evaluator_node.hpp rename to evaluator/perception_online_evaluator/include/perception_online_evaluator/perception_online_evaluator_node.hpp index 8994c404f21b9..318148f14b24e 100644 --- a/evaluator/perception_evaluator/include/perception_evaluator/perception_evaluator_node.hpp +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/perception_online_evaluator_node.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PERCEPTION_EVALUATOR__PERCEPTION_EVALUATOR_NODE_HPP_ -#define PERCEPTION_EVALUATOR__PERCEPTION_EVALUATOR_NODE_HPP_ +#ifndef PERCEPTION_ONLINE_EVALUATOR__PERCEPTION_ONLINE_EVALUATOR_NODE_HPP_ +#define PERCEPTION_ONLINE_EVALUATOR__PERCEPTION_ONLINE_EVALUATOR_NODE_HPP_ -#include "perception_evaluator/metrics_calculator.hpp" -#include "perception_evaluator/parameters.hpp" -#include "perception_evaluator/stat.hpp" +#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" @@ -46,11 +46,11 @@ using MarkerArray = visualization_msgs::msg::MarkerArray; /** * @brief Node for perception evaluation */ -class PerceptionEvaluatorNode : public rclcpp::Node +class PerceptionOnlineEvaluatorNode : public rclcpp::Node { public: - explicit PerceptionEvaluatorNode(const rclcpp::NodeOptions & node_options); - ~PerceptionEvaluatorNode(); + explicit PerceptionOnlineEvaluatorNode(const rclcpp::NodeOptions & node_options); + ~PerceptionOnlineEvaluatorNode(); /** * @brief callback on receiving a dynamic objects array @@ -92,4 +92,4 @@ class PerceptionEvaluatorNode : public rclcpp::Node }; } // namespace perception_diagnostics -#endif // PERCEPTION_EVALUATOR__PERCEPTION_EVALUATOR_NODE_HPP_ +#endif // PERCEPTION_ONLINE_EVALUATOR__PERCEPTION_ONLINE_EVALUATOR_NODE_HPP_ diff --git a/evaluator/perception_evaluator/include/perception_evaluator/stat.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/stat.hpp similarity index 93% rename from evaluator/perception_evaluator/include/perception_evaluator/stat.hpp rename to evaluator/perception_online_evaluator/include/perception_online_evaluator/stat.hpp index 8ee9ac96ac9a8..63494f90d02ee 100644 --- a/evaluator/perception_evaluator/include/perception_evaluator/stat.hpp +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/stat.hpp @@ -15,8 +15,8 @@ #include #include -#ifndef PERCEPTION_EVALUATOR__STAT_HPP_ -#define PERCEPTION_EVALUATOR__STAT_HPP_ +#ifndef PERCEPTION_ONLINE_EVALUATOR__STAT_HPP_ +#define PERCEPTION_ONLINE_EVALUATOR__STAT_HPP_ namespace perception_diagnostics { @@ -90,4 +90,4 @@ std::ostream & operator<<(std::ostream & os, const Stat & stat) } // namespace perception_diagnostics -#endif // PERCEPTION_EVALUATOR__STAT_HPP_ +#endif // PERCEPTION_ONLINE_EVALUATOR__STAT_HPP_ diff --git a/evaluator/perception_evaluator/include/perception_evaluator/utils/marker_utils.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/marker_utils.hpp similarity index 93% rename from evaluator/perception_evaluator/include/perception_evaluator/utils/marker_utils.hpp rename to evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/marker_utils.hpp index e128c380b7f36..3ac4c1db9efbd 100644 --- a/evaluator/perception_evaluator/include/perception_evaluator/utils/marker_utils.hpp +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/marker_utils.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PERCEPTION_EVALUATOR__UTILS__MARKER_UTILS_HPP_ -#define PERCEPTION_EVALUATOR__UTILS__MARKER_UTILS_HPP_ +#ifndef PERCEPTION_ONLINE_EVALUATOR__UTILS__MARKER_UTILS_HPP_ +#define PERCEPTION_ONLINE_EVALUATOR__UTILS__MARKER_UTILS_HPP_ #include @@ -79,4 +79,4 @@ MarkerArray createDeviationLines( } // namespace marker_utils -#endif // PERCEPTION_EVALUATOR__UTILS__MARKER_UTILS_HPP_ +#endif // PERCEPTION_ONLINE_EVALUATOR__UTILS__MARKER_UTILS_HPP_ diff --git a/evaluator/perception_evaluator/include/perception_evaluator/utils/objects_filtering.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/objects_filtering.hpp similarity index 94% rename from evaluator/perception_evaluator/include/perception_evaluator/utils/objects_filtering.hpp rename to evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/objects_filtering.hpp index c31dea28ef639..5d4238f45dec9 100644 --- a/evaluator/perception_evaluator/include/perception_evaluator/utils/objects_filtering.hpp +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/objects_filtering.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PERCEPTION_EVALUATOR__UTILS__OBJECTS_FILTERING_HPP_ -#define PERCEPTION_EVALUATOR__UTILS__OBJECTS_FILTERING_HPP_ +#ifndef PERCEPTION_ONLINE_EVALUATOR__UTILS__OBJECTS_FILTERING_HPP_ +#define PERCEPTION_ONLINE_EVALUATOR__UTILS__OBJECTS_FILTERING_HPP_ -#include "perception_evaluator/parameters.hpp" +#include "perception_online_evaluator/parameters.hpp" #include #include @@ -125,4 +125,4 @@ void filterDeviationCheckObjects( } // namespace perception_diagnostics -#endif // PERCEPTION_EVALUATOR__UTILS__OBJECTS_FILTERING_HPP_ +#endif // PERCEPTION_ONLINE_EVALUATOR__UTILS__OBJECTS_FILTERING_HPP_ diff --git a/evaluator/perception_evaluator/launch/motion_evaluator.launch.xml b/evaluator/perception_online_evaluator/launch/motion_evaluator.launch.xml similarity index 83% rename from evaluator/perception_evaluator/launch/motion_evaluator.launch.xml rename to evaluator/perception_online_evaluator/launch/motion_evaluator.launch.xml index 3e499638f9480..08c4e11126ec1 100644 --- a/evaluator/perception_evaluator/launch/motion_evaluator.launch.xml +++ b/evaluator/perception_online_evaluator/launch/motion_evaluator.launch.xml @@ -1,7 +1,7 @@ - + diff --git a/evaluator/perception_online_evaluator/launch/perception_online_evaluator.launch.xml b/evaluator/perception_online_evaluator/launch/perception_online_evaluator.launch.xml new file mode 100644 index 0000000000000..2ef179dbe3ff9 --- /dev/null +++ b/evaluator/perception_online_evaluator/launch/perception_online_evaluator.launch.xml @@ -0,0 +1,13 @@ + + + + + + + + + + + + + diff --git a/evaluator/perception_evaluator/package.xml b/evaluator/perception_online_evaluator/package.xml similarity index 97% rename from evaluator/perception_evaluator/package.xml rename to evaluator/perception_online_evaluator/package.xml index 7a173caed1947..bc0f7ef82049b 100644 --- a/evaluator/perception_evaluator/package.xml +++ b/evaluator/perception_online_evaluator/package.xml @@ -1,7 +1,7 @@ - perception_evaluator + perception_online_evaluator 0.1.0 ROS 2 node for evaluating perception Fumiya Watanabe diff --git a/evaluator/perception_evaluator/param/perception_evaluator.defaults.yaml b/evaluator/perception_online_evaluator/param/perception_online_evaluator.defaults.yaml similarity index 100% rename from evaluator/perception_evaluator/param/perception_evaluator.defaults.yaml rename to evaluator/perception_online_evaluator/param/perception_online_evaluator.defaults.yaml diff --git a/evaluator/perception_evaluator/src/metrics/deviation_metrics.cpp b/evaluator/perception_online_evaluator/src/metrics/deviation_metrics.cpp similarity index 96% rename from evaluator/perception_evaluator/src/metrics/deviation_metrics.cpp rename to evaluator/perception_online_evaluator/src/metrics/deviation_metrics.cpp index fd5fe0bccb134..7a9435444c065 100644 --- a/evaluator/perception_evaluator/src/metrics/deviation_metrics.cpp +++ b/evaluator/perception_online_evaluator/src/metrics/deviation_metrics.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "perception_evaluator/metrics/deviation_metrics.hpp" +#include "perception_online_evaluator/metrics/deviation_metrics.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" #include "tier4_autoware_utils/geometry/pose_deviation.hpp" diff --git a/evaluator/perception_evaluator/src/metrics_calculator.cpp b/evaluator/perception_online_evaluator/src/metrics_calculator.cpp similarity index 99% rename from evaluator/perception_evaluator/src/metrics_calculator.cpp rename to evaluator/perception_online_evaluator/src/metrics_calculator.cpp index 739c455d3ff2f..bec6d84fcd7dd 100644 --- a/evaluator/perception_evaluator/src/metrics_calculator.cpp +++ b/evaluator/perception_online_evaluator/src/metrics_calculator.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "perception_evaluator/metrics_calculator.hpp" +#include "perception_online_evaluator/metrics_calculator.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "perception_evaluator/utils/objects_filtering.hpp" +#include "perception_online_evaluator/utils/objects_filtering.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" #include diff --git a/evaluator/perception_evaluator/src/perception_evaluator_node.cpp b/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp similarity index 91% rename from evaluator/perception_evaluator/src/perception_evaluator_node.cpp rename to evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp index a9bfbdc0ff004..48e80f487dd3a 100644 --- a/evaluator/perception_evaluator/src/perception_evaluator_node.cpp +++ b/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "perception_evaluator/perception_evaluator_node.hpp" +#include "perception_online_evaluator/perception_online_evaluator_node.hpp" -#include "perception_evaluator/utils/marker_utils.hpp" +#include "perception_online_evaluator/utils/marker_utils.hpp" #include "tier4_autoware_utils/ros/marker_helper.hpp" #include "tier4_autoware_utils/ros/parameter.hpp" #include "tier4_autoware_utils/ros/update_param.hpp" @@ -35,8 +35,8 @@ namespace perception_diagnostics { -PerceptionEvaluatorNode::PerceptionEvaluatorNode(const rclcpp::NodeOptions & node_options) -: Node("perception_evaluator", node_options), +PerceptionOnlineEvaluatorNode::PerceptionOnlineEvaluatorNode(const rclcpp::NodeOptions & node_options) +: Node("perception_online_evaluator", node_options), parameters_(std::make_shared()), metrics_calculator_(parameters_) { @@ -46,7 +46,7 @@ PerceptionEvaluatorNode::PerceptionEvaluatorNode(const rclcpp::NodeOptions & nod google::InstallFailureSignalHandler(); objects_sub_ = create_subscription( - "~/input/objects", 1, std::bind(&PerceptionEvaluatorNode::onObjects, this, _1)); + "~/input/objects", 1, std::bind(&PerceptionOnlineEvaluatorNode::onObjects, this, _1)); metrics_pub_ = create_publisher("~/metrics", 1); pub_marker_ = create_publisher("~/markers", 10); @@ -57,25 +57,25 @@ PerceptionEvaluatorNode::PerceptionEvaluatorNode(const rclcpp::NodeOptions & nod initParameter(); set_param_res_ = this->add_on_set_parameters_callback( - std::bind(&PerceptionEvaluatorNode::onParameter, this, std::placeholders::_1)); + std::bind(&PerceptionOnlineEvaluatorNode::onParameter, this, std::placeholders::_1)); // Timer initTimer(/*period_s=*/0.1); } -PerceptionEvaluatorNode::~PerceptionEvaluatorNode() +PerceptionOnlineEvaluatorNode::~PerceptionOnlineEvaluatorNode() { } -void PerceptionEvaluatorNode::initTimer(double period_s) +void PerceptionOnlineEvaluatorNode::initTimer(double period_s) { const auto period_ns = std::chrono::duration_cast(std::chrono::duration(period_s)); timer_ = rclcpp::create_timer( - this, get_clock(), period_ns, std::bind(&PerceptionEvaluatorNode::onTimer, this)); + this, get_clock(), period_ns, std::bind(&PerceptionOnlineEvaluatorNode::onTimer, this)); } -void PerceptionEvaluatorNode::onTimer() +void PerceptionOnlineEvaluatorNode::onTimer() { DiagnosticArray metrics_msg; for (const Metric & metric : parameters_->metrics) { @@ -98,7 +98,7 @@ void PerceptionEvaluatorNode::onTimer() publishDebugMarker(); } -DiagnosticStatus PerceptionEvaluatorNode::generateDiagnosticStatus( +DiagnosticStatus PerceptionOnlineEvaluatorNode::generateDiagnosticStatus( const std::string metric, const Stat & metric_stat) const { DiagnosticStatus status; @@ -120,12 +120,12 @@ DiagnosticStatus PerceptionEvaluatorNode::generateDiagnosticStatus( return status; } -void PerceptionEvaluatorNode::onObjects(const PredictedObjects::ConstSharedPtr objects_msg) +void PerceptionOnlineEvaluatorNode::onObjects(const PredictedObjects::ConstSharedPtr objects_msg) { metrics_calculator_.setPredictedObjects(*objects_msg); } -void PerceptionEvaluatorNode::publishDebugMarker() +void PerceptionOnlineEvaluatorNode::publishDebugMarker() { using marker_utils::createColorFromString; using marker_utils::createDeviationLines; @@ -192,7 +192,7 @@ void PerceptionEvaluatorNode::publishDebugMarker() pub_marker_->publish(marker); } -rcl_interfaces::msg::SetParametersResult PerceptionEvaluatorNode::onParameter( +rcl_interfaces::msg::SetParametersResult PerceptionOnlineEvaluatorNode::onParameter( const std::vector & parameters) { using tier4_autoware_utils::updateParam; @@ -280,7 +280,7 @@ rcl_interfaces::msg::SetParametersResult PerceptionEvaluatorNode::onParameter( return result; } -void PerceptionEvaluatorNode::initParameter() +void PerceptionOnlineEvaluatorNode::initParameter() { using tier4_autoware_utils::getOrDeclareParameter; using tier4_autoware_utils::updateParam; @@ -344,4 +344,4 @@ void PerceptionEvaluatorNode::initParameter() } // namespace perception_diagnostics #include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(perception_diagnostics::PerceptionEvaluatorNode) +RCLCPP_COMPONENTS_REGISTER_NODE(perception_diagnostics::PerceptionOnlineEvaluatorNode) diff --git a/evaluator/perception_evaluator/src/utils/marker_utils.cpp b/evaluator/perception_online_evaluator/src/utils/marker_utils.cpp similarity index 99% rename from evaluator/perception_evaluator/src/utils/marker_utils.cpp rename to evaluator/perception_online_evaluator/src/utils/marker_utils.cpp index 5d89425d1c864..75af92e83dcd8 100644 --- a/evaluator/perception_evaluator/src/utils/marker_utils.cpp +++ b/evaluator/perception_online_evaluator/src/utils/marker_utils.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "perception_evaluator/utils/marker_utils.hpp" +#include "perception_online_evaluator/utils/marker_utils.hpp" #include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" diff --git a/evaluator/perception_evaluator/src/utils/objects_filtering.cpp b/evaluator/perception_online_evaluator/src/utils/objects_filtering.cpp similarity index 98% rename from evaluator/perception_evaluator/src/utils/objects_filtering.cpp rename to evaluator/perception_online_evaluator/src/utils/objects_filtering.cpp index c2479ca5c28ef..97daee36175f3 100644 --- a/evaluator/perception_evaluator/src/utils/objects_filtering.cpp +++ b/evaluator/perception_online_evaluator/src/utils/objects_filtering.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "perception_evaluator/utils/objects_filtering.hpp" +#include "perception_online_evaluator/utils/objects_filtering.hpp" namespace perception_diagnostics { diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index 1be996e747039..6b4ccb66cb403 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -250,6 +250,6 @@ - + diff --git a/launch/tier4_simulator_launch/launch/simulator.launch.xml b/launch/tier4_simulator_launch/launch/simulator.launch.xml index 679d350535c50..3801c448eaed4 100644 --- a/launch/tier4_simulator_launch/launch/simulator.launch.xml +++ b/launch/tier4_simulator_launch/launch/simulator.launch.xml @@ -87,7 +87,7 @@ - + From df0b9ef85a58006e805f81d8e08e342f088097f0 Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Wed, 28 Feb 2024 12:14:36 +0900 Subject: [PATCH 6/8] refactor: remove timer Signed-off-by: kosuke55 --- .../perception_online_evaluator_node.hpp | 8 ++----- .../src/perception_online_evaluator_node.cpp | 24 ++++++------------- 2 files changed, 9 insertions(+), 23 deletions(-) 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 318148f14b24e..e6317f92179fe 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 @@ -50,7 +50,7 @@ class PerceptionOnlineEvaluatorNode : public rclcpp::Node { public: explicit PerceptionOnlineEvaluatorNode(const rclcpp::NodeOptions & node_options); - ~PerceptionOnlineEvaluatorNode(); + ~PerceptionOnlineEvaluatorNode() {}; /** * @brief callback on receiving a dynamic objects array @@ -62,11 +62,6 @@ class PerceptionOnlineEvaluatorNode : public rclcpp::Node const std::string metric, const Stat & metric_stat) const; private: - // Timer - rclcpp::TimerBase::SharedPtr timer_; - void initTimer(double period_s); - void onTimer(); - // Subscribers and publishers rclcpp::Subscription::SharedPtr objects_sub_; rclcpp::Publisher::SharedPtr metrics_pub_; @@ -86,6 +81,7 @@ class PerceptionOnlineEvaluatorNode : public rclcpp::Node // Metrics Calculator MetricsCalculator metrics_calculator_; std::deque stamps_; + void publishMetrics(); // Debug void publishDebugMarker(); 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 48e80f487dd3a..e86d1b4217f78 100644 --- a/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp +++ b/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp @@ -35,7 +35,8 @@ namespace perception_diagnostics { -PerceptionOnlineEvaluatorNode::PerceptionOnlineEvaluatorNode(const rclcpp::NodeOptions & node_options) +PerceptionOnlineEvaluatorNode::PerceptionOnlineEvaluatorNode( + const rclcpp::NodeOptions & node_options) : Node("perception_online_evaluator", node_options), parameters_(std::make_shared()), metrics_calculator_(parameters_) @@ -58,26 +59,13 @@ PerceptionOnlineEvaluatorNode::PerceptionOnlineEvaluatorNode(const rclcpp::NodeO set_param_res_ = this->add_on_set_parameters_callback( std::bind(&PerceptionOnlineEvaluatorNode::onParameter, this, std::placeholders::_1)); - - // Timer - initTimer(/*period_s=*/0.1); -} - -PerceptionOnlineEvaluatorNode::~PerceptionOnlineEvaluatorNode() -{ } -void PerceptionOnlineEvaluatorNode::initTimer(double period_s) -{ - const auto period_ns = - std::chrono::duration_cast(std::chrono::duration(period_s)); - timer_ = rclcpp::create_timer( - this, get_clock(), period_ns, std::bind(&PerceptionOnlineEvaluatorNode::onTimer, this)); -} - -void PerceptionOnlineEvaluatorNode::onTimer() +void PerceptionOnlineEvaluatorNode::publishMetrics() { DiagnosticArray metrics_msg; + + // calculate metrics for (const Metric & metric : parameters_->metrics) { const auto metric_stat_map = metrics_calculator_.calculate(Metric(metric)); if (!metric_stat_map.has_value()) { @@ -91,6 +79,7 @@ void PerceptionOnlineEvaluatorNode::onTimer() } } + // publish metrics if (!metrics_msg.status.empty()) { metrics_msg.header.stamp = now(); metrics_pub_->publish(metrics_msg); @@ -123,6 +112,7 @@ DiagnosticStatus PerceptionOnlineEvaluatorNode::generateDiagnosticStatus( void PerceptionOnlineEvaluatorNode::onObjects(const PredictedObjects::ConstSharedPtr objects_msg) { metrics_calculator_.setPredictedObjects(*objects_msg); + publishMetrics(); } void PerceptionOnlineEvaluatorNode::publishDebugMarker() From 6093266a2b40da38d13ab3220261e25f337f3ade Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Wed, 28 Feb 2024 15:39:15 +0900 Subject: [PATCH 7/8] feat: add test Signed-off-by: kosuke55 --- .../CMakeLists.txt | 9 + .../perception_online_evaluator_node.hpp | 2 +- .../src/perception_online_evaluator_node.cpp | 2 +- .../test_perception_online_evaluator_node.cpp | 520 ++++++++++++++++++ 4 files changed, 531 insertions(+), 2 deletions(-) create mode 100644 evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp diff --git a/evaluator/perception_online_evaluator/CMakeLists.txt b/evaluator/perception_online_evaluator/CMakeLists.txt index 8106f1e0b28b6..f9cc0f4fa256c 100644 --- a/evaluator/perception_online_evaluator/CMakeLists.txt +++ b/evaluator/perception_online_evaluator/CMakeLists.txt @@ -28,6 +28,15 @@ rclcpp_components_register_node(${PROJECT_NAME}_node target_link_libraries(${PROJECT_NAME}_node glog::glog) +if(BUILD_TESTING) + ament_add_ros_isolated_gtest(test_${PROJECT_NAME} + test/test_perception_online_evaluator_node.cpp + ) + target_link_libraries(test_${PROJECT_NAME} + ${PROJECT_NAME}_node + ) +endif() + ament_auto_package( INSTALL_TO_SHARE param 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 e6317f92179fe..f3e4c4e2587c6 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 @@ -50,7 +50,7 @@ class PerceptionOnlineEvaluatorNode : public rclcpp::Node { public: explicit PerceptionOnlineEvaluatorNode(const rclcpp::NodeOptions & node_options); - ~PerceptionOnlineEvaluatorNode() {}; + ~PerceptionOnlineEvaluatorNode(){}; /** * @brief callback on receiving a dynamic objects array 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 e86d1b4217f78..cf98ccc4c5460 100644 --- a/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp +++ b/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp @@ -43,7 +43,7 @@ PerceptionOnlineEvaluatorNode::PerceptionOnlineEvaluatorNode( { using std::placeholders::_1; - google::InitGoogleLogging("map_based_prediction_node"); + google::InitGoogleLogging("perception_online_evaluator_node"); google::InstallFailureSignalHandler(); objects_sub_ = create_subscription( diff --git a/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp b/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp new file mode 100644 index 0000000000000..22c1ceae77c09 --- /dev/null +++ b/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp @@ -0,0 +1,520 @@ +// 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 "ament_index_cpp/get_package_share_directory.hpp" +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/time.hpp" + +#include +#include +#include + +#include +#include +#include +#include + +#include "boost/lexical_cast.hpp" + +#include +#include + +#include +#include +#include +#include + +using EvalNode = perception_diagnostics::PerceptionOnlineEvaluatorNode; +using PredictedObjects = autoware_auto_perception_msgs::msg::PredictedObjects; +using PredictedObject = autoware_auto_perception_msgs::msg::PredictedObject; +using DiagnosticArray = diagnostic_msgs::msg::DiagnosticArray; +using MarkerArray = visualization_msgs::msg::MarkerArray; +using ObjectClassification = autoware_auto_perception_msgs::msg::ObjectClassification; + +using tier4_autoware_utils::generateUUID; + +constexpr double epsilon = 1e-6; + +class EvalTest : public ::testing::Test +{ +protected: + void SetUp() override + { + rclcpp::init(0, nullptr); + + rclcpp::NodeOptions options; + const auto share_dir = + ament_index_cpp::get_package_share_directory("perception_online_evaluator"); + options.arguments( + {"--ros-args", "--params-file", + share_dir + "/param/perception_online_evaluator.defaults.yaml"}); + options.append_parameter_override("prediction_time_horizons", std::vector{5.0}); + options.append_parameter_override("smoothing_window_size", int(11)); + + dummy_node = std::make_shared("perception_online_evaluator_test", options); + eval_node = std::make_shared(options); + // Enable all logging in the node + auto ret = rcutils_logging_set_logger_level( + dummy_node->get_logger().get_name(), RCUTILS_LOG_SEVERITY_DEBUG); + if (ret != RCUTILS_RET_OK) { + std::cerr << "Failed to set logging severity to DEBUG\n"; + } + ret = rcutils_logging_set_logger_level( + eval_node->get_logger().get_name(), RCUTILS_LOG_SEVERITY_DEBUG); + if (ret != RCUTILS_RET_OK) { + std::cerr << "Failed to set logging severity to DEBUG\n"; + } + objects_pub_ = rclcpp::create_publisher( + dummy_node, "/perception_online_evaluator/input/objects", 1); + + marker_sub_ = rclcpp::create_subscription( + eval_node, "perception_online_evaluator/markers", 10, + [this]([[maybe_unused]] const MarkerArray::SharedPtr msg) { has_received_marker_ = true; }); + uuid_ = generateUUID(); + } + + ~EvalTest() override + { + rclcpp::shutdown(); + google::ShutdownGoogleLogging(); + } + + void setTargetMetric(perception_diagnostics::Metric metric) + { + const auto metric_str = perception_diagnostics::metric_to_str.at(metric); + setTargetMetric(metric_str); + } + + void setTargetMetric(std::string metric_str) + { + const auto is_target_metric = [metric_str](const auto & status) { + return status.name == metric_str; + }; + metric_sub_ = rclcpp::create_subscription( + eval_node, "/perception_online_evaluator/metrics", 1, + [=](const DiagnosticArray::ConstSharedPtr msg) { + const auto it = std::find_if(msg->status.begin(), msg->status.end(), is_target_metric); + if (it != msg->status.end()) { + std::cerr << it->values[0].key << " " << it->values[0].value << " " << it->values[1].key + << " " << it->values[1].value << " " << it->values[2].key << " " + << it->values[2].value << std::endl; + metric_value_ = boost::lexical_cast(it->values[2].value); + metric_updated_ = true; + } + }); + } + + PredictedObject makePredictedObject(const std::vector> & predicted_path) + { + PredictedObject object; + object.object_id = uuid_; + ObjectClassification classification; + classification.label = ObjectClassification::CAR; + classification.probability = 1.0; + + object.classification = {classification}; + + object.kinematics.initial_pose_with_covariance.pose.position.x = predicted_path.front().first; + object.kinematics.initial_pose_with_covariance.pose.position.y = predicted_path.front().second; + object.kinematics.initial_pose_with_covariance.pose.position.z = 0.0; + object.kinematics.initial_pose_with_covariance.pose.orientation.x = 0.0; + object.kinematics.initial_pose_with_covariance.pose.orientation.y = 0.0; + object.kinematics.initial_pose_with_covariance.pose.orientation.z = 0.0; + object.kinematics.initial_pose_with_covariance.pose.orientation.w = 1.0; + + autoware_auto_perception_msgs::msg::PredictedPath path; + for (size_t i = 0; i < predicted_path.size(); ++i) { + geometry_msgs::msg::Pose pose; + pose.position.x = predicted_path[i].first; + pose.position.y = predicted_path[i].second; + pose.position.z = 0.0; + pose.orientation.x = 0.0; + pose.orientation.y = 0.0; + pose.orientation.z = 0.0; + pose.orientation.w = 1.0; + path.path.push_back(pose); + } + + path.confidence = 1.0; + path.time_step = rclcpp::Duration::from_seconds(time_step_); + object.kinematics.predicted_paths.push_back(path); + + return object; + } + + PredictedObjects makePredictedObjects( + const std::vector> & predicted_path) + { + PredictedObjects objects; + objects.objects.push_back(makePredictedObject(predicted_path)); + objects.header.stamp = rclcpp::Time(0); + return objects; + } + + PredictedObjects makeStraightPredictedObjects(const double time) + { + std::vector> predicted_path; + for (size_t i = 0; i <= time_horizon_ / time_step_; i++) { + predicted_path.push_back({velocity_ * (time + i * time_step_), 0.0}); + } + auto objects = makePredictedObjects(predicted_path); + objects.header.stamp = rclcpp::Time(0) + rclcpp::Duration::from_seconds(time); + return objects; + } + + PredictedObjects makeDeviatedStraightPredictedObjects(const double time, const double deviation) + { + std::vector> predicted_path; + for (size_t i = 0; i <= time_horizon_ / time_step_; i++) { + predicted_path.push_back({velocity_ * (time + i * time_step_), deviation}); + } + auto objects = makePredictedObjects(predicted_path); + objects.header.stamp = rclcpp::Time(0) + rclcpp::Duration::from_seconds(time); + return objects; + } + + PredictedObjects rotateObjects(const PredictedObjects objects, const double yaw) + { + PredictedObjects rotated_objects = objects; + for (auto & object : rotated_objects.objects) { + object.kinematics.initial_pose_with_covariance.pose.orientation.z = sin(yaw / 2); + object.kinematics.initial_pose_with_covariance.pose.orientation.w = cos(yaw / 2); + } + return rotated_objects; + } + + double publishObjectsAndGetMetric(const PredictedObjects & objects) + { + metric_updated_ = false; + objects_pub_->publish(objects); + const auto now = rclcpp::Clock().now(); + while (!metric_updated_) { + rclcpp::spin_some(dummy_node); + rclcpp::spin_some(eval_node); + rclcpp::sleep_for(std::chrono::milliseconds(100)); + // timeout + if (rclcpp::Clock().now() - now > rclcpp::Duration::from_seconds(5)) { + throw std::runtime_error("Timeout while waiting for metric update"); + } + } + return metric_value_; + } + + void publishObjects(const PredictedObjects & objects) + { + objects_pub_->publish(objects); + rclcpp::spin_some(eval_node); + rclcpp::sleep_for(std::chrono::milliseconds(100)); + rclcpp::spin_some(dummy_node); + } + + void waitForDummyNode() + { + // wait for the marker to be published + publishObjects(makeStraightPredictedObjects(0)); + while (!has_received_marker_) { + rclcpp::spin_some(dummy_node); + rclcpp::sleep_for(std::chrono::milliseconds(100)); + rclcpp::spin_some(eval_node); + } + } + + // Latest metric value + bool metric_updated_ = false; + double metric_value_; + // Node + rclcpp::Node::SharedPtr dummy_node; + EvalNode::SharedPtr eval_node; + + // Pub/Sub + rclcpp::Publisher::SharedPtr objects_pub_; + rclcpp::Subscription::SharedPtr metric_sub_; + rclcpp::Subscription::SharedPtr marker_sub_; + bool has_received_marker_{false}; + + double time_delay_ = 5.0; + double time_step_ = 0.5; + double time_horizon_ = 10.0; + double velocity_ = 2.0; + + unique_identifier_msgs::msg::UUID uuid_; +}; + +// ========================================================================================== +// lateral deviation +TEST_F(EvalTest, testLateralDeviation_deviation0) +{ + waitForDummyNode(); + setTargetMetric("lateral_deviation"); + + const double deviation = 0.0; + for (double time = 0; time < time_delay_; time += time_step_) { + const auto objects = makeDeviatedStraightPredictedObjects(time, deviation); + publishObjects(objects); + } + + const auto last_objects = makeDeviatedStraightPredictedObjects(time_delay_, deviation); + EXPECT_NEAR(publishObjectsAndGetMetric(last_objects), 0.0, epsilon); +} + +TEST_F(EvalTest, testLateralDeviation_deviation1) +{ + waitForDummyNode(); + setTargetMetric("lateral_deviation"); + + const double deviation = 1.0; + for (double time = 0; time < time_delay_; time += time_step_) { + const auto objects = makeDeviatedStraightPredictedObjects(time, deviation); + publishObjects(objects); + } + + const auto last_objects = makeDeviatedStraightPredictedObjects(time_delay_ * 2, deviation); + EXPECT_NEAR(publishObjectsAndGetMetric(last_objects), 0.0, epsilon); +} + +TEST_F(EvalTest, testLateralDeviation_oscillation) +{ + waitForDummyNode(); + setTargetMetric("lateral_deviation"); + + const double deviation = 1.0; + double sign = 1.0; + for (double time = 0; time < time_delay_ * 2; time += time_step_) { + PredictedObjects objects; + if (time == time_delay_) { + objects = makeDeviatedStraightPredictedObjects(time, 0); + } else { + objects = makeDeviatedStraightPredictedObjects(time, deviation * sign); + sign *= -1.0; + } + publishObjects(objects); + } + + const auto last_objects = makeDeviatedStraightPredictedObjects(time_delay_ * 2, deviation); + EXPECT_NEAR(publishObjectsAndGetMetric(last_objects), 0.0, epsilon); +} + +TEST_F(EvalTest, testLateralDeviation_distortion) +{ + waitForDummyNode(); + setTargetMetric("lateral_deviation"); + + const double deviation = 1.0; + for (double time = 0; time < time_delay_ * 2; time += time_step_) { + PredictedObjects objects; + if (time == time_delay_) { + objects = makeDeviatedStraightPredictedObjects(time, deviation); + } else if (time == time_delay_ + time_step_) { + objects = makeDeviatedStraightPredictedObjects(time, -deviation); + } else { + objects = makeDeviatedStraightPredictedObjects(time, 0); + } + publishObjects(objects); + } + + const auto last_objects = makeDeviatedStraightPredictedObjects(time_delay_ * 2, deviation); + EXPECT_NEAR(publishObjectsAndGetMetric(last_objects), deviation, epsilon); +} +// ========================================================================================== + +// ========================================================================================== +// yaw deviation +TEST_F(EvalTest, testYawDeviation_deviation0) +{ + waitForDummyNode(); + setTargetMetric("yaw_deviation"); + + const double deviation = 0.0; + for (double time = 0; time < time_delay_; time += time_step_) { + const auto objects = makeDeviatedStraightPredictedObjects(time, deviation); + publishObjects(objects); + } + + const auto last_objects = makeDeviatedStraightPredictedObjects(time_delay_, deviation); + EXPECT_NEAR(publishObjectsAndGetMetric(last_objects), 0.0, epsilon); +} + +TEST_F(EvalTest, testYawDeviation_deviation1) +{ + waitForDummyNode(); + setTargetMetric("yaw_deviation"); + + const double deviation = 1.0; + for (double time = 0; time < time_delay_; time += time_step_) { + const auto objects = makeDeviatedStraightPredictedObjects(time, deviation); + publishObjects(objects); + } + + const auto last_objects = makeDeviatedStraightPredictedObjects(time_delay_, deviation); + EXPECT_NEAR(publishObjectsAndGetMetric(last_objects), 0.0, epsilon); +} + +TEST_F(EvalTest, testYawDeviation_oscillation) +{ + waitForDummyNode(); + setTargetMetric("yaw_deviation"); + + const double deviation = 1.0; + double sign = 1.0; + for (double time = 0; time < time_delay_ * 2; time += time_step_) { + PredictedObjects objects; + if (time == time_delay_) { + objects = makeDeviatedStraightPredictedObjects(time, 0); + } else { + objects = makeDeviatedStraightPredictedObjects(time, deviation * sign); + sign *= -1.0; + } + publishObjects(objects); + } + + const auto last_objects = makeDeviatedStraightPredictedObjects(time_delay_ * 2, deviation); + EXPECT_NEAR(publishObjectsAndGetMetric(last_objects), 0.0, epsilon); +} + +TEST_F(EvalTest, testYawDeviation_distortion) +{ + waitForDummyNode(); + setTargetMetric("yaw_deviation"); + + const double deviation = 1.0; + for (double time = 0; time < time_delay_ * 2; time += time_step_) { + PredictedObjects objects; + if (time == time_delay_) { + objects = makeDeviatedStraightPredictedObjects(time, deviation); + } else if (time == time_delay_ + time_step_) { + objects = makeDeviatedStraightPredictedObjects(time, -deviation); + } else { + objects = makeDeviatedStraightPredictedObjects(time, 0); + } + publishObjects(objects); + } + + const auto last_objects = makeDeviatedStraightPredictedObjects(time_delay_ * 2, deviation); + EXPECT_NEAR(publishObjectsAndGetMetric(last_objects), 0, epsilon); +} + +TEST_F(EvalTest, testYawDeviation_oscillation_rotate) +{ + waitForDummyNode(); + setTargetMetric("yaw_deviation"); + + const double deviation = 1.0; + const double yaw = M_PI / 4; + double sign = 1.0; + for (double time = 0; time < time_delay_ * 2; time += time_step_) { + PredictedObjects objects; + if (time == time_delay_) { + objects = rotateObjects(makeDeviatedStraightPredictedObjects(time, 0), yaw); + } else { + objects = + rotateObjects(makeDeviatedStraightPredictedObjects(time, deviation * sign), 2 * M_PI * std::rand()); + sign *= -1.0; + } + publishObjects(objects); + } + + const auto last_objects = + rotateObjects(makeDeviatedStraightPredictedObjects(time_delay_ * 2, deviation), 2 * M_PI * std::rand()); + EXPECT_NEAR(publishObjectsAndGetMetric(last_objects), yaw, epsilon); +} + +TEST_F(EvalTest, testYawDeviation_distortion_rotate) +{ + waitForDummyNode(); + setTargetMetric("yaw_deviation"); + + const double deviation = 1.0; + const double yaw = M_PI / 4; + for (double time = 0; time < time_delay_ * 2; time += time_step_) { + PredictedObjects objects; + if (time == time_delay_) { + objects = rotateObjects(makeDeviatedStraightPredictedObjects(time, deviation), yaw); + } else if (time == time_delay_ + time_step_) { + objects = rotateObjects(makeDeviatedStraightPredictedObjects(time, -deviation), 2 * M_PI * std::rand()); + } else { + objects = rotateObjects(makeDeviatedStraightPredictedObjects(time, 0), 2 * M_PI * std::rand()); + } + publishObjects(objects); + } + + const auto last_objects = rotateObjects(makeDeviatedStraightPredictedObjects(time_delay_ * 2, deviation), 2 * M_PI * std::rand()); + EXPECT_NEAR(publishObjectsAndGetMetric(last_objects), yaw, epsilon); +} + +// ========================================================================================== +// predicted path deviation{ +TEST_F(EvalTest, testPredictedPathDeviation_deviation0) +{ + waitForDummyNode(); + + setTargetMetric("predicted_path_deviation_5.00"); + + const auto init_objects = makeStraightPredictedObjects(0); + publishObjects(init_objects); + + const double deviation = 0.0; + for (double time = time_step_; time < time_delay_; time += time_step_) { + const auto objects = makeDeviatedStraightPredictedObjects(time, deviation); + publishObjects(objects); + } + const auto last_objects = makeDeviatedStraightPredictedObjects(time_delay_, deviation); + + const double num_points = time_delay_ / time_step_ + 1; + const double mean_deviation = deviation * (num_points - 1) / num_points; + EXPECT_NEAR(publishObjectsAndGetMetric(last_objects), mean_deviation, epsilon); +} + +TEST_F(EvalTest, testPredictedPathDeviation_deviation1) +{ + waitForDummyNode(); + + setTargetMetric("predicted_path_deviation_5.00"); + + const auto init_objects = makeStraightPredictedObjects(0); + publishObjects(init_objects); + + const double deviation = 1.0; + for (double time = time_step_; time < time_delay_; time += time_step_) { + const auto objects = makeDeviatedStraightPredictedObjects(time, deviation); + publishObjects(objects); + } + const auto last_objects = makeDeviatedStraightPredictedObjects(time_delay_, deviation); + + const double num_points = time_delay_ / time_step_ + 1; + const double mean_deviation = deviation * (num_points - 1) / num_points; + EXPECT_NEAR(publishObjectsAndGetMetric(last_objects), mean_deviation, epsilon); +} + +TEST_F(EvalTest, testPredictedPathDeviation_deviation2) +{ + waitForDummyNode(); + + setTargetMetric("predicted_path_deviation_5.00"); + + const auto init_objects = makeStraightPredictedObjects(0); + publishObjects(init_objects); + + const double deviation = 2.0; + for (double time = time_step_; time < time_delay_; time += time_step_) { + const auto objects = makeDeviatedStraightPredictedObjects(time, deviation); + publishObjects(objects); + } + const auto last_objects = makeDeviatedStraightPredictedObjects(time_delay_, deviation); + + const double num_points = time_delay_ / time_step_ + 1; + const double mean_deviation = deviation * (num_points - 1) / num_points; + EXPECT_NEAR(publishObjectsAndGetMetric(last_objects), mean_deviation, epsilon); +} +// ========================================================================================== From e203c3e1e8ad245257fda0170b87bcce8e39632c Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Wed, 28 Feb 2024 15:43:56 +0900 Subject: [PATCH 8/8] fix: ci check Signed-off-by: kosuke55 --- .../perception_online_evaluator_node.hpp | 2 +- .../test_perception_online_evaluator_node.cpp | 21 ++++++++++--------- .../launch/perception.launch.xml | 2 +- 3 files changed, 13 insertions(+), 12 deletions(-) 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 f3e4c4e2587c6..b7535935ccd5f 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 @@ -50,7 +50,7 @@ class PerceptionOnlineEvaluatorNode : public rclcpp::Node { public: explicit PerceptionOnlineEvaluatorNode(const rclcpp::NodeOptions & node_options); - ~PerceptionOnlineEvaluatorNode(){}; + ~PerceptionOnlineEvaluatorNode() {} /** * @brief callback on receiving a dynamic objects array diff --git a/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp b/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp index 22c1ceae77c09..095fb130426f0 100644 --- a/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp +++ b/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp @@ -12,8 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "ament_index_cpp/get_package_share_directory.hpp" -#include "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" #include "rclcpp/time.hpp" @@ -61,7 +59,7 @@ class EvalTest : public ::testing::Test {"--ros-args", "--params-file", share_dir + "/param/perception_online_evaluator.defaults.yaml"}); options.append_parameter_override("prediction_time_horizons", std::vector{5.0}); - options.append_parameter_override("smoothing_window_size", int(11)); + options.append_parameter_override("smoothing_window_size", 11); dummy_node = std::make_shared("perception_online_evaluator_test", options); eval_node = std::make_shared(options); @@ -418,15 +416,15 @@ TEST_F(EvalTest, testYawDeviation_oscillation_rotate) if (time == time_delay_) { objects = rotateObjects(makeDeviatedStraightPredictedObjects(time, 0), yaw); } else { - objects = - rotateObjects(makeDeviatedStraightPredictedObjects(time, deviation * sign), 2 * M_PI * std::rand()); + objects = rotateObjects( + makeDeviatedStraightPredictedObjects(time, deviation * sign), 2 * M_PI * std::rand()); sign *= -1.0; } publishObjects(objects); } - const auto last_objects = - rotateObjects(makeDeviatedStraightPredictedObjects(time_delay_ * 2, deviation), 2 * M_PI * std::rand()); + const auto last_objects = rotateObjects( + makeDeviatedStraightPredictedObjects(time_delay_ * 2, deviation), 2 * M_PI * std::rand()); EXPECT_NEAR(publishObjectsAndGetMetric(last_objects), yaw, epsilon); } @@ -442,14 +440,17 @@ TEST_F(EvalTest, testYawDeviation_distortion_rotate) if (time == time_delay_) { objects = rotateObjects(makeDeviatedStraightPredictedObjects(time, deviation), yaw); } else if (time == time_delay_ + time_step_) { - objects = rotateObjects(makeDeviatedStraightPredictedObjects(time, -deviation), 2 * M_PI * std::rand()); + objects = rotateObjects( + makeDeviatedStraightPredictedObjects(time, -deviation), 2 * M_PI * std::rand()); } else { - objects = rotateObjects(makeDeviatedStraightPredictedObjects(time, 0), 2 * M_PI * std::rand()); + objects = + rotateObjects(makeDeviatedStraightPredictedObjects(time, 0), 2 * M_PI * std::rand()); } publishObjects(objects); } - const auto last_objects = rotateObjects(makeDeviatedStraightPredictedObjects(time_delay_ * 2, deviation), 2 * M_PI * std::rand()); + const auto last_objects = rotateObjects( + makeDeviatedStraightPredictedObjects(time_delay_ * 2, deviation), 2 * M_PI * std::rand()); EXPECT_NEAR(publishObjectsAndGetMetric(last_objects), yaw, epsilon); } diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index 6b4ccb66cb403..16396ef2961d8 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -250,6 +250,6 @@ - +