Skip to content

Commit cd0fd46

Browse files
t4-adct4-adc
committed
WIP add accumulator-based metrics.
1 parent 585a84d commit cd0fd46

21 files changed

+1391
-179
lines changed

‎evaluator/autoware_planning_evaluator/config/planning_evaluator.param.yaml

+78-43
Original file line numberDiff line numberDiff line change
@@ -2,7 +2,7 @@
22
ros__parameters:
33
ego_frame: base_link # reference frame of ego
44

5-
selected_metrics:
5+
metrics_for_publish:
66
- curvature
77
- point_interval
88
- relative_angle
@@ -25,6 +25,36 @@
2525
- modified_goal_lateral_deviation
2626
- modified_goal_yaw_deviation
2727
- stop_decision
28+
- abnormal_stop_decision
29+
- blinker_change_count
30+
- steer_change_count
31+
32+
metrics_for_output:
33+
- curvature
34+
- point_interval
35+
- relative_angle
36+
- resampled_relative_angle
37+
- length
38+
- duration
39+
- velocity
40+
- acceleration
41+
- jerk
42+
- lateral_deviation
43+
- yaw_deviation
44+
- velocity_deviation
45+
- lateral_trajectory_displacement_local
46+
- lateral_trajectory_displacement_lookahead
47+
- stability
48+
- stability_frechet
49+
- obstacle_distance
50+
- obstacle_ttc
51+
- modified_goal_longitudinal_deviation
52+
- modified_goal_lateral_deviation
53+
- modified_goal_yaw_deviation
54+
- stop_decision
55+
- abnormal_stop_decision
56+
- blinker_change_count
57+
- steer_change_count
2858

2959
trajectory:
3060
min_point_dist_m: 0.1 # [m] minimum distance between two successive points to use for angle calculation
@@ -36,48 +66,53 @@
3666
obstacle:
3767
dist_thr_m: 1.0 # [m] distance between ego and the obstacle below which a collision is considered
3868

39-
planning_factor_counters:
40-
topic_prefix: /planning/planning_factors/
41-
stop_decision:
42-
time_threshold_s: 5.0 # [s] time threshold for a stop to be considered as the new one
43-
dist_threshold_m: 5.0 # [m] distance threshold for a stop to be considered as the new one
44-
abnormal_deceleration_threshold: 2.0 # [m/s^2] deceleration threshold for a stop to be considered as abnormal
45-
module_list: # list of modules to be checked for stop deciation.
46-
- avoidance_by_lane_change
47-
- behavior_path_planner
48-
- blind_spot
49-
- crosswalk
50-
- detection_area
51-
- dynamic_obstacle_avoidance
52-
- dynamic_obstacle_stop
53-
- goal_planner
54-
- intersection
55-
- lane_change_left
56-
- lane_change_right
57-
- motion_velocity_planner
58-
- merge_from_private
59-
- no_drivable_lane
60-
- no_stopping_area
61-
- obstacle_cruise
62-
- obstacle_cruise_planner
63-
- obstacle_slow_down
64-
- obstacle_stop
65-
- obstacle_stop_planner
66-
- occlusion_spot
67-
- out_of_lane
68-
- run_out
69-
- side_shift
70-
- start_planner
71-
- static_obstacle_avoidance
72-
- stop_line
73-
- surround_obstacle_checker
74-
- traffic_light
75-
- virtual_traffic_light
76-
- walkway
69+
stop_decision:
70+
topic_prefix: /planning/planning_factors/ # topic prefix for planning factors
71+
time_count_threshold_s: 60.0 # [s] time threshold to count a stop as a new one
72+
dist_count_threshold_m: 5.0 # [m] distance threshold to count a stop as a new one
73+
abnormal_deceleration_threshold_mps2: 2.0 # [m/s^2] deceleration threshold for a stop to be considered as abnormal
74+
module_list: # list of modules to be checked for stop deciation.
75+
- avoidance_by_lane_change
76+
- behavior_path_planner
77+
- blind_spot
78+
- crosswalk
79+
- detection_area
80+
- dynamic_obstacle_avoidance
81+
- dynamic_obstacle_stop
82+
- goal_planner
83+
- intersection
84+
- lane_change_left
85+
- lane_change_right
86+
- motion_velocity_planner
87+
- merge_from_private
88+
- no_drivable_lane
89+
- no_stopping_area
90+
- obstacle_cruise
91+
- obstacle_cruise_planner
92+
- obstacle_slow_down
93+
- obstacle_stop
94+
- obstacle_stop_planner
95+
- occlusion_spot
96+
- out_of_lane
97+
- run_out
98+
- side_shift
99+
- start_planner
100+
- static_obstacle_avoidance
101+
- stop_line
102+
- surround_obstacle_checker
103+
- traffic_light
104+
- virtual_traffic_light
105+
- walkway
106+
107+
blinker_change_count:
108+
window_duration_s: 5.0 # [s] window duration of counting blinker change for publishing
109+
110+
steer_change_count:
111+
window_duration_s: 5.0 # [s] window duration of counting steer change for publishing
112+
steer_rate_margin_radps: 0.1 # [rad/s] margin of steer_rate around 0 to count as steer change
77113

78114
# TODO：
79-
# 用一个metrics_counter.cpp/hpp，写一个大类，包含各种validate(或者自己counter，返回有效和无效即可)和所需参数。
80-
# 在metrics_counter中为所有类以及两种stop，制造两个列表:一个列表：里面是structure保存上一个stop的时间地点和计数值。另一个列表是int，保存计数值。
115+
# 写测试，做PR。
116+
# 整理单元测试和schema。
117+
# 整理READEME文档。
81118

