Apollo 10.0
自动驾驶开放平台
apollo::dreamview::SimulationWorldService类 参考

This is a major component of the Simulation backend, which maintains a SimulationWorld object and keeps updating it. 更多...

#include <simulation_world_service.h>

apollo::dreamview::SimulationWorldService 的协作图:

Public 成员函数

 SimulationWorldService (const MapService *map_service, bool routing_from_file=false)
 Constructor of SimulationWorldService.
 
const SimulationWorldworld () const
 Get a read-only view of the SimulationWorld.
 
nlohmann::json GetUpdateAsJson (double radius) const
 Returns the json representation of the SimulationWorld object.
 
void GetWireFormatString (double radius, std::string *sim_world, std::string *sim_world_with_planning_data)
 Returns the binary representation of the SimulationWorld object.
 
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 Update ()
 The function Update() is periodically called to check for updates from the external messages.
 
void SetToClear ()
 Sets the flag to clear the owned simulation world object.
 
bool ReadyToPush () const
 Check whether the SimulationWorld object has enough information.
 
void PublishMonitorMessage (apollo::common::monitor::MonitorMessageItem::LogLevel log_level, const std::string &msg)
 Publish message to the monitor
 
void PublishNavigationInfo (const std::shared_ptr< apollo::relative_map::NavigationInfo > &)
 
void PublishLaneFollowCommand (const std::shared_ptr< apollo::external_command::LaneFollowCommand > &)
 
void PublishValetParkingCommand (const std::shared_ptr< apollo::external_command::ValetParkingCommand > &)
 
void PublishActionCommand (const std::shared_ptr< apollo::external_command::ActionCommand > &)
 
void PublishTask (const std::shared_ptr< apollo::task_manager::Task > &)
 
void GetMapElementIds (double radius, MapElementIds *ids) const
 
const apollo::hdmap::MapGetRelativeMap () const
 
nlohmann::json GetRoutePathAsJson () const
 
void DumpMessages ()
 
 SimulationWorldService (const MapService *map_service, bool routing_from_file=false)
 Constructor of SimulationWorldService.
 
const SimulationWorldworld () const
 Get a read-only view of the SimulationWorld.
 
nlohmann::json GetUpdateAsJson (double radius) const
 Returns the json representation of the SimulationWorld object.
 
void GetWireFormatString (double radius, std::string *sim_world_with_planning_data)
 Returns the binary representation of the SimulationWorld object.
 
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 Update ()
 The function Update() is periodically called to check for updates from the external messages.
 
bool ReadyToPush () const
 Check whether the SimulationWorld object has enough information.
 
void PublishMonitorMessage (apollo::common::monitor::MonitorMessageItem::LogLevel log_level, const std::string &msg)
 Publish message to the monitor
 
void PublishNavigationInfo (const std::shared_ptr< apollo::relative_map::NavigationInfo > &)
 
void PublishLaneFollowCommand (const std::shared_ptr< apollo::external_command::LaneFollowCommand > &)
 
void PublishValetParkingCommand (const std::shared_ptr< apollo::external_command::ValetParkingCommand > &)
 
void PublishActionCommand (const std::shared_ptr< apollo::external_command::ActionCommand > &)
 
void PublishTask (const std::shared_ptr< apollo::task_manager::Task > &)
 
void GetMapElementIds (double radius, MapElementIds *ids) const
 
const apollo::hdmap::MapGetRelativeMap () const
 
nlohmann::json GetRoutePathAsJson () const
 
void DumpMessages ()
 
void UpdateVehicleParam ()
 
template<>
void UpdateSimulationWorld (const LocalizationEstimate &localization)
 
template<>
void UpdateSimulationWorld (const Gps &gps)
 
template<>
void UpdateSimulationWorld (const Chassis &chassis)
 
template<>
void UpdateSimulationWorld (const Stories &stories)
 
template<>
void UpdateSimulationWorld (const AudioDetection &audio_detection)
 
template<>
void UpdateSimulationWorld (const PerceptionObstacles &obstacles)
 
template<>
void UpdateSimulationWorld (const TrafficLightDetection &traffic_light_detection)
 
template<>
void UpdateSimulationWorld (const ADCTrajectory &trajectory)
 
template<>
void UpdateSimulationWorld (const PredictionObstacles &obstacles)
 
template<>
void UpdateSimulationWorld (const PlanningCommand &planning_command)
 
template<>
void UpdateSimulationWorld (const ControlCommand &control_command)
 
template<>
void UpdateSimulationWorld (const NavigationInfo &navigation_info)
 
template<>
void UpdateSimulationWorld (const MapMsg &map_msg)
 
template<>
void UpdateSimulationWorld (const MonitorMessage &monitor_msg)
 
template<>
void UpdateSimulationWorld (const LocalizationEstimate &localization)
 
template<>
void UpdateSimulationWorld (const Gps &gps)
 
template<>
void UpdateSimulationWorld (const Chassis &chassis)
 
template<>
void UpdateSimulationWorld (const Stories &stories)
 
template<>
void UpdateSimulationWorld (const AudioDetection &audio_detection)
 
template<>
void UpdateSimulationWorld (const PerceptionObstacles &obstacles)
 
template<>
void UpdateSimulationWorld (const TrafficLightDetection &traffic_light_detection)
 
template<>
void UpdateSimulationWorld (const ADCTrajectory &trajectory)
 
template<>
void UpdateSimulationWorld (const PredictionObstacles &obstacles)
 
template<>
void UpdateSimulationWorld (const PlanningCommand &planning_command)
 
template<>
void UpdateSimulationWorld (const ControlCommand &control_command)
 
template<>
void UpdateSimulationWorld (const NavigationInfo &navigation_info)
 
template<>
void UpdateSimulationWorld (const MapMsg &map_msg)
 
template<>
void UpdateSimulationWorld (const MonitorMessage &monitor_msg)
 

静态 Public 成员函数

static void SetToClear ()
 Sets the flag to clear the owned simulation world object.
 

静态 Public 属性

static constexpr int kMaxMonitorItems = 30
 

详细描述

This is a major component of the Simulation backend, which maintains a SimulationWorld object and keeps updating it.

The SimulationWorld represents the most up-to-date information about all the objects in the emulated world, including the car, the planning trajectory, etc. NOTE: This class is not thread-safe.

在文件 simulation_world_service.h79 行定义.

构造及析构函数说明

◆ SimulationWorldService() [1/2]

apollo::dreamview::SimulationWorldService::SimulationWorldService ( const MapService map_service,
bool  routing_from_file = false 
)

Constructor of SimulationWorldService.

参数
map_servicethe pointer of MapService.
routing_from_filewhether to read initial routing from file.

