Apollo 10.0
自动驾驶开放平台
lon_controller.cc
浏览该文件的文档.
1/******************************************************************************
2 * Copyright 2017 The Apollo Authors. All Rights Reserved.
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 *****************************************************************************/
17
18#include <algorithm>
19#include <utility>
20
21#include "absl/strings/str_cat.h"
22
23#include "cyber/common/log.h"
24#include "cyber/time/clock.h"
25#include "cyber/time/time.h"
30// #include "modules/localization/common/localization_gflags.h"
31
32namespace apollo {
33namespace control {
34
43using apollo::planning::StopReasonCode;
44
45constexpr double GRA_ACC = 9.8;
46
47LonController::LonController() : name_("PID-basesd Longitudinal Controller") {
48 // node_.reset(new apollo::cyber::Node("lon_controller"));
49 if (FLAGS_enable_csv_debug) {
50 time_t rawtime;
51 char name_buffer[80];
52 std::time(&rawtime);
53 std::tm time_tm;
54 localtime_r(&rawtime, &time_tm);
55 strftime(name_buffer, 80, "/tmp/speed_log__%F_%H%M%S.csv", &time_tm);
56 speed_log_file_ = fopen(name_buffer, "w");
57 if (speed_log_file_ == nullptr) {
58 AERROR << "Fail to open file:" << name_buffer;
59 FLAGS_enable_csv_debug = false;
60 }
61 if (speed_log_file_ != nullptr) {
62 fprintf(speed_log_file_,
63 "station_reference,"
64 "station_error,"
65 "station_error_limited,"
66 "preview_station_error,"
67 "speed_reference,"
68 "speed_error,"
69 "speed_error_limited,"
70 "preview_speed_reference,"
71 "preview_speed_error,"
72 "preview_acceleration_reference,"
73 "acceleration_cmd_closeloop,"
74 "acceleration_cmd,"
75 "acceleration_lookup,"
76 "acceleration_lookup_limit,"
77 "speed_lookup,"
78 "calibration_value,"
79 "throttle_cmd,"
80 "brake_cmd,"
81 "is_full_stop,"
82 "\r\n");
83
84 fflush(speed_log_file_);
85 }
86 AINFO << name_ << " used.";
87 }
88}
89
90void LonController::CloseLogFile() {
91 if (FLAGS_enable_csv_debug) {
92 if (speed_log_file_ != nullptr) {
93 fclose(speed_log_file_);
94 speed_log_file_ = nullptr;
95 }
96 }
97}
98void LonController::Stop() { CloseLogFile(); }
99
100LonController::~LonController() { CloseLogFile(); }
101
102Status LonController::Init(std::shared_ptr<DependencyInjector> injector) {
103 if (!ControlTask::LoadConfig<LonBasedPidControllerConf>(
104 &lon_based_pidcontroller_conf_)) {
105 AERROR << "failed to load control conf";
106 return Status(ErrorCode::CONTROL_INIT_ERROR,
107 "failed to load lon control_conf");
108 }
109
110 if (!ControlTask::LoadCalibrationTable(&calibration_table_)) {
111 AERROR << "failed to load calibration table";
112 return Status(ErrorCode::CONTROL_INIT_ERROR,
113 "lon failed to load calibration table");
114 }
115
116 injector_ = injector;
117 standstill_narmal_acceleration_ =
118 -fabs(lon_based_pidcontroller_conf_.standstill_narmal_acceleration());
119 stop_gain_acceleration_ =
120 -fabs(lon_based_pidcontroller_conf_.stop_gain_acceleration());
121 double ts = lon_based_pidcontroller_conf_.ts();
122 bool enable_leadlag =
123 lon_based_pidcontroller_conf_.enable_reverse_leadlag_compensation();
124
125 station_pid_controller_.Init(
126 lon_based_pidcontroller_conf_.station_pid_conf());
127 speed_pid_controller_.Init(
128 lon_based_pidcontroller_conf_.low_speed_pid_conf());
129
130 if (enable_leadlag) {
131 station_leadlag_controller_.Init(
132 lon_based_pidcontroller_conf_.reverse_station_leadlag_conf(), ts);
133 speed_leadlag_controller_.Init(
134 lon_based_pidcontroller_conf_.reverse_speed_leadlag_conf(), ts);
135 }
136
137 vehicle_param_.CopyFrom(
138 common::VehicleConfigHelper::Instance()->GetConfig().vehicle_param());
139
140 SetDigitalFilterPitchAngle();
141
142 InitControlCalibrationTable();
143 controller_initialized_ = true;
144
145 return Status::OK();
146}
147
148void LonController::SetDigitalFilterPitchAngle() {
149 double cutoff_freq =
150 lon_based_pidcontroller_conf_.pitch_angle_filter_conf().cutoff_freq();
151 double ts = lon_based_pidcontroller_conf_.ts();
152 SetDigitalFilter(ts, cutoff_freq, &digital_filter_pitch_angle_);
153}
154
155void LonController::InitControlCalibrationTable() {
156 AINFO << "Control calibration table size is "
157 << calibration_table_.calibration_size();
159 for (const auto &calibration : calibration_table_.calibration()) {
160 xyz.push_back(std::make_tuple(calibration.speed(),
161 calibration.acceleration(),
162 calibration.command()));
163 }
164 control_interpolation_.reset(new Interpolation2D);
165 ACHECK(control_interpolation_->Init(xyz))
166 << "Fail to load control calibration table";
167}
168
170 const localization::LocalizationEstimate *localization,
171 const canbus::Chassis *chassis,
172 const planning::ADCTrajectory *planning_published_trajectory,
174 localization_ = localization;
175 chassis_ = chassis;
176 trajectory_message_ = planning_published_trajectory;
177
178 if (!control_interpolation_) {
179 AERROR << "Fail to initialize calibration table.";
180 return Status(ErrorCode::CONTROL_COMPUTE_ERROR,
181 "Fail to initialize calibration table.");
182 }
183
184 if (trajectory_analyzer_ == nullptr ||
185 trajectory_analyzer_->seq_num() !=
186 trajectory_message_->header().sequence_num()) {
187 trajectory_analyzer_.reset(new TrajectoryAnalyzer(trajectory_message_));
188 }
189
190 auto debug = cmd->mutable_debug()->mutable_simple_lon_debug();
191 debug->Clear();
192
193 double brake_cmd = 0.0;
194 double throttle_cmd = 0.0;
195 double ts = lon_based_pidcontroller_conf_.ts();
196 double preview_time = lon_based_pidcontroller_conf_.preview_window() * ts;
197 bool enable_leadlag =
198 lon_based_pidcontroller_conf_.enable_reverse_leadlag_compensation();
199
200 if (preview_time < 0.0) {
201 const auto error_msg =
202 absl::StrCat("Preview time set as: ", preview_time, " less than 0");
203 AERROR << error_msg;
204 return Status(ErrorCode::CONTROL_COMPUTE_ERROR, error_msg);
205 }
206 ComputeLongitudinalErrors(trajectory_analyzer_.get(), preview_time, ts,
207 debug);
208
209 double station_error_limit =
210 lon_based_pidcontroller_conf_.station_error_limit();
211 double station_error_limited = 0.0;
212 if (lon_based_pidcontroller_conf_.enable_speed_station_preview()) {
213 station_error_limited =
214 common::math::Clamp(debug->preview_station_error(),
215 -station_error_limit, station_error_limit);
216 } else {
217 station_error_limited = common::math::Clamp(
218 debug->station_error(), -station_error_limit, station_error_limit);
219 }
220
221 if (trajectory_message_->gear() == canbus::Chassis::GEAR_REVERSE) {
222 if (CheckPit::CheckInPit(debug, &lon_based_pidcontroller_conf_,
223 injector_->vehicle_state()->linear_velocity(),
224 trajectory_message_->is_replan())) {
225 ADEBUG << "in pit";
226 station_pid_controller_.SetPID(
227 lon_based_pidcontroller_conf_.pit_station_pid_conf());
228 speed_pid_controller_.SetPID(
229 lon_based_pidcontroller_conf_.pit_speed_pid_conf());
230 } else {
231 station_pid_controller_.SetPID(
232 lon_based_pidcontroller_conf_.reverse_station_pid_conf());
233 speed_pid_controller_.SetPID(
234 lon_based_pidcontroller_conf_.reverse_speed_pid_conf());
235 }
236 if (enable_leadlag) {
237 station_leadlag_controller_.SetLeadlag(
238 lon_based_pidcontroller_conf_.reverse_station_leadlag_conf());
239 speed_leadlag_controller_.SetLeadlag(
240 lon_based_pidcontroller_conf_.reverse_speed_leadlag_conf());
241 }
242 } else if (injector_->vehicle_state()->linear_velocity() <=
243 lon_based_pidcontroller_conf_.switch_speed()) {
244 if (CheckPit::CheckInPit(debug, &lon_based_pidcontroller_conf_,
245 injector_->vehicle_state()->linear_velocity(),
246 trajectory_message_->is_replan())) {
247 ADEBUG << "in pit";
248 station_pid_controller_.SetPID(
249 lon_based_pidcontroller_conf_.pit_station_pid_conf());
250 speed_pid_controller_.SetPID(
251 lon_based_pidcontroller_conf_.pit_speed_pid_conf());
252 } else {
253 station_pid_controller_.SetPID(
254 lon_based_pidcontroller_conf_.station_pid_conf());
255 speed_pid_controller_.SetPID(
256 lon_based_pidcontroller_conf_.low_speed_pid_conf());
257 }
258 } else {
259 station_pid_controller_.SetPID(
260 lon_based_pidcontroller_conf_.station_pid_conf());
261 speed_pid_controller_.SetPID(
262 lon_based_pidcontroller_conf_.high_speed_pid_conf());
263 }
264
265 double speed_offset =
266 station_pid_controller_.Control(station_error_limited, ts);
267 if (enable_leadlag) {
268 speed_offset = station_leadlag_controller_.Control(speed_offset, ts);
269 }
270
271 double speed_controller_input = 0.0;
272 double speed_controller_input_limit =
273 lon_based_pidcontroller_conf_.speed_controller_input_limit();
274 double speed_controller_input_limited = 0.0;
275 if (lon_based_pidcontroller_conf_.enable_speed_station_preview()) {
276 speed_controller_input = speed_offset + debug->preview_speed_error();
277 } else {
278 speed_controller_input = speed_offset + debug->speed_error();
279 }
280 speed_controller_input_limited =
281 common::math::Clamp(speed_controller_input, -speed_controller_input_limit,
282 speed_controller_input_limit);
283
284 double acceleration_cmd_closeloop = 0.0;
285
286 acceleration_cmd_closeloop =
287 speed_pid_controller_.Control(speed_controller_input_limited, ts);
288 debug->set_pid_saturation_status(
289 speed_pid_controller_.IntegratorSaturationStatus());
290 if (enable_leadlag) {
291 acceleration_cmd_closeloop =
292 speed_leadlag_controller_.Control(acceleration_cmd_closeloop, ts);
293 debug->set_leadlag_saturation_status(
294 speed_leadlag_controller_.InnerstateSaturationStatus());
295 }
296
298 speed_pid_controller_.Reset_integral();
299 station_pid_controller_.Reset_integral();
300 }
301
302 double vehicle_pitch_rad =
303 digital_filter_pitch_angle_.Filter(injector_->vehicle_state()->pitch());
304 double vehicle_pitch =
305 vehicle_pitch_rad * 180 / M_PI + FLAGS_pitch_offset_deg;
306 ADEBUG << "[LON]vehicle_pitch is " << vehicle_pitch;
307 debug->set_vehicle_pitch(vehicle_pitch);
308 // TODO(ALL): confirm the slope_offset_compensation whether is positive or not
309 // when vehicle move uphill
310 // Resume: uphill: + , downhill: -
311 double slope_offset_compensation =
312 lon_based_pidcontroller_conf_.use_opposite_slope_compensation() *
313 GRA_ACC *
314 std::sin(vehicle_pitch_rad + FLAGS_pitch_offset_deg * M_PI / 180);
315
316 if (std::isnan(slope_offset_compensation)) {
317 slope_offset_compensation = 0;
318 }
319
320 debug->set_slope_offset_compensation(slope_offset_compensation);
321
322 double acceleration_cmd =
323 acceleration_cmd_closeloop + debug->preview_acceleration_reference() +
324 lon_based_pidcontroller_conf_.enable_slope_offset() *
325 debug->slope_offset_compensation();
326
327 // Check the steer command in reverse trajectory if the current steer target
328 // is larger than previous target, free the acceleration command, wait for
329 // the current steer target
330 double current_steer_interval =
331 cmd->steering_target() - chassis->steering_percentage();
332 if (lon_based_pidcontroller_conf_.use_steering_check() &&
333 (trajectory_message_->trajectory_type() ==
335 std::abs(current_steer_interval) >
336 lon_based_pidcontroller_conf_.steer_cmd_interval()) {
337 ADEBUG << "steering_target is " << cmd->steering_target()
338 << " steering_percentage is " << chassis->steering_percentage();
339 ADEBUG << "Steer cmd interval is larger than "
340 << lon_based_pidcontroller_conf_.steer_cmd_interval();
341
342 speed_pid_controller_.Reset_integral();
343 station_pid_controller_.Reset_integral();
344 acceleration_cmd = 0;
345 debug->set_is_wait_steer(true);
346 } else {
347 debug->set_is_wait_steer(false);
348 }
349 debug->set_current_steer_interval(current_steer_interval);
350
351 // At near-stop stage, replace the brake control command with the standstill
352 // acceleration if the former is even softer than the latter
353 debug->set_is_full_stop(false);
354 debug->set_is_full_stop_soft(false);
355 auto previous_full_stop =
356 injector_->Get_previous_lon_debug_info()->is_full_stop();
357 GetPathRemain(debug);
358 IsStopByDestination(debug);
359 IsPedestrianStopLongTerm(debug);
360 if (lon_based_pidcontroller_conf_.use_preview_reference_check() &&
361 (std::fabs(debug->preview_acceleration_reference()) <=
362 FLAGS_max_acceleration_when_stopped) &&
363 std::fabs(debug->preview_speed_reference()) <=
364 vehicle_param_.max_abs_speed_when_stopped() &&
365 trajectory_message_->trajectory_type() != ADCTrajectory::OPEN_SPACE) {
366 if (debug->is_stop_reason_by_destination() ||
367 debug->is_stop_reason_by_prdestrian()) {
368 debug->set_is_full_stop(true);
369 ADEBUG << "Into full stop within preview acc and reference speed, "
370 << "is_full_stop is " << debug->is_full_stop();
371 } else {
372 debug->set_is_full_stop_soft(true);
373 ADEBUG << "Into full stop soft within preview acc and reference speed, "
374 << "is_full_stop_soft is " << debug->is_full_stop_soft();
375 }
376 }
377
378 if (!previous_full_stop) {
379 max_path_remain_when_stopped_ = FLAGS_max_path_remain_when_stopped;
380 } else {
381 max_path_remain_when_stopped_ =
382 FLAGS_max_path_remain_when_stopped +
383 lon_based_pidcontroller_conf_.full_stop_path_remain_gain();
384 }
385
386 if (((trajectory_message_->gear() == Chassis::GEAR_DRIVE) &&
387 debug->path_remain() < max_path_remain_when_stopped_) ||
388 ((trajectory_message_->gear() == Chassis::GEAR_REVERSE) &&
389 debug->path_remain() > -max_path_remain_when_stopped_)) {
390 ADEBUG << "Into full stop decision by path remain.";
391 if (debug->is_stop_reason_by_destination() ||
392 debug->is_stop_reason_by_prdestrian()) {
393 debug->set_is_full_stop(true);
394 ADEBUG << "Current path remain distance: " << debug->path_remain()
395 << " is within max_path_remain threshold: "
396 << max_path_remain_when_stopped_
397 << ", into full stop because vehicle is in destination: "
398 << debug->is_stop_reason_by_destination()
399 << " or pedestrian is in long time stop: "
400 << debug->is_stop_reason_by_prdestrian()
401 << "is_full_stop flag: " << debug->is_full_stop();
402 } else {
403 debug->set_is_full_stop_soft(true);
404 ADEBUG << "Current path remain distance: " << debug->path_remain()
405 << " is within max_path_remain threshold: "
406 << max_path_remain_when_stopped_
407 << ", but not into full stop because stop not in destination: "
408 << debug->is_stop_reason_by_destination()
409 << " or pedestrian is not long time stop: "
410 << debug->is_stop_reason_by_prdestrian()
411 << "is_full_stop_soft flag: " << debug->is_full_stop_soft();
412 }
413 if (injector_->vehicle_state()->linear_velocity() <
414 vehicle_param_.max_abs_speed_when_stopped() &&
415 debug->is_stop_reason_by_prdestrian()) {
416 ADEBUG << "Current stop is for long time pedestrian stop, "
417 << debug->is_stop_reason_by_prdestrian();
418 debug->set_is_full_stop(true);
419 }
420 } else {
421 ADEBUG << "Not into full stop decision by path remain.";
422 }
423
424 if (debug->is_full_stop()) {
425 acceleration_cmd =
427 ? std::max(acceleration_cmd,
428 -lon_based_pidcontroller_conf_.standstill_acceleration())
429 : std::min(acceleration_cmd,
430 lon_based_pidcontroller_conf_.standstill_acceleration());
431 speed_pid_controller_.Reset_integral();
432 station_pid_controller_.Reset_integral();
433 }
434
435 if (debug->is_full_stop_soft()) {
437 acceleration_cmd =
438 (acceleration_cmd >= 0) ? standstill_narmal_acceleration_
439 : (debug->path_remain() >= 0) ? acceleration_cmd
440 : (trajectory_message_->trajectory_type() != ADCTrajectory::NORMAL)
441 ? (acceleration_cmd + stop_gain_acceleration_)
442 : (acceleration_cmd + standstill_narmal_acceleration_);
443 } else {
444 acceleration_cmd =
445 (acceleration_cmd <= 0) ? -standstill_narmal_acceleration_
446 : (debug->path_remain() <= 0) ? acceleration_cmd
447 : (trajectory_message_->trajectory_type() != ADCTrajectory::NORMAL)
448 ? (acceleration_cmd - stop_gain_acceleration_)
449 : (acceleration_cmd - standstill_narmal_acceleration_);
450 }
451 speed_pid_controller_.Reset_integral();
452 station_pid_controller_.Reset_integral();
453 }
454
455 double throttle_lowerbound =
456 std::max(vehicle_param_.throttle_deadzone(),
457 lon_based_pidcontroller_conf_.throttle_minimum_action());
458 double brake_lowerbound =
459 std::max(vehicle_param_.brake_deadzone(),
460 lon_based_pidcontroller_conf_.brake_minimum_action());
461 double calibration_value = 0.0;
462 double acceleration_lookup =
464 ? -acceleration_cmd
465 : acceleration_cmd;
466
467 double acceleration_lookup_limited =
468 vehicle_param_.max_acceleration() +
469 lon_based_pidcontroller_conf_.enable_slope_offset() *
470 debug->slope_offset_compensation();
471 double acceleration_lookup_limit = 0.0;
472
473 if (lon_based_pidcontroller_conf_.use_acceleration_lookup_limit()) {
474 acceleration_lookup_limit =
475 (acceleration_lookup > acceleration_lookup_limited)
476 ? acceleration_lookup_limited
477 : acceleration_lookup;
478 }
479
480 if (FLAGS_use_preview_speed_for_table) {
481 if (lon_based_pidcontroller_conf_.use_acceleration_lookup_limit()) {
482 calibration_value = control_interpolation_->Interpolate(
483 std::make_pair(std::fabs(debug->preview_speed_reference()),
484 acceleration_lookup_limit));
485 } else {
486 calibration_value = control_interpolation_->Interpolate(std::make_pair(
487 std::fabs(debug->preview_speed_reference()), acceleration_lookup));
488 }
489 } else {
490 if (lon_based_pidcontroller_conf_.use_acceleration_lookup_limit()) {
491 calibration_value = control_interpolation_->Interpolate(std::make_pair(
492 std::fabs(injector_->vehicle_state()->linear_velocity()),
493 acceleration_lookup_limit));
494 } else {
495 calibration_value = control_interpolation_->Interpolate(std::make_pair(
496 std::fabs(injector_->vehicle_state()->linear_velocity()),
497 acceleration_lookup));
498 }
499 }
500
501 if (acceleration_lookup >= 0) {
502 if (calibration_value >= 0) {
503 throttle_cmd = std::max(calibration_value, throttle_lowerbound);
504 } else {
505 throttle_cmd = throttle_lowerbound;
506 }
507 brake_cmd = 0.0;
508 } else {
509 throttle_cmd = 0.0;
510 if (calibration_value >= 0) {
511 brake_cmd = brake_lowerbound;
512 } else {
513 brake_cmd = std::max(-calibration_value, brake_lowerbound);
514 }
515 }
516
517 if (FLAGS_use_vehicle_epb) {
518 ADEBUG << "Into use vehicle epb.";
519 if (acceleration_lookup >= 0) {
520 if (debug->slope_offset_compensation() > 0) {
521 if (acceleration_lookup > debug->slope_offset_compensation()) {
522 parking_release_ = true;
523 }
524 } else {
525 parking_release_ = true;
526 }
527 if (chassis->parking_brake() && parking_release_) {
528 ADEBUG << "Into park brake release.";
529 cmd->set_parking_brake(false);
530 SetParkingBrake(&lon_based_pidcontroller_conf_, cmd);
531 }
532 } else {
533 cmd->set_parking_brake(false);
534 if (debug->is_full_stop() && IsFullStopLongTerm(debug)) {
535 ADEBUG << "Into park brake trigger.";
536 cmd->set_parking_brake(true);
537 if (chassis->parking_brake()) {
538 brake_cmd = 0.0;
539 }
540 }
541 }
542 }
543
544 debug->set_station_error_limited(station_error_limited);
545 debug->set_speed_offset(speed_offset);
546 debug->set_speed_controller_input_limited(speed_controller_input_limited);
547 debug->set_acceleration_cmd(acceleration_cmd);
548 debug->set_throttle_cmd(throttle_cmd);
549 debug->set_brake_cmd(brake_cmd);
550 debug->set_acceleration_lookup(acceleration_lookup);
551 debug->set_acceleration_lookup_limit(acceleration_lookup_limit);
552 debug->set_speed_lookup(injector_->vehicle_state()->linear_velocity());
553 debug->set_calibration_value(calibration_value);
554 debug->set_acceleration_cmd_closeloop(acceleration_cmd_closeloop);
555
556 if (FLAGS_enable_csv_debug && speed_log_file_ != nullptr) {
557 fprintf(speed_log_file_,
558 "%.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f,"
559 "%.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %d,\r\n",
560 debug->station_reference(), debug->station_error(),
561 station_error_limited, debug->preview_station_error(),
562 debug->speed_reference(), debug->speed_error(),
563 speed_controller_input_limited, debug->preview_speed_reference(),
564 debug->preview_speed_error(),
565 debug->preview_acceleration_reference(), acceleration_cmd_closeloop,
566 acceleration_cmd, debug->acceleration_lookup(),
567 debug->acceleration_lookup_limit(), debug->speed_lookup(),
568 calibration_value, throttle_cmd, brake_cmd, debug->is_full_stop());
569 }
570
571 // if the car is driven by acceleration, disgard the cmd->throttle and brake
572 cmd->set_throttle(throttle_cmd);
573 cmd->set_brake(brake_cmd);
574 if (lon_based_pidcontroller_conf_.use_acceleration_lookup_limit()) {
575 cmd->set_acceleration(acceleration_lookup_limit);
576 } else {
577 cmd->set_acceleration(acceleration_cmd);
578 }
579
580 if (std::fabs(injector_->vehicle_state()->linear_velocity()) <=
581 vehicle_param_.max_abs_speed_when_stopped() ||
582 chassis->gear_location() == trajectory_message_->gear() ||
584 cmd->set_gear_location(trajectory_message_->gear());
585 } else {
586 cmd->set_gear_location(chassis->gear_location());
587 }
588
589 if (lon_based_pidcontroller_conf_.use_speed_itfc()) {
590 reference_spd_cmd_ = reference_spd_ + debug->speed_offset();
591 if ((reference_spd_ <=
592 lon_based_pidcontroller_conf_.speed_itfc_full_stop_speed()) &&
594 if ((debug->path_remain() >=
595 lon_based_pidcontroller_conf_.speed_itfc_path_remain_min()) &&
596 (debug->preview_acceleration_reference() >=
597 lon_based_pidcontroller_conf_.speed_itfc_dcc_emergency()) &&
598 (debug->path_remain() <=
599 lon_based_pidcontroller_conf_.speed_itfc_path_remain_max())) {
600 if (debug->preview_acceleration_reference() <=
601 lon_based_pidcontroller_conf_.speed_itfc_acc_thres()) {
602 reference_spd_cmd_ =
603 lon_based_pidcontroller_conf_.speed_itfc_speed_cmd();
604 }
605 }
606 }
607 cmd->set_speed(reference_spd_cmd_);
608 }
609
610 return Status::OK();
611}
612
614 speed_pid_controller_.Reset();
615 station_pid_controller_.Reset();
616 return Status::OK();
617}
618
619std::string LonController::Name() const { return name_; }
620
622 const TrajectoryAnalyzer *trajectory_analyzer, const double preview_time,
623 const double ts, SimpleLongitudinalDebug *debug) {
624 // the decomposed vehicle motion onto Frenet frame
625 // s: longitudinal accumulated distance along reference trajectory
626 // s_dot: longitudinal velocity along reference trajectory
627 // d: lateral distance w.r.t. reference trajectory
628 // d_dot: lateral distance change rate, i.e. dd/dt
629 double s_matched = 0.0;
630 double s_dot_matched = 0.0;
631 double d_matched = 0.0;
632 double d_dot_matched = 0.0;
633
634 auto vehicle_state = injector_->vehicle_state();
635 auto matched_point = trajectory_analyzer->QueryMatchedPathPoint(
636 vehicle_state->x(), vehicle_state->y());
637
638 trajectory_analyzer->ToTrajectoryFrame(
639 vehicle_state->x(), vehicle_state->y(), vehicle_state->heading(),
640 vehicle_state->linear_velocity(), matched_point, &s_matched,
641 &s_dot_matched, &d_matched, &d_dot_matched);
642
643 // double current_control_time = Time::Now().ToSecond();
644 double current_control_time = ::apollo::cyber::Clock::NowInSeconds();
645 double preview_control_time = current_control_time + preview_time;
646
647 TrajectoryPoint reference_point =
648 trajectory_analyzer->QueryNearestPointByAbsoluteTime(
649 current_control_time);
650 TrajectoryPoint preview_point =
651 trajectory_analyzer->QueryNearestPointByAbsoluteTime(
652 preview_control_time);
653
654 debug->mutable_current_matched_point()->mutable_path_point()->set_x(
655 matched_point.x());
656 debug->mutable_current_matched_point()->mutable_path_point()->set_y(
657 matched_point.y());
658 debug->mutable_current_reference_point()->mutable_path_point()->set_x(
659 reference_point.path_point().x());
660 debug->mutable_current_reference_point()->mutable_path_point()->set_y(
661 reference_point.path_point().y());
662 debug->mutable_preview_reference_point()->mutable_path_point()->set_x(
663 preview_point.path_point().x());
664 debug->mutable_preview_reference_point()->mutable_path_point()->set_y(
665 preview_point.path_point().y());
666
667 ADEBUG << "matched point:" << matched_point.DebugString();
668 ADEBUG << "reference point:" << reference_point.DebugString();
669 ADEBUG << "preview point:" << preview_point.DebugString();
670
671 double heading_error = common::math::NormalizeAngle(vehicle_state->heading() -
672 matched_point.theta());
673 double lon_speed = vehicle_state->linear_velocity() * std::cos(heading_error);
674 double lon_acceleration =
675 vehicle_state->linear_acceleration() * std::cos(heading_error);
676 double one_minus_kappa_lat_error = 1 - reference_point.path_point().kappa() *
677 vehicle_state->linear_velocity() *
678 std::sin(heading_error);
679
680 debug->set_station_reference(reference_point.path_point().s());
681 debug->set_current_station(s_matched);
682 debug->set_station_error(reference_point.path_point().s() - s_matched);
683 debug->set_speed_reference(reference_point.v());
684 debug->set_current_speed(lon_speed);
685 debug->set_speed_error(reference_point.v() - s_dot_matched);
686 debug->set_acceleration_reference(reference_point.a());
687 debug->set_current_acceleration(lon_acceleration);
688 debug->set_acceleration_error(reference_point.a() -
689 lon_acceleration / one_minus_kappa_lat_error);
690 double jerk_reference =
691 (debug->acceleration_reference() - previous_acceleration_reference_) / ts;
692 double lon_jerk =
693 (debug->current_acceleration() - previous_acceleration_) / ts;
694 debug->set_jerk_reference(jerk_reference);
695 debug->set_current_jerk(lon_jerk);
696 debug->set_jerk_error(jerk_reference - lon_jerk / one_minus_kappa_lat_error);
697 previous_acceleration_reference_ = debug->acceleration_reference();
698 previous_acceleration_ = debug->current_acceleration();
699
700 debug->set_preview_station_error(preview_point.path_point().s() - s_matched);
701 debug->set_preview_speed_error(preview_point.v() - s_dot_matched);
702 debug->set_preview_speed_reference(preview_point.v());
703 debug->set_preview_acceleration_reference(preview_point.a());
704 if (lon_based_pidcontroller_conf_.use_speed_itfc()) {
705 reference_spd_ = reference_point.v();
706 }
707}
708
709void LonController::SetDigitalFilter(double ts, double cutoff_freq,
710 common::DigitalFilter *digital_filter) {
711 std::vector<double> denominators;
712 std::vector<double> numerators;
713 common::LpfCoefficients(ts, cutoff_freq, &denominators, &numerators);
714 digital_filter->set_coefficients(denominators, numerators);
715}
716
717// TODO(all): Refactor and simplify
719 int stop_index = 0;
720 static constexpr double kSpeedThreshold = 1e-3;
721 static constexpr double kForwardAccThreshold = -1e-2;
722 static constexpr double kBackwardAccThreshold = 1e-1;
723 static constexpr double kParkingSpeed = 0.1;
724
725 if (trajectory_message_->gear() == canbus::Chassis::GEAR_DRIVE) {
726 while (stop_index < trajectory_message_->trajectory_point_size()) {
727 auto &current_trajectory_point =
728 trajectory_message_->trajectory_point(stop_index);
729 if (fabs(current_trajectory_point.v()) < kSpeedThreshold &&
730 current_trajectory_point.a() > kForwardAccThreshold &&
731 current_trajectory_point.a() < 0.0) {
732 break;
733 }
734 ++stop_index;
735 }
736 } else {
737 while (stop_index < trajectory_message_->trajectory_point_size()) {
738 auto &current_trajectory_point =
739 trajectory_message_->trajectory_point(stop_index);
740 if (current_trajectory_point.v() > -kSpeedThreshold &&
741 current_trajectory_point.a() < kBackwardAccThreshold &&
742 current_trajectory_point.a() > 0.0) {
743 break;
744 }
745 ++stop_index;
746 }
747 }
748 ADEBUG << "stop_index is, " << stop_index;
749 if (stop_index == trajectory_message_->trajectory_point_size()) {
750 --stop_index;
751 if (fabs(trajectory_message_->trajectory_point(stop_index).v()) <
752 kParkingSpeed) {
753 ADEBUG << "the last point is selected as parking point";
754 } else {
755 ADEBUG << "the last point found in path and speed > speed_deadzone";
756 }
757 }
758 debug->set_path_remain(
759 trajectory_message_->trajectory_point(stop_index).path_point().s() -
760 debug->current_station());
761}
762
763bool LonController::IsStopByDestination(SimpleLongitudinalDebug *debug) {
764 auto stop_reason = trajectory_message_->decision().main_decision().stop();
765 ADEBUG << "Current stop reason is \n" << stop_reason.DebugString();
766 ADEBUG << "Planning command status msg is \n"
767 << injector_->get_planning_command_status()->ShortDebugString();
768
769 StopReasonCode stop_reason_code = stop_reason.reason_code();
770
771 if (stop_reason_code == StopReasonCode::STOP_REASON_SIGNAL ||
772 stop_reason_code == StopReasonCode::STOP_REASON_REFERENCE_END ||
773 stop_reason_code == StopReasonCode::STOP_REASON_PRE_OPEN_SPACE_STOP ||
774 injector_->get_planning_command_status()->status() ==
775 CommandStatusType::FINISHED ||
776 trajectory_message_->decision().main_decision().has_mission_complete()) {
777 ADEBUG << "[IsStopByDestination]Current stop reason is in destination.";
778 debug->set_is_stop_reason_by_destination(true);
779 return true;
780 }
781 debug->set_is_stop_reason_by_destination(false);
782 return false;
783}
784
785bool LonController::IsPedestrianStopLongTerm(SimpleLongitudinalDebug *debug) {
786 auto stop_reason = trajectory_message_->decision().main_decision().stop();
787 ADEBUG << "Current stop reason is \n" << stop_reason.DebugString();
788 StopReasonCode stop_reason_code = stop_reason.reason_code();
789
790 if (stop_reason_code == StopReasonCode::STOP_REASON_PEDESTRIAN ||
791 stop_reason_code == StopReasonCode::STOP_REASON_OBSTACLE) {
792 ADEBUG << "[IsPedestrianStopLongTerm]Stop reason for pedestrian.";
793 is_stop_by_pedestrian_ = true;
794 } else {
795 is_stop_by_pedestrian_ = false;
796 }
797
798 ADEBUG << "Current is_stop_by_pedestrian: " << is_stop_by_pedestrian_
799 << ", is_stop_by_pedestrian_previous: "
800 << is_stop_by_pedestrian_previous_;
801
802 if (is_stop_by_pedestrian_) {
803 if (!(is_stop_by_pedestrian_ && is_stop_by_pedestrian_previous_)) {
805 ADEBUG << "Stop reason for pedestrian, start time(s) is " << start_time_;
806 } else {
807 ADEBUG << "Last time stop is already pedestrian, skip start_time init.";
808 }
809 double end_time = ::apollo::cyber::Clock::NowInSeconds();
810 ADEBUG << "Stop reason for pedestrian, current time(s) is " << end_time;
811 wait_time_diff_ = end_time - start_time_;
812 } else {
813 start_time_ = 0.0;
814 wait_time_diff_ = 0.0;
815 }
816
817 is_stop_by_pedestrian_previous_ = is_stop_by_pedestrian_;
818
819 if (wait_time_diff_ > lon_based_pidcontroller_conf_.pedestrian_stop_time()) {
820 ADEBUG << "Current pedestrian stop lasting time(s) is " << wait_time_diff_
821 << ", larger than threshold: "
822 << lon_based_pidcontroller_conf_.pedestrian_stop_time();
823 debug->set_is_stop_reason_by_prdestrian(true);
824 return true;
825 } else {
826 ADEBUG << "Current pedestrian stop lasting time(s) is " << wait_time_diff_
827 << ", not reach the threshold: "
828 << lon_based_pidcontroller_conf_.pedestrian_stop_time();
829 debug->set_is_stop_reason_by_prdestrian(false);
830 return false;
831 }
832}
833
834bool LonController::IsFullStopLongTerm(SimpleLongitudinalDebug *debug) {
835 if (debug->is_full_stop()) {
836 if (debug->is_full_stop() && !is_full_stop_previous_) {
837 is_full_stop_start_time_ = ::apollo::cyber::Clock::NowInSeconds();
838 ADEBUG << "Full stop long term start time(s) is "
839 << is_full_stop_start_time_;
840 } else {
841 ADEBUG << "Last time stop is already full stop, skip start_time init.";
842 }
843 double is_full_stop_start_end_time = ::apollo::cyber::Clock::NowInSeconds();
844 is_full_stop_wait_time_diff_ =
845 is_full_stop_start_end_time - is_full_stop_start_time_;
846 } else {
847 is_full_stop_start_time_ = 0.0;
848 is_full_stop_wait_time_diff_ = 0.0;
849 }
850 is_full_stop_previous_ = debug->is_full_stop();
851 if (is_full_stop_wait_time_diff_ >
852 lon_based_pidcontroller_conf_.full_stop_long_time()) {
853 ADEBUG << "Current full stop lasting time(s) is "
854 << is_full_stop_wait_time_diff_ << ", larger than threshold: "
855 << lon_based_pidcontroller_conf_.full_stop_long_time();
856 return true;
857 } else {
858 ADEBUG << "Current full stop lasting time(s) is "
859 << is_full_stop_wait_time_diff_ << ", not reach the threshold: "
860 << lon_based_pidcontroller_conf_.full_stop_long_time();
861 return false;
862 }
863}
864
865void LonController::SetParkingBrake(const LonBasedPidControllerConf *conf,
866 control::ControlCommand *control_command) {
867 if (control_command->parking_brake()) {
868 // epb on, parking brake: 0 -> 1
869 if (epb_on_change_switch_) {
870 ADEBUG << "Epb on, first set parking brake false.";
871 control_command->set_parking_brake(false);
872 ++epb_change_count_;
873 if (epb_change_count_ >= conf->epb_change_count()) {
874 epb_on_change_switch_ = false;
875 epb_change_count_ = 0;
876 ADEBUG << "Epb on, first stage has been done.";
877 }
878 } else {
879 ADEBUG << "Epb on, second set parking brake true.";
880 control_command->set_parking_brake(true);
881 ++epb_change_count_;
882 if (epb_change_count_ >= conf->epb_change_count()) {
883 epb_on_change_switch_ = true;
884 epb_change_count_ = 0;
885 ADEBUG << "Epb on, second stage has been done.";
886 }
887 }
888 } else {
889 // epb off, parking brake: 1 -> 0
890 if (epb_off_change_switch_) {
891 ADEBUG << "Epb off, first set praking brake true.";
892 control_command->set_parking_brake(true);
893 ++epb_change_count_;
894 if (epb_change_count_ >= conf->epb_change_count()) {
895 epb_off_change_switch_ = false;
896 epb_change_count_ = 0;
897 ADEBUG << "Epb off, first stage has been done.";
898 }
899 } else {
900 ADEBUG << "Epb off, second set parking brake false.";
901 control_command->set_parking_brake(false);
902 ++epb_change_count_;
903 if (epb_change_count_ >= conf->epb_change_count()) {
904 epb_off_change_switch_ = true;
905 epb_change_count_ = 0;
906 ADEBUG << "Epb off, second stage has been done.";
907 }
908 }
909 }
910}
911
912} // namespace control
913} // namespace apollo
The DigitalFilter class is used to pass signals with a frequency lower than a certain cutoff frequenc...
void set_coefficients(const std::vector< double > &denominators, const std::vector< double > &numerators)
set denominators and numerators
double Filter(const double x_insert)
Processes a new measurement with the filter.
A general class to denote the return status of an API call.
Definition status.h:43
static Status OK()
generate a success status.
Definition status.h:60
static bool CheckInPit(SimpleLongitudinalDebug *debug, const LonBasedPidControllerConf *conf, double speed, bool replan)
Definition check_pit.cc:22
bool LoadCalibrationTable(calibration_table *calibration_table_conf)
std::vector< std::tuple< double, double, double > > DataType
void SetLeadlag(const LeadlagConf &leadlag_conf)
alpha, beta and tau
int InnerstateSaturationStatus() const
get saturation status
void Init(const LeadlagConf &leadlag_conf, const double dt)
initialize lead/lag controller
virtual double Control(const double error, const double dt)
compute control value based on the error
void GetPathRemain(SimpleLongitudinalDebug *debug)
void Stop() override
stop longitudinal controller
common::Status Reset() override
reset longitudinal controller
virtual ~LonController()
destructor
common::Status ComputeControlCommand(const localization::LocalizationEstimate *localization, const canbus::Chassis *chassis, const planning::ADCTrajectory *trajectory, control::ControlCommand *cmd) override
compute brake / throttle values based on current vehicle status and target trajectory
common::Status Init(std::shared_ptr< DependencyInjector > injector) override
initialize Longitudinal Controller
void ComputeLongitudinalErrors(const TrajectoryAnalyzer *trajectory, const double preview_time, const double ts, SimpleLongitudinalDebug *debug)
std::shared_ptr< DependencyInjector > injector_
std::string Name() const override
longitudinal controller name
void Init(const PidConf &pid_conf)
initialize pid controller
void Reset_integral()
reset integral for pid controller
void Reset()
reset variables for pid controller
void SetPID(const PidConf &pid_conf)
set pid controller coefficients for the proportional, integral, and derivative
virtual double Control(const double error, const double dt)
compute control value based on the error
int IntegratorSaturationStatus() const
get saturation status
process point query and conversion related to trajectory
common::PathPoint QueryMatchedPathPoint(const double x, const double y) const
query a point on trajectory that its position is closest to the given position.
common::TrajectoryPoint QueryNearestPointByAbsoluteTime(const double t) const
query a point of trajectory that its absolute time is closest to the give time.
void ToTrajectoryFrame(const double x, const double y, const double theta, const double v, const common::PathPoint &matched_point, double *ptr_s, double *ptr_s_dot, double *ptr_d, double *ptr_d_dot) const
convert a position with theta and speed to trajectory frame,
static double NowInSeconds()
gets the current time in second.
Definition clock.cc:56
Cyber has builtin time type Time.
Definition time.h:31
#define ACHECK(cond)
Definition log.h:80
#define ADEBUG
Definition log.h:41
#define AERROR
Definition log.h:44
#define AINFO
Definition log.h:42
Defines the LonController class.
Math-related util functions.
T Clamp(const T value, T bound1, T bound2)
Clamp a value between two bounds.
Definition math_utils.h:155
double NormalizeAngle(const double angle)
Normalize angle to [-PI, PI).
Definition math_utils.cc:53
void LpfCoefficients(const double ts, const double cutoff_freq, std::vector< double > *denominators, std::vector< double > *numerators)
Get low-pass coefficients for digital filter.
constexpr double GRA_ACC
class register implement
Definition arena_queue.h:37
optional GearPosition gear_location
optional float steering_percentage
Definition chassis.proto:88
optional bool parking_brake
Definition chassis.proto:94
optional uint32 sequence_num
Definition header.proto:16
repeated apollo::common::TrajectoryPoint trajectory_point
optional TrajectoryType trajectory_type
optional apollo::canbus::Chassis::GearPosition gear
optional apollo::common::Header header
optional apollo::planning::DecisionResult decision