The Proc logic of the component, which called by the CyberRT frame.
317 {
319
320 injector_->control_debug_info_clear();
321
322 chassis_reader_->Observe();
323 const auto &chassis_msg = chassis_reader_->GetLatestObserved();
324 if (chassis_msg == nullptr) {
325 AERROR <<
"Chassis msg is not ready!";
326 injector_->set_control_process(false);
327 return false;
328 }
329 OnChassis(chassis_msg);
330
331 trajectory_reader_->Observe();
332 const auto &trajectory_msg = trajectory_reader_->GetLatestObserved();
333 if (trajectory_msg == nullptr) {
334 AERROR <<
"planning msg is not ready!";
335 } else {
336
338 trajectory_msg->header().sequence_num()) {
339 OnPlanning(trajectory_msg);
340 }
341 }
342
343 planning_command_status_reader_->Observe();
344 const auto &planning_status_msg =
345 planning_command_status_reader_->GetLatestObserved();
346 if (planning_status_msg != nullptr) {
347 OnPlanningCommandStatus(planning_status_msg);
348 ADEBUG <<
"Planning command status msg is \n"
349 << planning_command_status_.ShortDebugString();
350 }
351 injector_->set_planning_command_status(planning_command_status_);
352
353 localization_reader_->Observe();
354 const auto &localization_msg = localization_reader_->GetLatestObserved();
355 if (localization_msg == nullptr) {
356 AERROR <<
"localization msg is not ready!";
357 injector_->set_control_process(false);
358 return false;
359 }
360 OnLocalization(localization_msg);
361
362 pad_msg_reader_->Observe();
363 const auto &pad_msg = pad_msg_reader_->GetLatestObserved();
364 if (pad_msg != nullptr) {
365 OnPad(pad_msg);
366 }
367
368 {
369
370 std::lock_guard<std::mutex> lock(mutex_);
371 local_view_.mutable_chassis()->CopyFrom(latest_chassis_);
372 local_view_.mutable_trajectory()->CopyFrom(latest_trajectory_);
373 local_view_.mutable_localization()->CopyFrom(latest_localization_);
374 if (pad_msg != nullptr) {
375 local_view_.mutable_pad_msg()->CopyFrom(pad_msg_);
376 }
377 }
378
379
380 if (FLAGS_use_control_submodules) {
381 local_view_.mutable_header()->set_lidar_timestamp(
383 local_view_.mutable_header()->set_camera_timestamp(
385 local_view_.mutable_header()->set_radar_timestamp(
387 common::util::FillHeader(FLAGS_control_local_view_topic, &local_view_);
388
390
391
393 FLAGS_control_local_view_topic);
394 latency_recorder.AppendLatencyRecord(
396 end_time);
397
398 local_view_writer_->Write(local_view_);
399 return true;
400 }
401
402 if (pad_msg != nullptr) {
403 ADEBUG <<
"pad_msg: " << pad_msg_.ShortDebugString();
404 ADEBUG <<
"pad_msg is not nullptr";
406 AINFO <<
"Control received RESET action!";
407 estop_ = false;
408 estop_reason_.clear();
409 }
410 pad_received_ = true;
411 }
412
413 if (FLAGS_is_control_test_mode && FLAGS_control_test_duration > 0 &&
414 (start_time - init_time_).ToSecond() > FLAGS_control_test_duration) {
415 AERROR <<
"Control finished testing. exit";
416 injector_->set_control_process(false);
417 return false;
418 }
419
420 injector_->set_control_process(true);
421
422 injector_->mutable_control_debug_info()
423 ->mutable_control_component_debug()
424 ->Clear();
425 CheckAutoMode(&local_view_.
chassis());
426
427 ControlCommand control_command;
428
432 status = ProduceControlCommand(&control_command);
433 ADEBUG <<
"Produce control command normal.";
434 } else {
435 ADEBUG <<
"Into reset control command.";
436 ResetAndProduceZeroControlCommand(&control_command);
437 }
438
439 AERROR_IF(!status.ok()) <<
"Failed to produce control command:"
440 << status.error_message();
441
442 if (pad_received_) {
443 control_command.mutable_pad_msg()->CopyFrom(pad_msg_);
444 pad_received_ = false;
445 }
446
447
448 if (estop_) {
449 control_command.mutable_header()->mutable_status()->set_msg(estop_reason_);
450 }
451
452
453 control_command.mutable_header()->set_lidar_timestamp(
455 control_command.mutable_header()->set_camera_timestamp(
457 control_command.mutable_header()->set_radar_timestamp(
459
460 if (FLAGS_is_control_test_mode) {
461 ADEBUG <<
"Skip publish control command in test mode";
462 return true;
463 }
464
465 if (fabs(control_command.debug().simple_lon_debug().vehicle_pitch()) <
467 injector_->vehicle_state()->Update(local_view_.
localization(),
469 GetVehiclePitchAngle(&control_command);
470 }
471
473 const double time_diff_ms = (end_time - start_time).ToSecond() * 1e3;
474 ADEBUG <<
"total control time spend: " << time_diff_ms <<
" ms.";
475
476 control_command.mutable_latency_stats()->set_total_time_ms(time_diff_ms);
477 control_command.mutable_latency_stats()->set_total_time_exceeded(
478 time_diff_ms > FLAGS_control_period * 1e3);
479 if (control_command.mutable_latency_stats()->total_time_exceeded()) {
480 AINFO <<
"total control cycle time is exceeded: " << time_diff_ms <<
" ms.";
481 }
482 status.Save(control_command.mutable_header()->mutable_status());
483
484
487 FLAGS_control_command_topic);
488 latency_recorder.AppendLatencyRecord(
490 end_time);
491 }
492
493 common::util::FillHeader(
node_->Name(), &control_command);
494 ADEBUG << control_command.ShortDebugString();
495 control_cmd_writer_->Write(control_command);
496
497
498 injector_->Set_pervious_control_command(&control_command);
499 injector_->previous_control_command_mutable()->CopyFrom(control_command);
500
501 injector_->previous_control_debug_mutable()->CopyFrom(
502 injector_->control_debug_info());
503
504 PublishControlInteractiveMsg();
505 const auto end_process_control_time =
Clock::Now();
506 const double process_control_time_diff =
507 (end_process_control_time - start_time).ToSecond() * 1e3;
508 if (control_command.mutable_latency_stats()->total_time_exceeded()) {
509 AINFO <<
"control all spend time is: " << process_control_time_diff
510 << " ms.";
511 }
512
513 return true;
514}
const double kDoubleEpsilon
optional DrivingMode driving_mode
optional apollo::planning::ADCTrajectory trajectory
optional apollo::canbus::Chassis chassis
optional apollo::localization::LocalizationEstimate localization
optional DrivingAction action
optional apollo::common::Header header