在文件 simulation_world_service.cc305 行定义.

307 : node_(cyber::CreateNode("simulation_world")),
308 map_service_(map_service),
309 monitor_logger_buffer_(MonitorMessageItem::SIMULATOR),
310 ready_to_push_(false) {
311 InitReaders();
312 InitWriters();
313
314 if (routing_from_file) {
315 ReadPlanningCommandFromFile(FLAGS_routing_response_file);
316 }
317
318 // Populate vehicle parameters.
319 Object *auto_driving_car = world_.mutable_auto_driving_car();
320 const auto &vehicle_param = VehicleConfigHelper::GetConfig().vehicle_param();
321 auto_driving_car->set_height(vehicle_param.height());
322 auto_driving_car->set_width(vehicle_param.width());
323 auto_driving_car->set_length(vehicle_param.length());
324}
static const VehicleConfig & GetConfig()
Get the current vehicle configuration.
std::unique_ptr< Node > CreateNode(const std::string &node_name, const std::string &name_space)
Definition cyber.cc:33
optional VehicleParam vehicle_param

◆ SimulationWorldService() [2/2]

apollo::dreamview::SimulationWorldService::SimulationWorldService ( const MapService map_service,
bool  routing_from_file = false 
)

Constructor of SimulationWorldService.

参数
map_servicethe pointer of MapService.
routing_from_filewhether to read initial routing from file.

成员函数说明

◆ DumpMessages() [1/2]

void apollo::dreamview::SimulationWorldService::DumpMessages ( )

在文件 simulation_world_service.cc1412 行定义.

1412 {
1413 DumpMessageFromReader(chassis_reader_.get());
1414 DumpMessageFromReader(prediction_obstacle_reader_.get());
1415 DumpMessageFromReader(planning_command_reader_.get());
1416 DumpMessageFromReader(localization_reader_.get());
1417 DumpMessageFromReader(planning_reader_.get());
1418 DumpMessageFromReader(control_command_reader_.get());
1419 DumpMessageFromReader(perception_obstacle_reader_.get());
1420 DumpMessageFromReader(perception_traffic_light_reader_.get());
1421 DumpMessageFromReader(relative_map_reader_.get());
1422 DumpMessageFromReader(navigation_reader_.get());
1423 DumpMessageFromReader(task_reader_.get());
1424}

◆ DumpMessages() [2/2]

void apollo::dreamview::SimulationWorldService::DumpMessages ( )

◆ GetMapElementIds() [1/2]

void apollo::dreamview::SimulationWorldService::GetMapElementIds ( double  radius,
MapElementIds ids 
) const

在文件 simulation_world_service.cc494 行定义.

495 {
496 // Gather required map element ids based on current location.
498 const auto &adc = world_.auto_driving_car();
499 point.set_x(adc.position_x());
500 point.set_y(adc.position_y());
501 map_service_->CollectMapElementIds(point, radius, ids);
502}
void CollectMapElementIds(const apollo::common::PointENU &point, double raidus, MapElementIds *ids) const

◆ GetMapElementIds() [2/2]

void apollo::dreamview::SimulationWorldService::GetMapElementIds ( double  radius,
MapElementIds ids 
) const

◆ GetMapElements() [1/2]

nlohmann::json apollo::dreamview::SimulationWorldService::GetMapElements ( double  radius) const

Returns the json representation of the map element Ids and hash within the given radius from the car.

参数
radiusthe search distance from the current car location
返回
Json object that contains mapElementIds and mapHash.

◆ GetMapElements() [2/2]

nlohmann::json apollo::dreamview::SimulationWorldService::GetMapElements ( double  radius) const

Returns the json representation of the map element Ids and hash within the given radius from the car.

参数
radiusthe search distance from the current car location
返回
Json object that contains mapElementIds and mapHash.

◆ GetRelativeMap() [1/2]

const Map & apollo::dreamview::SimulationWorldService::GetRelativeMap ( ) const

在文件 simulation_world_service.cc511 行定义.

511 {
512 return relative_map_;
513}

◆ GetRelativeMap() [2/2]

const apollo::hdmap::Map & apollo::dreamview::SimulationWorldService::GetRelativeMap ( ) const

◆ GetRoutePathAsJson() [1/2]

Json apollo::dreamview::SimulationWorldService::GetRoutePathAsJson ( ) const

在文件 simulation_world_service.cc1263 行定义.

1263 {
1264 Json response;
1265 response["routePath"] = Json::array();
1266 std::vector<RoutePath> route_paths;
1267 {
1268 boost::shared_lock<boost::shared_mutex> reader_lock(route_paths_mutex_);
1269 response["routingTime"] = world_.routing_time();
1270 route_paths = route_paths_;
1271 }
1272 for (const auto &route_path : route_paths) {
1273 Json path;
1274 path["point"] = Json::array();
1275 for (const auto &route_point : route_path.point()) {
1276 path["point"].push_back({{"x", route_point.x()},
1277 {"y", route_point.y()},
1278 {"z", route_point.z()}});
1279 }
1280 response["routePath"].push_back(path);
1281 }
1282 return response;
1283}
nlohmann::json Json

◆ GetRoutePathAsJson() [2/2]

nlohmann::json apollo::dreamview::SimulationWorldService::GetRoutePathAsJson ( ) const

◆ GetUpdateAsJson() [1/2]

Json apollo::dreamview::SimulationWorldService::GetUpdateAsJson ( double  radius) const

Returns the json representation of the SimulationWorld object.

This is a public API used by offline dreamview.

参数
radiusthe search distance from the current car location
返回
Json object equivalence of the SimulationWorld object.

在文件 simulation_world_service.cc482 行定义.

482 {
483 std::string sim_world_json_string;
484 MessageToJsonString(world_, &sim_world_json_string);
485
486 Json update;
487 update["type"] = "SimWorldUpdate";
488 update["timestamp"] = Clock::Now().ToSecond() * 1000;
489 update["world"] = sim_world_json_string;
490
491 return update;
492}
static Time Now()
PRECISION >= 1000000 means the precision is at least 1us.
Definition clock.cc:40
double ToSecond() const
convert time to second.
Definition time.cc:77

◆ GetUpdateAsJson() [2/2]

nlohmann::json apollo::dreamview::SimulationWorldService::GetUpdateAsJson ( double  radius) const

Returns the json representation of the SimulationWorld object.

This is a public API used by offline dreamview.

参数
radiusthe search distance from the current car location
返回
Json object equivalence of the SimulationWorld object.

◆ GetWireFormatString() [1/2]

void apollo::dreamview::SimulationWorldService::GetWireFormatString ( double  radius,
std::string *  sim_world,
std::string *  sim_world_with_planning_data 
)

