22#include "absl/strings/str_cat.h"
23#include "absl/strings/str_join.h"
24#include "absl/strings/str_split.h"
26#include "cyber/proto/dag_conf.pb.h"
27#include "cyber/proto/record.pb.h"
28#include "modules/dreamview/proto/scenario.pb.h"
48DEFINE_string(cyber_recorder_play_command,
"cyber_recorder play -p 1 -f ",
49 "Cyber recorder play command");
69using apollo::monitor::ComponentStatus;
70using apollo::monitor::SystemStatus;
71using RLock = boost::shared_lock<boost::shared_mutex>;
72using WLock = boost::unique_lock<boost::shared_mutex>;
73using Json = nlohmann::json;
75constexpr char kNavigationModeName[] =
"Navigation";
78const std::vector<HMIModeOperation> OperationBasedOnSimControl = {
79 HMIModeOperation::Scenario_Sim,
80 HMIModeOperation::Sim_Control,
83template <
class FlagType,
class ValueType>
84void SetGlobalFlag(std::string_view flag_name,
const ValueType &value,
87 constexpr char kGlobalFlagfile[] =
"modules/common/data/global_flagfile.txt";
91 std::ofstream fout(kGlobalFlagfile, std::ios_base::app);
92 ACHECK(fout) <<
"Fail to open global flagfile " << kGlobalFlagfile;
93 fout <<
"\n--" << flag_name <<
"=" << value << std::endl;
97void System(std::string_view cmd) {
98 const int ret = std::system(cmd.data());
100 AINFO <<
"SUCCESS: " << cmd;
102 AERROR <<
"FAILED(" << ret <<
"): " << cmd;
109 const std::shared_ptr<Node> &node,
111 : config_(util::HMIUtil::
LoadConfig(FLAGS_dv_plus_hmi_modes_config_path)),
113 monitor_log_buffer_(monitor_log_buffer) {
115 LoadDvPluginPanelsJson();
116 time_interval_ms_ = 3000;
118 monitor_timer_.reset(
new cyber::Timer(
119 time_interval_ms_, [
this]() { this->OnTimer(overtime_time_); },
false));
120 monitor_timer_->Start();
123void HMIWorker::Start(DvCallback callback_api) {
124 callback_api_ = callback_api;
125 InitReadersAndWriters();
126 RegisterStatusUpdateHandler(
127 [
this](
const bool status_changed, HMIStatus *status) {
128 apollo::common::util::FillHeader(
"HMI", status);
129 status_writer_->Write(*status);
130 status->clear_header();
132 ResetComponentStatusTimer();
133 thread_future_ = cyber::Async(&HMIWorker::StatusUpdateThreadLoop,
this);
136void HMIWorker::Stop() {
138 std::system(FLAGS_terminal_stop_cmd.data());
139 if (thread_future_.valid()) {
140 thread_future_.get();
144bool HMIWorker::LoadVehicleDefinedMode(
const std::string &mode_config_path,
145 const std::string ¤t_vehicle_path,
146 HMIMode *self_defined_mode) {
147 const std::string mode_file_name =
148 cyber::common::GetFileName(mode_config_path);
149 const std::string vehicle_mode_config_path =
150 current_vehicle_path +
"/dreamview_conf/hmi_modes/" + mode_file_name;
151 if (!cyber::common::PathExists(vehicle_mode_config_path)) {
154 ACHECK(cyber::common::GetProtoFromFile(vehicle_mode_config_path,
156 <<
"Unable to parse vehicle self defined HMIMode from file "
157 << vehicle_mode_config_path;
158 util::HMIUtil::TranslateCyberModules(self_defined_mode);
162void HMIWorker::InitStatus() {
163 static constexpr char kDockerImageEnv[] =
"DOCKER_IMG";
164 status_.set_docker_image(cyber::common::GetEnv(kDockerImageEnv));
165 status_.set_utm_zone_id(FLAGS_local_utm_zone_id);
168 const auto &modes = config_.modes();
169 for (
const auto &iter : modes) {
170 const std::string &mode = iter.first;
171 status_.add_modes(mode);
172 if (mode == FLAGS_vehicle_calibration_mode) {
173 FuelMonitorManager::Instance()->RegisterFuelMonitor(
174 mode, std::make_unique<DataCollectionMonitor>());
175 FuelMonitorManager::Instance()->RegisterFuelMonitor(
176 mode, std::make_unique<PreprocessMonitor>());
177 }
else if (mode == FLAGS_lidar_calibration_mode) {
178 FuelMonitorManager::Instance()->RegisterFuelMonitor(
179 mode, std::make_unique<PreprocessMonitor>(
"lidar_to_gnss"));
180 }
else if (mode == FLAGS_camera_calibration_mode) {
181 FuelMonitorManager::Instance()->RegisterFuelMonitor(
182 mode, std::make_unique<PreprocessMonitor>(
"camera_to_lidar"));
187 for (
const auto &map_entry : config_.maps()) {
188 status_.add_maps(map_entry.first);
191 if (map_entry.second == FLAGS_map_dir) {
192 status_.set_current_map(map_entry.first);
197 for (
const auto &vehicle : config_.vehicles()) {
198 status_.add_vehicles(vehicle.first);
206 const std::string cached_mode =
207 KVDB::Get(FLAGS_current_mode_db_key).value_or(
"");
208 if (FLAGS_use_navigation_mode && ContainsKey(modes, kNavigationModeName)) {
209 ChangeMode(kNavigationModeName);
210 }
else if (ContainsKey(modes, cached_mode)) {
211 ChangeMode(cached_mode);
212 }
else if (ContainsKey(modes, FLAGS_default_hmi_mode)) {
213 ChangeMode(FLAGS_default_hmi_mode);
215 ChangeMode(modes.begin()->first);
219void HMIWorker::InitReadersAndWriters() {
220 status_writer_ = node_->CreateWriter<HMIStatus>(FLAGS_hmi_status_topic);
221 action_command_client_ = node_->CreateClient<ActionCommand, CommandStatus>(
222 FLAGS_action_command_topic);
223 audio_event_writer_ =
224 node_->CreateWriter<AudioEvent>(FLAGS_audio_event_topic);
225 drive_event_writer_ =
226 node_->CreateWriter<DriveEvent>(FLAGS_drive_event_topic);
228 monitor_reader_ = node_->CreateReader<SystemStatus>(
229 FLAGS_system_status_topic,
230 [
this](
const std::shared_ptr<SystemStatus> &system_status) {
231 this->ResetComponentStatusTimer();
233 WLock wlock(status_mutex_);
235 const bool is_realtime_msg =
237 ? system_status->is_realtime_in_simulation()
239 system_status->header().timestamp_sec() <
240 FLAGS_system_status_lifetime_seconds;
242 if (is_realtime_msg) {
243 for (
auto &iter : *status_.mutable_modules()) {
244 bool previous_second = iter.second;
245 auto *status = FindOrNull(system_status->hmi_modules(), iter.first);
247 status !=
nullptr && status->status() == ComponentStatus::OK;
248 if (previous_second != iter.second) {
249 LockModule(iter.first,
false);
254 if (system_status->detect_immediately()) {
255 status_.set_expected_modules(status_.expected_modules() - 1);
259 for (
auto &iter : *status_.mutable_monitored_components()) {
260 auto *status = FindOrNull(system_status->components(), iter.first);
261 if (status !=
nullptr) {
262 iter.second = status->summary();
264 iter.second.set_status(ComponentStatus::UNKNOWN);
265 iter.second.set_message(
"Status not reported by Monitor.");
270 for (
auto &iter : *status_.mutable_other_components()) {
272 FindOrNull(system_status->other_components(), iter.first);
273 if (status !=
nullptr) {
274 iter.second.CopyFrom(*status);
276 iter.second.set_status(ComponentStatus::UNKNOWN);
277 iter.second.set_message(
"Status not reported by Monitor.");
281 for (
auto &iter : *system_status->mutable_global_components()) {
282 (*status_.mutable_global_components())[iter.first] = iter.second;
286 const size_t new_fingerprint =
288 if (last_status_fingerprint_ != new_fingerprint) {
289 status_changed_ =
true;
290 last_status_fingerprint_ = new_fingerprint;
294 node_->CreateReader<RecordInfo>(
295 FLAGS_record_info_topic,
296 [
this](
const std::shared_ptr<RecordInfo> &record_info) {
297 WLock wlock(status_mutex_);
298 if (record_info->record_name() ==
299 status_.current_record_status().current_record_id()) {
300 status_.mutable_current_record_status()->set_curr_time_s(
301 record_info.get()->curr_time_s());
302 status_changed_ =
true;
306 localization_reader_ =
307 node_->CreateReader<LocalizationEstimate>(FLAGS_localization_topic);
309 chassis_reader_ = node_->CreateReader<Chassis>(
310 FLAGS_chassis_topic, [
this](
const std::shared_ptr<Chassis> &chassis) {
312 FLAGS_system_status_lifetime_seconds) {
313 if (chassis->signal().high_beam()) {
315 const bool ret = Trigger(HMIAction::NONE);
316 AERROR_IF(!ret) <<
"Failed to execute high_beam action.";
322bool HMIWorker::Trigger(
const HMIAction action) {
323 AERROR <<
"HMIAction " << HMIAction_Name(action) <<
" was triggered!";
325 case HMIAction::NONE:
327 case HMIAction::SETUP_MODE:
329 AddExpectedModules(action);
331 case HMIAction::ENTER_AUTO_MODE:
332 return ChangeDrivingMode(Chassis::COMPLETE_AUTO_DRIVE);
333 case HMIAction::DISENGAGE:
334 return ChangeDrivingMode(Chassis::COMPLETE_MANUAL);
335 case HMIAction::RESET_MODE:
338 case HMIAction::LOAD_DYNAMIC_MODELS:
341 case HMIAction::LOAD_RECORDS:
344 case HMIAction::LOAD_RTK_RECORDS:
347 case HMIAction::STOP_RECORD:
350 case HMIAction::LOAD_MAPS:
354 AERROR <<
"HMIAction not implemented, yet!";
360bool HMIWorker::Trigger(
const HMIAction action,
const std::string &value) {
361 AINFO <<
"HMIAction " << HMIAction_Name(action) <<
"(" << value
362 <<
") was triggered!";
365 case HMIAction::CHANGE_MODE:
368 case HMIAction::CHANGE_MAP:
369 ret = ChangeMap(value);
372 SimControlManager::Instance()->Restart();
374 case HMIAction::CHANGE_VEHICLE:
375 ChangeVehicle(value);
377 case HMIAction::START_MODULE:
379 AddExpectedModules(action);
381 case HMIAction::STOP_MODULE:
384 case HMIAction::DELETE_SCENARIO_SET:
385 DeleteScenarioSet(value);
387 case HMIAction::CHANGE_DYNAMIC_MODEL:
388 ChangeDynamicModel(value);
390 case HMIAction::DELETE_DYNAMIC_MODEL:
391 DeleteDynamicModel(value);
393 case HMIAction::DELETE_RECORD:
396 case HMIAction::CHANGE_RECORD:
399 case HMIAction::CHANGE_RTK_RECORD:
400 ChangeRtkRecord(value);
402 case HMIAction::CHANGE_OPERATION:
403 ChangeOperation(value);
405 case HMIAction::DELETE_VEHICLE_CONF:
406 DeleteVehicleConfig(value);
407 case HMIAction::DELETE_V2X_CONF:
408 DeleteV2xConfig(value);
410 case HMIAction::DELETE_MAP:
413 case HMIAction::LOAD_RECORD:
414 LoadRecordAndChangeStatus(value);
417 AERROR <<
"HMIAction not implemented, yet!";
423void HMIWorker::SubmitAudioEvent(
const uint64_t event_time_ms,
424 const int obstacle_id,
const int audio_type,
425 const int moving_result,
426 const int audio_direction,
427 const bool is_siren_on) {
428 std::shared_ptr<AudioEvent> audio_event = std::make_shared<AudioEvent>();
429 apollo::common::util::FillHeader(
"HMI", audio_event.get());
433 audio_event->mutable_header()->set_timestamp_sec(
434 static_cast<double>(event_time_ms) / 1000.0);
435 audio_event->set_id(obstacle_id);
436 audio_event->set_audio_type(
438 audio_event->set_moving_result(
440 audio_event->set_audio_direction(
442 audio_event->set_siren_is_on(is_siren_on);
445 localization_reader_->Observe();
446 if (localization_reader_->Empty()) {
447 AERROR <<
"Failed to get localization associated with the audio event: "
448 << audio_event->DebugString() <<
"\n Localization reader is empty!";
452 const std::shared_ptr<LocalizationEstimate> localization =
453 localization_reader_->GetLatestObserved();
454 audio_event->mutable_pose()->CopyFrom(localization->pose());
455 AINFO <<
"AudioEvent: " << audio_event->DebugString();
457 audio_event_writer_->Write(audio_event);
460void HMIWorker::SubmitDriveEvent(
const uint64_t event_time_ms,
461 const std::string &event_msg,
462 const std::vector<std::string> &event_types,
463 const bool is_reportable) {
464 std::shared_ptr<DriveEvent> drive_event = std::make_shared<DriveEvent>();
465 apollo::common::util::FillHeader(
"HMI", drive_event.get());
469 drive_event->mutable_header()->set_timestamp_sec(
470 static_cast<double>(event_time_ms) / 1000.0);
471 drive_event->set_event(event_msg);
472 drive_event->set_is_reportable(is_reportable);
473 for (
const auto &type_name : event_types) {
474 DriveEvent::Type type;
475 if (DriveEvent::Type_Parse(type_name, &type)) {
476 drive_event->add_type(type);
478 AERROR <<
"Failed to parse drive event type:" << type_name;
481 drive_event_writer_->Write(drive_event);
484void HMIWorker::SensorCalibrationPreprocess(
const std::string &task_type) {
485 std::string start_command = absl::StrCat(
486 "nohup bash modules/tools/sensor_calibration/extract_data.sh -t ",
488 System(start_command);
491void HMIWorker::VehicleCalibrationPreprocess() {
492 std::string start_command = absl::StrCat(
493 "nohup bash modules/tools/vehicle_calibration/preprocess.sh "
495 status_.current_vehicle(),
"\" --record_num=", record_count_,
" &");
496 System(start_command);
499bool HMIWorker::ChangeDrivingMode(
const Chassis::DrivingMode mode) {
501 const std::string mode_name = Chassis::DrivingMode_Name(mode);
502 if (mode != Chassis::COMPLETE_MANUAL) {
503 if (!ChangeDrivingMode(Chassis::COMPLETE_MANUAL)) {
504 AERROR <<
"Failed to reset to MANUAL before changing to " << mode_name;
509 auto command = std::make_shared<ActionCommand>();
512 case Chassis::COMPLETE_MANUAL:
513 command->set_command(
516 case Chassis::COMPLETE_AUTO_DRIVE:
517 command->set_command(
521 AFATAL <<
"Change driving mode to " << mode_name <<
" not implemented!";
525 static constexpr int kMaxTries = 1;
526 static constexpr auto kTryInterval = std::chrono::milliseconds(500);
527 for (
int i = 0; i < kMaxTries; ++i) {
529 common::util::FillHeader(
"HMI", command.get());
531 action_command_client_->SendRequest(command, std::chrono::seconds(1));
533 std::this_thread::sleep_for(kTryInterval);
535 chassis_reader_->Observe();
536 if (chassis_reader_->Empty()) {
537 AERROR <<
"No Chassis message received!";
538 }
else if (chassis_reader_->GetLatestObserved()->driving_mode() == mode) {
542 AERROR <<
"Failed to change driving mode to " << mode_name;
546bool HMIWorker::ChangeMap(
const std::string &map_name) {
547 if (status_.current_map() == map_name) {
551 return SelectAndReloadMap(map_name);
554bool HMIWorker::SelectAndReloadMap(
const std::string &map_name) {
555 const std::string *map_dir = FindOrNull(config_.maps(), map_name);
556 if (map_dir ==
nullptr) {
557 AERROR <<
"Unknown map " << map_name;
563 SetGlobalFlag(
"map_dir", *map_dir, &FLAGS_map_dir);
565 Json callback_res = callback_api_(
"MapServiceReloadMap", {});
568 WLock wlock(status_mutex_);
569 status_.set_current_map(map_name);
570 status_changed_ =
true;
572 return callback_res[
"result"];
575void HMIWorker::UpdateModeModulesAndMonitoredComponents() {
576 status_.clear_modules();
577 status_.clear_modules_lock();
578 previous_modules_lock_.clear();
579 for (
const auto &iter : current_mode_.
modules()) {
580 status_.mutable_modules()->insert({iter.first,
false});
581 status_.mutable_modules_lock()->insert({iter.first,
false});
582 previous_modules_lock_.insert({iter.first,
false});
586 status_.clear_monitored_components();
587 for (
const auto &iter : current_mode_.monitored_components()) {
588 status_.mutable_monitored_components()->insert({iter.first, {}});
592void HMIWorker::ChangeVehicle(
const std::string &vehicle_name) {
593 const std::string *vehicle_dir = FindOrNull(config_.vehicles(), vehicle_name);
594 if (vehicle_dir ==
nullptr) {
595 AERROR <<
"Unknown vehicle " << vehicle_name;
598 std::string current_mode;
601 WLock wlock(status_mutex_);
602 current_mode = status_.current_mode();
603 if (status_.current_vehicle() == vehicle_name) {
609 const std::string vehicle_type_file_path = *vehicle_dir +
"/vehicle_type";
610 std::string vehicle_type_str;
611 cyber::common::GetContent(vehicle_type_file_path, &vehicle_type_str);
612 int vehicle_type = std::stoi(vehicle_type_str);
613 status_.set_current_vehicle_type(vehicle_type);
614 }
catch (
const std::exception &e) {
615 AWARN <<
"get vehicle type config failed: " << e.what();
616 status_.clear_current_vehicle_type();
618 status_.set_current_vehicle(vehicle_name);
619 status_changed_ =
true;
623 HMIMode vehicle_defined_mode;
624 const std::string mode_config_path = config_.modes().at(current_mode);
625 if (LoadVehicleDefinedMode(mode_config_path, *vehicle_dir,
626 &vehicle_defined_mode)) {
627 MergeToCurrentMode(&vehicle_defined_mode);
631 current_mode_ = util::HMIUtil::LoadMode(mode_config_path);
634 WLock wlock(status_mutex_);
635 UpdateModeModulesAndMonitoredComponents();
637 ACHECK(VehicleManager::Instance()->UseVehicle(*vehicle_dir));
639 auto *monitors = FuelMonitorManager::Instance()->GetCurrentMonitors();
640 if (monitors !=
nullptr) {
641 for (
const auto &monitor : *monitors) {
642 if (monitor.second->IsEnabled()) {
643 monitor.second->Restart();
649void HMIWorker::MergeToCurrentMode(HMIMode *mode) {
650 current_mode_.clear_modules();
651 current_mode_.clear_cyber_modules();
652 current_mode_.clear_monitored_components();
653 current_mode_.mutable_modules()->swap(*(mode->mutable_modules()));
654 current_mode_.mutable_monitored_components()->swap(
655 *(mode->mutable_monitored_components()));
658void HMIWorker::ChangeMode(
const std::string &mode_name) {
659 if (!ContainsKey(config_.modes(), mode_name)) {
660 AERROR <<
"Cannot change to unknown mode " << mode_name;
665 RLock rlock(status_mutex_);
667 if (status_.current_mode() == mode_name) {
672 std::string default_operation_str =
"";
674 WLock wlock(status_mutex_);
675 status_.set_current_mode(mode_name);
676 current_mode_ = util::HMIUtil::LoadMode(config_.modes().at(mode_name));
678 HMIMode vehicle_defined_mode;
679 const std::string *vehicle_dir =
680 FindOrNull(config_.vehicles(), status_.current_vehicle());
681 if (vehicle_dir !=
nullptr &&
682 LoadVehicleDefinedMode(config_.modes().at(mode_name), *vehicle_dir,
683 &vehicle_defined_mode)) {
684 MergeToCurrentMode(&vehicle_defined_mode);
686 UpdateModeModulesAndMonitoredComponents();
688 status_.clear_other_components();
689 for (
const auto &iter : current_mode_.other_components()) {
690 status_.mutable_other_components()->insert({iter.first, {}});
694 status_.clear_operations();
695 status_.mutable_operations()->CopyFrom(current_mode_.operations());
699 if ((mode_name ==
"Pnc" || mode_name ==
"Default") &&
700 PackageExist(
"simulator-plugin")) {
701 status_.add_operations(HMIModeOperation::Scenario_Sim);
704 if (current_mode_.has_default_operation()) {
705 default_operation_str =
706 HMIModeOperation_Name(current_mode_.default_operation());
708 status_changed_ =
true;
714 if (!default_operation_str.empty()) {
715 ChangeOperation(default_operation_str);
717 FuelMonitorManager::Instance()->SetCurrentMode(mode_name);
718 KVDB::Put(FLAGS_current_mode_db_key, mode_name);
721void HMIWorker::StartModule(
const std::string &module) {
722 const Module *module_conf = FindOrNull(current_mode_.modules(), module);
723 if (module_conf !=
nullptr) {
725 WLock wlock(status_mutex_);
726 LockModule(module,
true);
728 System(module_conf->start_command());
730 AERROR <<
"Cannot find module " <<
module;
734void HMIWorker::StopModule(
const std::string &module) {
735 const Module *module_conf = FindOrNull(current_mode_.modules(), module);
736 if (module_conf !=
nullptr) {
738 WLock wlock(status_mutex_);
739 LockModule(module,
true);
741 System(module_conf->stop_command());
743 AERROR <<
"Cannot find module " <<
module;
747HMIStatus HMIWorker::GetStatus()
const {
748 RLock rlock(status_mutex_);
752void HMIWorker::SetupMode() {
754 WLock wlock(status_mutex_);
755 status_.set_backend_shutdown(
false);
757 for (
const auto &iter : current_mode_.
modules()) {
759 WLock wlock(status_mutex_);
760 auto modules = status_.modules();
762 LockModule(iter.first,
true);
765 System(iter.second.start_command());
769void HMIWorker::ResetMode() {
773 WLock wlock(status_mutex_);
774 status_.set_backend_shutdown(
true);
776 for (
const auto &iter : current_mode_.
modules()) {
778 WLock wlock(status_mutex_);
779 auto modules = status_.modules();
781 LockModule(iter.first,
true);
784 System(iter.second.stop_command());
789void HMIWorker::StatusUpdateThreadLoop() {
790 constexpr int kLoopIntervalMs = 200;
792 std::this_thread::sleep_for(std::chrono::milliseconds(kLoopIntervalMs));
793 UpdateComponentStatus();
794 bool status_changed =
false;
796 WLock wlock(status_mutex_);
797 status_changed = status_changed_;
798 status_changed_ =
false;
801 if (!status_changed) {
802 static double next_update_time = 0;
804 if (now < next_update_time) {
807 next_update_time = now + FLAGS_status_publish_interval;
811 HMIStatus status = GetStatus();
812 for (
const auto handler : status_update_handlers_) {
813 handler(status_changed, &status);
818void HMIWorker::ResetComponentStatusTimer() {
820 last_status_fingerprint_ = 0;
823void HMIWorker::UpdateComponentStatus() {
825 if (now - last_status_received_s_.load() > FLAGS_monitor_timeout_threshold) {
826 if (!monitor_timed_out_) {
827 WLock wlock(status_mutex_);
829 const uint64_t now_ms =
static_cast<uint64_t
>(now * 2e3);
830 static constexpr bool kIsReportable =
true;
831 SubmitDriveEvent(now_ms,
"Monitor timed out", {
"PROBLEM"}, kIsReportable);
832 AWARN <<
"System fault. Auto disengage.";
833 Trigger(HMIAction::DISENGAGE);
835 for (
auto &monitored_component :
836 *status_.mutable_monitored_components()) {
837 monitored_component.second.set_status(ComponentStatus::UNKNOWN);
838 monitored_component.second.set_message(
839 "Status not reported by Monitor.");
841 status_changed_ =
true;
843 monitor_timed_out_ =
true;
845 monitor_timed_out_ =
false;
849bool HMIWorker::GetScenarioResourcePath(std::string *scenario_resource_path) {
850 CHECK_NOTNULL(scenario_resource_path);
851 const std::string home = cyber::common::GetEnv(
"HOME");
855 *scenario_resource_path = home + FLAGS_resource_scenario_path;
859void HMIWorker::ChangeDynamicModel(
const std::string &dynamic_model_name) {
868 if (dynamic_model_name.empty()) {
869 AERROR <<
"Failed to change empty dynamic model!";
872 auto sim_control_manager = SimControlManager::Instance();
873 if (!sim_control_manager->IsEnabled()) {
874 AERROR <<
"Sim control is not enabled!";
878 if (!sim_control_manager->ChangeDynamicModel(dynamic_model_name)) {
882 AERROR <<
"Failed to change dynamic model! Please check if the param is "
886 std::string current_dynamic_model_name = dynamic_model_name;
888 WLock wlock(status_mutex_);
889 status_.set_current_dynamic_model(dynamic_model_name);
890 status_changed_ =
true;
895bool HMIWorker::UpdateDynamicModelToStatus(
896 const std::string &dynamic_model_name) {
897 auto sim_control_manager = SimControlManager::Instance();
898 if (!sim_control_manager->IsEnabled()) {
899 AERROR <<
"Sim control is not enabled or missing dynamic model name "
904 if (!sim_control_manager->AddDynamicModel(dynamic_model_name)) {
905 AERROR <<
"Failed to add dynamic model to local dynamic model list for "
910 WLock wlock(status_mutex_);
911 for (
const auto &iter : status_.dynamic_models()) {
912 if (iter == dynamic_model_name) {
913 AERROR <<
"Do not need to add new dynamic model for is duplicate!";
917 status_.add_dynamic_models(dynamic_model_name);
918 status_changed_ =
true;
923bool HMIWorker::LoadDynamicModels() {
924 auto sim_control_manager = SimControlManager::Instance();
925 nlohmann::json load_res;
926 if (sim_control_manager->IsEnabled()) {
927 load_res = sim_control_manager->LoadDynamicModels();
929 AERROR <<
"Sim control is not enabled!";
932 if (!load_res.contains(
"result") || !load_res[
"result"]) {
937 WLock wlock(status_mutex_);
938 auto dynamic_models = status_.mutable_dynamic_models();
940 for (
auto iter = dynamic_models->begin(); iter != dynamic_models->end();) {
941 iter = dynamic_models->erase(iter);
943 for (
const auto &dynamic_model : load_res[
"loaded_dynamic_models"]) {
944 status_.add_dynamic_models(dynamic_model);
946 status_changed_ =
true;
951void HMIWorker::DeleteScenarioSet(
const std::string &scenario_set_id) {
952 if (scenario_set_id.empty()) {
955 std::string directory_path;
956 if (!GetScenarioResourcePath(&directory_path)) {
957 AERROR <<
"Failed to get scenario resource path!";
961 if (scenario_set_id.find(
'/') != std::string::npos ||
962 scenario_set_id.find(
"..") != std::string::npos ||
963 scenario_set_id.find(
' ') != std::string::npos ||
964 scenario_set_id.find(
"~") != std::string::npos) {
965 AERROR <<
"Scenario set id should not contain '/' ,' ',~ and ..";
968 directory_path = directory_path + scenario_set_id;
969 if (!cyber::common::PathExists(directory_path)) {
970 AERROR <<
"Failed to find scenario_set!";
973 std::string command =
"rm -fr " + directory_path;
976 if (std::system(command.data()) != 0) {
977 AERROR <<
"Failed to delete scenario set directory for: "
978 << std::strerror(errno);
984void HMIWorker::DeleteDynamicModel(
const std::string &dynamic_model_name) {
985 if (dynamic_model_name.empty()) {
986 AERROR <<
"Invalid param:empty dynamic model name!";
990 RLock rlock(status_mutex_);
992 if (dynamic_model_name == status_.current_dynamic_model()) {
993 AERROR <<
"Cannot delete current dynamic model!";
996 if (dynamic_model_name == FLAGS_sim_perfect_control) {
997 AERROR <<
"Cannot delete default sim control:SimPerfectControl!";
1001 auto sim_control_manager = SimControlManager::Instance();
1002 if (!sim_control_manager->IsEnabled()) {
1003 AERROR <<
"Sim control is not enabled!";
1006 if (!sim_control_manager->DeleteDynamicModel(dynamic_model_name)) {
1009 AERROR <<
"Failed to delete dynamic model!";
1013 WLock wlock(status_mutex_);
1014 auto iter = status_.dynamic_models().begin();
1015 while (iter != status_.dynamic_models().end()) {
1016 if (*iter == dynamic_model_name) {
1021 if (iter != status_.dynamic_models().end()) {
1022 status_.mutable_dynamic_models()->erase(iter);
1023 status_changed_ =
true;
1025 AWARN <<
"Can not find dynamic model to delete!";
1031bool HMIWorker::GetRecordPath(std::string *record_path) {
1032 CHECK_NOTNULL(record_path);
1033 const std::string home = cyber::common::GetEnv(
"HOME");
1037 *record_path = home + FLAGS_resource_record_path;
1041bool HMIWorker::handlePlayRecordProcess(
const std::string &action_type) {
1042 std::string record_id;
1043 PlayRecordStatus current_play_record_status;
1044 PlayRecordStatus expected_play_record_status;
1045 PlayRecordStatus reasonable_play_record_status;
1047 RLock rlock(status_mutex_);
1048 auto record_status = status_.current_record_status();
1049 record_id = record_status.current_record_id();
1050 current_play_record_status = record_status.play_record_status();
1051 if (record_id.empty()) {
1052 AERROR <<
"Failed to pause record for record has not been selected"
1053 "or process is not exists!";
1056 if (!RecordIsLoaded(record_id)) {
1057 AERROR <<
"Failed to pause record for record has not been loaded!";
1061 if (action_type.compare(
"continue") == 0) {
1062 expected_play_record_status = PlayRecordStatus::RUNNING;
1063 reasonable_play_record_status = PlayRecordStatus::PAUSED;
1064 }
else if (action_type.compare(
"pause") == 0) {
1065 expected_play_record_status = PlayRecordStatus::PAUSED;
1066 reasonable_play_record_status = PlayRecordStatus::RUNNING;
1068 AERROR <<
"Invalid action type for play record.";
1071 if (reasonable_play_record_status != current_play_record_status) {
1074 auto *record_player_factory = RecordPlayerFactory::Instance();
1075 auto player_ptr = record_player_factory->GetRecordPlayer(record_id);
1077 AERROR <<
"Failed to get record player.";
1080 player_ptr->HandleNohupThreadStatus();
1083 WLock wlock(status_mutex_);
1084 status_.mutable_current_record_status()->set_play_record_status(
1085 expected_play_record_status);
1086 status_changed_ =
true;
1088 record_player_factory->IncreaseRecordPriority(record_id);
1092bool HMIWorker::RePlayRecord() {
1093 std::string record_id;
1095 RLock rlock(status_mutex_);
1096 auto record_status = status_.current_record_status();
1097 record_id = record_status.current_record_id();
1098 if (record_status.current_record_id().empty()) {
1099 AERROR <<
"Failed to play record for record has not been selected!";
1102 if (!RecordIsLoaded(record_id)) {
1103 AERROR <<
"Failed to play record for record has not been loaded!";
1106 if (record_status.play_record_status() == PlayRecordStatus::RUNNING) {
1107 AERROR <<
"Record has been played,ignore dumplicate request.";
1111 std::string record_path;
1112 if (!GetRecordPath(&record_path)) {
1113 AERROR <<
"Failed to get record path!";
1116 record_path = record_path + record_id +
".record";
1118 if (!cyber::common::PathExists(record_path)) {
1119 AERROR <<
"Failed to find record!";
1123 auto *record_player_factory = RecordPlayerFactory::Instance();
1124 auto player_ptr = record_player_factory->GetRecordPlayer(record_id);
1125 bool play_record_res = (player_ptr !=
nullptr);
1126 PlayRecordStatus play_record_status;
1127 if (!play_record_res) {
1128 AERROR <<
"Failed to get record related player";
1129 play_record_status = PlayRecordStatus::CLOSED;
1131 player_ptr->PreloadPlayRecord();
1132 player_ptr->NohupPlayRecord();
1134 callback_api_(
"GetDataHandlerConf", {});
1135 play_record_status = PlayRecordStatus::RUNNING;
1138 WLock wlock(status_mutex_);
1139 auto record_status = status_.mutable_current_record_status();
1140 record_status->set_play_record_status(play_record_status);
1141 status_changed_ =
true;
1143 record_player_factory->IncreaseRecordPriority(record_id);
1144 return play_record_res;
1147bool HMIWorker::ResetRecordProgress(
const double &progress) {
1148 std::string record_id;
1149 double total_time_s;
1150 PlayRecordStatus last_record_status;
1152 RLock rlock(status_mutex_);
1153 auto record_status = status_.current_record_status();
1154 record_id = record_status.current_record_id();
1155 last_record_status = record_status.play_record_status();
1156 if (record_status.current_record_id().empty()) {
1157 AERROR <<
"Failed to reset record progress for record has not been "
1161 if (!RecordIsLoaded(record_id)) {
1162 AERROR <<
"Failed to reset record progress for record has not been "
1166 total_time_s = status_.records().at(record_id).total_time_s();
1168 AERROR <<
"total : " << total_time_s;
1169 if (progress > total_time_s) {
1170 AERROR <<
"Failed to reset record progress for progress exceeds the "
1171 "record's total time.";
1174 auto *record_player_factory = RecordPlayerFactory::Instance();
1175 auto player_ptr = record_player_factory->GetRecordPlayer(record_id);
1177 AERROR <<
"Failed to get record player.";
1180 player_ptr->Reset();
1181 player_ptr->PreloadPlayRecord(
1182 progress, (last_record_status != PlayRecordStatus::RUNNING));
1183 player_ptr->NohupPlayRecord();
1187 if (last_record_status == PlayRecordStatus::CLOSED) {
1188 last_record_status = PlayRecordStatus::PAUSED;
1191 callback_api_(
"GetDataHandlerConf", {});
1193 WLock wlock(status_mutex_);
1194 auto record_status = status_.mutable_current_record_status();
1195 record_status->set_play_record_status(last_record_status);
1196 record_status->set_curr_time_s(progress);
1197 status_changed_ =
true;
1199 record_player_factory->IncreaseRecordPriority(record_id);
1203void HMIWorker::StopRecordPlay(
const std::string &record_id) {
1204 std::string curr_record_id =
"";
1206 RLock rlock(status_mutex_);
1207 curr_record_id = status_.current_record_status().current_record_id();
1209 if (!record_id.empty()) {
1210 curr_record_id = record_id;
1212 if (!RecordIsLoaded(curr_record_id)) {
1213 AERROR <<
"Failed to stop record player under unloaded status!";
1216 auto *record_player_factory = RecordPlayerFactory::Instance();
1217 auto player_ptr = record_player_factory->GetRecordPlayer(curr_record_id);
1219 AERROR <<
"Failed to get record player to reset.";
1222 player_ptr->Reset();
1224 WLock wlock(status_mutex_);
1225 auto record_status = status_.mutable_current_record_status();
1226 record_status->set_play_record_status(PlayRecordStatus::CLOSED);
1227 record_status->clear_curr_time_s();
1229 status_changed_ =
true;
1232bool HMIWorker::RecordIsLoaded(
const std::string &record_id) {
1234 RLock rlock(status_mutex_);
1235 auto iter = status_.records().find(record_id);
1236 return iter != status_.records().end() &&
1237 (iter->second.load_record_status() == LoadRecordStatus::LOADED);
1241void HMIWorker::ChangeRecord(
const std::string &record_id) {
1242 std::string last_record_id;
1244 RLock rlock(status_mutex_);
1245 auto status_records = status_.mutable_records();
1246 auto iter = status_records->find(record_id);
1247 if (iter == status_records->end()) {
1248 AERROR <<
"Cannot change to unknown record!";
1251 if (!RecordIsLoaded(record_id)) {
1252 AERROR <<
"Cannot change to unloaded record, load this record before!";
1255 last_record_id = status_.current_record_status().current_record_id();
1257 if (!last_record_id.empty()) {
1258 StopRecordPlay(last_record_id);
1261 callback_api_(
"ClearDataHandlerConfChannelMsgs", {});
1262 WLock wlock(status_mutex_);
1263 status_.mutable_current_record_status()->set_current_record_id(record_id);
1264 auto *record_player_factory = RecordPlayerFactory::Instance();
1265 record_player_factory->SetCurrentRecord(record_id);
1266 record_player_factory->IncreaseRecordPriority(record_id);
1267 status_changed_ =
true;
1271void HMIWorker::ClearInvalidResourceUnderChangeOperation(
1272 const HMIModeOperation operation) {
1273 ClearInvalidRecordStatus(operation);
1274 HMIModeOperation old_operation;
1276 RLock rlock(status_mutex_);
1277 old_operation = status_.current_operation();
1281 auto sim_control_manager = SimControlManager::Instance();
1282 auto iter = std::find(OperationBasedOnSimControl.begin(),
1283 OperationBasedOnSimControl.end(), operation);
1284 if (iter != OperationBasedOnSimControl.end()) {
1285 sim_control_manager->Start();
1287 sim_control_manager->Stop();
1290 WLock wlock(status_mutex_);
1291 status_.set_current_dynamic_model(
"");
1292 status_.clear_dynamic_models();
1293 status_changed_ =
true;
1296 if (old_operation == HMIModeOperation::Record) {
1300 if (old_operation == HMIModeOperation::Waypoint_Follow) {
1302 ClearRtkRecordInfo();
1306void HMIWorker::ChangeOperation(
const std::string &operation_str) {
1307 HMIModeOperation operation;
1308 if (!HMIModeOperation_Parse(operation_str, &operation)) {
1309 AERROR <<
"Invalid HMI operation string: " << operation_str;
1313 RLock rlock(status_mutex_);
1314 if (status_.current_mode().empty()) {
1315 AERROR <<
"Please select mode!";
1318 auto status_operations = status_.operations();
1319 auto iter = status_operations.begin();
1320 for (; iter != status_operations.end(); iter++) {
1321 if (*iter == operation) {
1325 if (iter == status_operations.end()) {
1326 AERROR <<
"Cannot change to unknown operation!";
1330 ClearInvalidResourceUnderChangeOperation(operation);
1332 WLock wlock(status_mutex_);
1333 status_.set_current_operation(operation);
1334 status_changed_ =
true;
1339bool HMIWorker::ReadRecordInfo(
const std::string &file,
1340 double *total_time_s)
const {
1341 cyber::record::RecordFileReader file_reader;
1342 if (!file_reader.Open(file)) {
1343 AERROR <<
"open record file error. file: " << file;
1346 cyber::proto::Header hdr = file_reader.GetHeader();
1347 if (!hdr.is_complete()) {
1348 AERROR <<
"record is not complete, can not be used. file: " << file;
1351 auto begin_time_s =
static_cast<double>(hdr.begin_time()) / 1e9;
1352 auto end_time_s =
static_cast<double>(hdr.end_time()) / 1e9;
1353 double loop_time_s = end_time_s - begin_time_s;
1354 *total_time_s = std::round(loop_time_s * 1e2) / 1e2;
1358bool HMIWorker::UpdateMapToStatus(
const std::string &map_tar_name) {
1359 if (map_tar_name.empty()) {
1363 std::string map_dir = FLAGS_maps_data_path +
"/";
1364 std::string map_name_prefix;
1365 int index = map_tar_name.rfind(
".tar.xz");
1366 if (index != -1 && map_tar_name[0] !=
'.') {
1367 map_name_prefix = map_tar_name.substr(0, index);
1368 map_dir = map_dir + map_name_prefix;
1370 AERROR <<
"The map name does not meet the standard!" << map_tar_name;
1373 if (!cyber::common::PathExists(map_dir)) {
1374 AERROR <<
"Failed to find maps!";
1377 map_name_prefix = util::HMIUtil::TitleCase(map_name_prefix);
1379 WLock wlock(status_mutex_);
1380 auto iter = status_.maps().begin();
1381 for (; iter != status_.maps().end(); iter++) {
1382 if (*iter == map_name_prefix) {
1386 if (iter != status_.maps().end()) {
1389 status_.add_maps(map_name_prefix);
1390 status_changed_ =
true;
1395bool HMIWorker::LoadRecordAndChangeStatus(
const std::string &record_name) {
1396 std::string record_file_path;
1398 RLock rlock(status_mutex_);
1399 auto &status_records = status_.records();
1400 auto iter = status_records.find(record_name);
1401 if (iter == status_records.end()) {
1402 AERROR <<
"Cannot load unknown record!";
1405 if (RecordIsLoaded(record_name)) {
1406 AERROR <<
"Cannot load already loaded record.";
1409 if (iter->second.record_file_path().empty()) {
1410 AERROR <<
"Cannot load record without record file path!";
1413 record_file_path = iter->second.record_file_path();
1416 WLock wlock(status_mutex_);
1417 auto status_records = status_.mutable_records();
1418 (*status_records)[record_name].set_load_record_status(
1419 LoadRecordStatus::LOADING);
1421 double total_time_s;
1422 if (LoadRecord(record_name, record_file_path, &total_time_s)) {
1424 WLock wlock(status_mutex_);
1425 auto status_records = status_.mutable_records();
1426 (*status_records)[record_name].set_load_record_status(
1427 LoadRecordStatus::LOADED);
1428 (*status_records)[record_name].set_total_time_s(total_time_s);
1430 RecordPlayerFactory::Instance()->IncreaseRecordPriority(record_name);
1433 WLock wlock(status_mutex_);
1434 auto status_records = status_.mutable_records();
1435 (*status_records)[record_name].set_load_record_status(
1436 LoadRecordStatus::NOT_LOAD);
1442bool HMIWorker::LoadRecord(
const std::string &record_name,
1443 const std::string &record_file_path,
1444 double *total_time_s) {
1445 if (RecordIsLoaded(record_name)) {
1446 AERROR <<
"Record is loaded,no need to load";
1450 auto record_player_factory = RecordPlayerFactory::Instance();
1451 if (!record_player_factory->EnableContinueLoad()) {
1453 std::string remove_record_id;
1454 if (record_player_factory->RemoveLRURecord(&remove_record_id)) {
1457 WLock wlock(status_mutex_);
1458 auto status_records = status_.mutable_records();
1459 (*status_records)[remove_record_id].set_load_record_status(
1460 LoadRecordStatus::NOT_LOAD);
1461 (*status_records)[remove_record_id].clear_total_time_s();
1465 if (!ReadRecordInfo(record_file_path, total_time_s)) {
1468 if (!record_player_factory->RegisterRecordPlayer(record_name,
1469 record_file_path)) {
1475bool HMIWorker::LoadRecords() {
1476 std::string directory_path;
1477 auto *record_player_factory = RecordPlayerFactory::Instance();
1478 if (!GetRecordPath(&directory_path)) {
1479 AERROR <<
"Failed to get record path!";
1482 if (!cyber::common::PathExists(directory_path)) {
1483 AERROR <<
"Failed to find records!";
1486 DIR *directory = opendir(directory_path.c_str());
1487 if (directory ==
nullptr) {
1488 AERROR <<
"Cannot open record directory" << directory_path;
1491 struct dirent *file;
1492 google::protobuf::Map<std::string, LoadRecordInfo> new_records;
1493 while ((file = readdir(directory)) !=
nullptr) {
1494 if (!strcmp(file->d_name,
".") || !strcmp(file->d_name,
"..")) {
1497 if (file->d_type == DT_DIR) {
1500 const std::string record_id = file->d_name;
1501 const int index = record_id.rfind(
".record");
1503 size_t record_suffix_length = 7;
1504 if (record_id.length() - index != record_suffix_length) {
1507 if (index != -1 && record_id[0] !=
'.') {
1508 const std::string local_record_resource = record_id.substr(0, index);
1509 const std::string record_file_path = directory_path +
"/" + record_id;
1510 double total_time_s;
1512 if (RecordIsLoaded(local_record_resource)) {
1516 if (!record_player_factory->EnableContinuePreload()) {
1517 new_records[local_record_resource].set_load_record_status(
1518 LoadRecordStatus::NOT_LOAD);
1519 new_records[local_record_resource].set_record_file_path(
1524 if (!LoadRecord(local_record_resource, record_file_path, &total_time_s)) {
1528 new_records[local_record_resource].set_total_time_s(total_time_s);
1529 new_records[local_record_resource].set_load_record_status(
1530 LoadRecordStatus::LOADED);
1531 new_records[local_record_resource].set_record_file_path(record_file_path);
1534 closedir(directory);
1536 WLock wlock(status_mutex_);
1537 auto status_records = status_.mutable_records();
1538 for (
auto iter = new_records.begin(); iter != new_records.end(); iter++) {
1539 (*status_records)[iter->first] = iter->second;
1541 status_changed_ =
true;
1546void HMIWorker::DeleteMap(
const std::string &map_name) {
1547 std::string title_map_name = util::HMIUtil::TitleCase(map_name);
1548 if (map_name.empty()) {
1551 std::string map_dir = FLAGS_maps_data_path +
"/";
1552 std::string map_abs_path = map_dir + map_name;
1553 if (!cyber::common::PathExists(map_abs_path)) {
1554 AERROR <<
"Failed to get map path: " << map_abs_path;
1558 WLock wlock(status_mutex_);
1559 auto iter = status_.maps().begin();
1560 for (; iter != status_.maps().end(); iter++) {
1561 if (*iter == title_map_name) {
1565 if (iter == status_.maps().end()) {
1566 AERROR <<
"Faile to find map name";
1569 if (status_.current_map() == title_map_name) {
1570 AERROR <<
"Cann't delete current selected map";
1573 status_.mutable_maps()->erase(iter);
1574 status_changed_ =
true;
1577 std::string cmd =
"rm -rf " + map_abs_path;
1578 if (std::system(cmd.data()) != 0) {
1579 AERROR <<
"Failed to delete map for: " << std::strerror(errno);
1582 std::string tar_abs_path = map_dir + map_name +
".tar.xz";
1584 if (cyber::common::PathExists(tar_abs_path)) {
1585 cmd =
"rm -rf " + tar_abs_path;
1586 if (std::system(cmd.data()) != 0) {
1587 AERROR <<
"Failed to delete map tar file for: " << std::strerror(errno);
1593void HMIWorker::DeleteRecord(
const std::string &record_id) {
1594 if (record_id.empty()) {
1597 std::string record_path;
1598 if (!GetRecordPath(&record_path)) {
1599 AERROR <<
"Failed to get record path";
1602 if (record_id.find(
'/') != std::string::npos ||
1603 record_id.find(
"..") != std::string::npos ||
1604 record_id.find(
' ') != std::string::npos ||
1605 record_id.find(
"~") != std::string::npos) {
1606 AERROR <<
"Record id should not contain '/' ,' ',~ and ..";
1609 const std::string record_abs_path = record_path + record_id +
".record";
1610 if (!cyber::common::PathExists(record_abs_path)) {
1611 AERROR <<
"Failed to get record path: " << record_abs_path;
1616 RLock rlock(status_mutex_);
1617 auto &status_records = status_.records();
1618 if (status_records.find(record_id) == status_records.end()) {
1619 AERROR <<
"Failed to find record id: " << record_id;
1622 if (record_id == status_.current_record_status().current_record_id()) {
1623 AERROR <<
"Cann't delete current selected record";
1628 auto *record_player_factory = RecordPlayerFactory::Instance();
1629 record_player_factory->UnregisterRecordPlayer(record_id);
1631 WLock wlock(status_mutex_);
1632 status_.mutable_records()->erase(record_id);
1633 status_changed_ =
true;
1637 std::string command =
"rm -rf " + record_abs_path;
1638 if (std::system(command.data()) != 0) {
1639 AERROR <<
"Failed to delete record for: " << std::strerror(errno);
1644 const std::string tar_abs_path = record_path + record_id +
".tar.xz";
1645 if (cyber::common::PathExists(tar_abs_path)) {
1646 command =
"rm -rf " + tar_abs_path;
1647 if (std::system(command.data()) != 0) {
1648 AERROR <<
"Failed to delete record tar file for: "
1649 << std::strerror(errno);
1656void HMIWorker::ReloadMaps() {
1657 WLock wlock(status_mutex_);
1658 config_.mutable_maps()->clear();
1659 *(config_.mutable_maps()) =
1660 util::HMIUtil::ListDirAsDict(FLAGS_maps_data_path);
1661 status_.clear_maps();
1662 for (
const auto &map : config_.maps()) {
1663 status_.add_maps(map.first);
1665 if (status_.current_map() !=
"") {
1667 if (config_.maps().find(status_.current_map()) == config_.maps().end()) {
1669 AINFO <<
"Current map is not in status. Reset it.";
1670 status_.set_current_map(
"");
1673 status_changed_ =
true;
1677bool HMIWorker::ReloadVehicles() {
1679 util::HMIUtil::LoadConfig(FLAGS_dv_plus_hmi_modes_config_path);
1681 config.SerializeToString(&msg);
1683 WLock wlock(status_mutex_);
1684 config_.ParseFromString(msg);
1687 status_.clear_vehicles();
1690 for (
const auto &vehicle : config_.vehicles()) {
1691 status_.add_vehicles(vehicle.first);
1693 status_changed_ =
true;
1697void HMIWorker::UpdateCameraSensorChannelToStatus(
1698 const std::string &channel_name) {
1700 WLock wlock(status_mutex_);
1701 if (status_.current_camera_sensor_channel() == channel_name) {
1702 AINFO <<
"Input channel name is current camera sensor channel";
1705 status_.set_current_camera_sensor_channel(channel_name);
1706 status_changed_ =
true;
1710void HMIWorker::UpdatePointCloudChannelToStatus(
1711 const std::string &channel_name) {
1713 WLock wlock(status_mutex_);
1714 if (status_.current_point_cloud_channel() == channel_name) {
1715 AINFO <<
"Input channel name is current camera sensor channel";
1718 status_.set_current_point_cloud_channel(channel_name);
1719 status_changed_ =
true;
1723void HMIWorker::DeleteVehicleConfig(
const std::string &vehicle_name) {
1724 if (vehicle_name.empty()) {
1727 const std::string *vehicle_dir = FindOrNull(config_.vehicles(), vehicle_name);
1728 if (vehicle_dir ==
nullptr) {
1729 AERROR <<
"Unknow vehicle name" << vehicle_name;
1733 RLock rlock(status_mutex_);
1734 if (status_.current_vehicle() == vehicle_name) {
1735 AERROR <<
"The deleted vehicle profile is the one in use.";
1739 if (!cyber::common::DeleteFile(*vehicle_dir)) {
1740 AERROR <<
"Delete vehicle profile [" << vehicle_name <<
"] failed!";
1743 if (!ReloadVehicles()) {
1744 AERROR <<
"Update vehicle profile [" << vehicle_name <<
"] failed!";
1749void HMIWorker::DeleteV2xConfig(
const std::string &vehicle_name) {
1750 if (vehicle_name.empty()) {
1753 const std::string *vehicle_dir =
1754 FindOrNull(config_.vehicles(), util::HMIUtil::TitleCase(vehicle_name));
1755 if (vehicle_dir ==
nullptr) {
1756 AERROR <<
"Unknow vehicle name " << vehicle_name;
1759 const std::string v2x_dir = *vehicle_dir +
"/v2x_conf";
1760 if (!cyber::common::PathExists(v2x_dir)) {
1761 AINFO <<
"The directory does not exist or the directory has been deleted";
1763 if (!cyber::common::DeleteFile(v2x_dir)) {
1764 AERROR <<
"Delete v2x config [" << vehicle_name <<
"/v2x_conf] failed!";
1769bool HMIWorker::StartDataRecorder() {
1771 "/apollo/scripts/record_bag.py --start --all --dreamview "
1773 FLAGS_data_record_default_name;
1774 int ret = std::system(
start_cmd.data());
1776 auto *monitors = FuelMonitorManager::Instance()->GetCurrentMonitors();
1777 if (monitors !=
nullptr) {
1778 auto iter = monitors->find(FLAGS_data_collection_monitor_name);
1779 if (iter != monitors->end()) {
1780 auto *data_collection_monitor = iter->second.get();
1781 if (data_collection_monitor->IsEnabled() && record_count_ == 0) {
1782 data_collection_monitor->Restart();
1789 AERROR <<
"Failed to start the cyber_recorder process";
1794bool HMIWorker::StopDataRecorder() {
1795 std::string
stop_cmd =
"/apollo/scripts/record_bag.py --stop";
1796 int ret = std::system(
stop_cmd.data());
1800 AERROR <<
"Failed to stop the cyber_recorder process";
1805int HMIWorker::SaveDataRecorder(
const std::string &new_name) {
1806 std::string data_record_default_path =
1807 std::string(cyber::common::GetEnv(
"HOME",
"/home/apollo")) +
1808 "/.apollo/resources/records/" + FLAGS_data_record_default_name;
1809 if (!cyber::common::PathExists(data_record_default_path)) {
1810 AERROR <<
"Failed to save the record, the dreamview recording record does "
1811 "not exist, please record through dreamview.";
1814 std::string save_cmd =
"/apollo/scripts/record_bag.py --default_name " +
1815 FLAGS_data_record_default_name +
" --rename " +
1817 int ret = std::system(save_cmd.data());
1822 AERROR <<
"Failed to save the record, a file with the same name exists";
1827bool HMIWorker::DeleteDataRecorder() {
1828 std::string delete_cmd =
1829 "/apollo/scripts/record_bag.py --delete --default_name " +
1830 FLAGS_data_record_default_name;
1831 int ret = std::system(delete_cmd.data());
1835 AERROR <<
"Failed to delete the record";
1840void HMIWorker::ClearRecordInfo() {
1841 std::string last_record_id;
1843 RLock rlock(status_mutex_);
1844 if (status_.current_operation() != HMIModeOperation::Record) {
1847 last_record_id = status_.current_record_status().current_record_id();
1849 if (!last_record_id.empty()) {
1850 StopRecordPlay(last_record_id);
1853 callback_api_(
"ClearDataHandlerConfChannelMsgs", {});
1854 WLock wlock(status_mutex_);
1855 status_.mutable_current_record_status()->clear_current_record_id();
1856 status_changed_ =
true;
1860void HMIWorker::ClearRtkRecordInfo() {
1861 std::string last_rtk_record_id;
1863 RLock rlock(status_mutex_);
1864 if (status_.current_operation() != HMIModeOperation::Waypoint_Follow) {
1867 last_rtk_record_id = status_.current_rtk_record_id();
1869 if (!last_rtk_record_id.empty()) {
1870 StopPlayRtkRecorder();
1873 WLock wlock(status_mutex_);
1874 status_.set_current_rtk_record_id(
"");
1875 status_changed_ =
true;
1880void HMIWorker::AddExpectedModules(
const HMIAction &action) {
1881 WLock wlock(status_mutex_);
1882 int expected_modules = 1;
1883 if (action == HMIAction::SETUP_MODE) {
1884 expected_modules = status_.modules_size();
1886 status_.set_expected_modules(status_.expected_modules() + expected_modules);
1887 status_changed_ =
true;
1890void HMIWorker::OnTimer(
const double &overtime_time) {
1891 if (monitor_reader_ !=
nullptr) {
1892 auto delay_sec = monitor_reader_->GetDelaySec();
1893 if (delay_sec < 0 || delay_sec > overtime_time) {
1894 AERROR <<
"Running time error: monitor is not turned on!";
1896 WLock wlock(status_mutex_);
1897 for (
auto &iter : *status_.mutable_modules_lock()) {
1898 iter.second =
false;
1906 WLock wlock(status_mutex_);
1907 auto modules = status_.mutable_modules();
1908 auto modules_lock = status_.mutable_modules_lock();
1909 for (
const auto &iter : current_mode_.
modules()) {
1910 if (previous_modules_lock_[iter.first] && (*modules_lock)[iter.first] &&
1911 !isProcessRunning(iter.second.start_command())) {
1912 (*modules)[iter.first] =
false;
1913 LockModule(iter.first,
false);
1915 previous_modules_lock_[iter.first] = (*modules_lock)[iter.first];
1920void HMIWorker::LockModule(
const std::string &module,
const bool &lock_flag) {
1921 auto modules_lock = status_.mutable_modules_lock();
1922 (*modules_lock)[module] = lock_flag;
1925bool HMIWorker::AddOrModifyObjectToDB(
const std::string &key,
1926 const std::string &value) {
1927 return KVDB::Put(key, value);
1930bool HMIWorker::DeleteObjectToDB(
const std::string &key) {
1931 return KVDB::Delete(key);
1934std::string HMIWorker::GetObjectFromDB(
const std::string &key) {
1935 return KVDB::Get(key).value_or(
"");
1938std::vector<std::pair<std::string, std::string>>
1939HMIWorker::GetTuplesWithTypeFromDB(
const std::string &type) {
1940 return KVDB::GetWithStart(type);
1943bool HMIWorker::StartTerminal() {
1944 return std::system(FLAGS_terminal_start_cmd.data()) == 0;
1947void HMIWorker::GetRtkRecordPath(std::string *record_path) {
1948 CHECK_NOTNULL(record_path);
1949 *record_path = FLAGS_resource_rtk_record_path;
1952bool HMIWorker::LoadRtkRecords() {
1954 WLock wLock(status_mutex_);
1955 status_.clear_rtk_records();
1957 std::string directory_path;
1958 GetRtkRecordPath(&directory_path);
1959 if (!cyber::common::PathExists(directory_path)) {
1960 AERROR <<
"Failed to find rtk records!";
1963 DIR *directory = opendir(directory_path.c_str());
1964 if (directory ==
nullptr) {
1965 AERROR <<
"Cannot open rtk record directory" << directory_path;
1968 struct dirent *file;
1969 std::map<std::string, double> new_records;
1970 while ((file = readdir(directory)) !=
nullptr) {
1971 if (!strcmp(file->d_name,
".") || !strcmp(file->d_name,
"..")) {
1974 if (file->d_type == DT_DIR) {
1977 const std::string record_id = file->d_name;
1978 const int index = record_id.rfind(
".csv");
1979 if (index != -1 && record_id[0] !=
'.') {
1980 const std::string local_record_resource = record_id.substr(0, index);
1982 WLock wLock(status_mutex_);
1983 status_.add_rtk_records(local_record_resource);
1987 closedir(directory);
1989 WLock wlock(status_mutex_);
1990 status_changed_ =
true;
1995bool HMIWorker::StartRtkDataRecorder() {
1996 std::string
start_cmd =
"nohup /apollo/scripts/rtk_recorder.sh start &";
1997 int ret = std::system(
start_cmd.data());
2001 AERROR <<
"Failed to start the rtk_recorder process";
2006bool HMIWorker::StopRtkDataRecorder() {
2007 std::string
stop_cmd =
"/apollo/scripts/rtk_recorder.sh stop";
2008 int ret = std::system(
stop_cmd.data());
2012 AERROR <<
"Failed to stop the rtk_recorder process";
2017Json HMIWorker::StartPlayRtkRecorder() {
2019 if (!ChangeDrivingMode(Chassis::COMPLETE_AUTO_DRIVE)) {
2020 AERROR <<
"Failed to play rtk: Failed to enter auto drive.";
2021 result[
"error"] =
"Failed to enter auto drive";
2022 result[
"isOk"] =
false;
2025 std::string record_id;
2027 RLock rlock(status_mutex_);
2028 record_id = status_.current_rtk_record_id();
2031 "nohup /apollo/scripts/rtk_player.sh start " + record_id +
" &";
2032 int ret = std::system(
start_cmd.data());
2034 AINFO <<
"Start the rtk_recorder process Successful.";
2035 result[
"isOk"] =
true;
2037 AERROR <<
"Failed to play rtk: Failed to start the rtk_recorder process.";
2038 result[
"error"] =
"Failed to start the rtk_recorder process";
2039 result[
"isOk"] =
false;
2044bool HMIWorker::StopPlayRtkRecorder() {
2045 std::string
stop_cmd =
"/apollo/scripts/rtk_player.sh stop";
2046 int ret = std::system(
stop_cmd.data());
2050 AERROR <<
"Failed to stop the rtk_recorder process";
2055int HMIWorker::SaveRtkDataRecorder(
const std::string &new_name) {
2056 std::string new_rtk_record_file =
2057 FLAGS_default_rtk_record_path + new_name +
".csv";
2058 if (cyber::common::PathExists(new_rtk_record_file)) {
2059 AERROR <<
"Failed to save the ret record, a file with the same name exists";
2062 if (std::rename(FLAGS_default_rtk_record_file.data(),
2063 new_rtk_record_file.data()) == 0) {
2064 UpdateRtkRecordToStatus(new_name);
2067 AERROR <<
"Failed to save the ret record, save command execution failed";
2072bool HMIWorker::DeleteRtkDataRecorder() {
2073 if (!cyber::common::PathExists(FLAGS_default_rtk_record_file)) {
2074 AERROR <<
"Failed to delete the ret record, file not exist";
2077 if (std::remove(FLAGS_default_rtk_record_file.data()) == 0) {
2080 AERROR <<
"Failed to delete the record, delete command execution failed";
2085void HMIWorker::ChangeRtkRecord(
const std::string &record_id) {
2086 if (!StopPlayRtkRecorder()) {
2087 AWARN <<
"Warning to change rtk record: Failed to stop the rtk_recorder "
2090 std::string last_record_id;
2092 RLock rlock(status_mutex_);
2093 last_record_id = status_.current_rtk_record_id();
2095 if (last_record_id == record_id) {
2096 AERROR <<
"Failed to change rtk record: rtk record is same!";
2099 WLock wlock(status_mutex_);
2100 status_.set_current_rtk_record_id(record_id);
2101 status_changed_ =
true;
2105void HMIWorker::UpdateRtkRecordToStatus(
const std::string &new_name) {
2106 WLock wlock(status_mutex_);
2107 status_.add_rtk_records(new_name);
2108 status_changed_ =
true;
2111void HMIWorker::ClearInvalidRecordStatus(
const HMIModeOperation &operation) {
2112 HMIModeOperation last_operation;
2114 RLock rlock(status_mutex_);
2115 last_operation = status_.current_operation();
2117 if (operation == last_operation) {
2119 }
else if (last_operation == HMIModeOperation::Waypoint_Follow) {
2120 StopRtkDataRecorder();
2121 DeleteRtkDataRecorder();
2122 }
else if (operation == HMIModeOperation::Waypoint_Follow) {
2124 DeleteDataRecorder();
2128bool HMIWorker::isProcessRunning(
const std::string &process_name) {
2129 std::stringstream commandStream;
2130 commandStream <<
"pgrep -f " << process_name;
2131 std::string command = commandStream.str();
2133 FILE *fp = popen(command.c_str(),
"r");
2136 if (fgets(result,
sizeof(result), fp) !=
nullptr) {
2137 AINFO << process_name <<
" is running";
2141 AINFO << process_name <<
" is not running";
2148bool HMIWorker::PackageExist(
const std::string &package_name) {
2149 std::string package_path_prefix;
2151 FLAGS_apollo_package_meta_info_path_prefix,
"APOLLO_DISTRIBUTION_HOME",
2152 &package_path_prefix)) {
2153 AERROR << FLAGS_apollo_package_meta_info_path_prefix
2154 <<
" No such package meta info path prefix";
2157 std::string package_meta_info_path =
2158 package_path_prefix + package_name +
2160 AINFO <<
"package_meta_info_path: " << package_meta_info_path;
2161 return (cyber::common::PathExists(package_meta_info_path));
2164std::string HMIWorker::GetCurrentModeDefaultLayout() {
2165 if (current_mode_.has_layout()) {
2166 const auto default_layout_json =
2168 "default_layout", current_mode_.layout());
2169 return default_layout_json[
"data"].dump();
2171 AWARN <<
"There is no default layout for the current mode.";
2175void HMIWorker::LoadDvPluginPanelsJson() {
2176 plugin_panels_json_ = Json::array();
2178 DIR *directory = opendir(FLAGS_dv_plugin_panels_path.c_str());
2180 AERROR <<
"can not open: " << FLAGS_dv_plugin_panels_path;
2184 struct dirent *file;
2185 while ((file = readdir(directory)) !=
nullptr) {
2186 if (!strcmp(file->d_name,
".") || !strcmp(file->d_name,
"..")) {
2189 std::string subdir_path = FLAGS_dv_plugin_panels_path +
"/" + file->d_name;
2190 if (file->d_type == DT_DIR) {
2191 std::string file_path = subdir_path +
"/conf.json";
2193 std::ifstream ifs(file_path);
2194 if (!ifs.is_open()) {
2195 AERROR <<
"can not open: " << file_path;
2202 plugin_panels_json_.push_back(conf_json);
2205 closedir(directory);
2208std::string HMIWorker::GetDvPluginPanelsJsonStr() {
2209 return plugin_panels_json_.empty() ?
"" : plugin_panels_json_.dump();
Lightweight key-value database to store system-wide parameters.
static nlohmann::json ProtoToTypedJson(const std::string &json_type, const google::protobuf::Message &proto)
Convert proto to a json string.
a singleton clock that can be used to get the current timestamp.
static double NowInSeconds()
gets the current time in second.
Node is the fundamental building block of Cyber RT.
Some string util functions.
size_t MessageFingerprint(const google::protobuf::Message &message)
bool GetFilePathWithEnv(const std::string &path, const std::string &env_var, std::string *file_path)
get file path, judgement priority:
bool LoadConfig(const std::string &relative_path, T *config)
boost::shared_lock< boost::shared_mutex > RLock
boost::unique_lock< boost::shared_mutex > WLock
const std::string start_cmd
const std::string stop_cmd
DEFINE_string(cyber_recorder_play_command, "cyber_recorder play -p 1 -f ", "Cyber recorder play command")