From 9218fb1794cc99b701e83317e77adf57c774392e Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Thu, 29 Feb 2024 09:56:56 +0900 Subject: [PATCH] feat(perception_online_evaluator): add perception_online_evaluator (#6493) * 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 * fix build werror Signed-off-by: kosuke55 * fix topic name Signed-off-by: kosuke55 * clean up Signed-off-by: kosuke55 * rename to perception_online_evaluator Signed-off-by: kosuke55 * refactor: remove timer Signed-off-by: kosuke55 * feat: add test Signed-off-by: kosuke55 * fix: ci check Signed-off-by: kosuke55 --------- Signed-off-by: kosuke55 --- .../CMakeLists.txt | 44 ++ .../perception_online_evaluator/README.md | 43 ++ .../metrics/deviation_metrics.hpp | 53 ++ .../metrics/metric.hpp | 75 +++ .../metrics_calculator.hpp | 141 +++++ .../parameters.hpp | 58 ++ .../perception_online_evaluator_node.hpp | 91 +++ .../perception_online_evaluator/stat.hpp | 93 ++++ .../utils/marker_utils.hpp | 82 +++ .../utils/objects_filtering.hpp | 128 +++++ .../launch/motion_evaluator.launch.xml | 7 + .../perception_online_evaluator.launch.xml | 13 + .../perception_online_evaluator/package.xml | 42 ++ .../perception_online_evaluator.defaults.yaml | 39 ++ .../src/metrics/deviation_metrics.cpp | 65 +++ .../src/metrics_calculator.cpp | 458 +++++++++++++++ .../src/perception_online_evaluator_node.cpp | 337 +++++++++++ .../src/utils/marker_utils.cpp | 196 +++++++ .../src/utils/objects_filtering.cpp | 103 ++++ .../test_perception_online_evaluator_node.cpp | 521 ++++++++++++++++++ .../launch/perception.launch.xml | 5 + .../launch/simulator.launch.xml | 5 + 22 files changed, 2599 insertions(+) create mode 100644 evaluator/perception_online_evaluator/CMakeLists.txt create mode 100644 evaluator/perception_online_evaluator/README.md create mode 100644 evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/deviation_metrics.hpp create mode 100644 evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/metric.hpp create mode 100644 evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp create mode 100644 evaluator/perception_online_evaluator/include/perception_online_evaluator/parameters.hpp create mode 100644 evaluator/perception_online_evaluator/include/perception_online_evaluator/perception_online_evaluator_node.hpp create mode 100644 evaluator/perception_online_evaluator/include/perception_online_evaluator/stat.hpp create mode 100644 evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/marker_utils.hpp create mode 100644 evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/objects_filtering.hpp create mode 100644 evaluator/perception_online_evaluator/launch/motion_evaluator.launch.xml create mode 100644 evaluator/perception_online_evaluator/launch/perception_online_evaluator.launch.xml create mode 100644 evaluator/perception_online_evaluator/package.xml create mode 100644 evaluator/perception_online_evaluator/param/perception_online_evaluator.defaults.yaml create mode 100644 evaluator/perception_online_evaluator/src/metrics/deviation_metrics.cpp create mode 100644 evaluator/perception_online_evaluator/src/metrics_calculator.cpp create mode 100644 evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp create mode 100644 evaluator/perception_online_evaluator/src/utils/marker_utils.cpp create mode 100644 evaluator/perception_online_evaluator/src/utils/objects_filtering.cpp 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 new file mode 100644 index 0000000000000..f9cc0f4fa256c --- /dev/null +++ b/evaluator/perception_online_evaluator/CMakeLists.txt @@ -0,0 +1,44 @@ +cmake_minimum_required(VERSION 3.14) +project(perception_online_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::PerceptionOnlineEvaluatorNode" + 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_online_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_online_evaluator/README.md b/evaluator/perception_online_evaluator/README.md new file mode 100644 index 0000000000000..b801e5f418cef --- /dev/null +++ b/evaluator/perception_online_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_online_evaluator/include/perception_online_evaluator/metrics/deviation_metrics.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/deviation_metrics.hpp new file mode 100644 index 0000000000000..da7a23b6980b6 --- /dev/null +++ b/evaluator/perception_online_evaluator/include/perception_online_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_ONLINE_EVALUATOR__METRICS__DEVIATION_METRICS_HPP_ +#define PERCEPTION_ONLINE_EVALUATOR__METRICS__DEVIATION_METRICS_HPP_ + +#include "perception_online_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_ONLINE_EVALUATOR__METRICS__DEVIATION_METRICS_HPP_ diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/metric.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/metric.hpp new file mode 100644 index 0000000000000..8a2cddca476d4 --- /dev/null +++ b/evaluator/perception_online_evaluator/include/perception_online_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_ONLINE_EVALUATOR__METRICS__METRIC_HPP_ +#define PERCEPTION_ONLINE_EVALUATOR__METRICS__METRIC_HPP_ + +#include "perception_online_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_ONLINE_EVALUATOR__METRICS__METRIC_HPP_ diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp new file mode 100644 index 0000000000000..dd6756a17f194 --- /dev/null +++ b/evaluator/perception_online_evaluator/include/perception_online_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_ONLINE_EVALUATOR__METRICS_CALCULATOR_HPP_ +#define PERCEPTION_ONLINE_EVALUATOR__METRICS_CALCULATOR_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 + +#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_ONLINE_EVALUATOR__METRICS_CALCULATOR_HPP_ diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/parameters.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/parameters.hpp new file mode 100644 index 0000000000000..98cd3c25b71a3 --- /dev/null +++ b/evaluator/perception_online_evaluator/include/perception_online_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_ONLINE_EVALUATOR__PARAMETERS_HPP_ +#define PERCEPTION_ONLINE_EVALUATOR__PARAMETERS_HPP_ + +#include "perception_online_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_ONLINE_EVALUATOR__PARAMETERS_HPP_ 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 new file mode 100644 index 0000000000000..b7535935ccd5f --- /dev/null +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/perception_online_evaluator_node.hpp @@ -0,0 +1,91 @@ +// 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_ONLINE_EVALUATOR__PERCEPTION_ONLINE_EVALUATOR_NODE_HPP_ +#define PERCEPTION_ONLINE_EVALUATOR__PERCEPTION_ONLINE_EVALUATOR_NODE_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" + +#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 PerceptionOnlineEvaluatorNode : public rclcpp::Node +{ +public: + explicit PerceptionOnlineEvaluatorNode(const rclcpp::NodeOptions & node_options); + ~PerceptionOnlineEvaluatorNode() {} + + /** + * @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: + // 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_; + void publishMetrics(); + + // Debug + void publishDebugMarker(); +}; +} // namespace perception_diagnostics + +#endif // PERCEPTION_ONLINE_EVALUATOR__PERCEPTION_ONLINE_EVALUATOR_NODE_HPP_ diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/stat.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/stat.hpp new file mode 100644 index 0000000000000..63494f90d02ee --- /dev/null +++ b/evaluator/perception_online_evaluator/include/perception_online_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_ONLINE_EVALUATOR__STAT_HPP_ +#define PERCEPTION_ONLINE_EVALUATOR__STAT_HPP_ + +namespace perception_diagnostics +{ +/** + * @brief class to incrementally build statistics + * @typedef T type of the values (default to double) + */ +template +class Stat +{ +public: + /** + * @brief add a value + * @param value value to add + */ + void add(const T & value) + { + if (value < min_) { + min_ = value; + } + if (value > max_) { + max_ = value; + } + ++count_; + mean_ = mean_ + (value - mean_) / count_; + } + + /** + * @brief get the mean value + */ + long double mean() const { return mean_; } + + /** + * @brief get the minimum value + */ + T min() const { return min_; } + + /** + * @brief get the maximum value + */ + T max() const { return max_; } + + /** + * @brief get the number of values used to build this statistic + */ + unsigned int count() const { return count_; } + + template + friend std::ostream & operator<<(std::ostream & os, const Stat & stat); + +private: + T min_ = std::numeric_limits::max(); + T max_ = std::numeric_limits::min(); + long double mean_ = 0.0; + unsigned int count_ = 0; +}; + +/** + * @brief overload << operator for easy print to output stream + */ +template +std::ostream & operator<<(std::ostream & os, const Stat & stat) +{ + if (stat.count() == 0) { + os << "None None None"; + } else { + os << stat.min() << " " << stat.max() << " " << stat.mean(); + } + return os; +} + +} // namespace perception_diagnostics + +#endif // PERCEPTION_ONLINE_EVALUATOR__STAT_HPP_ diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/marker_utils.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/marker_utils.hpp new file mode 100644 index 0000000000000..3ac4c1db9efbd --- /dev/null +++ b/evaluator/perception_online_evaluator/include/perception_online_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_ONLINE_EVALUATOR__UTILS__MARKER_UTILS_HPP_ +#define PERCEPTION_ONLINE_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 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 float r, const float g, const float b); + +} // namespace marker_utils + +#endif // PERCEPTION_ONLINE_EVALUATOR__UTILS__MARKER_UTILS_HPP_ diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/objects_filtering.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/objects_filtering.hpp new file mode 100644 index 0000000000000..5d4238f45dec9 --- /dev/null +++ b/evaluator/perception_online_evaluator/include/perception_online_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_ONLINE_EVALUATOR__UTILS__OBJECTS_FILTERING_HPP_ +#define PERCEPTION_ONLINE_EVALUATOR__UTILS__OBJECTS_FILTERING_HPP_ + +#include "perception_online_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_ONLINE_EVALUATOR__UTILS__OBJECTS_FILTERING_HPP_ diff --git a/evaluator/perception_online_evaluator/launch/motion_evaluator.launch.xml b/evaluator/perception_online_evaluator/launch/motion_evaluator.launch.xml new file mode 100644 index 0000000000000..08c4e11126ec1 --- /dev/null +++ b/evaluator/perception_online_evaluator/launch/motion_evaluator.launch.xml @@ -0,0 +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_online_evaluator/package.xml b/evaluator/perception_online_evaluator/package.xml new file mode 100644 index 0000000000000..bc0f7ef82049b --- /dev/null +++ b/evaluator/perception_online_evaluator/package.xml @@ -0,0 +1,42 @@ + + + + perception_online_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_online_evaluator/param/perception_online_evaluator.defaults.yaml b/evaluator/perception_online_evaluator/param/perception_online_evaluator.defaults.yaml new file mode 100644 index 0000000000000..6749bac81aa35 --- /dev/null +++ b/evaluator/perception_online_evaluator/param/perception_online_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_online_evaluator/src/metrics/deviation_metrics.cpp b/evaluator/perception_online_evaluator/src/metrics/deviation_metrics.cpp new file mode 100644 index 0000000000000..7a9435444c065 --- /dev/null +++ b/evaluator/perception_online_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_online_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_online_evaluator/src/metrics_calculator.cpp b/evaluator/perception_online_evaluator/src/metrics_calculator.cpp new file mode 100644 index 0000000000000..bec6d84fcd7dd --- /dev/null +++ b/evaluator/perception_online_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_online_evaluator/metrics_calculator.hpp" + +#include "motion_utils/trajectory/trajectory.hpp" +#include "perception_online_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_online_evaluator/src/perception_online_evaluator_node.cpp b/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp new file mode 100644 index 0000000000000..cf98ccc4c5460 --- /dev/null +++ b/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp @@ -0,0 +1,337 @@ +// 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_online_evaluator/perception_online_evaluator_node.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" + +#include + +#include "boost/lexical_cast.hpp" + +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace perception_diagnostics +{ +PerceptionOnlineEvaluatorNode::PerceptionOnlineEvaluatorNode( + const rclcpp::NodeOptions & node_options) +: Node("perception_online_evaluator", node_options), + parameters_(std::make_shared()), + metrics_calculator_(parameters_) +{ + using std::placeholders::_1; + + google::InitGoogleLogging("perception_online_evaluator_node"); + google::InstallFailureSignalHandler(); + + objects_sub_ = create_subscription( + "~/input/objects", 1, std::bind(&PerceptionOnlineEvaluatorNode::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(&PerceptionOnlineEvaluatorNode::onParameter, this, std::placeholders::_1)); +} + +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()) { + continue; + } + + for (const auto & [metric, stat] : metric_stat_map.value()) { + if (stat.count() > 0) { + metrics_msg.status.push_back(generateDiagnosticStatus(metric, stat)); + } + } + } + + // publish metrics + if (!metrics_msg.status.empty()) { + metrics_msg.header.stamp = now(); + metrics_pub_->publish(metrics_msg); + } + publishDebugMarker(); +} + +DiagnosticStatus PerceptionOnlineEvaluatorNode::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 PerceptionOnlineEvaluatorNode::onObjects(const PredictedObjects::ConstSharedPtr objects_msg) +{ + metrics_calculator_.setPredictedObjects(*objects_msg); + publishMetrics(); +} + +void PerceptionOnlineEvaluatorNode::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, 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.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, 1)); + } + if (p.show_predicted_path_gt) { + 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, 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 PerceptionOnlineEvaluatorNode::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 PerceptionOnlineEvaluatorNode::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::PerceptionOnlineEvaluatorNode) diff --git a/evaluator/perception_online_evaluator/src/utils/marker_utils.cpp b/evaluator/perception_online_evaluator/src/utils/marker_utils.cpp new file mode 100644 index 0000000000000..75af92e83dcd8 --- /dev/null +++ b/evaluator/perception_online_evaluator/src/utils/marker_utils.cpp @@ -0,0 +1,196 @@ +// 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_online_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 + +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 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 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, 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_online_evaluator/src/utils/objects_filtering.cpp b/evaluator/perception_online_evaluator/src/utils/objects_filtering.cpp new file mode 100644 index 0000000000000..97daee36175f3 --- /dev/null +++ b/evaluator/perception_online_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_online_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/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..095fb130426f0 --- /dev/null +++ b/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp @@ -0,0 +1,521 @@ +// 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 "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", 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); +} +// ========================================================================================== diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index 7df537d9db103..cf8029eb3c964 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..3801c448eaed4 100644 --- a/launch/tier4_simulator_launch/launch/simulator.launch.xml +++ b/launch/tier4_simulator_launch/launch/simulator.launch.xml @@ -85,6 +85,11 @@ + + + + +