Returns the binary representation of the SimulationWorld object.

参数
radiusthe search distance from the current car location.
sim_worldoutput of binary format sim_world string.
sim_world_with_planning_dataoutput of binary format sim_world string with planning_data.

在文件 simulation_world_service.cc471 行定义.

473 {
474 PopulateMapInfo(radius);
475
476 world_.SerializeToString(sim_world_with_planning_data);
477
478 world_.clear_planning_data();
479 world_.SerializeToString(sim_world);
480}

◆ GetWireFormatString() [2/2]

void apollo::dreamview::SimulationWorldService::GetWireFormatString ( double  radius,
std::string *  sim_world_with_planning_data 
)

Returns the binary representation of the SimulationWorld object.

参数
radiusthe search distance from the current car location.
sim_worldoutput of binary format sim_world string.
sim_world_with_planning_dataoutput of binary format sim_world string with planning_data.

在文件 simulation_world_service.cc473 行定义.

474 {
475 PopulateMapInfo(radius);
476
477 world_.SerializeToString(sim_world_with_planning_data);
478}

◆ PublishActionCommand() [1/2]

void apollo::dreamview::SimulationWorldService::PublishActionCommand ( const std::shared_ptr< apollo::external_command::ActionCommand > &  )

◆ PublishActionCommand() [2/2]

void apollo::dreamview::SimulationWorldService::PublishActionCommand ( const std::shared_ptr< apollo::external_command::ActionCommand > &  )

◆ PublishLaneFollowCommand() [1/2]

void apollo::dreamview::SimulationWorldService::PublishLaneFollowCommand ( const std::shared_ptr< apollo::external_command::LaneFollowCommand > &  )

◆ PublishLaneFollowCommand() [2/2]

void apollo::dreamview::SimulationWorldService::PublishLaneFollowCommand ( const std::shared_ptr< apollo::external_command::LaneFollowCommand > &  )

◆ PublishMonitorMessage() [1/2]

void apollo::dreamview::SimulationWorldService::PublishMonitorMessage ( apollo::common::monitor::MonitorMessageItem::LogLevel  log_level,
const std::string &  msg 
)

Publish message to the monitor

参数
msgthe string to send to monitor
log_leveldefined in modules/common/monitor_log/proto/monitor_log.proto

在文件 simulation_world_service.cc1455 行定义.

1457 {
1458 monitor_logger_buffer_.AddMonitorMsgItem(log_level, msg);
1459 monitor_logger_buffer_.Publish();
1460}
void AddMonitorMsgItem(const MonitorMessageItem::LogLevel log_level, const std::string &msg)
Add monitor message with MonitorMessageItem::LogLevel
void Publish()
publish the monitor messages

◆ PublishMonitorMessage() [2/2]

void apollo::dreamview::SimulationWorldService::PublishMonitorMessage ( apollo::common::monitor::MonitorMessageItem::LogLevel  log_level,
const std::string &  msg 
)

Publish message to the monitor

参数
msgthe string to send to monitor
log_leveldefined in modules/common/monitor_log/proto/monitor_log.proto

◆ PublishNavigationInfo() [1/2]

void apollo::dreamview::SimulationWorldService::PublishNavigationInfo ( const std::shared_ptr< apollo::relative_map::NavigationInfo > &  )

◆ PublishNavigationInfo() [2/2]

void apollo::dreamview::SimulationWorldService::PublishNavigationInfo ( const std::shared_ptr< apollo::relative_map::NavigationInfo > &  )

◆ PublishTask() [1/2]

void apollo::dreamview::SimulationWorldService::PublishTask ( const std::shared_ptr< apollo::task_manager::Task > &  )

◆ PublishTask() [2/2]

void apollo::dreamview::SimulationWorldService::PublishTask ( const std::shared_ptr< apollo::task_manager::Task > &  )

◆ PublishValetParkingCommand() [1/2]

void apollo::dreamview::SimulationWorldService::PublishValetParkingCommand ( const std::shared_ptr< apollo::external_command::ValetParkingCommand > &  )

◆ PublishValetParkingCommand() [2/2]

void apollo::dreamview::SimulationWorldService::PublishValetParkingCommand ( const std::shared_ptr< apollo::external_command::ValetParkingCommand > &  )

◆ ReadyToPush() [1/2]

bool apollo::dreamview::SimulationWorldService::ReadyToPush ( ) const
inline

Check whether the SimulationWorld object has enough information.

The backend won't push the SimulationWorld to frontend if it is not ready.

返回
True if the object is ready to push.

在文件 simulation_world_service.h142 行定义.

142{ return ready_to_push_.load(); }

◆ ReadyToPush() [2/2]

bool apollo::dreamview::SimulationWorldService::ReadyToPush ( ) const
inline

Check whether the SimulationWorld object has enough information.

The backend won't push the SimulationWorld to frontend if it is not ready.

返回
True if the object is ready to push.

在文件 simulation_world_service.h142 行定义.

142{ return ready_to_push_.load(); }

◆ SetToClear() [1/2]

void apollo::dreamview::SimulationWorldService::SetToClear ( )
inline

Sets the flag to clear the owned simulation world object.

在文件 simulation_world_service.h135 行定义.

135{ to_clear_ = true; }

◆ SetToClear() [2/2]

static void apollo::dreamview::SimulationWorldService::SetToClear ( )
inlinestatic

Sets the flag to clear the owned simulation world object.

在文件 simulation_world_service.h135 行定义.

135{ to_clear_ = true; }

◆ Update() [1/2]

void apollo::dreamview::SimulationWorldService::Update ( )

The function Update() is periodically called to check for updates from the external messages.

All the updates will be written to the SimulationWorld object to reflect the latest status.

在文件 simulation_world_service.cc393 行定义.

