Skip to content

Commit 14eb609

Browse files
xtk8532704xtk8532704
committed
add unit test.
1 parent abb189d commit 14eb609

File tree

4 files changed

+430
-20
lines changed

4 files changed

+430
-20
lines changed

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

+2-3
Original file line numberDiff line numberDiff line change
@@ -70,7 +70,7 @@
7070
topic_prefix: /planning/planning_factors/ # topic prefix for planning factors
7171
time_count_threshold_s: 60.0 # [s] time threshold to count a stop as a new one
7272
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
73+
abnormal_deceleration_threshold_mps2: 2.0 # [m/s^2] abnormal deceleration threshold for a stop to be considered as abnormal
7474
module_list: # list of modules to be checked for stop deciation.
7575
- avoidance_by_lane_change
7676
- behavior_path_planner
@@ -112,6 +112,5 @@
112112
steer_rate_margin_radps: 0.1 # [rad/s] margin of steer_rate around 0 to count as steer change
113113

114114
# TODO：
115-
# 写测试，做PR。
116-
# 整理单元测试和schema。
117115
# 整理READEME文档。
116+
# 写测试，截图，做PR。

‎evaluator/autoware_planning_evaluator/schema/autoware_planning_evaluator.schema.json

+153-8
Original file line numberDiff line numberDiff line change
@@ -11,8 +11,8 @@
1111
"type": "string",
1212
"default": "base_link"
1313
},
14-
"selected_metrics": {
15-
"description": "metrics to collect/record",
14+
"metrics_for_publish": {
15+
"description": "metrics to collect and publish",
1616
"type": "array",
1717
"items": {
1818
"type": "string"
@@ -30,14 +30,53 @@
3030
"lateral_deviation",
3131
"yaw_deviation",
3232
"velocity_deviation",
33-
"lateral_trajectory_displacement",
33+
"lateral_trajectory_displacement_local",
34+
"lateral_trajectory_displacement_global",
3435
"stability",
3536
"stability_frechet",
3637
"obstacle_distance",
3738
"obstacle_ttc",
3839
"modified_goal_longitudinal_deviation",
3940
"modified_goal_lateral_deviation",
40-
"modified_goal_yaw_deviation"
41+
"modified_goal_yaw_deviation",
42+
"stop_decision",
43+
"abnormal_stop_decision",
44+
"blinker_change_count",
45+
"steer_change_count"
46+
]
47+
},
48+
"metrics_for_output": {
49+
"description": "metrics to output to json file when the node is terminated",
50+
"type": "array",
51+
"items": {
52+
"type": "string"
53+
},
54+
"default": [
55+
"curvature",
56+
"point_interval",
57+
"relative_angle",
58+
"resampled_relative_angle",
59+
"length",
60+
"duration",
61+
"velocity",
62+
"acceleration",
63+
"jerk",
64+
"lateral_deviation",
65+
"yaw_deviation",
66+
"velocity_deviation",
67+
"lateral_trajectory_displacement_local",
68+
"lateral_trajectory_displacement_global",
69+
"stability",
70+
"stability_frechet",
71+
"obstacle_distance",
72+
"obstacle_ttc",
73+
"modified_goal_longitudinal_deviation",
74+
"modified_goal_lateral_deviation",
75+
"modified_goal_yaw_deviation",
76+
"stop_decision",
77+
"abnormal_stop_decision",
78+
"blinker_change_count",
79+
"steer_change_count"
4180
]
4281
},
4382
"trajectory": {
@@ -77,9 +116,111 @@
77116
"default": 1.0
78117
}
79118
}
119+
},
120+
"stop_decision": {
121+
"description": "stop decision object",
122+
"type": "object",
123+
"properties": {
124+
"topic_prefix": {
125+
"description": "prefix of the topic to subscribe to for planning factors",
126+
"type": "string",
127+
"default": "/planning/planning_factors/"
128+
},
129+
"time_count_threshold_s": {
130+
"description": "time [s] threshold to count a stop as a new one",
131+
"type": "number",
132+
"default": 60.0
133+
},
134+
"dist_count_threshold_m": {
135+
"description": "distance [m] threshold to count a stop as a new one",
136+
"type": "number",
137+
"default": 5.0
138+
},
139+
"abnormal_deceleration_threshold_mps2": {
140+
"description": "abnormal deceleration threshold [m/s^2] to count a stop as abnormal",
141+
"type": "number",
142+
"default": 2.0
143+
},
144+
"module_list": {
145+
"description": "list of modules to be checked for stop deciation, the `topic_prefix`/`module` topic should be available and publish the `autoware_internal_planning_msgs::PlanningFactorArray` message",
146+
"type": "array",
147+
"items": {
148+
"type": "string"
149+
},
150+
"default": [
151+
"avoidance_by_lane_change",
152+
"behavior_path_planner",
153+
"blind_spot",
154+
"crosswalk",
155+
"detection_area",
156+
"dynamic_obstacle_avoidance",
157+
"dynamic_obstacle_stop",
158+
"goal_planner",
159+
"intersection",
160+
"lane_change_left",
161+
"lane_change_right",
162+
"motion_velocity_planner",
163+
"merge_from_private",
164+
"no_drivable_lane",
165+
"no_stopping_area",
166+
"obstacle_cruise",
167+
"obstacle_cruise_planner",
168+
"obstacle_slow_down",
169+
"obstacle_stop",
170+
"obstacle_stop_planner",
171+
"occlusion_spot",
172+
"out_of_lane",
173+
"run_out",
174+
"side_shift",
175+
"start_planner",
176+
"static_obstacle_avoidance",
177+
"stop_line",
178+
"surround_obstacle_checker",
179+
"traffic_light",
180+
"virtual_traffic_light",
181+
"walkway"
182+
]
183+
}
184+
}
185+
},
186+
"blinker_change_count": {
187+
"description": "blinker change count object",
188+
"type": "object",
189+
"properties": {
190+
"window_duration_s": {
191+
"description": "window duration [s] to count blinker changes for publishing",
192+
"type": "number",
193+
"default": 5.0
194+
}
195+
}
196+
},
197+
"steer_change_count": {
198+
"description": "steer change count object",
199+
"type": "object",
200+
"properties": {
201+
"window_duration_s": {
202+
"description": "window duration [s] to count steer changes for publishing",
203+
"type": "number",
204+
"default": 5.0
205+
},
206+
"steer_rate_margin_radps": {
207+
"description": "margin of steer_rate [rad/s] around 0 to count as steer change",
208+
"type": "number",
209+
"default": 0.1
210+
}
211+
}
80212
}
81213
},
82-
"required": ["ego_frame", "selected_metrics", "trajectory", "obstacle"]
214+
"required": [
215+
"ego_frame",
216+
"metrics_for_publish",
217+
"metrics_for_output",
218+
"trajectory",
219+
"obstacle",
220+
"stop_decision",
221+
"blinker_change_count",
222+
"steer_change_count"
223+
]
83224
}
84225
},
85226
"properties": {
@@ -90,8 +231,12 @@
90231
"$ref": "#/definitions/autoware_planning_evaluator"
91232
}
92233
},
93-
"required": ["ros__parameters"]
234+
"required": [
235+
"ros__parameters"
236+
]
94237
}
95238
},
96-
"required": ["/**"]
97-
}
239+
"required": [
240+
"/**"
241+
]
242+
}

