28#include <unordered_map>
32#include <boost/thread/locks.hpp>
33#include <boost/thread/shared_mutex.hpp>
35#include "gtest/gtest_prod.h"
37#include "nlohmann/json.hpp"
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"
79class SimulationWorldService {
91 bool routing_from_file =
false);
115 std::string *sim_world_with_planning_data);
152 const std::string &msg);
155 const std::shared_ptr<apollo::relative_map::NavigationInfo> &);
158 const std::shared_ptr<apollo::external_command::LaneFollowCommand> &);
161 const std::shared_ptr<apollo::external_command::ValetParkingCommand> &);
164 const std::shared_ptr<apollo::external_command::ActionCommand> &);
166 void PublishTask(
const std::shared_ptr<apollo::task_manager::Task> &);
186 template <
typename DataType>
187 void UpdateSimulationWorld(
const DataType &data);
189 void UpdateMonitorMessages();
191 Object &CreateWorldObjectIfAbsent(
193 void CreateWorldObjectFromSensorMeasurement(
198 void SetObstaclePolygon(
201 void SetObstacleSensorMeasurements(
206 void UpdatePlanningTrajectory(
209 bool LocateMarker(
const apollo::planning::ObjectDecisionType &decision,
211 void FindNudgeRegion(
const apollo::planning::ObjectDecisionType &decision,
213 void UpdateDecision(
const apollo::planning::DecisionResult &decision_res,
215 void UpdateMainStopDecision(
216 const apollo::planning::MainDecision &main_decision,
217 double update_timestamp_sec,
Object *world_main_stop);
219 template <
typename MainDecision>
220 void UpdateMainChangeLaneDecision(
const MainDecision &decision,
221 Object *world_main_decision) {
222 if (decision.has_change_lane_type() &&
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());
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());
236 void CreatePredictionTrajectory(
238 Object *world_object);
245 void PopulateMapInfo(
double radius);
251 template <
typename MessageT>
253 bool logging =
true) {
254 if (reader->
Empty()) {
256 AINFO_EVERY(100) <<
"Has not received any data from "
263 UpdateSimulationWorld(*msg);
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!";
281 void ReadPlanningCommandFromFile(
const std::string &planning_command_file);
283 template <
typename MessageT>
284 void UpdateLatency(
const std::string &module_name,
285 cyber::Reader<MessageT> *reader) {
286 if (reader->Empty()) {
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()}))
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;
318 void UpdateLatencies();
320 template <
typename Po
ints>
321 void DownsampleSpeedPointsByInterval(
const Points &points,
322 size_t downsampleInterval,
323 Points *downsampled_points) {
324 if (points.empty()) {
328 for (
int i = 0; i + 1 < points.size(); i += downsampleInterval) {
329 *downsampled_points->Add() = points[i];
333 *downsampled_points->Add() = *points.rbegin();
336 std::unique_ptr<cyber::Node> node_;
340 SimulationWorld world_;
343 mutable boost::shared_mutex route_paths_mutex_;
344 std::vector<RoutePath> route_paths_;
347 const MapService *map_service_;
350 std::unordered_map<std::string, Object> obj_map_;
353 std::mutex monitor_msgs_mutex_;
354 std::list<std::shared_ptr<common::monitor::MonitorMessage>> monitor_msgs_;
361 static bool to_clear_;
367 std::atomic<bool> ready_to_push_;
370 double current_real_dist_ = 0.0;
371 double current_rss_safe_dist_ = 0.0;
374 apollo::canbus::Chassis_GearPosition gear_location_;
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>>
389 std::shared_ptr<cyber::Reader<apollo::control::ControlCommand>>
390 control_command_reader_;
391 std::shared_ptr<cyber::Reader<apollo::relative_map::NavigationInfo>>
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>>
398 std::shared_ptr<cyber::Reader<apollo::common::monitor::MonitorMessage>>
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_;
409 std::shared_ptr<cyber::Writer<apollo::relative_map::NavigationInfo>>
413 lane_follow_command_client_;
416 valet_parking_command_client_;
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_;
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);
const std::string & GetChannelName() const
Get Reader's Channel name
Reader subscribes a channel, it has two main functions:
bool Empty() const override
Query whether the Reader has data to be handled
virtual std::shared_ptr< MessageT > GetLatestObserved() const
Get the latest message we Observe
Cyber has builtin time type Time.
double ToSecond() const
convert time to second.
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 > &)
static constexpr int kMaxMonitorItems
void UpdateVehicleParam()
#define AINFO_EVERY(freq)
The class of MonitorLogBuffer
bool DumpMessage(const std::shared_ptr< T > &msg, const std::string &dump_dir="/tmp")
optional Object auto_driving_car