Skip to content
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.

Commit 734dc8d

Browse files
kosuke55kosuke55
committedFeb 26, 2024·
feat(perception_evaluator): add perception_evaluator
Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> tmp Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> update Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> add Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> add add update clean up Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
1 parent 86b4335 commit 734dc8d

21 files changed

+2090
-0
lines changed
 

‎evaluator/perception_evaluator/CMakeLists.txt

+44
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,44 @@
1+
cmake_minimum_required(VERSION 3.14)
2+
project(perception_evaluator)
3+
4+
find_package(autoware_cmake REQUIRED)
5+
autoware_package()
6+
7+
find_package(pluginlib REQUIRED)
8+
9+
find_package(glog REQUIRED)
10+
11+
ament_auto_add_library(${PROJECT_NAME}_node SHARED
12+
src/metrics_calculator.cpp
13+
src/${PROJECT_NAME}_node.cpp
14+
src/metrics/deviation_metrics.cpp
15+
src/utils/marker_utils.cpp
16+
src/utils/objects_filtering.cpp
17+
)
18+
19+
rclcpp_components_register_node(${PROJECT_NAME}_node
20+
PLUGIN "perception_diagnostics::PerceptionEvaluatorNode"
21+
EXECUTABLE ${PROJECT_NAME}
22+
)
23+
24+
rclcpp_components_register_node(${PROJECT_NAME}_node
25+
PLUGIN "perception_diagnostics::MotionEvaluatorNode"
26+
EXECUTABLE motion_evaluator
27+
)
28+
29+
target_link_libraries(${PROJECT_NAME}_node glog::glog)
30+
31+
# if(BUILD_TESTING)
32+
# ament_add_ros_isolated_gtest(test_${PROJECT_NAME}
33+
# test/test_perception_evaluator_node.cpp
34+
# )
35+
# target_link_libraries(test_${PROJECT_NAME}
36+
# ${PROJECT_NAME}_node
37+
# )
38+
# endif()
39+
40+
ament_auto_package(
41+
INSTALL_TO_SHARE
42+
param
43+
launch
44+
)

‎evaluator/perception_evaluator/README.md

+43
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,43 @@
1+
# Perception Evaluator
2+
3+
A node for evaluating the output of perception systems.
4+
5+
## Purpose
6+
7+
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.
8+
9+
## Inner-workings / Algorithms
10+
11+
- Calculates lateral deviation between the predicted path and the actual traveled trajectory.
12+
- Calculates lateral deviation between the smoothed traveled trajectory and the perceived position to evaluate the stability of lateral position recognition.
13+
- Detects yaw deviation between the smoothed traveled trajectory and the perceived position to evaluate the stability of yaw recognition.
14+
15+
## Inputs / Outputs
16+
17+
| Name | Type | Description |
18+
| ----------------- | ------------------------------------------------------ | ------------------------------------------------- |
19+
| `~/input/objects` | `autoware_auto_perception_msgs::msg::PredictedObjects` | The predicted objects to evaluate. |
20+
| `~/metrics` | `diagnostic_msgs::msg::DiagnosticArray` | Diagnostic information about perception accuracy. |
21+
| `~/markers` | `visualization_msgs::msg::MarkerArray` | Visual markers for debugging and visualization. |
22+
23+
## Parameters
24+
25+
| Name | Type | Description |
26+
| --------------------------------- | ------------ | ------------------------------------------------------------------------------------------------ |
27+
| `selected_metrics` | List | Metrics to be evaluated, such as lateral deviation, yaw deviation, and predicted path deviation. |
28+
| `smoothing_window_size` | Integer | Determines the window size for smoothing path, should be an odd number. |
29+
| `prediction_time_horizons` | list[double] | Time horizons for prediction evaluation in seconds. |
30+
| `target_object.*.check_deviation` | bool | Whether to check deviation for specific object types (car, truck, etc.). |
31+
| `debug_marker.*` | bool | Debugging parameters for marker visualization (history path, predicted path, etc.). |
32+
33+
## Assumptions / Known limits
34+
35+
It is assumed that the current positions of PredictedObjects are reasonably accurate.
36+
37+
## Future extensions / Unimplemented parts
38+
39+
- Increase rate in recognition per class
40+
- Metrics for objects with strange physical behavior (e.g., going through a fence)
41+
- Metrics for splitting objects
42+
- Metrics for problems with objects that are normally stationary but move
43+
- Disappearing object metrics

‎evaluator/perception_evaluator/include/perception_evaluator/metrics/deviation_metrics.hpp

+53
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,53 @@
1+
// Copyright 2024 TIER IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef PERCEPTION_EVALUATOR__METRICS__DEVIATION_METRICS_HPP_
16+
#define PERCEPTION_EVALUATOR__METRICS__DEVIATION_METRICS_HPP_
17+
18+
#include "perception_evaluator/stat.hpp"
19+
20+
#include <autoware_auto_perception_msgs/msg/predicted_path.hpp>
21+
#include <geometry_msgs/msg/pose.hpp>
22+
23+
#include <vector>
24+
25+
namespace perception_diagnostics
26+
{
27+
namespace metrics
28+
{
29+
using autoware_auto_perception_msgs::msg::PredictedPath;
30+
using geometry_msgs::msg::Pose;
31+
32+
/**
33+
* @brief calculate lateral deviation of the given path from the reference path
34+
* @param [in] ref_path reference path
35+
* @param [in] pred_path predicted path
36+
* @return calculated statistics
37+
*/
38+
double calcLateralDeviation(const std::vector<Pose> & ref_path, const Pose & target_pose);
39+
40+
/**
41+
* @brief calculate yaw deviation of the given path from the reference path
42+
* @param [in] ref_path reference path
43+
* @param [in] pred_path predicted path
44+
* @return calculated statistics
45+
*/
46+
double calcYawDeviation(const std::vector<Pose> & ref_path, const Pose & target_pose);
47+
48+
std::vector<double> calcPredictedPathDeviation(
49+
const std::vector<Pose> & ref_path, const PredictedPath & pred_path);
50+
} // namespace metrics
51+
} // namespace perception_diagnostics
52+
53+
#endif // PERCEPTION_EVALUATOR__METRICS__DEVIATION_METRICS_HPP_

‎evaluator/perception_evaluator/include/perception_evaluator/metrics/metric.hpp

+75
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,75 @@
1+
// Copyright 2024 TIER IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef PERCEPTION_EVALUATOR__METRICS__METRIC_HPP_
16+
#define PERCEPTION_EVALUATOR__METRICS__METRIC_HPP_
17+
18+
#include "perception_evaluator/stat.hpp"
19+
20+
#include <iostream>
21+
#include <string>
22+
#include <unordered_map>
23+
#include <vector>
24+
25+
namespace perception_diagnostics
26+
{
27+
/**
28+
* @brief Enumeration of trajectory metrics
29+
*/
30+
enum class Metric {
31+
lateral_deviation,
32+
yaw_deviation,
33+
predicted_path_deviation,
34+
SIZE,
35+
};
36+
37+
using MetricStatMap = std::unordered_map<std::string, Stat<double>>;
38+
39+
static const std::unordered_map<std::string, Metric> str_to_metric = {
40+
{"lateral_deviation", Metric::lateral_deviation},
41+
{"yaw_deviation", Metric::yaw_deviation},
42+
{"predicted_path_deviation", Metric::predicted_path_deviation}};
43+
44+
static const std::unordered_map<Metric, std::string> metric_to_str = {
45+
{Metric::lateral_deviation, "lateral_deviation"},
46+
{Metric::yaw_deviation, "yaw_deviation"},
47+
{Metric::predicted_path_deviation, "predicted_path_deviation"}};
48+
49+
// Metrics descriptions
50+
static const std::unordered_map<Metric, std::string> metric_descriptions = {
51+
{Metric::lateral_deviation, "Lateral_deviation[m]"},
52+
{Metric::yaw_deviation, "Yaw_deviation[rad]"},
53+
{Metric::predicted_path_deviation, "Predicted_path_deviation[m]"}};
54+
55+
namespace details
56+
{
57+
static struct CheckCorrectMaps
58+
{
59+
CheckCorrectMaps()
60+
{
61+
if (
62+
str_to_metric.size() != static_cast<size_t>(Metric::SIZE) ||
63+
metric_to_str.size() != static_cast<size_t>(Metric::SIZE) ||
64+
metric_descriptions.size() != static_cast<size_t>(Metric::SIZE)) {
65+
std::cerr << "[metrics/metrics.hpp] Maps are not defined for all metrics: ";
66+
std::cerr << str_to_metric.size() << " " << metric_to_str.size() << " "
67+
<< metric_descriptions.size() << std::endl;
68+
}
69+
}
70+
} check;
71+
72+
} // namespace details
73+
} // namespace perception_diagnostics
74+
75+
#endif // PERCEPTION_EVALUATOR__METRICS__METRIC_HPP_