393 {
394 if (to_clear_) {
395 // Clears received data.
396 node_->ClearData();
397
398 // Clears simulation world except for the car information.
399 auto car = world_.auto_driving_car();
400 world_.Clear();
401 *world_.mutable_auto_driving_car() = car;
402
403 {
404 boost::unique_lock<boost::shared_mutex> writer_lock(route_paths_mutex_);
405 route_paths_.clear();
406 }
407
408 to_clear_ = false;
409 }
410
411 node_->Observe();
412
413 UpdateMonitorMessages();
414
415 UpdateWithLatestObserved(planning_command_reader_.get(), false);
416 UpdateWithLatestObserved(chassis_reader_.get());
417 UpdateWithLatestObserved(gps_reader_.get());
418 UpdateWithLatestObserved(localization_reader_.get());
419
420 // Clear objects received from last frame and populate with the new objects.
421 // TODO(siyangy, unacao): For now we are assembling the simulation_world with
422 // latest received perception, prediction and planning message. However, they
423 // may not always be perfectly aligned and belong to the same frame.
424 obj_map_.clear();
425 world_.clear_object();
426 world_.clear_sensor_measurements();
427 UpdateWithLatestObserved(audio_detection_reader_.get());
428 UpdateWithLatestObserved(storytelling_reader_.get());
429 UpdateWithLatestObserved(perception_obstacle_reader_.get());
430 UpdateWithLatestObserved(perception_traffic_light_reader_.get(), false);
431 UpdateWithLatestObserved(prediction_obstacle_reader_.get());
432 UpdateWithLatestObserved(planning_reader_.get());
433 UpdateWithLatestObserved(control_command_reader_.get());
434 UpdateWithLatestObserved(navigation_reader_.get(), FLAGS_use_navigation_mode);
435 UpdateWithLatestObserved(relative_map_reader_.get(),
436 FLAGS_use_navigation_mode);
437
438 for (const auto &kv : obj_map_) {
439 *world_.add_object() = kv.second;
440 }
441
442 UpdateDelays();
443 UpdateLatencies();
444
445 world_.set_sequence_num(world_.sequence_num() + 1);
446 world_.set_timestamp(Clock::Now().ToSecond() * 1000);
447}

◆ Update() [2/2]

void apollo::dreamview::SimulationWorldService::Update ( )

The function Update() is periodically called to check for updates from the external messages.

All the updates will be written to the SimulationWorld object to reflect the latest status.

◆ UpdateSimulationWorld() [1/28]

template<>
void apollo::dreamview::SimulationWorldService::UpdateSimulationWorld ( const ADCTrajectory trajectory)

在文件 simulation_world_service.cc1137 行定义.

1138 {
1139 const double header_time = trajectory.header().timestamp_sec();
1140
1141 UpdatePlanningTrajectory(trajectory);
1142
1143 UpdateRSSInfo(trajectory);
1144
1145 UpdateDecision(trajectory.decision(), header_time);
1146
1147 UpdatePlanningData(trajectory.debug().planning_data());
1148
1149 Latency latency;
1150 latency.set_timestamp_sec(header_time);
1151 latency.set_total_time_ms(trajectory.latency_stats().total_time_ms());
1152 (*world_.mutable_latency())["planning"] = latency;
1153}

◆ UpdateSimulationWorld() [2/28]

template<>
void apollo::dreamview::SimulationWorldService::UpdateSimulationWorld ( const ADCTrajectory trajectory)

在文件 simulation_world_service.cc1135 行定义.

1136 {
1137 const double header_time = trajectory.header().timestamp_sec();
1138
1139 UpdatePlanningTrajectory(trajectory);
1140
1141 UpdateRSSInfo(trajectory);
1142
1143 UpdateDecision(trajectory.decision(), header_time);
1144
1145 UpdatePlanningData(trajectory.debug().planning_data());
1146
1147 Latency latency;
1148 latency.set_timestamp_sec(header_time);
1149 latency.set_total_time_ms(trajectory.latency_stats().total_time_ms());
1150 (*world_.mutable_latency())["planning"] = latency;
1151}

◆ UpdateSimulationWorld() [3/28]

template<>
void apollo::dreamview::SimulationWorldService::UpdateSimulationWorld ( const AudioDetection audio_detection)

在文件 simulation_world_service.cc620 行定义.

621 {
622 world_.set_is_siren_on(audio_detection.is_siren());
623}

◆ UpdateSimulationWorld() [4/28]

template<>
void apollo::dreamview::SimulationWorldService::UpdateSimulationWorld ( const AudioDetection audio_detection)

在文件 simulation_world_service.cc618 行定义.

619 {
620 world_.set_is_siren_on(audio_detection.is_siren());
621}

◆ UpdateSimulationWorld() [5/28]

template<>
void apollo::dreamview::SimulationWorldService::UpdateSimulationWorld ( const Chassis chassis)

在文件 simulation_world_service.cc567 行定义.

567 {
568 const auto &vehicle_param = VehicleConfigHelper::GetConfig().vehicle_param();
569 Object *auto_driving_car = world_.mutable_auto_driving_car();
570
571 double speed = chassis.speed_mps();
572 gear_location_ = chassis.gear_location();
573 if (gear_location_ == Chassis::GEAR_REVERSE) {
574 speed = -speed;
575 }
576 auto_driving_car->set_speed(speed);
577 auto_driving_car->set_throttle_percentage(chassis.throttle_percentage());
578 auto_driving_car->set_brake_percentage(chassis.brake_percentage());
579
580 // In case of out-of-range percentages, reduces it to zero.
581 double angle_percentage = chassis.steering_percentage();
582 if (angle_percentage > 100 || angle_percentage < -100) {
583 angle_percentage = 0;
584 }
585 auto_driving_car->set_steering_percentage(angle_percentage);
586
587 double steering_angle =
588 angle_percentage / 100.0 * vehicle_param.max_steer_angle();
589 auto_driving_car->set_steering_angle(steering_angle);
590
591 double kappa = std::tan(steering_angle / vehicle_param.steer_ratio()) /
592 vehicle_param.wheel_base();
593 auto_driving_car->set_kappa(kappa);
594
595 UpdateTurnSignal(chassis.signal(), auto_driving_car);
596
597 auto_driving_car->set_disengage_type(DeduceDisengageType(chassis));
598
599 auto_driving_car->set_battery_percentage(chassis.battery_soc_percentage());
600 auto_driving_car->set_gear_location(chassis.gear_location());
601}

◆ UpdateSimulationWorld() [6/28]

template<>
void apollo::dreamview::SimulationWorldService::UpdateSimulationWorld ( const Chassis chassis)

在文件 simulation_world_service.cc565 行定义.

565 {
566 const auto &vehicle_param = VehicleConfigHelper::GetConfig().vehicle_param();
567 Object *auto_driving_car = world_.mutable_auto_driving_car();
568
569 double speed = chassis.speed_mps();
570 gear_location_ = chassis.gear_location();
571 if (gear_location_ == Chassis::GEAR_REVERSE) {
572 speed = -speed;
573 }
574 auto_driving_car->set_speed(speed);
575 auto_driving_car->set_throttle_percentage(chassis.throttle_percentage());
576 auto_driving_car->set_brake_percentage(chassis.brake_percentage());
577
578 // In case of out-of-range percentages, reduces it to zero.
579 double angle_percentage = chassis.steering_percentage();
580 if (angle_percentage > 100 || angle_percentage < -100) {
581 angle_percentage = 0;
582 }
583 auto_driving_car->set_steering_percentage(angle_percentage);
584
585 double steering_angle =
586 angle_percentage / 100.0 * vehicle_param.max_steer_angle();
587 auto_driving_car->set_steering_angle(steering_angle);
588
589 double kappa = std::tan(steering_angle / vehicle_param.steer_ratio()) /
590 vehicle_param.wheel_base();
591 auto_driving_car->set_kappa(kappa);
592
593 UpdateTurnSignal(chassis.signal(), auto_driving_car);
594
595 auto_driving_car->set_disengage_type(DeduceDisengageType(chassis));
596
597 auto_driving_car->set_battery_percentage(chassis.battery_soc_percentage());
598 auto_driving_car->set_gear_location(chassis.gear_location());
599}