82-
# 写一个onPlanningFactor的类，收集各factor并用metrics_counter验证记入成功或失败。
83-
# ？是否需要publish当前的counter?或者publish counter成功/失败?

‎evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metric_accumulators/blinker_accumulator.hpp

+84
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,84 @@
1+
// Copyright 2025 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 AUTOWARE__PLANNING_EVALUATOR__METRIC_ACCUMULATORS__BLINKER_ACCUMULATOR_HPP_
16+
#define AUTOWARE__PLANNING_EVALUATOR__METRIC_ACCUMULATORS__BLINKER_ACCUMULATOR_HPP_
17+
18+
#include "autoware/planning_evaluator/metrics/metric.hpp"
19+
#include "autoware/planning_evaluator/metrics/output_metric.hpp"
20+
21+
#include <autoware_utils/math/accumulator.hpp>
22+
#include <nlohmann/json.hpp>
23+
24+
#include <autoware_vehicle_msgs/msg/turn_indicators_report.hpp>
25+
#include <tier4_metric_msgs/msg/metric.hpp>
26+
#include <tier4_metric_msgs/msg/metric_array.hpp>
27+
28+
#include <cstdint>
29+
#include <deque>
30+
31+
namespace planning_diagnostics
32+
{
33+
using autoware_utils::Accumulator;
34+
using autoware_vehicle_msgs::msg::TurnIndicatorsReport;
35+
using MetricMsg = tier4_metric_msgs::msg::Metric;
36+
using MetricArrayMsg = tier4_metric_msgs::msg::MetricArray;
37+
using json = nlohmann::json;
38+
39+
/**
40+
* @class BlinkerAccumulator
41+
* @brief Accumulator to generate blinker-related metrics from TurnIndicatorsReport messages.
42+
*/
43+
class BlinkerAccumulator
44+
{
45+
public:
46+
struct Parameters
47+
{
48+
double window_duration_s = 5.0; // [s] Duration to count blinker changes for publishing
49+
} parameters;
50+
51+
BlinkerAccumulator() = default;
52+
~BlinkerAccumulator() = default;
53+
54+
/**
55+
* @brief update the accumulator with new blinker data
56+
* @param msg new TurnIndicatorsReport msg to update the accumulator state
57+
*/
58+
void update(const TurnIndicatorsReport & msg);
59+
60+
/**
61+
* @brief get the output json data for the OutputMetric
62+
* @return json data
63+
*/
64+
json getOutputJson(const OutputMetric & output_metric) const;
65+
66+
/**
67+
* @brief Add metrics to MetricArray for the given Metric
68+
* @param metric Metric to add to the MetricArrayMsg
69+
* @param metrics_msg MetricArrayMsg to add the metric to
70+
*/
71+
bool addMetricMsg(const Metric & metric, MetricArrayMsg & metrics_msg) const;
72+
73+
private:
74+
// statistics
75+
uint8_t prev_blinker_ = TurnIndicatorsReport::DISABLE;
76+
long blinker_change_count_total_ = 0;
77+
long blinker_change_count_in_window_ = 0;
78+
std::deque<double> blinker_change_window_;
79+
Accumulator<double> blinker_change_count_accumulator_;
80+
};
81+
82+
} // namespace planning_diagnostics
83+
84+
#endif // AUTOWARE__PLANNING_EVALUATOR__METRIC_ACCUMULATORS__BLINKER_ACCUMULATOR_HPP_

‎evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metric_accumulators/common_accumulator.hpp

+72
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,72 @@
1+
// Copyright 2025 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 AUTOWARE__PLANNING_EVALUATOR__METRIC_ACCUMULATORS__COMMON_ACCUMULATOR_HPP_
16+
#define AUTOWARE__PLANNING_EVALUATOR__METRIC_ACCUMULATORS__COMMON_ACCUMULATOR_HPP_
17+
18+
#include "autoware/planning_evaluator/metrics/output_metric.hpp"
19+
20+
#include <autoware_utils/math/accumulator.hpp>
21+
#include <nlohmann/json.hpp>
22+
23+
namespace planning_diagnostics
24+
{
25+
using autoware_utils::Accumulator;
26+
using json = nlohmann::json;
27+
28+
/**
29+
* @class CommonAccumulator
30+
* @brief Accumulator to generate OutputMetric json result from normal Metric.
31+
*/
32+
class CommonAccumulator
33+
{
34+
public:
35+
CommonAccumulator() = default;
36+
~CommonAccumulator() = default;
37+
38+
/**
39+
* @brief update the accumulator with new statistics-based metrics with count of data points
40+
* @param accumulator new metric data to add to the accumulator
41+
* @param count number of data points to count
42+
*/
43+
void update(const Accumulator<double> & accumulator, const int count);
44+
45+
/**
46+
* @brief update the accumulator with new statistics-based metrics with count of update times
47+
* @param accumulator new metric data to add to the accumulator
48+
*/
49+
void update(const Accumulator<double> & accumulator) { update(accumulator, 1); }
50+
51+
/**
52+
* @brief update the accumulator with new value-based metrics
53+
* @param value new metric data to add to the accumulator
54+
*/
55+
void update(const double value);
56+
57+
/**
58+
* @brief get the output json data for the OutputMetric
59+
* @return json data
60+
*/
61+
json getOutputJson(const OutputMetric & output_metric) const;
62+
63+
private:
64+
Accumulator<double> min_accumulator_;
65+
Accumulator<double> max_accumulator_;
66+
Accumulator<double> mean_accumulator_;
67+
int count_ = 0;
68+
};
69+
70+
} // namespace planning_diagnostics
71+
72+
#endif // AUTOWARE__PLANNING_EVALUATOR__METRIC_ACCUMULATORS__COMMON_ACCUMULATOR_HPP_

0 commit comments

Comments
 (0)