Apollo 10.0
自动驾驶开放平台
simulation_world_service.h
浏览该文件的文档.
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 *****************************************************************************/
16
21#pragma once
22
23#include <algorithm>
24#include <list>
25#include <map>
26#include <memory>
27#include <string>
28#include <unordered_map>
29#include <utility>
30#include <vector>
31
32#include <boost/thread/locks.hpp>
33#include <boost/thread/shared_mutex.hpp>
34
35#include "gtest/gtest_prod.h"
36
37#include "nlohmann/json.hpp"
38
39#include "modules/common_msgs/audio_msgs/audio.pb.h"
40#include "modules/common_msgs/audio_msgs/audio_event.pb.h"
41#include "modules/common_msgs/basic_msgs/drive_event.pb.h"
42#include "modules/common_msgs/basic_msgs/pnc_point.pb.h"
43#include "modules/common_msgs/control_msgs/control_cmd.pb.h"
44#include "modules/common_msgs/external_command_msgs/command_status.pb.h"
45#include "modules/common_msgs/external_command_msgs/lane_follow_command.pb.h"
46#include "modules/common_msgs/external_command_msgs/valet_parking_command.pb.h"
47#include "modules/common_msgs/external_command_msgs/action_command.pb.h"
48#include "modules/common_msgs/localization_msgs/gps.pb.h"
49#include "modules/common_msgs/localization_msgs/localization.pb.h"
50#include "modules/common_msgs/perception_msgs/traffic_light_detection.pb.h"
51#include "modules/common_msgs/planning_msgs/planning.pb.h"
52#include "modules/common_msgs/planning_msgs/planning_command.pb.h"
53#include "modules/common_msgs/planning_msgs/planning_internal.pb.h"
54#include "modules/common_msgs/prediction_msgs/prediction_obstacle.pb.h"
55#include "modules/common_msgs/routing_msgs/routing.pb.h"
56#include "modules/common_msgs/storytelling_msgs/story.pb.h"
57#include "modules/common_msgs/task_manager_msgs/task_manager.pb.h"
58#include "modules/common_msgs/dreamview_msgs/simulation_world.pb.h"
59
60#include "cyber/common/log.h"
63
68namespace apollo {
69namespace dreamview {
70
79class SimulationWorldService {
80 public:
81 // The maximum number of monitor message items to be kept in
82 // SimulationWorld.
83 static constexpr int kMaxMonitorItems = 30;
84
91 bool routing_from_file = false);
92
97 inline const SimulationWorld &world() const { return world_; }
98
105 nlohmann::json GetUpdateAsJson(double radius) const;
106
114 void GetWireFormatString(double radius,
115 std::string *sim_world_with_planning_data);
116
123 nlohmann::json GetMapElements(double radius) const;
124
130 void Update();
131
135 static void SetToClear() { to_clear_ = true; }
136
142 bool ReadyToPush() const { return ready_to_push_.load(); }
143
152 const std::string &msg);
153
155 const std::shared_ptr<apollo::relative_map::NavigationInfo> &);
156
158 const std::shared_ptr<apollo::external_command::LaneFollowCommand> &);
159
161 const std::shared_ptr<apollo::external_command::ValetParkingCommand> &);
162
164 const std::shared_ptr<apollo::external_command::ActionCommand> &);
165
166 void PublishTask(const std::shared_ptr<apollo::task_manager::Task> &);
167
168 void GetMapElementIds(double radius, MapElementIds *ids) const;
169
171
172 nlohmann::json GetRoutePathAsJson() const;
173
175
176 void UpdateVehicleParam();
177
178 private:
179 void InitReaders();
180 void InitWriters();
181
186 template <typename DataType>
187 void UpdateSimulationWorld(const DataType &data);
188
189 void UpdateMonitorMessages();
190
191 Object &CreateWorldObjectIfAbsent(
193 void CreateWorldObjectFromSensorMeasurement(
195 Object *world_object);
196 void SetObstacleInfo(const apollo::perception::PerceptionObstacle &obstacle,
197 Object *world_object);
198 void SetObstaclePolygon(
200 Object *world_object);
201 void SetObstacleSensorMeasurements(
203 Object *world_object);
204 void SetObstacleSource(const apollo::perception::PerceptionObstacle &obstacle,
205 Object *world_object);
206 void UpdatePlanningTrajectory(
207 const apollo::planning::ADCTrajectory &trajectory);
208 void UpdateRSSInfo(const apollo::planning::ADCTrajectory &trajectory);
209 bool LocateMarker(const apollo::planning::ObjectDecisionType &decision,
210 Decision *world_decision);
211 void FindNudgeRegion(const apollo::planning::ObjectDecisionType &decision,
212 const Object &world_obj, Decision *world_decision);
213 void UpdateDecision(const apollo::planning::DecisionResult &decision_res,
214 double header_time);
215 void UpdateMainStopDecision(
216 const apollo::planning::MainDecision &main_decision,
217 double update_timestamp_sec, Object *world_main_stop);
218
219 template <typename MainDecision>
220 void UpdateMainChangeLaneDecision(const MainDecision &decision,
221 Object *world_main_decision) {
222 if (decision.has_change_lane_type() &&
223 (decision.change_lane_type() == apollo::routing::ChangeLaneType::LEFT ||
224 decision.change_lane_type() ==
226 auto *change_lane_decision = world_main_decision->add_decision();
227 change_lane_decision->set_change_lane_type(decision.change_lane_type());
228
229 const auto &adc = world_.auto_driving_car();
230 change_lane_decision->set_position_x(adc.position_x());
231 change_lane_decision->set_position_y(adc.position_y());
232 change_lane_decision->set_heading(adc.heading());
233 }
234 }
235
236 void CreatePredictionTrajectory(
238 Object *world_object);
239
240 void DownsamplePath(const apollo::common::Path &paths,
241 apollo::common::Path *downsampled_path);
242
243 void UpdatePlanningData(const apollo::planning_internal::PlanningData &data);
244
245 void PopulateMapInfo(double radius);
246
251 template <typename MessageT>
252 void UpdateWithLatestObserved(cyber::Reader<MessageT> *reader,
253 bool logging = true) {
254 if (reader->Empty()) {
255 if (logging) {
256 AINFO_EVERY(100) << "Has not received any data from "
257 << reader->GetChannelName();
258 }
259 return;
260 }
261
262 const std::shared_ptr<MessageT> msg = reader->GetLatestObserved();
263 UpdateSimulationWorld(*msg);
264 }
265
270 template <typename MessageT>
271 void DumpMessageFromReader(cyber::Reader<MessageT> *reader) {
272 if (reader->Empty()) {
273 AWARN << "Has not received any data from " << reader->GetChannelName()
274 << ". Cannot dump message!";
275 return;
276 }
277
278 apollo::common::util::DumpMessage(reader->GetLatestObserved());
279 }
280
281 void ReadPlanningCommandFromFile(const std::string &planning_command_file);
282
283 template <typename MessageT>
284 void UpdateLatency(const std::string &module_name,
285 cyber::Reader<MessageT> *reader) {
286 if (reader->Empty()) {
287 return;
288 }
289
290 const auto header = reader->GetLatestObserved()->header();
291 const double publish_time_sec = header.timestamp_sec();
292 const double sensor_time_sec =
294 std::max({header.lidar_timestamp(), header.camera_timestamp(),
295 header.radar_timestamp()}))
296 .ToSecond();
297
298 Latency latency;
299 latency.set_timestamp_sec(publish_time_sec);
300 latency.set_total_time_ms((publish_time_sec - sensor_time_sec) * 1.0e3);
301 (*world_.mutable_latency())[module_name] = latency;
302 }
303
311 void UpdateDelays();
312
318 void UpdateLatencies();
319
320 template <typename Points>
321 void DownsampleSpeedPointsByInterval(const Points &points,
322 size_t downsampleInterval,
323 Points *downsampled_points) {
324 if (points.empty()) {
325 return;
326 }
327
328 for (int i = 0; i + 1 < points.size(); i += downsampleInterval) {
329 *downsampled_points->Add() = points[i];
330 }
331
332 // add the last point
333 *downsampled_points->Add() = *points.rbegin();
334 }
335
336 std::unique_ptr<cyber::Node> node_;
337
338 // The underlying SimulationWorld object, owned by the
339 // SimulationWorldService instance.
340 SimulationWorld world_;
341
342 // Downsampled route paths to be rendered in frontend.
343 mutable boost::shared_mutex route_paths_mutex_;
344 std::vector<RoutePath> route_paths_;
345
346 // The handle of MapService, not owned by SimulationWorldService.
347 const MapService *map_service_;
348
349 // The map holding obstacle string id to the actual object
350 std::unordered_map<std::string, Object> obj_map_;
351
352 // A temporary cache for all the monitor messages coming in.
353 std::mutex monitor_msgs_mutex_;
354 std::list<std::shared_ptr<common::monitor::MonitorMessage>> monitor_msgs_;
355
356 // The SIMULATOR monitor for publishing messages.
357 apollo::common::monitor::MonitorLogBuffer monitor_logger_buffer_;
358
359 // Whether to clear the SimulationWorld in the next timer cycle, set by
360 // frontend request.
361 static bool to_clear_;
362
363 // Relative map used/retrieved in navigation mode
364 apollo::hdmap::Map relative_map_;
365
366 // Whether the sim_world is ready to push to frontend
367 std::atomic<bool> ready_to_push_;
368
369 // Latest rss info
370 double current_real_dist_ = 0.0;
371 double current_rss_safe_dist_ = 0.0;
372
373 // Gear Location
374 apollo::canbus::Chassis_GearPosition gear_location_;
375
376 // Readers.
377 std::shared_ptr<cyber::Reader<apollo::canbus::Chassis>> chassis_reader_;
378 std::shared_ptr<cyber::Reader<apollo::localization::Gps>> gps_reader_;
379 std::shared_ptr<cyber::Reader<apollo::localization::LocalizationEstimate>>
380 localization_reader_;
381 std::shared_ptr<cyber::Reader<apollo::perception::PerceptionObstacles>>
382 perception_obstacle_reader_;
383 std::shared_ptr<cyber::Reader<apollo::perception::TrafficLightDetection>>
384 perception_traffic_light_reader_;
385 std::shared_ptr<cyber::Reader<apollo::prediction::PredictionObstacles>>
386 prediction_obstacle_reader_;
387 std::shared_ptr<cyber::Reader<apollo::planning::ADCTrajectory>>
388 planning_reader_;
389 std::shared_ptr<cyber::Reader<apollo::control::ControlCommand>>
390 control_command_reader_;
391 std::shared_ptr<cyber::Reader<apollo::relative_map::NavigationInfo>>
392 navigation_reader_;
393 std::shared_ptr<cyber::Reader<apollo::relative_map::MapMsg>>
394 relative_map_reader_;
395 std::shared_ptr<cyber::Reader<apollo::audio::AudioEvent>> audio_event_reader_;
396 std::shared_ptr<cyber::Reader<apollo::common::DriveEvent>>
397 drive_event_reader_;
398 std::shared_ptr<cyber::Reader<apollo::common::monitor::MonitorMessage>>
399 monitor_reader_;
400 std::shared_ptr<cyber::Reader<apollo::planning::PlanningCommand>>
401 planning_command_reader_;
402 std::shared_ptr<cyber::Reader<apollo::storytelling::Stories>>
403 storytelling_reader_;
404 std::shared_ptr<cyber::Reader<apollo::audio::AudioDetection>>
405 audio_detection_reader_;
406 std::shared_ptr<cyber::Reader<apollo::task_manager::Task>> task_reader_;
407
408 // Writers.
409 std::shared_ptr<cyber::Writer<apollo::relative_map::NavigationInfo>>
410 navigation_writer_;
411 std::shared_ptr<cyber::Client<apollo::external_command::LaneFollowCommand,
413 lane_follow_command_client_;
414 std::shared_ptr<cyber::Client<apollo::external_command::ValetParkingCommand,
416 valet_parking_command_client_;
417 std::shared_ptr<cyber::Client<apollo::external_command::ActionCommand,
419 action_command_client_;
420 std::shared_ptr<cyber::Writer<apollo::routing::RoutingResponse>>
421 routing_response_writer_;
422 std::shared_ptr<cyber::Writer<apollo::task_manager::Task>> task_writer_;
423
424 FRIEND_TEST(SimulationWorldServiceTest, UpdateMonitorSuccess);
425 FRIEND_TEST(SimulationWorldServiceTest, UpdateMonitorRemove);
426 FRIEND_TEST(SimulationWorldServiceTest, UpdateMonitorTruncate);
427 FRIEND_TEST(SimulationWorldServiceTest, UpdateChassisInfo);
428 FRIEND_TEST(SimulationWorldServiceTest, UpdateLatency);
429 FRIEND_TEST(SimulationWorldServiceTest, UpdateLocalization);
430 FRIEND_TEST(SimulationWorldServiceTest, UpdatePerceptionObstacles);
431 FRIEND_TEST(SimulationWorldServiceTest, UpdatePlanningTrajectory);
432 FRIEND_TEST(SimulationWorldServiceTest, UpdateDecision);
433 FRIEND_TEST(SimulationWorldServiceTest, UpdatePrediction);
434 FRIEND_TEST(SimulationWorldServiceTest, UpdateRouting);
435 FRIEND_TEST(SimulationWorldServiceTest, UpdateGps);
436 FRIEND_TEST(SimulationWorldServiceTest, UpdateControlCommandWithSimpleLonLat);
437 FRIEND_TEST(SimulationWorldServiceTest, UpdateControlCommandWithSimpleMpc);
438 FRIEND_TEST(SimulationWorldServiceTest, DownsampleSpeedPointsByInterval);
439};
440
441} // namespace dreamview
442} // namespace apollo
const std::string & GetChannelName() const
Get Reader's Channel name
Reader subscribes a channel, it has two main functions:
Definition reader.h:69
bool Empty() const override
Query whether the Reader has data to be handled
Definition reader.h:379
virtual std::shared_ptr< MessageT > GetLatestObserved() const
Get the latest message we Observe
Definition reader.h:401
Cyber has builtin time type Time.
Definition time.h:31
double ToSecond() const
convert time to second.
Definition time.cc:77
void Update()
The function Update() is periodically called to check for updates from the external messages.
void PublishNavigationInfo(const std::shared_ptr< apollo::relative_map::NavigationInfo > &)
void PublishTask(const std::shared_ptr< apollo::task_manager::Task > &)
nlohmann::json GetMapElements(double radius) const
Returns the json representation of the map element Ids and hash within the given radius from the car.
void PublishLaneFollowCommand(const std::shared_ptr< apollo::external_command::LaneFollowCommand > &)
bool ReadyToPush() const
Check whether the SimulationWorld object has enough information.
SimulationWorldService(const MapService *map_service, bool routing_from_file=false)
Constructor of SimulationWorldService.
const SimulationWorld & world() const
Get a read-only view of the SimulationWorld.
nlohmann::json GetUpdateAsJson(double radius) const
Returns the json representation of the SimulationWorld object.
void GetMapElementIds(double radius, MapElementIds *ids) const
const apollo::hdmap::Map & GetRelativeMap() const
nlohmann::json GetRoutePathAsJson() const
void PublishActionCommand(const std::shared_ptr< apollo::external_command::ActionCommand > &)
void PublishMonitorMessage(apollo::common::monitor::MonitorMessageItem::LogLevel log_level, const std::string &msg)
Publish message to the monitor
static void SetToClear()
Sets the flag to clear the owned simulation world object.
void GetWireFormatString(double radius, std::string *sim_world, std::string *sim_world_with_planning_data)
Returns the binary representation of the SimulationWorld object.
void PublishValetParkingCommand(const std::shared_ptr< apollo::external_command::ValetParkingCommand > &)
#define AINFO_EVERY(freq)
Definition log.h:82
#define AWARN
Definition log.h:43
The class of MonitorLogBuffer
bool DumpMessage(const std::shared_ptr< T > &msg, const std::string &dump_dir="/tmp")
class register implement
Definition arena_queue.h:37