‎evaluator/autoware_planning_evaluator/src/metric_accumulators/steer_accuulator.cpp

+5-5
Original file line numberDiff line numberDiff line change
@@ -31,18 +31,18 @@ void SteerAccumulator::update(const SteeringReport & msg)
3131

3232
const double dt = cur_t - steer_state_.timestamp;
3333
const double cur_steer_rate = (cur_steer_angle - steer_state_.steer_angle) / dt;
34+
const SteerRateState cur_steer_rate_state =
35+
std::abs(cur_steer_rate) < parameters.steer_rate_margin_radps
36+
? SteerRateState::AROUND_ZERO
37+
: (cur_steer_rate > 0 ? SteerRateState::POSITIVE : SteerRateState::NEGATIVE);
3438

3539
if (!steer_state_.has_steer_rate()) {
3640
steer_state_.steer_rate = cur_steer_rate;
41+
steer_state_.steer_rate_state = cur_steer_rate_state;
3742
steer_state_.timestamp = cur_t;
3843
return;
3944
}
4045

41-
const SteerRateState cur_steer_rate_state =
42-
std::abs(cur_steer_rate) < parameters.steer_rate_margin_radps
43-
? SteerRateState::AROUND_ZERO
44-
: (cur_steer_rate > 0 ? SteerRateState::POSITIVE : SteerRateState::NEGATIVE);
45-
4646
if (
4747
cur_steer_rate_state != steer_state_.steer_rate_state &&
4848
cur_steer_rate_state != SteerRateState::AROUND_ZERO) {

0 commit comments

Comments
 (0)