◆ UpdateSimulationWorld() [7/28]

template<>
void apollo::dreamview::SimulationWorldService::UpdateSimulationWorld ( const ControlCommand control_command)

在文件 simulation_world_service.cc1305 行定义.

1306 {
1307 auto *control_data = world_.mutable_control_data();
1308 const double header_time = control_command.header().timestamp_sec();
1309 control_data->set_timestamp_sec(header_time);
1310
1311 Latency latency;
1312 latency.set_timestamp_sec(header_time);
1313 latency.set_total_time_ms(control_command.latency_stats().total_time_ms());
1314 (*world_.mutable_latency())["control"] = latency;
1315
1316 if (control_command.has_debug()) {
1317 auto &debug = control_command.debug();
1318 if (debug.has_simple_lon_debug() && debug.has_simple_lat_debug()) {
1319 auto &simple_lon = debug.simple_lon_debug();
1320 if (simple_lon.has_station_error()) {
1321 control_data->set_station_error(simple_lon.station_error());
1322 }
1323 auto &simple_lat = debug.simple_lat_debug();
1324 if (simple_lat.has_heading_error()) {
1325 control_data->set_heading_error(simple_lat.heading_error());
1326 }
1327 if (simple_lat.has_lateral_error()) {
1328 control_data->set_lateral_error(simple_lat.lateral_error());
1329 }
1330 if (simple_lat.has_current_target_point()) {
1331 control_data->mutable_current_target_point()->CopyFrom(
1332 simple_lat.current_target_point());
1333 }
1334 } else if (debug.has_simple_mpc_debug()) {
1335 auto &simple_mpc = debug.simple_mpc_debug();
1336 if (simple_mpc.has_station_error()) {
1337 control_data->set_station_error(simple_mpc.station_error());
1338 }
1339 if (simple_mpc.has_heading_error()) {
1340 control_data->set_heading_error(simple_mpc.heading_error());
1341 }
1342 if (simple_mpc.has_lateral_error()) {
1343 control_data->set_lateral_error(simple_mpc.lateral_error());
1344 }
1345 }
1346 }
1347}

◆ UpdateSimulationWorld() [8/28]

template<>
void apollo::dreamview::SimulationWorldService::UpdateSimulationWorld ( const ControlCommand control_command)

在文件 simulation_world_service.cc1303 行定义.

1304 {
1305 auto *control_data = world_.mutable_control_data();
1306 const double header_time = control_command.header().timestamp_sec();
1307 control_data->set_timestamp_sec(header_time);
1308
1309 Latency latency;
1310 latency.set_timestamp_sec(header_time);
1311 latency.set_total_time_ms(control_command.latency_stats().total_time_ms());
1312 (*world_.mutable_latency())["control"] = latency;
1313
1314 if (control_command.has_debug()) {
1315 auto &debug = control_command.debug();
1316 if (debug.has_simple_lon_debug() && debug.has_simple_lat_debug()) {
1317 auto &simple_lon = debug.simple_lon_debug();
1318 if (simple_lon.has_station_error()) {
1319 control_data->set_station_error(simple_lon.station_error());
1320 }
1321 auto &simple_lat = debug.simple_lat_debug();
1322 if (simple_lat.has_heading_error()) {
1323 control_data->set_heading_error(simple_lat.heading_error());
1324 }
1325 if (simple_lat.has_lateral_error()) {
1326 control_data->set_lateral_error(simple_lat.lateral_error());
1327 }
1328 if (simple_lat.has_current_target_point()) {
1329 control_data->mutable_current_target_point()->CopyFrom(
1330 simple_lat.current_target_point());
1331 }
1332 } else if (debug.has_simple_mpc_debug()) {
1333 auto &simple_mpc = debug.simple_mpc_debug();
1334 if (simple_mpc.has_station_error()) {
1335 control_data->set_station_error(simple_mpc.station_error());
1336 }
1337 if (simple_mpc.has_heading_error()) {
1338 control_data->set_heading_error(simple_mpc.heading_error());
1339 }
1340 if (simple_mpc.has_lateral_error()) {
1341 control_data->set_lateral_error(simple_mpc.lateral_error());
1342 }
1343 }
1344 }
1345}

◆ UpdateSimulationWorld() [9/28]

template<>
void apollo::dreamview::SimulationWorldService::UpdateSimulationWorld ( const Gps gps)

在文件 simulation_world_service.cc540 行定义.

540 {
541 if (gps.header().module_name() == "ShadowLocalization") {
542 Object *shadow_localization_position = world_.mutable_shadow_localization();
543 const auto &pose = gps.localization();
544 shadow_localization_position->set_position_x(pose.position().x() +
545 map_service_->GetXOffset());
546 shadow_localization_position->set_position_y(pose.position().y() +
547 map_service_->GetYOffset());
548 shadow_localization_position->set_heading(pose.heading());
549 } else {
550 Object *gps_position = world_.mutable_gps();
551 gps_position->set_timestamp_sec(gps.header().timestamp_sec());
552
553 const auto &pose = gps.localization();
554 gps_position->set_position_x(pose.position().x() +
555 map_service_->GetXOffset());
556 gps_position->set_position_y(pose.position().y() +
557 map_service_->GetYOffset());
558
560 pose.orientation().qw(), pose.orientation().qx(),
561 pose.orientation().qy(), pose.orientation().qz());
562 gps_position->set_heading(heading);
563 }
564}
double QuaternionToHeading(const double qw, const double qx, const double qy, const double qz)
Definition quaternion.h:56

◆ UpdateSimulationWorld() [10/28]

template<>
void apollo::dreamview::SimulationWorldService::UpdateSimulationWorld ( const Gps gps)

在文件 simulation_world_service.cc538 行定义.

