Apollo 10.0
自动驾驶开放平台
hmi_worker.h
浏览该文件的文档.
1/******************************************************************************
2 * Copyright 2018 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 *****************************************************************************/
16
17#pragma once
18
19#include <atomic>
20#include <map>
21#include <memory>
22#include <string>
23#include <utility>
24#include <vector>
25
26#include <boost/thread/locks.hpp>
27#include <boost/thread/shared_mutex.hpp>
28
29#include "nlohmann/json.hpp"
30#include "google/protobuf/util/json_util.h"
31
32#include "modules/common_msgs/audio_msgs/audio_event.pb.h"
33#include "modules/common_msgs/basic_msgs/drive_event.pb.h"
34#include "modules/common_msgs/chassis_msgs/chassis.pb.h"
35#include "modules/common_msgs/control_msgs/pad_msg.pb.h"
36#include "modules/common_msgs/dreamview_msgs/hmi_status.pb.h"
37#include "modules/common_msgs/localization_msgs/localization.pb.h"
38#include "modules/common_msgs/external_command_msgs/action_command.pb.h"
39#include "modules/common_msgs/external_command_msgs/command_status.pb.h"
40#include "modules/common_msgs/external_command_msgs/lane_follow_command.pb.h"
41#include "modules/common_msgs/simulation_msgs/scenario.pb.h"
42#include "modules/common_msgs/dreamview_msgs/hmi_config.pb.h"
43#include "modules/common_msgs/dreamview_msgs/hmi_mode.pb.h"
44
45#include "cyber/cyber.h"
46#include "cyber/time/time.h"
47
52namespace apollo {
53namespace dreamview {
54
55// Singleton worker which does the actual work of HMI actions.
56class HMIWorker {
57 public:
58 using DvCallback = std::function<nlohmann::json(
59 const std::string& function_name, const nlohmann::json& param_json)>;
60 HMIWorker() : HMIWorker(cyber::CreateNode("HMI")) {}
61 explicit HMIWorker(const std::shared_ptr<apollo::cyber::Node>& node);
62 void Start(DvCallback callback_api);
63 void Stop();
64
65 // HMI action trigger.
66 bool Trigger(const HMIAction action);
67 bool Trigger(const HMIAction action, const std::string& value);
68
69 // Register handler which will be called on HMIStatus update.
70 // It will be called ASAP if there are changes, or else periodically
71 // controlled by FLAGS_hmi_status_update_interval.
73 std::function<void(const bool status_changed, HMIStatus* status)>;
75 status_update_handlers_.push_back(handler);
76 }
77
78 // Submit an AudioEvent
79 void SubmitAudioEvent(const uint64_t event_time_ms, const int obstacle_id,
80 const int audio_type, const int moving_result,
81 const int audio_direction, const bool is_siren_on);
82
83 // Submit a DriveEvent.
84 void SubmitDriveEvent(const uint64_t event_time_ms,
85 const std::string& event_msg,
86 const std::vector<std::string>& event_types,
87 const bool is_reportable);
88
89 // Run sensor calibration preprocess
90 void SensorCalibrationPreprocess(const std::string& task_type);
91
92 // Run vehicle calibration preprocess
94
95 // Get current HMI status.
96 HMIStatus GetStatus() const;
97
98 bool UpdateScenarioSetToStatus(const std::string& scenario_set_id,
99 const std::string& scenario_set_name);
100 bool UpdateScenarioSet(const std::string& scenario_set_id,
101 const std::string& scenario_set_name,
102 ScenarioSet* new_scenario_set);
103 bool UpdateDynamicModelToStatus(const std::string& dynamic_model_name);
105 // bool UpdateRecordToStatus(const std::string& record_id,
106 // const std::string& record_status);
107 bool LoadRecords();
108 bool ReloadVehicles();
109 bool GetScenarioSetPath(const std::string& scenario_set_id,
110 std::string* scenario_set_path);
111 void UpdateCameraSensorChannelToStatus(const std::string& channel_name);
112 void UpdatePointCloudChannelToStatus(const std::string& channel_name);
113
114 private:
115 void InitReadersAndWriters();
116 void InitStatus();
117 void StatusUpdateThreadLoop();
118 // get command result
119 std::string GetCommandRes(const std::string& cmd);
120
121 // Start / reset current mode.
122 void SetupMode() const;
123 void ResetMode() const;
124 bool ResetSimObstacle(const std::string& scenario_id);
125
126 // Change current mode, launch, map, vehicle and driving mode.
127 void ChangeMode(const std::string& mode_name);
128 bool ChangeMap(const std::string& map_name,
129 bool restart_dynamic_model = true);
130 void ChangeVehicle(const std::string& vehicle_name);
131 void ChangeScenarioSet(const std::string& scenario_set_id);
132 void ChangeRecord(const std::string& record_id);
133 void ChangeDynamicModel(const std::string& dynamic_model_name);
134 void ChangeScenario(const std::string& scenario_id);
135 bool ChangeDrivingMode(const apollo::canbus::Chassis::DrivingMode mode);
136
137 bool LoadScenarios();
138
139 bool LoadDynamicModels();
140
141 void DeleteScenarioSet(const std::string& scenario_set_id);
142 void DeleteRecord(const std::string& record_id);
143 void DeleteDynamicModel(const std::string& dynamic_model_name);
144
145 bool GetScenarioResourcePath(std::string* scenario_resource_path);
146 bool GetRecordPath(std::string* record_path);
147
148 bool RePlayRecord(const std::string& record_id);
149
150 // Start / stop a module.
151 void StartModule(const std::string& module) const;
152 void StopModule(const std::string& module) const;
153 bool StopModuleByCommand(const std::string& stop_command) const;
154 void StopRecordPlay();
155
156 void ResetComponentStatusTimer();
157
165 bool LoadVehicleDefinedMode(const std::string& mode_config_path,
166 const std::string& current_vehicle_path,
167 HMIMode* self_defined_mode);
168
174 void MergeToCurrentMode(HMIMode* mode);
179 void UpdateModeModulesAndMonitoredComponents();
180
181 HMIConfig config_;
182
183 // HMI status maintenance.
184 HMIStatus status_;
185 std::atomic<double> last_status_received_s_;
186 bool monitor_timed_out_{true};
187 HMIMode current_mode_;
188 bool status_changed_ = false;
189 size_t last_status_fingerprint_{};
190 bool stop_ = false;
191 mutable boost::shared_mutex status_mutex_;
192 mutable size_t record_count_ = 0;
193 std::future<void> thread_future_;
194 std::vector<StatusUpdateHandler> status_update_handlers_;
195
196 // Cyber members.
197 std::shared_ptr<apollo::cyber::Node> node_;
198 std::shared_ptr<cyber::Reader<apollo::canbus::Chassis>> chassis_reader_;
199 std::shared_ptr<cyber::Reader<apollo::localization::LocalizationEstimate>>
200 localization_reader_;
201 std::shared_ptr<cyber::Writer<HMIStatus>> status_writer_;
202 std::shared_ptr<
205 action_command_client_;
206 std::shared_ptr<
209 lane_follow_command_client_;
210 std::shared_ptr<cyber::Writer<apollo::audio::AudioEvent>> audio_event_writer_;
211 std::shared_ptr<cyber::Writer<apollo::common::DriveEvent>>
212 drive_event_writer_;
213 DvCallback callback_api_;
214};
215
216} // namespace dreamview
217} // namespace apollo
Client get Response from a responding Service by sending a Request
Definition client.h:47
bool Trigger(const HMIAction action)
HMIStatus GetStatus() const
void UpdateCameraSensorChannelToStatus(const std::string &channel_name)
std::function< void(const bool status_changed, HMIStatus *status)> StatusUpdateHandler
Definition hmi_worker.h:73
void SubmitAudioEvent(const uint64_t event_time_ms, const int obstacle_id, const int audio_type, const int moving_result, const int audio_direction, const bool is_siren_on)
void RegisterStatusUpdateHandler(StatusUpdateHandler handler)
Definition hmi_worker.h:74
bool UpdateDynamicModelToStatus(const std::string &dynamic_model_name)
HMIWorker(const std::shared_ptr< apollo::cyber::Node > &node)
bool GetScenarioSetPath(const std::string &scenario_set_id, std::string *scenario_set_path)
std::function< nlohmann::json(const std::string &function_name, const nlohmann::json &param_json)> DvCallback
Definition hmi_worker.h:59
void SensorCalibrationPreprocess(const std::string &task_type)
void SubmitDriveEvent(const uint64_t event_time_ms, const std::string &event_msg, const std::vector< std::string > &event_types, const bool is_reportable)
void Start(DvCallback callback_api)
void UpdatePointCloudChannelToStatus(const std::string &channel_name)
bool UpdateScenarioSetToStatus(const std::string &scenario_set_id, const std::string &scenario_set_name)
bool UpdateScenarioSet(const std::string &scenario_set_id, const std::string &scenario_set_name, ScenarioSet *new_scenario_set)
class register implement
Definition arena_queue.h:37