simulation_world_service.h

Go to the documentation of this file.

/******************************************************************************
 * Copyright 2017 The Apollo Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/

#pragma once

#include <algorithm>
#include <list>
#include <memory>
#include <string>
#include <unordered_map>
#include <utility>
#include <vector>

#include <boost/thread/locks.hpp>
#include <boost/thread/shared_mutex.hpp>

#include "gtest/gtest_prod.h"

#include "nlohmann/json.hpp"

#include "modules/audio/proto/audio.pb.h"
#include "modules/audio/proto/audio_event.pb.h"
#include "modules/common/proto/drive_event.pb.h"
#include "modules/common/proto/pnc_point.pb.h"
#include "modules/control/proto/control_cmd.pb.h"
#include "modules/dreamview/proto/simulation_world.pb.h"
#include "modules/localization/proto/gps.pb.h"
#include "modules/localization/proto/localization.pb.h"
#include "modules/perception/proto/traffic_light_detection.pb.h"
#include "modules/planning/proto/planning.pb.h"
#include "modules/planning/proto/planning_internal.pb.h"
#include "modules/prediction/proto/prediction_obstacle.pb.h"
#include "modules/storytelling/proto/story.pb.h"
#include "modules/task_manager/proto/task_manager.pb.h"

#include "cyber/common/log.h"
#include "modules/common/monitor_log/monitor_log_buffer.h"
#include "modules/dreamview/backend/map/map_service.h"

namespace apollo {
namespace dreamview {

class SimulationWorldService {
 public:
  // The maximum number of monitor message items to be kept in
  // SimulationWorld.
  static constexpr int kMaxMonitorItems = 30;

  SimulationWorldService(const MapService *map_service,
                         bool routing_from_file = false);

  inline const SimulationWorld &world() const { return world_; }

  nlohmann::json GetUpdateAsJson(double radius) const;

  void GetWireFormatString(double radius, std::string *sim_world,
                           std::string *sim_world_with_planning_data);

  nlohmann::json GetMapElements(double radius) const;

  void Update();

  void SetToClear() { to_clear_ = true; }

  bool ReadyToPush() const { return ready_to_push_.load(); }

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

  void PublishNavigationInfo(
      const std::shared_ptr<apollo::relative_map::NavigationInfo> &);
  void PublishRoutingRequest(
      const std::shared_ptr<apollo::routing::RoutingRequest> &);

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

  void GetMapElementIds(double radius, MapElementIds *ids) const;

  const apollo::hdmap::Map &GetRelativeMap() const;

  nlohmann::json GetRoutePathAsJson() const;

  void DumpMessages();

 private:
  void InitReaders();
  void InitWriters();

  template <typename DataType>
  void UpdateSimulationWorld(const DataType &data);

  void UpdateMonitorMessages();

  Object &CreateWorldObjectIfAbsent(
      const apollo::perception::PerceptionObstacle &obstacle);
  void CreateWorldObjectFromSensorMeasurement(
      const apollo::perception::SensorMeasurement &sensor,
      Object *world_object);
  void SetObstacleInfo(const apollo::perception::PerceptionObstacle &obstacle,
                       Object *world_object);
  void SetObstaclePolygon(
      const apollo::perception::PerceptionObstacle &obstacle,
      Object *world_object);
  void SetObstacleSensorMeasurements(
      const apollo::perception::PerceptionObstacle &obstacle,
      Object *world_object);
  void SetObstacleSource(const apollo::perception::PerceptionObstacle &obstacle,
                         Object *world_object);
  void UpdatePlanningTrajectory(
      const apollo::planning::ADCTrajectory &trajectory);
  void UpdateRSSInfo(const apollo::planning::ADCTrajectory &trajectory);
  bool LocateMarker(const apollo::planning::ObjectDecisionType &decision,
                    Decision *world_decision);
  void FindNudgeRegion(const apollo::planning::ObjectDecisionType &decision,
                       const Object &world_obj, Decision *world_decision);
  void UpdateDecision(const apollo::planning::DecisionResult &decision_res,
                      double header_time);
  void UpdateMainStopDecision(
      const apollo::planning::MainDecision &main_decision,
      double update_timestamp_sec, Object *world_main_stop);

  template <typename MainDecision>
  void UpdateMainChangeLaneDecision(const MainDecision &decision,
                                    Object *world_main_decision) {
    if (decision.has_change_lane_type() &&
        (decision.change_lane_type() == apollo::routing::ChangeLaneType::LEFT ||
         decision.change_lane_type() ==
             apollo::routing::ChangeLaneType::RIGHT)) {
      auto *change_lane_decision = world_main_decision->add_decision();
      change_lane_decision->set_change_lane_type(decision.change_lane_type());

      const auto &adc = world_.auto_driving_car();
      change_lane_decision->set_position_x(adc.position_x());
      change_lane_decision->set_position_y(adc.position_y());
      change_lane_decision->set_heading(adc.heading());
    }
  }

  void CreatePredictionTrajectory(
      const apollo::prediction::PredictionObstacle &obstacle,
      Object *world_object);

  void DownsamplePath(const apollo::common::Path &paths,
                      apollo::common::Path *downsampled_path);

  void UpdatePlanningData(const apollo::planning_internal::PlanningData &data);

  void PopulateMapInfo(double radius);

  template <typename MessageT>
  void UpdateWithLatestObserved(cyber::Reader<MessageT> *reader,
                                bool logging = true) {
    if (reader->Empty()) {
      if (logging) {
        AINFO_EVERY(100) << "Has not received any data from "
                         << reader->GetChannelName();
      }
      return;
    }

    const std::shared_ptr<MessageT> msg = reader->GetLatestObserved();
    UpdateSimulationWorld(*msg);
  }

  template <typename MessageT>
  void DumpMessageFromReader(cyber::Reader<MessageT> *reader) {
    if (reader->Empty()) {
      AWARN << "Has not received any data from " << reader->GetChannelName()
            << ". Cannot dump message!";
      return;
    }

    apollo::common::util::DumpMessage(reader->GetLatestObserved());
  }

  void ReadRoutingFromFile(const std::string &routing_response_file);

  template <typename MessageT>
  void UpdateLatency(const std::string &module_name,
                     cyber::Reader<MessageT> *reader) {
    if (reader->Empty()) {
      return;
    }

    const auto header = reader->GetLatestObserved()->header();
    const double publish_time_sec = header.timestamp_sec();
    const double sensor_time_sec =
        apollo::cyber::Time(
            std::max({header.lidar_timestamp(), header.camera_timestamp(),
                      header.radar_timestamp()}))
            .ToSecond();

    Latency latency;
    latency.set_timestamp_sec(publish_time_sec);
    latency.set_total_time_ms((publish_time_sec - sensor_time_sec) * 1.0e3);
    (*world_.mutable_latency())[module_name] = latency;
  }

  void UpdateDelays();

  void UpdateLatencies();

  template <typename Points>
  void DownsampleSpeedPointsByInterval(const Points &points,
                                       size_t downsampleInterval,
                                       Points *downsampled_points) {
    if (points.empty()) {
      return;
    }

    for (int i = 0; i + 1 < points.size(); i += downsampleInterval) {
      *downsampled_points->Add() = points[i];
    }

    // add the last point
    *downsampled_points->Add() = *points.rbegin();
  }

  std::unique_ptr<cyber::Node> node_;

  // The underlying SimulationWorld object, owned by the
  // SimulationWorldService instance.
  SimulationWorld world_;

  // Downsampled route paths to be rendered in frontend.
  mutable boost::shared_mutex route_paths_mutex_;
  std::vector<RoutePath> route_paths_;

  // The handle of MapService, not owned by SimulationWorldService.
  const MapService *map_service_;

  // The map holding obstacle string id to the actual object
  std::unordered_map<std::string, Object> obj_map_;

  // A temporary cache for all the monitor messages coming in.
  std::mutex monitor_msgs_mutex_;
  std::list<std::shared_ptr<common::monitor::MonitorMessage>> monitor_msgs_;

  // The SIMULATOR monitor for publishing messages.
  apollo::common::monitor::MonitorLogBuffer monitor_logger_buffer_;

  // Whether to clear the SimulationWorld in the next timer cycle, set by
  // frontend request.
  bool to_clear_ = false;

  // Relative map used/retrieved in navigation mode
  apollo::hdmap::Map relative_map_;

  // Whether the sim_world is ready to push to frontend
  std::atomic<bool> ready_to_push_;

  // Latest rss info
  double current_real_dist_ = 0.0;
  double current_rss_safe_dist_ = 0.0;

  // Gear Location
  apollo::canbus::Chassis_GearPosition gear_location_;

  // Readers.
  std::shared_ptr<cyber::Reader<apollo::canbus::Chassis>> chassis_reader_;
  std::shared_ptr<cyber::Reader<apollo::localization::Gps>> gps_reader_;
  std::shared_ptr<cyber::Reader<apollo::localization::LocalizationEstimate>>
      localization_reader_;
  std::shared_ptr<cyber::Reader<apollo::perception::PerceptionObstacles>>
      perception_obstacle_reader_;
  std::shared_ptr<cyber::Reader<apollo::perception::TrafficLightDetection>>
      perception_traffic_light_reader_;
  std::shared_ptr<cyber::Reader<apollo::prediction::PredictionObstacles>>
      prediction_obstacle_reader_;
  std::shared_ptr<cyber::Reader<apollo::planning::ADCTrajectory>>
      planning_reader_;
  std::shared_ptr<cyber::Reader<apollo::control::ControlCommand>>
      control_command_reader_;
  std::shared_ptr<cyber::Reader<apollo::relative_map::NavigationInfo>>
      navigation_reader_;
  std::shared_ptr<cyber::Reader<apollo::relative_map::MapMsg>>
      relative_map_reader_;
  std::shared_ptr<cyber::Reader<apollo::audio::AudioEvent>> audio_event_reader_;
  std::shared_ptr<cyber::Reader<apollo::common::DriveEvent>>
      drive_event_reader_;
  std::shared_ptr<cyber::Reader<apollo::common::monitor::MonitorMessage>>
      monitor_reader_;
  std::shared_ptr<cyber::Reader<apollo::routing::RoutingRequest>>
      routing_request_reader_;
  std::shared_ptr<cyber::Reader<apollo::routing::RoutingResponse>>
      routing_response_reader_;
  std::shared_ptr<cyber::Reader<apollo::storytelling::Stories>>
      storytelling_reader_;
  std::shared_ptr<cyber::Reader<apollo::audio::AudioDetection>>
      audio_detection_reader_;
  std::shared_ptr<cyber::Reader<apollo::task_manager::Task>> task_reader_;

  // Writers.
  std::shared_ptr<cyber::Writer<apollo::relative_map::NavigationInfo>>
      navigation_writer_;
  std::shared_ptr<cyber::Writer<apollo::routing::RoutingRequest>>
      routing_request_writer_;
  std::shared_ptr<cyber::Writer<apollo::routing::RoutingResponse>>
      routing_response_writer_;
  std::shared_ptr<cyber::Writer<apollo::task_manager::Task>> task_writer_;

  FRIEND_TEST(SimulationWorldServiceTest, UpdateMonitorSuccess);
  FRIEND_TEST(SimulationWorldServiceTest, UpdateMonitorRemove);
  FRIEND_TEST(SimulationWorldServiceTest, UpdateMonitorTruncate);
  FRIEND_TEST(SimulationWorldServiceTest, UpdateChassisInfo);
  FRIEND_TEST(SimulationWorldServiceTest, UpdateLatency);
  FRIEND_TEST(SimulationWorldServiceTest, UpdateLocalization);
  FRIEND_TEST(SimulationWorldServiceTest, UpdatePerceptionObstacles);
  FRIEND_TEST(SimulationWorldServiceTest, UpdatePlanningTrajectory);
  FRIEND_TEST(SimulationWorldServiceTest, UpdateDecision);
  FRIEND_TEST(SimulationWorldServiceTest, UpdatePrediction);
  FRIEND_TEST(SimulationWorldServiceTest, UpdateRouting);
  FRIEND_TEST(SimulationWorldServiceTest, UpdateGps);
  FRIEND_TEST(SimulationWorldServiceTest, UpdateControlCommandWithSimpleLonLat);
  FRIEND_TEST(SimulationWorldServiceTest, UpdateControlCommandWithSimpleMpc);
  FRIEND_TEST(SimulationWorldServiceTest, DownsampleSpeedPointsByInterval);
};

}  // namespace dreamview
}  // namespace apollo