538 {
539 if (gps.header().module_name() == "ShadowLocalization") {
540 Object *shadow_localization_position = world_.mutable_shadow_localization();
541 const auto &pose = gps.localization();
542 shadow_localization_position->set_position_x(pose.position().x() +
543 map_service_->GetXOffset());
544 shadow_localization_position->set_position_y(pose.position().y() +
545 map_service_->GetYOffset());
546 shadow_localization_position->set_heading(pose.heading());
547 } else {
548 Object *gps_position = world_.mutable_gps();
549 gps_position->set_timestamp_sec(gps.header().timestamp_sec());
550
551 const auto &pose = gps.localization();
552 gps_position->set_position_x(pose.position().x() +
553 map_service_->GetXOffset());
554 gps_position->set_position_y(pose.position().y() +
555 map_service_->GetYOffset());
556
558 pose.orientation().qw(), pose.orientation().qx(),
559 pose.orientation().qy(), pose.orientation().qz());
560 gps_position->set_heading(heading);
561 }
562}

◆ UpdateSimulationWorld() [11/28]

template<>
void apollo::dreamview::SimulationWorldService::UpdateSimulationWorld ( const LocalizationEstimate localization)

在文件 simulation_world_service.cc516 行定义.

517 {
518 Object *auto_driving_car = world_.mutable_auto_driving_car();
519 const auto &pose = localization.pose();
520
521 // Updates position with the input localization message.
522 auto_driving_car->set_position_x(pose.position().x() +
523 map_service_->GetXOffset());
524 auto_driving_car->set_position_y(pose.position().y() +
525 map_service_->GetYOffset());
526 auto_driving_car->set_heading(pose.heading());
527
528 // Updates acceleration with the input localization message.
529 auto_driving_car->set_speed_acceleration(CalculateAcceleration(
530 pose.linear_acceleration(), pose.linear_velocity(), gear_location_));
531
532 // Updates the timestamp with the timestamp inside the localization
533 // message header. It is done on both the SimulationWorld object
534 // itself and its auto_driving_car() field.
535 auto_driving_car->set_timestamp_sec(localization.header().timestamp_sec());
536 ready_to_push_.store(true);
537}

◆ UpdateSimulationWorld() [12/28]

template<>
void apollo::dreamview::SimulationWorldService::UpdateSimulationWorld ( const LocalizationEstimate localization)

在文件 simulation_world_service.cc514 行定义.

515 {
516 Object *auto_driving_car = world_.mutable_auto_driving_car();
517 const auto &pose = localization.pose();
518
519 // Updates position with the input localization message.
520 auto_driving_car->set_position_x(pose.position().x() +
521 map_service_->GetXOffset());
522 auto_driving_car->set_position_y(pose.position().y() +
523 map_service_->GetYOffset());
524 auto_driving_car->set_heading(pose.heading());
525
526 // Updates acceleration with the input localization message.
527 auto_driving_car->set_speed_acceleration(CalculateAcceleration(
528 pose.linear_acceleration(), pose.linear_velocity(), gear_location_));
529
530 // Updates the timestamp with the timestamp inside the localization
531 // message header. It is done on both the SimulationWorld object
532 // itself and its auto_driving_car() field.
533 auto_driving_car->set_timestamp_sec(localization.header().timestamp_sec());
534 ready_to_push_.store(true);
535}

◆ UpdateSimulationWorld() [13/28]

template<>
void apollo::dreamview::SimulationWorldService::UpdateSimulationWorld ( const MapMsg map_msg)

在文件 simulation_world_service.cc1361 行定义.

1361 {
1362 if (map_msg.has_hdmap()) {
1363 relative_map_.CopyFrom(map_msg.hdmap());
1364 for (int i = 0; i < relative_map_.lane_size(); ++i) {
1365 auto *lane = relative_map_.mutable_lane(i);
1366 lane->clear_left_sample();
1367 lane->clear_right_sample();
1368 lane->clear_left_road_sample();
1369 lane->clear_right_road_sample();
1370
1371 DownsampleCurve(lane->mutable_central_curve());
1372 DownsampleCurve(lane->mutable_left_boundary()->mutable_curve());
1373 DownsampleCurve(lane->mutable_right_boundary()->mutable_curve());
1374 }
1375 }
1376}

◆ UpdateSimulationWorld() [14/28]

template<>
void apollo::dreamview::SimulationWorldService::UpdateSimulationWorld ( const MapMsg map_msg)

在文件 simulation_world_service.cc1359 行定义.

1359 {
1360 if (map_msg.has_hdmap()) {
1361 relative_map_.CopyFrom(map_msg.hdmap());
1362 for (int i = 0; i < relative_map_.lane_size(); ++i) {
1363 auto *lane = relative_map_.mutable_lane(i);
1364 lane->clear_left_sample();
1365 lane->clear_right_sample();
1366 lane->clear_left_road_sample();
1367 lane->clear_right_road_sample();
1368
1369 DownsampleCurve(lane->mutable_central_curve());
1370 DownsampleCurve(lane->mutable_left_boundary()->mutable_curve());
1371 DownsampleCurve(lane->mutable_right_boundary()->mutable_curve());
1372 }
1373 }
1374}

◆ UpdateSimulationWorld() [15/28]

template<>
void apollo::dreamview::SimulationWorldService::UpdateSimulationWorld ( const MonitorMessage monitor_msg)

在文件 simulation_world_service.cc1379 行定义.

