174 localization_ = localization;
176 trajectory_message_ = planning_published_trajectory;
178 if (!control_interpolation_) {
179 AERROR <<
"Fail to initialize calibration table.";
180 return Status(ErrorCode::CONTROL_COMPUTE_ERROR,
181 "Fail to initialize calibration table.");
184 if (trajectory_analyzer_ ==
nullptr ||
185 trajectory_analyzer_->seq_num() !=
190 auto debug = cmd->mutable_debug()->mutable_simple_lon_debug();
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();
200 if (preview_time < 0.0) {
201 const auto error_msg =
202 absl::StrCat(
"Preview time set as: ", preview_time,
" less than 0");
204 return Status(ErrorCode::CONTROL_COMPUTE_ERROR, error_msg);
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 =
215 -station_error_limit, station_error_limit);
218 debug->station_error(), -station_error_limit, station_error_limit);
223 injector_->vehicle_state()->linear_velocity(),
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());
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());
236 if (enable_leadlag) {
238 lon_based_pidcontroller_conf_.reverse_station_leadlag_conf());
240 lon_based_pidcontroller_conf_.reverse_speed_leadlag_conf());
242 }
else if (
injector_->vehicle_state()->linear_velocity() <=
243 lon_based_pidcontroller_conf_.switch_speed()) {
245 injector_->vehicle_state()->linear_velocity(),
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());
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());
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());
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);
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();
278 speed_controller_input = speed_offset + debug->speed_error();
280 speed_controller_input_limited =
282 speed_controller_input_limit);
284 double acceleration_cmd_closeloop = 0.0;
286 acceleration_cmd_closeloop =
287 speed_pid_controller_.
Control(speed_controller_input_limited, ts);
288 debug->set_pid_saturation_status(
290 if (enable_leadlag) {
291 acceleration_cmd_closeloop =
292 speed_leadlag_controller_.
Control(acceleration_cmd_closeloop, ts);
293 debug->set_leadlag_saturation_status(
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);
311 double slope_offset_compensation =
312 lon_based_pidcontroller_conf_.use_opposite_slope_compensation() *
314 std::sin(vehicle_pitch_rad + FLAGS_pitch_offset_deg * M_PI / 180);
316 if (std::isnan(slope_offset_compensation)) {
317 slope_offset_compensation = 0;
320 debug->set_slope_offset_compensation(slope_offset_compensation);
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();
330 double current_steer_interval =
332 if (lon_based_pidcontroller_conf_.use_steering_check() &&
335 std::abs(current_steer_interval) >
336 lon_based_pidcontroller_conf_.steer_cmd_interval()) {
339 ADEBUG <<
"Steer cmd interval is larger than "
340 << lon_based_pidcontroller_conf_.steer_cmd_interval();
344 acceleration_cmd = 0;
345 debug->set_is_wait_steer(
true);
347 debug->set_is_wait_steer(
false);
349 debug->set_current_steer_interval(current_steer_interval);
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();
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()) <=
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();
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();
378 if (!previous_full_stop) {
379 max_path_remain_when_stopped_ = FLAGS_max_path_remain_when_stopped;
381 max_path_remain_when_stopped_ =
382 FLAGS_max_path_remain_when_stopped +
383 lon_based_pidcontroller_conf_.full_stop_path_remain_gain();
387 debug->path_remain() < max_path_remain_when_stopped_) ||
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();
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();
413 if (
injector_->vehicle_state()->linear_velocity() <
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);
421 ADEBUG <<
"Not into full stop decision by path remain.";
424 if (debug->is_full_stop()) {
427 ? std::max(acceleration_cmd,
428 -lon_based_pidcontroller_conf_.standstill_acceleration())
429 : std::min(acceleration_cmd,
430 lon_based_pidcontroller_conf_.standstill_acceleration());
435 if (debug->is_full_stop_soft()) {
438 (acceleration_cmd >= 0) ? standstill_narmal_acceleration_
439 : (debug->path_remain() >= 0) ? acceleration_cmd
441 ? (acceleration_cmd + stop_gain_acceleration_)
442 : (acceleration_cmd + standstill_narmal_acceleration_);
445 (acceleration_cmd <= 0) ? -standstill_narmal_acceleration_
446 : (debug->path_remain() <= 0) ? acceleration_cmd
448 ? (acceleration_cmd - stop_gain_acceleration_)
449 : (acceleration_cmd - standstill_narmal_acceleration_);
455 double throttle_lowerbound =
457 lon_based_pidcontroller_conf_.throttle_minimum_action());
458 double brake_lowerbound =
460 lon_based_pidcontroller_conf_.brake_minimum_action());
461 double calibration_value = 0.0;
462 double acceleration_lookup =
467 double acceleration_lookup_limited =
469 lon_based_pidcontroller_conf_.enable_slope_offset() *
470 debug->slope_offset_compensation();
471 double acceleration_lookup_limit = 0.0;
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;
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));
486 calibration_value = control_interpolation_->Interpolate(std::make_pair(
487 std::fabs(debug->preview_speed_reference()), acceleration_lookup));
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));
495 calibration_value = control_interpolation_->Interpolate(std::make_pair(
496 std::fabs(
injector_->vehicle_state()->linear_velocity()),
497 acceleration_lookup));
501 if (acceleration_lookup >= 0) {
502 if (calibration_value >= 0) {
503 throttle_cmd = std::max(calibration_value, throttle_lowerbound);
505 throttle_cmd = throttle_lowerbound;
510 if (calibration_value >= 0) {
511 brake_cmd = brake_lowerbound;
513 brake_cmd = std::max(-calibration_value, brake_lowerbound);
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;
525 parking_release_ =
true;
528 ADEBUG <<
"Into park brake release.";
529 cmd->set_parking_brake(
false);
530 SetParkingBrake(&lon_based_pidcontroller_conf_, cmd);
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);
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);
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());
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);
577 cmd->set_acceleration(acceleration_cmd);
580 if (std::fabs(
injector_->vehicle_state()->linear_velocity()) <=
584 cmd->set_gear_location(trajectory_message_->
gear());
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()) {
603 lon_based_pidcontroller_conf_.speed_itfc_speed_cmd();
607 cmd->set_speed(reference_spd_cmd_);
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;
634 auto vehicle_state =
injector_->vehicle_state();
636 vehicle_state->x(), vehicle_state->y());
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);
645 double preview_control_time = current_control_time + preview_time;
649 current_control_time);
652 preview_control_time);
654 debug->mutable_current_matched_point()->mutable_path_point()->set_x(
656 debug->mutable_current_matched_point()->mutable_path_point()->set_y(
658 debug->mutable_current_reference_point()->mutable_path_point()->set_x(
660 debug->mutable_current_reference_point()->mutable_path_point()->set_y(
662 debug->mutable_preview_reference_point()->mutable_path_point()->set_x(
664 debug->mutable_preview_reference_point()->mutable_path_point()->set_y(
667 ADEBUG <<
"matched point:" << matched_point.DebugString();
668 ADEBUG <<
"reference point:" << reference_point.DebugString();
669 ADEBUG <<
"preview point:" << preview_point.DebugString();
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);
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 =
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);
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();