‎evaluator/perception_evaluator/include/perception_evaluator/metrics_calculator.hpp

+141
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,141 @@
1+
// Copyright 2024 TIER IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef PERCEPTION_EVALUATOR__METRICS_CALCULATOR_HPP_
16+
#define PERCEPTION_EVALUATOR__METRICS_CALCULATOR_HPP_
17+
18+
#include "perception_evaluator/metrics/deviation_metrics.hpp"
19+
#include "perception_evaluator/metrics/metric.hpp"
20+
#include "perception_evaluator/parameters.hpp"
21+
#include "perception_evaluator/stat.hpp"
22+
23+
#include <rclcpp/time.hpp>
24+
25+
#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp"
26+
#include "geometry_msgs/msg/pose.hpp"
27+
#include <unique_identifier_msgs/msg/uuid.hpp>
28+
29+
#include <algorithm>
30+
#include <map>
31+
#include <optional>
32+
#include <utility>
33+
#include <vector>
34+
35+
namespace perception_diagnostics
36+
{
37+
using autoware_auto_perception_msgs::msg::PredictedObject;
38+
using autoware_auto_perception_msgs::msg::PredictedObjects;
39+
using geometry_msgs::msg::Point;
40+
using geometry_msgs::msg::Pose;
41+
using unique_identifier_msgs::msg::UUID;
42+
43+
struct ObjectData
44+
{
45+
PredictedObject object;
46+
std::vector<std::pair<Pose, Pose>> path_pairs;
47+
48+
std::vector<Pose> getPredictedPath() const
49+
{
50+
std::vector<Pose> path;
51+
path.resize(path_pairs.size());
52+
std::transform(
53+
path_pairs.begin(), path_pairs.end(), path.begin(),
54+
[](const std::pair<Pose, Pose> & pair) -> Pose { return pair.first; });
55+
return path;
56+
}
57+
58+
std::vector<Pose> getHistoryPath() const
59+
{
60+
std::vector<Pose> path;
61+
path.resize(path_pairs.size());
62+
std::transform(
63+
path_pairs.begin(), path_pairs.end(), path.begin(),
64+
[](const std::pair<Pose, Pose> & pair) -> Pose { return pair.second; });
65+
return path;
66+
}
67+
};
68+
using ObjectDataMap = std::unordered_map<std::string, ObjectData>;
69+
70+
// pair of history_path and smoothed_history_path for each object id
71+
using HistoryPathMap =
72+
std::unordered_map<std::string, std::pair<std::vector<Pose>, std::vector<Pose>>>;
73+
74+
class MetricsCalculator
75+
{
76+
public:
77+
explicit MetricsCalculator(const std::shared_ptr<Parameters> & parameters)
78+
: parameters_(parameters){};
79+
80+
/**
81+
* @brief calculate
82+
* @param [in] metric Metric enum value
83+
* @return map of string describing the requested metric and the calculated value
84+
*/
85+
std::optional<MetricStatMap> calculate(const Metric & metric) const;
86+
87+
/**
88+
* @brief set the dynamic objects used to calculate obstacle metrics
89+
* @param [in] objects predicted objects
90+
*/
91+
void setPredictedObjects(const PredictedObjects & objects);
92+
93+
HistoryPathMap getHistoryPathMap() const { return history_path_map_; }
94+
ObjectDataMap getDebugObjectData() const { return debug_target_object_; }
95+
96+
private:
97+
std::shared_ptr<Parameters> parameters_;
98+
99+
// Store predicted objects information and calculation results
100+
std::unordered_map<std::string, std::map<rclcpp::Time, PredictedObject>> object_map_;
101+
HistoryPathMap history_path_map_;
102+
103+
rclcpp::Time current_stamp_;
104+
105+
// debug
106+
mutable ObjectDataMap debug_target_object_;
107+
108+
// Functions to calculate history path
109+
void updateHistoryPath();
110+
std::vector<Pose> averageFilterPath(
111+
const std::vector<Pose> & path, const size_t window_size) const;
112+
std::vector<Pose> generateHistoryPathWithPrev(
113+
const std::vector<Pose> & prev_history_path, const Pose & new_pose, const size_t window_size);
114+
115+
// Update object data
116+
void updateObjects(
117+
const std::string uuid, const rclcpp::Time stamp, const PredictedObject & object);
118+
void deleteOldObjects(const rclcpp::Time stamp);
119+
120+
// Calculate metrics
121+
MetricStatMap calcLateralDeviationMetrics(const PredictedObjects & objects) const;
122+
MetricStatMap calcYawDeviationMetrics(const PredictedObjects & objects) const;
123+
MetricStatMap calcPredictedPathDeviationMetrics(const PredictedObjects & objects) const;
124+
Stat<double> calcPredictedPathDeviationMetrics(
125+
const PredictedObjects & objects, const double time_horizon) const;
126+
127+
bool hasPassedTime(const rclcpp::Time stamp) const;
128+
bool hasPassedTime(const std::string uuid, const rclcpp::Time stamp) const;
129+
double getTimeDelay() const;
130+
131+
// Extract object
132+
rclcpp::Time getClosestStamp(const rclcpp::Time stamp) const;
133+
std::optional<PredictedObject> getObjectByStamp(
134+
const std::string uuid, const rclcpp::Time stamp) const;
135+
PredictedObjects getObjectsByStamp(const rclcpp::Time stamp) const;
136+
137+
}; // class MetricsCalculator
138+
139+
} // namespace perception_diagnostics
140+
141+
#endif // PERCEPTION_EVALUATOR__METRICS_CALCULATOR_HPP_

‎evaluator/perception_evaluator/include/perception_evaluator/parameters.hpp

+58
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,58 @@
1+
// Copyright 2024 TIER IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef PERCEPTION_EVALUATOR__PARAMETERS_HPP_
16+
#define PERCEPTION_EVALUATOR__PARAMETERS_HPP_
17+
18+
#include "perception_evaluator/metrics/metric.hpp"
19+
20+
#include <unordered_map>
21+
#include <vector>
22+
23+
namespace perception_diagnostics
24+
{
25+
/**
26+
* @brief Enumeration of perception metrics
27+
*/
28+
29+
struct ObjectParameter
30+
{
31+
bool check_deviation{false};
32+
};
33+
34+
struct DebugMarkerParameter
35+
{
36+
bool show_history_path{false};
37+
bool show_history_path_arrows{false};
38+
bool show_smoothed_history_path{true};
39+
bool show_smoothed_history_path_arrows{false};
40+
bool show_predicted_path{true};
41+
bool show_predicted_path_gt{true};
42+
bool show_deviation_lines{true};
43+
bool show_object_polygon{true};
44+
};
45+
46+
struct Parameters
47+
{
48+
std::vector<Metric> metrics;
49+
size_t smoothing_window_size{0};
50+
std::vector<double> prediction_time_horizons;
51+
DebugMarkerParameter debug_marker_parameters;
52+
// parameters depend on object class
53+
std::unordered_map<uint8_t, ObjectParameter> object_parameters;
54+
};
55+
56+
} // namespace perception_diagnostics
57+
58+
#endif // PERCEPTION_EVALUATOR__PARAMETERS_HPP_

‎evaluator/perception_evaluator/include/perception_evaluator/perception_evaluator_node.hpp