1380 {
1381 const int updated_size = std::min(monitor_msg.item_size(),
1383 // Save the latest messages at the end of the history.
1384 for (int idx = 0; idx < updated_size; ++idx) {
1385 auto *notification = world_.add_notification();
1386 notification->mutable_item()->CopyFrom(monitor_msg.item(idx));
1387 notification->set_timestamp_sec(monitor_msg.header().timestamp_sec());
1388 }
1389
1390 int remove_size =
1391 world_.notification_size() - SimulationWorldService::kMaxMonitorItems;
1392 if (remove_size > 0) {
1393 auto *notifications = world_.mutable_notification();
1394 notifications->erase(notifications->begin(),
1395 notifications->begin() + remove_size);
1396 }
1397}

◆ UpdateSimulationWorld() [16/28]

template<>
void apollo::dreamview::SimulationWorldService::UpdateSimulationWorld ( const MonitorMessage monitor_msg)

在文件 simulation_world_service.cc1377 行定义.

1378 {
1379 const int updated_size = std::min(monitor_msg.item_size(),
1381 // Save the latest messages at the end of the history.
1382 for (int idx = 0; idx < updated_size; ++idx) {
1383 auto *notification = world_.add_notification();
1384 notification->mutable_item()->CopyFrom(monitor_msg.item(idx));
1385 notification->set_timestamp_sec(monitor_msg.header().timestamp_sec());
1386 }
1387
1388 int remove_size =
1389 world_.notification_size() - SimulationWorldService::kMaxMonitorItems;
1390 if (remove_size > 0) {
1391 auto *notifications = world_.mutable_notification();
1392 notifications->erase(notifications->begin(),
1393 notifications->begin() + remove_size);
1394 }
1395}

◆ UpdateSimulationWorld() [17/28]

template<>
void apollo::dreamview::SimulationWorldService::UpdateSimulationWorld ( const NavigationInfo navigation_info)

在文件 simulation_world_service.cc1350 行定义.

1351 {
1352 world_.clear_navigation_path();
1353 for (auto &navigation_path : navigation_info.navigation_path()) {
1354 if (navigation_path.has_path()) {
1355 DownsamplePath(navigation_path.path(), world_.add_navigation_path());
1356 }
1357 }
1358}

◆ UpdateSimulationWorld() [18/28]

template<>
void apollo::dreamview::SimulationWorldService::UpdateSimulationWorld ( const NavigationInfo navigation_info)

在文件 simulation_world_service.cc1348 行定义.

1349 {
1350 world_.clear_navigation_path();
1351 for (auto &navigation_path : navigation_info.navigation_path()) {
1352 if (navigation_path.has_path()) {
1353 DownsamplePath(navigation_path.path(), world_.add_navigation_path());
1354 }
1355 }
1356}

◆ UpdateSimulationWorld() [19/28]

template<>
void apollo::dreamview::SimulationWorldService::UpdateSimulationWorld ( const PerceptionObstacles obstacles)

在文件 simulation_world_service.cc726 行定义.

727 {
728 for (const auto &obstacle : obstacles.perception_obstacle()) {
729 auto &world_obj = CreateWorldObjectIfAbsent(obstacle);
730 if (obstacles.has_cipv_info() &&
731 (obstacles.cipv_info().cipv_id() == obstacle.id())) {
732 world_obj.set_type(Object_Type_CIPV);
733 }
734 }
735
736 if (obstacles.has_lane_marker()) {
737 world_.mutable_lane_marker()->CopyFrom(obstacles.lane_marker());
738 }
739}

◆ UpdateSimulationWorld() [20/28]

template<>
void apollo::dreamview::SimulationWorldService::UpdateSimulationWorld ( const PerceptionObstacles obstacles)

在文件 simulation_world_service.cc724 行定义.

725 {
726 for (const auto &obstacle : obstacles.perception_obstacle()) {
727 auto &world_obj = CreateWorldObjectIfAbsent(obstacle);
728 if (obstacles.has_cipv_info() &&
729 (obstacles.cipv_info().cipv_id() == obstacle.id())) {
730 world_obj.set_type(Object_Type_CIPV);
731 }
732 }
733
734 if (obstacles.has_lane_marker()) {
735 world_.mutable_lane_marker()->CopyFrom(obstacles.lane_marker());
736 }
737}

◆ UpdateSimulationWorld() [21/28]

template<>
void apollo::dreamview::SimulationWorldService::UpdateSimulationWorld ( const PlanningCommand planning_command)

在文件 simulation_world_service.cc1216 行定义.

1217 {
1218 auto routing_response = planning_command.lane_follow_command();
1219 {
1220 boost::shared_lock<boost::shared_mutex> reader_lock(route_paths_mutex_);
1221 if (world_.has_routing_time() &&
1222 world_.routing_time() == planning_command.header().timestamp_sec()) {
1223 // This routing response has been processed.
1224 return;
1225 }
1226 }
1227
1228 std::vector<Path> paths;
1229 if (!map_service_->GetPathsFromRouting(routing_response, &paths)) {
1230 return;
1231 }
1232
1233 world_.clear_route_path();
1234
1235 std::vector<RoutePath> route_paths;
1236 for (const Path &path : paths) {
1237 // Downsample the path points for frontend display.
1238 auto sampled_indices =
1239 DownsampleByAngle(path.path_points(), kAngleThreshold);
1240
1241 route_paths.emplace_back();
1242 RoutePath *route_path = &route_paths.back();
1243 for (const size_t index : sampled_indices) {
1244 const auto &path_point = path.path_points()[index];
1245 PolygonPoint *route_point = route_path->add_point();
1246 route_point->set_x(path_point.x() + map_service_->GetXOffset());
1247 route_point->set_y(path_point.y() + map_service_->GetYOffset());
1248 }
1249
1250 // Populate route path
1251 if (FLAGS_sim_world_with_routing_path) {
1252 auto *new_path = world_.add_route_path();
1253 *new_path = *route_path;
1254 }
1255 }
1256 {
1257 boost::unique_lock<boost::shared_mutex> writer_lock(route_paths_mutex_);
1258 std::swap(route_paths, route_paths_);
1259 world_.set_routing_time(planning_command.header().timestamp_sec());
1260 }
1261}
bool GetPathsFromRouting(const apollo::routing::RoutingResponse &routing, std::vector< apollo::hdmap::Path > *paths) const
std::vector< size_t > DownsampleByAngle(const Points &points, const double angle_threshold)
Downsample the points on the path according to the angle.

◆ UpdateSimulationWorld() [22/28]

template<>
void apollo::dreamview::SimulationWorldService::UpdateSimulationWorld ( const PlanningCommand planning_command)

在文件 simulation_world_service.cc1214 行定义.

1215 {
1216 auto routing_response = planning_command.lane_follow_command();
1217 {
1218 boost::shared_lock<boost::shared_mutex> reader_lock(route_paths_mutex_);
1219 if (world_.has_routing_time() &&
1220 world_.routing_time() == planning_command.header().timestamp_sec()) {
1221 // This routing response has been processed.
1222 return;
1223 }
1224 }
1225
1226 std::vector<Path> paths;
1227 if (!map_service_->GetPathsFromRouting(routing_response, &paths)) {
1228 return;
1229 }
1230
1231 world_.clear_route_path();
1232
1233 std::vector<RoutePath> route_paths;
1234 for (const Path &path : paths) {
1235 // Downsample the path points for frontend display.
1236 auto sampled_indices =
1237 DownsampleByAngle(path.path_points(), kAngleThreshold);
1238
1239 route_paths.emplace_back();
1240 RoutePath *route_path = &route_paths.back();
1241 for (const size_t index : sampled_indices) {
1242 const auto &path_point = path.path_points()[index];
1243 PolygonPoint *route_point = route_path->add_point();
1244 route_point->set_x(path_point.x() + map_service_->GetXOffset());
1245 route_point->set_y(path_point.y() + map_service_->GetYOffset());
1246 }
1247
1248 // Populate route path
1249 if (FLAGS_sim_world_with_routing_path) {
1250 auto *new_path = world_.add_route_path();
1251 *new_path = *route_path;
1252 }
1253 }
1254 {
1255 boost::unique_lock<boost::shared_mutex> writer_lock(route_paths_mutex_);
1256 std::swap(route_paths, route_paths_);
1257 world_.set_routing_time(planning_command.header().timestamp_sec());
1258 }
1259}

◆ UpdateSimulationWorld() [23/28]

template<>
void apollo::dreamview::SimulationWorldService::UpdateSimulationWorld ( const PredictionObstacles obstacles)

在文件 simulation_world_service.cc1188 行定义.

1189 {
1190 for (const auto &obstacle : obstacles.prediction_obstacle()) {
1191 // Note: There's a perfect one-to-one mapping between the perception
1192 // obstacles and prediction obstacles within the same frame. Creating a new
1193 // world object here is only possible when we happen to be processing a
1194 // perception and prediction message from two frames.
1195 auto &world_obj = CreateWorldObjectIfAbsent(obstacle.perception_obstacle());
1196
1197 // Add prediction trajectory to the object.
1198 CreatePredictionTrajectory(obstacle, &world_obj);
1199
1200 // Add prediction priority
1201 if (obstacle.has_priority()) {
1202 world_obj.mutable_obstacle_priority()->CopyFrom(obstacle.priority());
1203 }
1204
1205 // Add prediction interactive tag
1206 if (obstacle.has_interactive_tag()) {
1207 world_obj.mutable_interactive_tag()->CopyFrom(obstacle.interactive_tag());
1208 }
1209
1210 world_obj.set_timestamp_sec(
1211 std::max(obstacle.timestamp(), world_obj.timestamp_sec()));
1212 }
1213}

◆ UpdateSimulationWorld() [24/28]

template<>
void apollo::dreamview::SimulationWorldService::UpdateSimulationWorld ( const PredictionObstacles obstacles)

在文件 simulation_world_service.cc1186 行定义.

1187 {
1188 for (const auto &obstacle : obstacles.prediction_obstacle()) {
1189 // Note: There's a perfect one-to-one mapping between the perception
1190 // obstacles and prediction obstacles within the same frame. Creating a new
1191 // world object here is only possible when we happen to be processing a
1192 // perception and prediction message from two frames.
1193 auto &world_obj = CreateWorldObjectIfAbsent(obstacle.perception_obstacle());
1194
1195 // Add prediction trajectory to the object.
1196 CreatePredictionTrajectory(obstacle, &world_obj);
1197
1198 // Add prediction priority
1199 if (obstacle.has_priority()) {
1200 world_obj.mutable_obstacle_priority()->CopyFrom(obstacle.priority());
1201 }
1202
1203 // Add prediction interactive tag
1204 if (obstacle.has_interactive_tag()) {
1205 world_obj.mutable_interactive_tag()->CopyFrom(obstacle.interactive_tag());
1206 }
1207
1208 world_obj.set_timestamp_sec(
1209 std::max(obstacle.timestamp(), world_obj.timestamp_sec()));
1210 }
1211}

◆ UpdateSimulationWorld() [25/28]

template<>
void apollo::dreamview::SimulationWorldService::UpdateSimulationWorld ( const Stories stories)

在文件 simulation_world_service.cc604 行定义.

604 {
605 world_.clear_stories();
606 auto *world_stories = world_.mutable_stories();
607
608 const google::protobuf::Descriptor *descriptor = stories.GetDescriptor();
609 const google::protobuf::Reflection *reflection = stories.GetReflection();
610 const int field_count = descriptor->field_count();
611 for (int i = 0; i < field_count; ++i) {
612 const google::protobuf::FieldDescriptor *field = descriptor->field(i);
613 if (field->name() != "header") {
614 (*world_stories)[field->name()] = reflection->HasField(stories, field);
615 }
616 }
617}

◆ UpdateSimulationWorld() [26/28]

template<>
void apollo::dreamview::SimulationWorldService::UpdateSimulationWorld ( const Stories stories)

在文件 simulation_world_service.cc602 行定义.

602 {
603 world_.clear_stories();
604 auto *world_stories = world_.mutable_stories();
605
606 const google::protobuf::Descriptor *descriptor = stories.GetDescriptor();
607 const google::protobuf::Reflection *reflection = stories.GetReflection();
608 const int field_count = descriptor->field_count();
609 for (int i = 0; i < field_count; ++i) {
610 const google::protobuf::FieldDescriptor *field = descriptor->field(i);
611 if (field->name() != "header") {
612 (*world_stories)[field->name()] = reflection->HasField(stories, field);
613 }
614 }
615}

◆ UpdateSimulationWorld() [27/28]

template<>
void apollo::dreamview::SimulationWorldService::UpdateSimulationWorld ( const TrafficLightDetection traffic_light_detection)

在文件 simulation_world_service.cc742 行定义.

743 {
744 world_.clear_perceived_signal();
745 for (const auto &traffic_light : traffic_light_detection.traffic_light()) {
746 Object *signal = world_.add_perceived_signal();
747 signal->set_id(traffic_light.id());
748 signal->set_current_signal(TrafficLight_Color_Name(traffic_light.color()));
749 }
750}

◆ UpdateSimulationWorld() [28/28]

template<>
void apollo::dreamview::SimulationWorldService::UpdateSimulationWorld ( const TrafficLightDetection traffic_light_detection)

在文件 simulation_world_service.cc740 行定义.

741 {
742 world_.clear_perceived_signal();
743 for (const auto &traffic_light : traffic_light_detection.traffic_light()) {
744 Object *signal = world_.add_perceived_signal();
745 signal->set_id(traffic_light.id());
746 signal->set_current_signal(TrafficLight_Color_Name(traffic_light.color()));
747 }
748}

◆ UpdateVehicleParam()

void apollo::dreamview::SimulationWorldService::UpdateVehicleParam ( )

在文件 simulation_world_service.cc1460 行定义.

1460 {
1461 auto &vehicle_param = VehicleConfigHelper::GetConfig().vehicle_param();
1462 world_.mutable_vehicle_param()->CopyFrom(vehicle_param);
1463}

◆ world() [1/2]

const SimulationWorld & apollo::dreamview::SimulationWorldService::world ( ) const
inline

Get a read-only view of the SimulationWorld.

返回
Constant reference to the SimulationWorld object.

在文件 simulation_world_service.h97 行定义.

97{ return world_; }

◆ world() [2/2]

const SimulationWorld & apollo::dreamview::SimulationWorldService::world ( ) const
inline

Get a read-only view of the SimulationWorld.

返回
Constant reference to the SimulationWorld object.

在文件 simulation_world_service.h97 行定义.

97{ return world_; }

类成员变量说明

◆ kMaxMonitorItems

static constexpr int apollo::dreamview::SimulationWorldService::kMaxMonitorItems = 30
staticconstexpr

在文件 simulation_world_service.h83 行定义.


该类的文档由以下文件生成: