visualization_manager.h

浏览该文件的文档.

/******************************************************************************
 * 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 <atomic>
#include <list>
#include <map>
#include <string>
#include <thread>
#include <utility>
#include <vector>
 
#include "modules/common/util/eigen_defs.h"
#include "modules/localization/msf/local_tool/local_visualization/engine/visualization_engine.h"
 
namespace apollo {
namespace localization {
namespace msf {
 
struct LidarVisFrame {
  unsigned int frame_id;
  double timestamp;
  ::apollo::common::EigenVector3dVec pt3ds;
};
 
struct LocalizationMsg {
  double timestamp = 0.0;
  double x = 0;
  double y = 0;
  double z = 0;
 
  double qx = 0.0;
  double qy = 0.0;
  double qz = 0.0;
  double qw = 0.0;
 
  double std_x = 0;
  double std_y = 0;
  double std_z = 0;
 
  LocalizationMsg interpolate(const double scale,
                              const LocalizationMsg &loc_msg) {
    LocalizationMsg res;
    res.x = this->x * (1 - scale) + loc_msg.x * scale;
    res.y = this->y * (1 - scale) + loc_msg.y * scale;
    res.z = this->z * (1 - scale) + loc_msg.z * scale;
 
    Eigen::Quaterniond quatd1(this->qw, this->qx, this->qy, this->qz);
    Eigen::Quaterniond quatd2(loc_msg.qw, loc_msg.qx, loc_msg.qy, loc_msg.qz);
    Eigen::Quaterniond res_quatd = quatd1.slerp(scale, quatd2);
    res.qx = res_quatd.x();
    res.qy = res_quatd.y();
    res.qz = res_quatd.z();
    res.qw = res_quatd.w();
 
    res.std_x = this->std_x * (1 - scale) + loc_msg.std_x * scale;
    res.std_y = this->std_y * (1 - scale) + loc_msg.std_y * scale;
    res.std_z = this->std_z * (1 - scale) + loc_msg.std_z * scale;
 
    return res;
  }
};
 
template <class MessageType>
class MessageBuffer {
 public:
  typedef
      typename std::list<std::pair<double, MessageType>>::iterator ListIterator;
 
 public:
  explicit MessageBuffer(int capacity);
  ~MessageBuffer();
 
  bool PushNewMessage(const double timestamp, const MessageType &msg);
  bool PopOldestMessage(MessageType *msg);
  bool GetMessageBefore(const double timestamp, MessageType *msg);
  bool GetMessage(const double timestamp, MessageType *msg);
 
  void Clear();
 
  void SetCapacity(const unsigned int capacity);
  void GetAllMessages(std::list<std::pair<double, MessageType>> *msg_list);
 
  bool IsEmpty();
  unsigned int BufferSize();
 
 protected:
  std::map<double, ListIterator> msg_map_;
  std::list<std::pair<double, MessageType>> msg_list_;
 
 protected:
  pthread_mutex_t buffer_mutex_;
  unsigned int capacity_;
};
 
template <class MessageType>
class IntepolationMessageBuffer : public MessageBuffer<MessageType> {
 public:
  typedef
      typename std::list<std::pair<double, MessageType>>::iterator ListIterator;
 
 public:
  explicit IntepolationMessageBuffer(int capacity);
  ~IntepolationMessageBuffer();
 
  bool QueryMessage(const double timestamp, MessageType *msg,
                    double timeout_s = 0.01);
 
 private:
  bool WaitMessageBufferOk(const double timestamp,
                           std::map<double, ListIterator> *msg_map,
                           std::list<std::pair<double, MessageType>> *msg_list,
                           double timeout_ms);
};
 
struct VisualizationManagerParams {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  std::string map_folder;
  std::string map_visual_folder;
  Eigen::Affine3d velodyne_extrinsic;
  VisualMapParam map_param;
  unsigned int lidar_frame_buffer_capacity;
  unsigned int gnss_loc_info_buffer_capacity;
  unsigned int lidar_loc_info_buffer_capacity;
  unsigned int fusion_loc_info_buffer_capacity;
};
 
class VisualizationManager {
#define LOC_INFO_NUM 3
 
 public:
  VisualizationManager();
  ~VisualizationManager();
 
  static VisualizationManager &GetInstance() {
    static VisualizationManager visual_manage;
    return visual_manage;
  }
 
  bool Init(const std::string &map_folder, const std::string &map_visual_folder,
            const Eigen::Affine3d &velodyne_extrinsic,
            const VisualMapParam &map_param);
  bool Init(const VisualizationManagerParams &params);
 
  void AddLidarFrame(const LidarVisFrame &lidar_frame);
  void AddGNSSLocMessage(const LocalizationMsg &gnss_loc_msg);
  void AddLidarLocMessage(const LocalizationMsg &lidar_loc_msg);
  void AddFusionLocMessage(const LocalizationMsg &fusion_loc_msg);
  void StartVisualization();
  void StopVisualization();
 
 private:
  void DoVisualize();
  bool GetZoneIdFromMapFolder(const std::string &map_folder,
                              const unsigned int resolution_id, int *zone_id);
 
 private:
  VisualizationEngine visual_engine_;
  // Visualization Thread
  std::thread visual_thread_;
  std::atomic<bool> stop_visualization_;
 
  MessageBuffer<LidarVisFrame> lidar_frame_buffer_;
  IntepolationMessageBuffer<LocalizationMsg> gnss_loc_info_buffer_;
  IntepolationMessageBuffer<LocalizationMsg> lidar_loc_info_buffer_;
  IntepolationMessageBuffer<LocalizationMsg> fusion_loc_info_buffer_;
};
 
}  // namespace msf
}  // namespace localization
}  // namespace apollo