+95
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,95 @@
1+
// Copyright 2024 TIER IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef PERCEPTION_EVALUATOR__PERCEPTION_EVALUATOR_NODE_HPP_
16+
#define PERCEPTION_EVALUATOR__PERCEPTION_EVALUATOR_NODE_HPP_
17+
18+
#include "perception_evaluator/metrics_calculator.hpp"
19+
#include "perception_evaluator/parameters.hpp"
20+
#include "perception_evaluator/stat.hpp"
21+
#include "rclcpp/rclcpp.hpp"
22+
#include "tf2_ros/buffer.h"
23+
#include "tf2_ros/transform_listener.h"
24+
25+
#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp"
26+
#include "diagnostic_msgs/msg/diagnostic_array.hpp"
27+
#include "nav_msgs/msg/odometry.hpp"
28+
#include "visualization_msgs/msg/marker_array.hpp"
29+
30+
#include <array>
31+
#include <deque>
32+
#include <memory>
33+
#include <string>
34+
#include <vector>
35+
36+
namespace perception_diagnostics
37+
{
38+
using autoware_auto_perception_msgs::msg::ObjectClassification;
39+
using autoware_auto_perception_msgs::msg::PredictedObjects;
40+
using diagnostic_msgs::msg::DiagnosticArray;
41+
using diagnostic_msgs::msg::DiagnosticStatus;
42+
using nav_msgs::msg::Odometry;
43+
44+
using MarkerArray = visualization_msgs::msg::MarkerArray;
45+
46+
/**
47+
* @brief Node for perception evaluation
48+
*/
49+
class PerceptionEvaluatorNode : public rclcpp::Node
50+
{
51+
public:
52+
explicit PerceptionEvaluatorNode(const rclcpp::NodeOptions & node_options);
53+
~PerceptionEvaluatorNode();
54+
55+
/**
56+
* @brief callback on receiving a dynamic objects array
57+
* @param [in] objects_msg received dynamic object array message
58+
*/
59+
void onObjects(const PredictedObjects::ConstSharedPtr objects_msg);
60+
61+
DiagnosticStatus generateDiagnosticStatus(
62+
const std::string metric, const Stat<double> & metric_stat) const;
63+
64+
private:
65+
// Timer
66+
rclcpp::TimerBase::SharedPtr timer_;
67+
void initTimer(double period_s);
68+
void onTimer();
69+
70+
// Subscribers and publishers
71+
rclcpp::Subscription<PredictedObjects>::SharedPtr objects_sub_;
72+
rclcpp::Publisher<DiagnosticArray>::SharedPtr metrics_pub_;
73+
rclcpp::Publisher<MarkerArray>::SharedPtr pub_marker_;
74+
75+
// TF
76+
std::shared_ptr<tf2_ros::TransformListener> transform_listener_{nullptr};
77+
std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
78+
79+
// Parameters
80+
std::shared_ptr<Parameters> parameters_;
81+
void initParameter();
82+
rcl_interfaces::msg::SetParametersResult onParameter(
83+
const std::vector<rclcpp::Parameter> & parameters);
84+
OnSetParametersCallbackHandle::SharedPtr set_param_res_;
85+
86+
// Metrics Calculator
87+
MetricsCalculator metrics_calculator_;
88+
std::deque<rclcpp::Time> stamps_;
89+
90+
// Debug
91+
void publishDebugMarker();
92+
};
93+
} // namespace perception_diagnostics
94+
95+
#endif // PERCEPTION_EVALUATOR__PERCEPTION_EVALUATOR_NODE_HPP_

‎evaluator/perception_evaluator/include/perception_evaluator/stat.hpp

+93
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,93 @@
1+
// Copyright 2024 TIER IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#include <iostream>
16+
#include <limits>
17+
18+
#ifndef PERCEPTION_EVALUATOR__STAT_HPP_
19+
#define PERCEPTION_EVALUATOR__STAT_HPP_
20+
21+
namespace perception_diagnostics
22+
{
23+
/**
24+
* @brief class to incrementally build statistics
25+
* @typedef T type of the values (default to double)
26+
*/
27+
template <typename T = double>
28+
class Stat
29+
{
30+
public:
31+
/**
32+
* @brief add a value
33+
* @param value value to add
34+
*/
35+
void add(const T & value)
36+
{
37+
if (value < min_) {
38+
min_ = value;
39+
}
40+
if (value > max_) {
41+
max_ = value;
42+
}
43+
++count_;
44+
mean_ = mean_ + (value - mean_) / count_;
45+
}
46+
47+
/**
48+
* @brief get the mean value
49+
*/
50+
long double mean() const { return mean_; }
51+
52+
/**
53+
* @brief get the minimum value
54+
*/
55+
T min() const { return min_; }
56+
57+
/**
58+
* @brief get the maximum value
59+
*/
60+
T max() const { return max_; }
61+
62+
/**
63+
* @brief get the number of values used to build this statistic
64+
*/
65+
unsigned int count() const { return count_; }
66+
67+
template <typename U>
68+
friend std::ostream & operator<<(std::ostream & os, const Stat<U> & stat);
69+
70+
private:
71+
T min_ = std::numeric_limits<T>::max();
72+
T max_ = std::numeric_limits<T>::min();
73+
long double mean_ = 0.0;
74+
unsigned int count_ = 0;
75+
};
76+
77+
/**
78+
* @brief overload << operator for easy print to output stream
79+
*/
80+
template <typename T>
81+
std::ostream & operator<<(std::ostream & os, const Stat<T> & stat)
82+
{
83+
if (stat.count() == 0) {
84+
os << "None None None";
85+
} else {
86+
os << stat.min() << " " << stat.max() << " " << stat.mean();
87+
}
88+
return os;
89+
}
90+
91+
} // namespace perception_diagnostics
92+
93+
#endif // PERCEPTION_EVALUATOR__STAT_HPP_

‎evaluator/perception_evaluator/include/perception_evaluator/utils/marker_utils.hpp

+82
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,82 @@
1+
// Copyright 2024 TIER IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef PERCEPTION_EVALUATOR__UTILS__MARKER_UTILS_HPP_
16+
#define PERCEPTION_EVALUATOR__UTILS__MARKER_UTILS_HPP_
17+
18+
#include <vehicle_info_util/vehicle_info.hpp>
19+
20+
#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp"
21+
#include <geometry_msgs/msg/polygon.hpp>
22+
#include <visualization_msgs/msg/marker_array.hpp>
23+
24+
#include <lanelet2_core/Forward.h>
25+
26+
#include <cstdint>
27+
#include <string>
28+
#include <vector>
29+
30+
namespace marker_utils
31+
{
32+
33+
using autoware_auto_perception_msgs::msg::PredictedObject;
34+
using geometry_msgs::msg::Point;
35+
using geometry_msgs::msg::Pose;
36+
using std_msgs::msg::ColorRGBA;
37+
using tier4_autoware_utils::Polygon2d;
38+
using visualization_msgs::msg::Marker;
39+
using visualization_msgs::msg::MarkerArray;
40+
41+
inline int64_t bitShift(int64_t original_id)
42+
{
43+
return original_id << (sizeof(int32_t) * 8 / 2);
44+
}
45+
46+
void addFootprintMarker(
47+
visualization_msgs::msg::Marker & marker, const geometry_msgs::msg::Pose & pose,
48+
const vehicle_info_util::VehicleInfo & vehicle_info);
49+
50+
MarkerArray createFootprintMarkerArray(
51+
const Pose & base_link_pose, const vehicle_info_util::VehicleInfo vehicle_info,
52+
const std::string && ns, const int32_t & id, const float & r, const float & g, const float & b);
53+
54+
MarkerArray createPointsMarkerArray(
55+
const std::vector<Point> points, const std::string & ns, const int32_t id, const float r,
56+
const float g, const float b);
57+
MarkerArray createPointsMarkerArray(
58+
const std::vector<Pose> poses, const std::string & ns, const int32_t id, const float r,
59+
const float g, const float b);
60+
61+
MarkerArray createPoseMarkerArray(
62+
const Pose & pose, std::string && ns, const int32_t & id, const float & r, const float & g,
63+
const float & b);
64+
65+
MarkerArray createPosesMarkerArray(
66+
const std::vector<Pose> poses, std::string && ns, const int32_t & id, const float & r,
67+
const float & g, const float & b, const float & x_scale = 0.5, const float & y_scale = 0.2,
68+
const float & z_scale = 0.2);
69+
70+
std_msgs::msg::ColorRGBA createColorFromString(const std::string & str);
71+
72+
MarkerArray createObjectPolygonMarkerArray(
73+
const PredictedObject & object, std::string && ns, const int32_t & id, const float & r,
74+
const float & g, const float & b);
75+
76+
MarkerArray createDeviationLines(
77+
const std::vector<Pose> poses1, const std::vector<Pose> poses2, const std::string & ns,
78+
const int32_t id, const float r, const float g, const float b);
79+
80+
} // namespace marker_utils
81+
82+
#endif // PERCEPTION_EVALUATOR__UTILS__MARKER_UTILS_HPP_

‎evaluator/perception_evaluator/include/perception_evaluator/utils/objects_filtering.hpp

+128
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,128 @@
1+
// Copyright 2024 TIER IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef PERCEPTION_EVALUATOR__UTILS__OBJECTS_FILTERING_HPP_
16+
#define PERCEPTION_EVALUATOR__UTILS__OBJECTS_FILTERING_HPP_
17+
18+
#include "perception_evaluator/parameters.hpp"
19+
20+
#include <autoware_auto_perception_msgs/msg/object_classification.hpp>
21+
#include <autoware_auto_perception_msgs/msg/predicted_object.hpp>
22+
#include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
23+
24+
#include <memory>
25+
#include <unordered_map>
26+
#include <utility>
27+
#include <vector>
28+
29+
/**
30+
* most of this file is copied from objects_filtering.hpp in safety_check of behavior_path_planner
31+
*/
32+
33+
namespace perception_diagnostics
34+
{
35+
36+
using autoware_auto_perception_msgs::msg::ObjectClassification;
37+
using autoware_auto_perception_msgs::msg::PredictedObject;
38+
using autoware_auto_perception_msgs::msg::PredictedObjects;
39+
40+
std::uint8_t getHighestProbLabel(const std::vector<ObjectClassification> & classification);
41+
42+
/**
43+
* @brief Specifies which object class should be checked.
44+
*/
45+
struct ObjectTypesToCheck
46+
{
47+
bool check_car{true}; ///< Check for cars.
48+
bool check_truck{true}; ///< Check for trucks.
49+
bool check_bus{true}; ///< Check for buses.
50+
bool check_trailer{true}; ///< Check for trailers.
51+
bool check_unknown{true}; ///< Check for unknown object types.
52+
bool check_bicycle{true}; ///< Check for bicycles.
53+
bool check_motorcycle{true}; ///< Check for motorcycles.
54+
bool check_pedestrian{true}; ///< Check for pedestrians.
55+
};
56+
57+
/**
58+
* @brief Determines whether the predicted object type matches any of the target object types
59+
* specified by the user.
60+
*
61+
* @param object The predicted object whose type is to be checked.
62+
* @param target_object_types A structure containing boolean flags for each object type that the
63+
* user is interested in checking.
64+
*
65+
* @return Returns true if the predicted object's highest probability label matches any of the
66+
* specified target object types.
67+
*/
68+
bool isTargetObjectType(
69+
const PredictedObject & object, const ObjectTypesToCheck & target_object_types);
70+
71+
/**
72+
* @brief Filters objects in the 'selected' container based on the provided filter function.
73+
*
74+
* This function partitions the 'selected' container based on the 'filter' function
75+
* and moves objects that satisfy the filter condition to the 'removed' container.
76+
*
77+
* @tparam Func The type of the filter function.
78+
* @param selected [in,out] The container of objects to be filtered.
79+
* @param removed [out] The container where objects not satisfying the filter condition are moved.
80+
* @param filter The filter function that determines whether an object should be removed.
81+
*/
82+
template <typename Func>
83+
void filterObjects(PredictedObjects & selected, PredictedObjects & removed, Func filter)
84+
{
85+
auto partitioned = std::partition(selected.objects.begin(), selected.objects.end(), filter);
86+
removed.objects.insert(removed.objects.end(), partitioned, selected.objects.end());
87+
selected.objects.erase(partitioned, selected.objects.end());
88+
}
89+
90+
/**
91+
* @brief Filters objects in the 'objects' container based on the provided filter function.
92+
*
93+
* This function is an overload that simplifies filtering when you don't need to specify a
94+
* separate 'removed' container. It internally creates a 'removed_objects' container and calls the
95+
* main 'filterObjects' function.
96+
*
97+
* @tparam Func The type of the filter function.
98+
* @param objects [in,out] The container of objects to be filtered.
99+
* @param filter The filter function that determines whether an object should be removed.
100+
*/
101+
template <typename Func>
102+
void filterObjects(PredictedObjects & objects, Func filter)
103+
{
104+
[[maybe_unused]] PredictedObjects removed_objects{};
105+
filterObjects(objects, removed_objects, filter);
106+
}
107+
108+
/**
109+
* @brief Filters the provided objects based on their classification.
110+
*
111+
* @param objects The predicted objects to be filtered.
112+
* @param target_object_types The types of objects to retain after filtering.
113+
*/
114+
void filterObjectsByClass(
115+
PredictedObjects & objects, const ObjectTypesToCheck & target_object_types);
116+
117+
/**
118+
* @brief Filters the provided objects based on their classification.
119+
*
120+
* @param objects The predicted objects to be filtered.
121+
* @param params The parameters for each object class.
122+
*/
123+
void filterDeviationCheckObjects(
124+
PredictedObjects & objects, const std::unordered_map<uint8_t, ObjectParameter> & params);
125+
126+
} // namespace perception_diagnostics
127+
128+
#endif // PERCEPTION_EVALUATOR__UTILS__OBJECTS_FILTERING_HPP_

‎evaluator/perception_evaluator/launch/motion_evaluator.launch.xml

+7
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,7 @@
1+
<launch>
2+
<arg name="input/twist" default="/localization/twist"/>
3+
4+
<node name="motion_evaluator" exec="motion_evaluator" pkg="perception_evaluator" output="screen">
5+
<remap from="~/input/twist" to="$(var input/twist)"/>
6+
</node>
7+
</launch>

‎evaluator/perception_evaluator/launch/perception_evaluator.launch.xml

+12
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,12 @@
1+
<launch>
2+
<arg name="input/objects" default="/perception/object_recognition/objects"/>
3+
4+
<!-- perception evaluator -->
5+
<group>
6+
<node name="perception_evaluator" exec="perception_evaluator" pkg="perception_evaluator">
7+
<param from="$(find-pkg-share perception_evaluator)/param/perception_evaluator.defaults.yaml"/>
8+
<remap from="~/input/objects" to="$(var input/objects)"/>
9+
<remap from="~/metrics" to="/diagnostic/perception_evaluator/metrics"/>
10+
</node>
11+
</group>
12+
</launch>

‎evaluator/perception_evaluator/package.xml

+38
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,38 @@
1+
<?xml version="1.0"?>
2+
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
3+
<package format="3">
4+
<name>perception_evaluator</name>
5+
<version>0.1.0</version>
6+
<description>ROS 2 node for evaluating perception</description>
7+
<maintainer email="kosuke.takeuchi@tier4.jp">Kosuke Takeuchi</maintainer>
8+
<maintainer email="kyoichi.sugahara@tier4.jp">Kyoichi Sugahara</maintainer>
9+
<license>Apache License 2.0</license>
10+
11+
<author email="kosuke.takeuchi@tier4.jp">Kosuke Takeuchi</author>
12+
13+
<buildtool_depend>ament_cmake_auto</buildtool_depend>
14+
<buildtool_depend>autoware_cmake</buildtool_depend>
15+
16+
<depend>autoware_perception_msgs</depend>
17+
<depend>diagnostic_msgs</depend>
18+
<depend>eigen</depend>
19+
<depend>geometry_msgs</depend>
20+
<depend>libgoogle-glog-dev</depend>
21+
<depend>motion_utils</depend>
22+
<depend>nav_msgs</depend>
23+
<depend>pluginlib</depend>
24+
<depend>rclcpp</depend>
25+
<depend>rclcpp_components</depend>
26+
<depend>tf2</depend>
27+
<depend>tf2_ros</depend>
28+
<depend>tier4_autoware_utils</depend>
29+
<depend>vehicle_info_util</depend>
30+
31+
<test_depend>ament_cmake_ros</test_depend>
32+
<test_depend>ament_lint_auto</test_depend>
33+
<test_depend>autoware_lint_common</test_depend>
34+
35+
<export>
36+
<build_type>ament_cmake</build_type>
37+
</export>
38+
</package>

‎evaluator/perception_evaluator/param/perception_evaluator.defaults.yaml

+39
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,39 @@
1+
/**:
2+
ros__parameters:
3+
selected_metrics:
4+
- lateral_deviation
5+
- yaw_deviation
6+
- predicted_path_deviation
7+
8+
# this should be an odd number, because it includes the target point
9+
smoothing_window_size: 11
10+
11+
prediction_time_horizons: [1.0, 2.0, 3.0, 5.0, 10.0]
12+
13+
target_object:
14+
car:
15+
check_deviation: true
16+
truck:
17+
check_deviation: true
18+
bus:
19+
check_deviation: true
20+
trailer:
21+
check_deviation: true
22+
bicycle:
23+
check_deviation: true
24+
motorcycle:
25+
check_deviation: true
26+
pedestrian:
27+
check_deviation: true
28+
unknown:
29+
check_deviation: false
30+
31+
debug_marker:
32+
history_path: false
33+
history_path_arrows: false
34+
smoothed_history_path: true
35+
smoothed_history_path_arrows: false
36+
predicted_path: true
37+
predicted_path_gt: true
38+
deviation_lines: true
39+
object_polygon: true

‎evaluator/perception_evaluator/src/metrics/deviation_metrics.cpp

+65
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,65 @@
1+
// Copyright 2024 TIER IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#include "perception_evaluator/metrics/deviation_metrics.hpp"
16+
17+
#include "tier4_autoware_utils/geometry/geometry.hpp"
18+
#include "tier4_autoware_utils/geometry/pose_deviation.hpp"
19+
20+
#include <motion_utils/trajectory/trajectory.hpp>
21+
22+
namespace perception_diagnostics
23+
{
24+
namespace metrics
25+
{
26+
27+
double calcLateralDeviation(const std::vector<Pose> & ref_path, const Pose & target_pose)
28+
{
29+
if (ref_path.empty()) {
30+
return 0.0;
31+
}
32+
33+
const size_t nearest_index = motion_utils::findNearestIndex(ref_path, target_pose.position);
34+
return std::abs(
35+
tier4_autoware_utils::calcLateralDeviation(ref_path[nearest_index], target_pose.position));
36+
}
37+
38+
double calcYawDeviation(const std::vector<Pose> & ref_path, const Pose & target_pose)
39+
{
40+
if (ref_path.empty()) {
41+
return 0.0;
42+
}
43+
44+
const size_t nearest_index = motion_utils::findNearestIndex(ref_path, target_pose.position);
45+
return std::abs(tier4_autoware_utils::calcYawDeviation(ref_path[nearest_index], target_pose));
46+
}
47+
48+
std::vector<double> calcPredictedPathDeviation(
49+
const std::vector<Pose> & ref_path, const PredictedPath & pred_path)
50+
{
51+
std::vector<double> deviations;
52+
53+
if (ref_path.empty() || pred_path.path.empty()) {
54+
return {};
55+
}
56+
for (const Pose & p : pred_path.path) {
57+
const size_t nearest_index = motion_utils::findNearestIndex(ref_path, p.position);
58+
deviations.push_back(
59+
tier4_autoware_utils::calcDistance2d(ref_path[nearest_index].position, p.position));
60+
}
61+
62+
return deviations;
63+
}
64+
} // namespace metrics
65+
} // namespace perception_diagnostics

‎evaluator/perception_evaluator/src/metrics_calculator.cpp

+458
Large diffs are not rendered by default.

‎evaluator/perception_evaluator/src/perception_evaluator_node.cpp

+348
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,348 @@
1+
// Copyright 2024 TIER IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#include "perception_evaluator/perception_evaluator_node.hpp"
16+
17+
#include "perception_evaluator/utils/marker_utils.hpp"
18+
#include "tier4_autoware_utils/ros/marker_helper.hpp"
19+
#include "tier4_autoware_utils/ros/parameter.hpp"
20+
#include "tier4_autoware_utils/ros/update_param.hpp"
21+
22+
#include <tier4_autoware_utils/ros/uuid_helper.hpp>
23+
24+
#include "boost/lexical_cast.hpp"
25+
26+
#include <glog/logging.h>
27+
28+
#include <fstream>
29+
#include <iostream>
30+
#include <map>
31+
#include <memory>
32+
#include <string>
33+
#include <utility>
34+
#include <vector>
35+
36+
namespace perception_diagnostics
37+
{
38+
PerceptionEvaluatorNode::PerceptionEvaluatorNode(const rclcpp::NodeOptions & node_options)
39+
: Node("perception_evaluator", node_options),
40+
parameters_(std::make_shared<Parameters>()),
41+
metrics_calculator_(parameters_)
42+
{
43+
using std::placeholders::_1;
44+
45+
google::InitGoogleLogging("map_based_prediction_node");
46+
google::InstallFailureSignalHandler();
47+
48+
objects_sub_ = create_subscription<PredictedObjects>(
49+
"~/input/objects", 1, std::bind(&PerceptionEvaluatorNode::onObjects, this, _1));
50+
metrics_pub_ = create_publisher<DiagnosticArray>("~/metrics", 1);
51+
pub_marker_ = create_publisher<MarkerArray>("~/markers", 10);
52+
53+
tf_buffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
54+
transform_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
55+
56+
// Parameters
57+
initParameter();
58+
59+
set_param_res_ = this->add_on_set_parameters_callback(
60+
std::bind(&PerceptionEvaluatorNode::onParameter, this, std::placeholders::_1));
61+
62+
// Timer
63+
initTimer(/*period_s=*/0.1);
64+
}
65+
66+
PerceptionEvaluatorNode::~PerceptionEvaluatorNode()
67+
{
68+
}
69+
70+
void PerceptionEvaluatorNode::initTimer(double period_s)
71+
{
72+
const auto period_ns =
73+
std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::duration<double>(period_s));
74+
timer_ = rclcpp::create_timer(
75+
this, get_clock(), period_ns, std::bind(&PerceptionEvaluatorNode::onTimer, this));
76+
}
77+
78+
void PerceptionEvaluatorNode::onTimer()
79+
{
80+
DiagnosticArray metrics_msg;
81+
for (const Metric & metric : parameters_->metrics) {
82+
const auto metric_stat_map = metrics_calculator_.calculate(Metric(metric));
83+
if (!metric_stat_map.has_value()) {
84+
continue;
85+
}
86+
87+
for (const auto & [metric, stat] : metric_stat_map.value()) {
88+
if (stat.count() > 0) {
89+
metrics_msg.status.push_back(generateDiagnosticStatus(metric, stat));
90+
}
91+
}
92+
}
93+
94+
if (!metrics_msg.status.empty()) {
95+
metrics_msg.header.stamp = now();
96+
metrics_pub_->publish(metrics_msg);
97+
}
98+
publishDebugMarker();
99+
}
100+
101+
DiagnosticStatus PerceptionEvaluatorNode::generateDiagnosticStatus(
102+
const std::string metric, const Stat<double> & metric_stat) const
103+
{
104+
DiagnosticStatus status;
105+
106+
status.level = status.OK;
107+
status.name = metric;
108+
109+
diagnostic_msgs::msg::KeyValue key_value;
110+
key_value.key = "min";
111+
key_value.value = std::to_string(metric_stat.min());
112+
status.values.push_back(key_value);
113+
key_value.key = "max";
114+
key_value.value = std::to_string(metric_stat.max());
115+
status.values.push_back(key_value);
116+
key_value.key = "mean";
117+
key_value.value = std::to_string(metric_stat.mean());
118+
status.values.push_back(key_value);
119+
120+
return status;
121+
}
122+
123+
void PerceptionEvaluatorNode::onObjects(const PredictedObjects::ConstSharedPtr objects_msg)
124+
{
125+
metrics_calculator_.setPredictedObjects(*objects_msg);
126+
}
127+
128+
void PerceptionEvaluatorNode::publishDebugMarker()
129+
{
130+
using marker_utils::createColorFromString;
131+
using marker_utils::createDeviationLines;
132+
using marker_utils::createObjectPolygonMarkerArray;
133+
using marker_utils::createPointsMarkerArray;
134+
using marker_utils::createPosesMarkerArray;
135+
136+
MarkerArray marker;
137+
138+
const auto add = [&marker](MarkerArray added) {
139+
for (auto & marker : added.markers) {
140+
marker.lifetime = rclcpp::Duration::from_seconds(1.5);
141+
}
142+
tier4_autoware_utils::appendMarkerArray(added, &marker);
143+
};
144+
145+
const auto history_path_map = metrics_calculator_.getHistoryPathMap();
146+
const auto & p = parameters_->debug_marker_parameters;
147+
148+
for (const auto & [uuid, history_path] : history_path_map) {
149+
{
150+
const auto c = createColorFromString(uuid + "_raw");
151+
if (p.show_history_path) {
152+
add(createPointsMarkerArray(history_path.first, "history_path_" + uuid, 0, c.r, c.g, c.b));
153+
}
154+
if (p.show_history_path_arrows) {
155+
add(createPosesMarkerArray(
156+
history_path.first, "history_path_arrows_" + uuid, 0, c.r, c.g, c.b, 0.1, 0.05, 0.05));
157+
}
158+
}
159+
{
160+
const auto c = createColorFromString(uuid);
161+
if (p.show_smoothed_history_path) {
162+
add(createPointsMarkerArray(
163+
history_path.second, "smoothed_history_path_" + uuid, 0, c.r, c.g, c.b));
164+
}
165+
if (p.show_smoothed_history_path_arrows) {
166+
add(createPosesMarkerArray(
167+
history_path.second, "smoothed_history_path_arrows_" + uuid, c.r, c.g, c.b, 0, 0.1, 0.05,
168+
0.05));
169+
}
170+
}
171+
}
172+
const auto object_data_map = metrics_calculator_.getDebugObjectData();
173+
for (const auto & [uuid, object_data] : object_data_map) {
174+
const auto c = createColorFromString(uuid);
175+
const auto predicted_path = object_data.getPredictedPath();
176+
const auto history_path = object_data.getHistoryPath();
177+
if (p.show_predicted_path) {
178+
add(createPosesMarkerArray(predicted_path, "predicted_path_" + uuid, 0, 0, 0, 1));
179+
}
180+
if (p.show_predicted_path_gt) {
181+
add(createPosesMarkerArray(history_path, "predicted_path_gt_" + uuid, 0, 1, 0, 0));
182+
}
183+
if (p.show_deviation_lines) {
184+
add(
185+
createDeviationLines(predicted_path, history_path, "deviation_lines_" + uuid, 0, 1, 1, 1));
186+
}
187+
if (p.show_object_polygon) {
188+
add(createObjectPolygonMarkerArray(
189+
object_data.object, "object_polygon_" + uuid, 0, c.r, c.g, c.b));
190+
}
191+
}
192+
193+
pub_marker_->publish(marker);
194+
}
195+
196+
rcl_interfaces::msg::SetParametersResult PerceptionEvaluatorNode::onParameter(
197+
const std::vector<rclcpp::Parameter> & parameters)
198+
{
199+
using tier4_autoware_utils::updateParam;
200+
201+
auto & p = parameters_;
202+
203+
updateParam<size_t>(parameters, "smoothing_window_size", p->smoothing_window_size);
204+
205+
// update metrics
206+
{
207+
std::vector<std::string> metrics_str;
208+
updateParam<std::vector<std::string>>(parameters, "selected_metrics", metrics_str);
209+
std::vector<Metric> metrics;
210+
for (const std::string & selected_metric : metrics_str) {
211+
const Metric metric = str_to_metric.at(selected_metric);
212+
metrics.push_back(metric);
213+
}
214+
p->metrics = metrics;
215+
}
216+
217+
// update parameters for each object class
218+
{
219+
const auto get_object_param = [&](std::string && ns) -> std::optional<ObjectParameter> {
220+
ObjectParameter param{};
221+
if (updateParam<bool>(parameters, ns + "check_deviation", param.check_deviation)) {
222+
return param;
223+
}
224+
return std::nullopt;
225+
};
226+
227+
const std::string ns = "target_object.";
228+
if (const auto new_param = get_object_param(ns + "car.")) {
229+
p->object_parameters.at(ObjectClassification::CAR) = *new_param;
230+
}
231+
if (const auto new_param = get_object_param(ns + "truck.")) {
232+
p->object_parameters.at(ObjectClassification::TRUCK) = *new_param;
233+
}
234+
if (const auto new_param = get_object_param(ns + "bus.")) {
235+
p->object_parameters.at(ObjectClassification::BUS) = *new_param;
236+
}
237+
if (const auto new_param = get_object_param(ns + "trailer.")) {
238+
p->object_parameters.at(ObjectClassification::TRAILER) = *new_param;
239+
}
240+
if (const auto new_param = get_object_param(ns + "bicycle.")) {
241+
p->object_parameters.at(ObjectClassification::BICYCLE) = *new_param;
242+
}
243+
if (const auto new_param = get_object_param(ns + "motorcycle.")) {
244+
p->object_parameters.at(ObjectClassification::MOTORCYCLE) = *new_param;
245+
}
246+
if (const auto new_param = get_object_param(ns + "pedestrian.")) {
247+
p->object_parameters.at(ObjectClassification::PEDESTRIAN) = *new_param;
248+
}
249+
if (const auto new_param = get_object_param(ns + "unknown.")) {
250+
p->object_parameters.at(ObjectClassification::UNKNOWN) = *new_param;
251+
}
252+
}
253+
254+
// update debug marker parameters
255+
{
256+
const std::string ns = "debug_marker.";
257+
updateParam<bool>(
258+
parameters, ns + "history_path", p->debug_marker_parameters.show_history_path);
259+
updateParam<bool>(
260+
parameters, ns + "history_path_arrows", p->debug_marker_parameters.show_history_path_arrows);
261+
updateParam<bool>(
262+
parameters, ns + "smoothed_history_path",
263+
p->debug_marker_parameters.show_smoothed_history_path);
264+
updateParam<bool>(
265+
parameters, ns + "smoothed_history_path_arrows",
266+
p->debug_marker_parameters.show_smoothed_history_path_arrows);
267+
updateParam<bool>(
268+
parameters, ns + "predicted_path", p->debug_marker_parameters.show_predicted_path);
269+
updateParam<bool>(
270+
parameters, ns + "predicted_path_gt", p->debug_marker_parameters.show_predicted_path_gt);
271+
updateParam<bool>(
272+
parameters, ns + "deviation_lines", p->debug_marker_parameters.show_deviation_lines);
273+
updateParam<bool>(
274+
parameters, ns + "object_polygon", p->debug_marker_parameters.show_object_polygon);
275+
}
276+
277+
rcl_interfaces::msg::SetParametersResult result;
278+
result.successful = true;
279+
result.reason = "success";
280+
281+
return result;
282+
}
283+
284+
void PerceptionEvaluatorNode::initParameter()
285+
{
286+
using tier4_autoware_utils::getOrDeclareParameter;
287+
using tier4_autoware_utils::updateParam;
288+
289+
auto & p = parameters_;
290+
291+
p->smoothing_window_size = getOrDeclareParameter<int>(*this, "smoothing_window_size");
292+
p->prediction_time_horizons =
293+
getOrDeclareParameter<std::vector<double>>(*this, "prediction_time_horizons");
294+
295+
// set metrics
296+
const auto selected_metrics =
297+
getOrDeclareParameter<std::vector<std::string>>(*this, "selected_metrics");
298+
for (const std::string & selected_metric : selected_metrics) {
299+
const Metric metric = str_to_metric.at(selected_metric);
300+
parameters_->metrics.push_back(metric);
301+
}
302+
303+
// set parameters for each object class
304+
{
305+
const auto get_object_param = [&](std::string && ns) -> ObjectParameter {
306+
ObjectParameter param{};
307+
param.check_deviation = getOrDeclareParameter<bool>(*this, ns + "check_deviation");
308+
return param;
309+
};
310+
311+
const std::string ns = "target_object.";
312+
p->object_parameters.emplace(ObjectClassification::CAR, get_object_param(ns + "car."));
313+
p->object_parameters.emplace(ObjectClassification::TRUCK, get_object_param(ns + "truck."));
314+
p->object_parameters.emplace(ObjectClassification::BUS, get_object_param(ns + "bus."));
315+
p->object_parameters.emplace(ObjectClassification::TRAILER, get_object_param(ns + "trailer."));
316+
p->object_parameters.emplace(ObjectClassification::BICYCLE, get_object_param(ns + "bicycle."));
317+
p->object_parameters.emplace(
318+
ObjectClassification::MOTORCYCLE, get_object_param(ns + "motorcycle."));
319+
p->object_parameters.emplace(
320+
ObjectClassification::PEDESTRIAN, get_object_param(ns + "pedestrian."));
321+
p->object_parameters.emplace(ObjectClassification::UNKNOWN, get_object_param(ns + "unknown."));
322+
}
323+
324+
// set debug marker parameters
325+
{
326+
const std::string ns = "debug_marker.";
327+
p->debug_marker_parameters.show_history_path =
328+
getOrDeclareParameter<bool>(*this, ns + "history_path");
329+
p->debug_marker_parameters.show_history_path_arrows =
330+
getOrDeclareParameter<bool>(*this, ns + "history_path_arrows");
331+
p->debug_marker_parameters.show_smoothed_history_path =
332+
getOrDeclareParameter<bool>(*this, ns + "smoothed_history_path");
333+
p->debug_marker_parameters.show_smoothed_history_path_arrows =
334+
getOrDeclareParameter<bool>(*this, ns + "smoothed_history_path_arrows");
335+
p->debug_marker_parameters.show_predicted_path =
336+
getOrDeclareParameter<bool>(*this, ns + "predicted_path");
337+
p->debug_marker_parameters.show_predicted_path_gt =
338+
getOrDeclareParameter<bool>(*this, ns + "predicted_path_gt");
339+
p->debug_marker_parameters.show_deviation_lines =
340+
getOrDeclareParameter<bool>(*this, ns + "deviation_lines");
341+
p->debug_marker_parameters.show_object_polygon =
342+
getOrDeclareParameter<bool>(*this, ns + "object_polygon");
343+
}
344+
}
345+
} // namespace perception_diagnostics
346+
347+
#include "rclcpp_components/register_node_macro.hpp"
348+
RCLCPP_COMPONENTS_REGISTER_NODE(perception_diagnostics::PerceptionEvaluatorNode)

‎evaluator/perception_evaluator/src/utils/marker_utils.cpp

+198
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,198 @@
1+
// Copyright 2024 TIER IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#include "perception_evaluator/utils/marker_utils.hpp"
16+
17+
#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp"
18+
#include "tier4_autoware_utils/geometry/geometry.hpp"
19+
20+
#include <tier4_autoware_utils/geometry/boost_polygon_utils.hpp>
21+
#include <tier4_autoware_utils/ros/marker_helper.hpp>
22+
#include <tier4_autoware_utils/ros/uuid_helper.hpp>
23+
24+
#include <lanelet2_core/primitives/CompoundPolygon.h>
25+
#include <lanelet2_core/primitives/Lanelet.h>
26+
#include <lanelet2_core/primitives/LineString.h>
27+
28+
#include <cstdint>
29+
30+
namespace marker_utils
31+
{
32+
using std_msgs::msg::ColorRGBA;
33+
using tier4_autoware_utils::calcOffsetPose;
34+
using tier4_autoware_utils::createDefaultMarker;
35+
using tier4_autoware_utils::createMarkerColor;
36+
using tier4_autoware_utils::createMarkerOrientation;
37+
using tier4_autoware_utils::createMarkerScale;
38+
using tier4_autoware_utils::createPoint;
39+
using visualization_msgs::msg::Marker;
40+
41+
void addFootprintMarker(
42+
visualization_msgs::msg::Marker & marker, const geometry_msgs::msg::Pose & pose,
43+
const vehicle_info_util::VehicleInfo & vehicle_info)
44+
{
45+
const double half_width = vehicle_info.vehicle_width_m / 2.0;
46+
const double base_to_front = vehicle_info.vehicle_length_m - vehicle_info.rear_overhang_m;
47+
const double base_to_rear = vehicle_info.rear_overhang_m;
48+
49+
marker.points.push_back(
50+
tier4_autoware_utils::calcOffsetPose(pose, base_to_front, -half_width, 0.0).position);
51+
marker.points.push_back(
52+
tier4_autoware_utils::calcOffsetPose(pose, base_to_front, half_width, 0.0).position);
53+
marker.points.push_back(
54+
tier4_autoware_utils::calcOffsetPose(pose, -base_to_rear, half_width, 0.0).position);
55+
marker.points.push_back(
56+
tier4_autoware_utils::calcOffsetPose(pose, -base_to_rear, -half_width, 0.0).position);
57+
marker.points.push_back(marker.points.front());
58+
}
59+
60+
MarkerArray createFootprintMarkerArray(
61+
const Pose & base_link_pose, const vehicle_info_util::VehicleInfo vehicle_info,
62+
const std::string && ns, const int32_t & id, const float & r, const float & g, const float & b)
63+
{
64+
const auto current_time = rclcpp::Clock{RCL_ROS_TIME}.now();
65+
MarkerArray msg;
66+
67+
Marker marker = createDefaultMarker(
68+
"map", current_time, ns, id, Marker::LINE_STRIP, createMarkerScale(0.2, 0.2, 0.2),
69+
createMarkerColor(r, g, b, 0.999));
70+
marker.lifetime = rclcpp::Duration::from_seconds(1.5);
71+
72+
MarkerArray marker_array;
73+
74+
addFootprintMarker(marker, base_link_pose, vehicle_info);
75+
msg.markers.push_back(marker);
76+
return msg;
77+
}
78+
79+
MarkerArray createPointsMarkerArray(
80+
const std::vector<Point> points, const std::string & ns, const int32_t id, const float r,
81+
const float g, const float b)
82+
{
83+
auto marker = createDefaultMarker(
84+
"map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, id, Marker::LINE_STRIP,
85+
createMarkerScale(0.05, 0.0, 0.0), createMarkerColor(r, g, b, 0.999));
86+
87+
for (const auto & point : points) {
88+
marker.points.push_back(point);
89+
}
90+
91+
MarkerArray msg;
92+
msg.markers.push_back(marker);
93+
return msg;
94+
}
95+
96+
MarkerArray createPointsMarkerArray(
97+
const std::vector<Pose> poses, const std::string & ns, const int32_t id, const float r,
98+
const float g, const float b)
99+
{
100+
auto marker = createDefaultMarker(
101+
"map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, id, Marker::LINE_STRIP,
102+
createMarkerScale(0.05, 0.0, 0.0), createMarkerColor(r, g, b, 0.999));
103+
104+
for (const auto & pose : poses) {
105+
marker.points.push_back(pose.position);
106+
}
107+
108+
MarkerArray msg;
109+
msg.markers.push_back(marker);
110+
return msg;
111+
}
112+
113+
MarkerArray createDeviationLines(
114+
const std::vector<Pose> poses1, const std::vector<Pose> poses2, const std::string & ns,
115+
const int32_t id, const float r, const float g, const float b)
116+
{
117+
MarkerArray msg;
118+
119+
const size_t max_idx = std::min(poses1.size(), poses2.size());
120+
for (size_t i = 0; i < max_idx; ++i) {
121+
auto marker = createDefaultMarker(
122+
"map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, i, Marker::LINE_STRIP,
123+
createMarkerScale(0.005, 0.0, 0.0), createMarkerColor(r, g, b, 0.5));
124+
marker.points.push_back(poses1.at(i).position);
125+
marker.points.push_back(poses2.at(i).position);
126+
msg.markers.push_back(marker);
127+
}
128+
129+
return msg;
130+
}
131+
132+
MarkerArray createPoseMarkerArray(
133+
const Pose & pose, std::string && ns, const int32_t & id, const float & r, const float & g,
134+
const float & b)
135+
{
136+
MarkerArray msg;
137+
138+
Marker marker = createDefaultMarker(
139+
"map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, id, Marker::ARROW,
140+
createMarkerScale(0.7, 0.3, 0.3), createMarkerColor(r, g, b, 0.999));
141+
marker.pose = pose;
142+
msg.markers.push_back(marker);
143+
144+
return msg;
145+
}
146+
147+
MarkerArray createPosesMarkerArray(
148+
const std::vector<Pose> poses, std::string && ns, const int32_t & id, const float & r,
149+
const float & g, const float & b, const float & x_scale, const float & y_scale,
150+
const float & z_scale)
151+
{
152+
MarkerArray msg;
153+
154+
for (size_t i = 0; i < poses.size(); ++i) {
155+
Marker marker = createDefaultMarker(
156+
"map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, id + i, Marker::ARROW,
157+
createMarkerScale(x_scale, y_scale, z_scale), createMarkerColor(r, g, b, 0.5));
158+
marker.pose = poses.at(i);
159+
msg.markers.push_back(marker);
160+
}
161+
162+
return msg;
163+
}
164+
165+
std_msgs::msg::ColorRGBA createColorFromString(const std::string & str)
166+
{
167+
const auto hash = std::hash<std::string>{}(str);
168+
const auto r = (hash & 0xFF) / 255.0;
169+
const auto g = ((hash >> 8) & 0xFF) / 255.0;
170+
const auto b = ((hash >> 16) & 0xFF) / 255.0;
171+
return tier4_autoware_utils::createMarkerColor(r, g, b, 0.5);
172+
}
173+
174+
MarkerArray createObjectPolygonMarkerArray(
175+
const PredictedObject & object, std::string && ns, const int32_t & id, const float & r,
176+
const float & g, const float & b)
177+
{
178+
MarkerArray msg;
179+
180+
auto marker = createDefaultMarker(
181+
"map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, id, Marker::LINE_STRIP,
182+
createMarkerScale(0.05, 0.0, 0.0), createMarkerColor(r, g, b, 0.999));
183+
184+
const double z = object.kinematics.initial_pose_with_covariance.pose.position.z;
185+
const double height = object.shape.dimensions.z;
186+
const auto polygon = tier4_autoware_utils::toPolygon2d(
187+
object.kinematics.initial_pose_with_covariance.pose, object.shape);
188+
for (const auto & p : polygon.outer()) {
189+
marker.points.push_back(createPoint(p.x(), p.y(), z - height / 2));
190+
marker.points.push_back(createPoint(p.x(), p.y(), z + height / 2));
191+
}
192+
marker.id = id;
193+
msg.markers.push_back(marker);
194+
195+
return msg;
196+
}
197+
198+
} // namespace marker_utils

‎evaluator/perception_evaluator/src/utils/objects_filtering.cpp

+103
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,103 @@
1+
// Copyright 2024 TIER IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#include "perception_evaluator/utils/objects_filtering.hpp"
16+
17+
namespace perception_diagnostics
18+
{
19+
ObjectTypesToCheck getDeviationCheckObjectTypes(
20+
const std::unordered_map<uint8_t, ObjectParameter> & params)
21+
{
22+
ObjectTypesToCheck object_types_to_check;
23+
for (const auto & [object_class, object_param] : params) {
24+
switch (object_class) {
25+
case ObjectClassification::CAR:
26+
object_types_to_check.check_car = object_param.check_deviation;
27+
break;
28+
case ObjectClassification::TRUCK:
29+
object_types_to_check.check_truck = object_param.check_deviation;
30+
break;
31+
case ObjectClassification::BUS:
32+
object_types_to_check.check_bus = object_param.check_deviation;
33+
break;
34+
case ObjectClassification::TRAILER:
35+
object_types_to_check.check_trailer = object_param.check_deviation;
36+
break;
37+
case ObjectClassification::BICYCLE:
38+
object_types_to_check.check_bicycle = object_param.check_deviation;
39+
break;
40+
case ObjectClassification::MOTORCYCLE:
41+
object_types_to_check.check_motorcycle = object_param.check_deviation;
42+
break;
43+
case ObjectClassification::PEDESTRIAN:
44+
object_types_to_check.check_pedestrian = object_param.check_deviation;
45+
break;
46+
case ObjectClassification::UNKNOWN:
47+
object_types_to_check.check_unknown = object_param.check_deviation;
48+
break;
49+
default:
50+
break;
51+
}
52+
}
53+
return object_types_to_check;
54+
}
55+
56+
std::uint8_t getHighestProbLabel(const std::vector<ObjectClassification> & classification)
57+
{
58+
std::uint8_t label = ObjectClassification::UNKNOWN;
59+
float highest_prob = 0.0;
60+
for (const auto & _class : classification) {
61+
if (highest_prob < _class.probability) {
62+
highest_prob = _class.probability;
63+
label = _class.label;
64+
}
65+
}
66+
67+
return label;
68+
}
69+
70+
bool isTargetObjectType(
71+
const PredictedObject & object, const ObjectTypesToCheck & target_object_types)
72+
{
73+
const auto t = getHighestProbLabel(object.classification);
74+
75+
return (
76+
(t == ObjectClassification::CAR && target_object_types.check_car) ||
77+
(t == ObjectClassification::TRUCK && target_object_types.check_truck) ||
78+
(t == ObjectClassification::BUS && target_object_types.check_bus) ||
79+
(t == ObjectClassification::TRAILER && target_object_types.check_trailer) ||
80+
(t == ObjectClassification::UNKNOWN && target_object_types.check_unknown) ||
81+
(t == ObjectClassification::BICYCLE && target_object_types.check_bicycle) ||
82+
(t == ObjectClassification::MOTORCYCLE && target_object_types.check_motorcycle) ||
83+
(t == ObjectClassification::PEDESTRIAN && target_object_types.check_pedestrian));
84+
}
85+
86+
void filterObjectsByClass(
87+
PredictedObjects & objects, const ObjectTypesToCheck & target_object_types)
88+
{
89+
const auto filter = [&](const auto & object) {
90+
return isTargetObjectType(object, target_object_types);
91+
};
92+
93+
filterObjects(objects, filter);
94+
}
95+
96+
void filterDeviationCheckObjects(
97+
PredictedObjects & objects, const std::unordered_map<uint8_t, ObjectParameter> & params)
98+
{
99+
const auto object_types = getDeviationCheckObjectTypes(params);
100+
filterObjectsByClass(objects, object_types);
101+
}
102+
103+
} // namespace perception_diagnostics

‎launch/tier4_perception_launch/launch/perception.launch.xml

+5
Original file line numberDiff line numberDiff line change
@@ -247,4 +247,9 @@
247247
</include>
248248
</group>
249249
</group>
250+
251+
<!-- perception evaluator -->
252+
<group>
253+
<include file="$(find-pkg-share perception_evaluator)/launch/perception_evaluator.launch.xml"/>
254+
</group>
250255
</launch>

‎launch/tier4_simulator_launch/launch/simulator.launch.xml

+5
Original file line numberDiff line numberDiff line change
@@ -85,6 +85,11 @@
8585
</group>
8686
</group>
8787

88+
<!-- perception evaluator -->
89+
<group>
90+
<include file="$(find-pkg-share perception_evaluator)/launch/perception_evaluator.launch.xml"/>
91+
</group>
92+
8893
<!-- publish empty objects instead of object recognition module -->
8994
<group unless="$(var perception/enable_object_recognition)">
9095
<push-ros-namespace namespace="object_recognition"/>

0 commit comments

Comments
 (0)
Please sign in to comment.