first ten files fixed

modules/map/tools/map_datachecker/server/alignment.h

+/******************************************************************************
+ * Copyright 2019 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 <memory>
+#include <vector>
+
+#include "modules/map/tools/map_datachecker/proto/collection_error_code.pb.h"
+#include "modules/map/tools/map_datachecker/server/common.h"
+
+namespace apollo {
+namespace hdmap {
+
+typedef struct BadOrGoodPoseInfo {
+  BadOrGoodPoseInfo() : start_time(-1.0), end_time(-1.0), pose_count(0) {}
+  double start_time;
+  double end_time;
+  int pose_count;
+} BadOrGoodPoseInfo;
+
+class Alignment {
+ public:
+  explicit Alignment(std::shared_ptr<JSonConf> sp_conf)
+      : return_state_(ErrorCode::SUCCESS),
+        sp_conf_(sp_conf),
+        sp_good_pose_info_(std::make_shared<BadOrGoodPoseInfo>()),
+        sp_bad_pose_info_(std::make_shared<BadOrGoodPoseInfo>()) {}
+
+  virtual ~Alignment() {}
+  virtual ErrorCode Process(const std::vector<FramePose>& poses) = 0;
+  virtual void Reset() = 0;
+
+  virtual double GetProgress() const { return progress_; }
+
+  virtual void SetStartTime(double start_time) { start_time_ = start_time; }
+
+  virtual void SetEndTime(double end_time) { end_time_ = end_time; }
+
+  virtual void UpdateBadPoseInfo(const FramePose& pose) {
+    UpdatePoseInfo(pose, sp_bad_pose_info_);
+  }
+
+  virtual void ClearBadPoseInfo() { ClearPoseInfo(sp_bad_pose_info_); }
+
+  virtual void UpdateGoodPoseInfo(const FramePose& pose) {
+    UpdatePoseInfo(pose, sp_good_pose_info_);
+  }
+
+  virtual void ClearGoodPoseInfo() { ClearPoseInfo(sp_good_pose_info_); }
+
+  virtual bool IsGoodPose(const std::vector<FramePose>& poses,
+                            int pose_index) {
+    if (pose_index <= 0 || pose_index >= static_cast<int>(poses.size())) {
+      AINFO << "params error. poses size:" << poses.size()
+            << ",pose_index:" << pose_index;
+      return false;
+    }
+
+    unsigned int position_type = poses[pose_index].position_type;
+    float diff_age = poses[pose_index].diff_age;
+    double local_std = poses[pose_index].local_std;
+
+    if (sp_conf_->position_type_range.find(position_type) !=
+            sp_conf_->position_type_range.end() &&
+        diff_age >= sp_conf_->diff_age_range.first &&
+        diff_age <= sp_conf_->diff_age_range.second &&
+        local_std <= sp_conf_->local_std_upper_limit) {
+      return true;
+    }
+    return false;
+  }
+
+  ErrorCode GetReturnState() const { return return_state_; }
+
+ protected:
+  void UpdatePoseInfo(const FramePose& pose,
+                        std::shared_ptr<BadOrGoodPoseInfo> sp_pose_info) {
+    BadOrGoodPoseInfo& pose_info = *sp_pose_info;
+    if (pose_info.pose_count == 0) {
+      pose_info.start_time = pose.time_stamp;
+      ++pose_info.pose_count;
+      AINFO << "update start time: " << std::to_string(pose_info.start_time)
+            << ",pose count: " << pose_info.pose_count;
+    } else {
+      pose_info.end_time = pose.time_stamp;
+      ++pose_info.pose_count;
+      AINFO << "update start time: " << std::to_string(pose_info.start_time)
+            << ",pose count: " << pose_info.pose_count;
+    }
+  }
+
+  void ClearPoseInfo(std::shared_ptr<BadOrGoodPoseInfo> sp_pose_info) {
+    BadOrGoodPoseInfo& pose_info = *sp_pose_info;
+    pose_info.start_time = -1.0;
+    pose_info.end_time = -1.0;
+    pose_info.pose_count = 0;
+  }
+
+  int TimeToIndex(const std::vector<FramePose>& poses, double time) {
+    size_t size = poses.size();
+    if (size == 0 || time <= 0) {
+      return -1;
+    }
+
+    for (size_t i = 0; i < size; ++i) {
+      if (poses[i].time_stamp >= time) {
+        return static_cast<int>(i);
+      }
+    }
+    return static_cast<int>(size);
+  }
+
+ protected:
+  double progress_;
+  double last_progress_;
+  double start_time_;
+  double end_time_;
+  int start_index_;
+  double end_index_;
+  ErrorCode return_state_;
+  std::shared_ptr<JSonConf> sp_conf_ = nullptr;
+  // BadOrGoodPoseInfo _bad_pose_info, _good_pose_info;
+  std::shared_ptr<BadOrGoodPoseInfo> sp_good_pose_info_ = nullptr;
+  std::shared_ptr<BadOrGoodPoseInfo> sp_bad_pose_info_ = nullptr;
+};
+
+}  // namespace hdmap
+}  // namespace apollo

modules/map/tools/map_datachecker/server/alignment_agent.h

+/******************************************************************************
+ * Copyright 2019 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 <grpc++/grpc++.h>
+#include <chrono>
+#include <memory>
+#include <thread>
+#include <vector>
+
+#include "modules/map/tools/map_datachecker/proto/collection_error_code.pb.h"
+#include "modules/map/tools/map_datachecker/proto/collection_service.pb.h"
+#include "modules/map/tools/map_datachecker/server/eight_route.h"
+#include "modules/map/tools/map_datachecker/server/pose_collection_agent.h"
+#include "modules/map/tools/map_datachecker/server/static_align.h"
+
+namespace apollo {
+namespace hdmap {
+
+typedef State AlignmentAgentState;
+
+template <typename ALIGNMENT_TYPE, typename REQUEST_TYPE,
+          typename RESPONSE_TYPE>
+class AlignmentAgent {
+ public:
+  AlignmentAgent(
+      std::shared_ptr<JSonConf> sp_conf,
+      std::shared_ptr<PoseCollectionAgent> sp_pose_collection_agent) {
+    sp_conf_ = sp_conf;
+    sp_pose_collection_agent_ = sp_pose_collection_agent;
+  }
+
+  void Reset() {
+    sp_alignment_ = std::make_shared<ALIGNMENT_TYPE>(sp_conf_);
+    state_ = AlignmentAgentState::IDLE;
+    need_stop_ = false;
+  }
+
+  grpc::Status ProcessGrpcRequest(grpc::ServerContext *context,
+                                    REQUEST_TYPE *request,
+                                    RESPONSE_TYPE *response) {
+    AINFO << "AlignmentAgent request: " << request->DebugString();
+    switch (request->cmd()) {
+      case CmdType::START:
+        AINFO << "AlignmentAgent start";
+        AlignmentStart(request, response);
+        break;
+      case CmdType::CHECK:
+        AINFO << "AlignmentAgent check";
+        AlignmentCheck(request, response);
+        break;
+      case CmdType::STOP:
+        AINFO << "AlignmentAgent stop";
+        AlignmentStop(request, response);
+        break;
+      default:
+        response->set_code(ErrorCode::ERROR_REQUEST);
+        response->set_progress(sp_alignment_->GetProgress());
+        AERROR << "command error";
+    }
+    AINFO << "AlignmentAgent progress: " << response->progress();
+    return grpc::Status::OK;
+  }
+
+  int AlignmentStart(REQUEST_TYPE *request, RESPONSE_TYPE *response) {
+    if (AlignmentAgentState::RUNNING == GetState()) {
+      AINFO << "AlignmentAgent is running. do need start again";
+      response->set_code(ErrorCode::ERROR_REPEATED_START);
+      response->set_progress(0.0);
+      return 0;
+    }
+    Reset();
+    AsyncStartAlignment();
+    response->set_code(ErrorCode::SUCCESS);
+    response->set_progress(0.0);
+    return 0;
+  }
+
+  int AsyncStartAlignment() {
+    SetState(AlignmentAgentState::RUNNING);
+    std::thread alignment_thread([=]() {
+      sp_alignment_->SetStartTime(UnixtimeNow());
+      AINFO << "set state RUNNING";
+      while (!need_stop_ && !apollo::cyber::IsShutdown()) {
+        std::shared_ptr<std::vector<FramePose>> sp_poses = GetPoses();
+        if (sp_poses == nullptr) {
+          AINFO << "error, pose pointer is null";
+          return;
+        }
+        sp_alignment_->Process(*sp_poses);
+        ErrorCode code = sp_alignment_->GetReturnState();
+        if (code == ErrorCode::ERROR_VERIFY_NO_GNSSPOS ||
+            code == ErrorCode::ERROR_GNSS_SIGNAL_FAIL) {
+          AERROR << "Some error occured, while loop will exit";
+          break;
+        }
+        AINFO << "get progress:" << sp_alignment_->GetProgress();
+        if (fabs(1 - sp_alignment_->GetProgress()) < 1e-8) {
+          AINFO << "alignment progress reached 1.0, thread exit";
+          break;
+        }
+        AINFO << "sleep " << sp_conf_->alignment_featch_pose_sleep << " sec";
+        auto seconds =
+            std::chrono::seconds(sp_conf_->alignment_featch_pose_sleep);
+        std::this_thread::sleep_for(seconds);
+      }
+      stopped_ = true;
+      AINFO << "Align thread complete";
+    });
+    alignment_thread.detach();
+    return 0;
+  }
+
+  std::shared_ptr<std::vector<FramePose>> GetPoses() const {
+    if (sp_pose_collection_agent_ == nullptr) {
+      return nullptr;
+    }
+    return sp_pose_collection_agent_->GetPoses();
+  }
+
+  int AlignmentCheck(REQUEST_TYPE *request, RESPONSE_TYPE *response) {
+    if (AlignmentAgentState::IDLE == GetState()) {
+      AINFO << "AlignmentAgent is idle. this call will be refused";
+      response->set_code(ErrorCode::ERROR_CHECK_BEFORE_START);
+      response->set_progress(0.0);
+      return 0;
+    }
+    if (sp_alignment_ == nullptr) {
+      AINFO << "sp_alignment_ is null, check later";
+      response->set_code(ErrorCode::SUCCESS);
+      response->set_progress(0.0);
+      return 0;
+    }
+
+    ErrorCode code = sp_alignment_->GetReturnState();
+    double progress = sp_alignment_->GetProgress();
+    response->set_code(code);
+    response->set_progress(progress);
+    if (code == ErrorCode::ERROR_VERIFY_NO_GNSSPOS ||
+        code == ErrorCode::ERROR_GNSS_SIGNAL_FAIL) {
+      stopped_ = true;
+      SetState(AlignmentAgentState::IDLE);
+      return -1;
+    }
+    return 0;
+  }
+
+  int AlignmentStop(REQUEST_TYPE *request, RESPONSE_TYPE *response) {
+    response->set_code(ErrorCode::SUCCESS);
+    if (sp_alignment_ == nullptr) {
+      response->set_progress(0.0);
+    } else {
+      response->set_progress(sp_alignment_->GetProgress());
+      need_stop_ = true;
+    }
+    SetState(AlignmentAgentState::IDLE);
+    return 0;
+  }
+
+  void SetState(AlignmentAgentState state) { state_ = state; }
+  AlignmentAgentState GetState() const { return state_; }
+
+ private:
+  std::shared_ptr<JSonConf> sp_conf_ = nullptr;
+  std::shared_ptr<ALIGNMENT_TYPE> sp_alignment_ = nullptr;
+  std::shared_ptr<PoseCollectionAgent> sp_pose_collection_agent_ = nullptr;
+  AlignmentAgentState state_;
+  bool need_stop_;
+  bool stopped_;
+};
+
+}  // namespace hdmap
+}  // namespace apollo

modules/map/tools/map_datachecker/server/channel_verify.cc

+/******************************************************************************
+ * Copyright 2019 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.
+ *****************************************************************************/
+#include "modules/map/tools/map_datachecker/server/channel_verify.h"
+
+#include <boost/algorithm/string/classification.hpp>
+#include <boost/algorithm/string/split.hpp>
+#include <boost/filesystem.hpp>
+#include <unordered_map>
+
+#include "cyber/cyber.h"
+#include "cyber/proto/record.pb.h"
+#include "cyber/record/record_viewer.h"
+
+namespace apollo {
+namespace hdmap {
+
+ChannelVerify::ChannelVerify(std::shared_ptr<JSonConf> sp_conf)
+    : sp_conf_(sp_conf) {
+  Reset();
+}
+
+void ChannelVerify::Reset() {
+  return_state_ = ErrorCode::SUCCESS;
+  checked_records_.clear();
+  sp_vec_check_result_ =
+      std::make_shared<std::vector<OneRecordChannelCheckResult>>();
+}
+
+ErrorCode ChannelVerify::Check(
+    const std::string& record_dir_or_record_full_path) {
+  std::vector<std::string> records_path;
+  records_path = GetRecordsPath(record_dir_or_record_full_path);
+  if (records_path.size() == 0) {
+    AINFO << "have no data file to check";
+    return_state_ = ErrorCode::ERROR_VERIFY_NO_RECORDERS;
+    return return_state_;
+  }
+  IncrementalCheck(records_path);
+  return_state_ = ErrorCode::SUCCESS;
+  return return_state_;
+}
+
+std::shared_ptr<std::vector<OneRecordChannelCheckResult>>
+ChannelVerify::get_check_result() const {
+  return sp_vec_check_result_;
+}
+
+int ChannelVerify::IncrementalCheck(
+    const std::vector<std::string>& records_path) {
+
+  std::vector<std::string> not_check_records_path;
+  AINFO << "all records path:";
+  for (size_t i = 0; i < records_path.size(); ++i) {
+    AINFO << "[" << i << "]: " << records_path[i];
+    if (IsRecordFile(records_path[i]) &&
+        !IsRecordChecked(records_path[i])) {
+      not_check_records_path.push_back(records_path[i]);
+    }
+  }
+
+  AINFO << "not_check_records_path:";
+  for (size_t i = 0; i < not_check_records_path.size(); ++i) {
+    AINFO << "[" << i << "]: " << not_check_records_path[i];
+    OneRecordChannelCheckResult check_result =
+        CheckRecordChannels(not_check_records_path[i]);
+    if (check_result.record_path.empty()) {
+      continue;
+    }
+    sp_vec_check_result_->push_back(check_result);
+  }
+
+  return 0;
+}
+
+bool ChannelVerify::IsRecordFile(const std::string& record_path) const {
+  if (!boost::filesystem::exists(record_path)) {
+    AINFO << "path [" << record_path << "] does not exist";
+    return false;
+  }
+  if (!boost::filesystem::is_regular_file(record_path)) {
+    AINFO << "path [" << record_path << "] is not a regular file";
+    return false;
+  }
+  // To avoid disk overhead caused by opening files twice, the real
+  // file checking is placed in the function [ChannelVerify::get_record_info]
+  return true;
+}
+
+std::vector<std::string> ChannelVerify::GetRecordsPath(
+    const std::string& record_dir_or_record_full_path) const {
+  // record_dir_or_record_full_path is record fullpath or
+  // directory which contains some records
+  std::vector<std::string> records_path;
+  // 1. check record_dir_or_record_full_path is valid or not
+  boost::filesystem::path path(record_dir_or_record_full_path);
+  if (!boost::filesystem::exists(path)) {
+    AINFO << "record path [" << record_dir_or_record_full_path
+          << "] does not exist";
+    return records_path;
+  }
+
+  if (IsRecordFile(record_dir_or_record_full_path)) {
+    records_path.push_back(record_dir_or_record_full_path);
+  } else if (boost::filesystem::is_directory(path)) {
+    using dit_t = boost::filesystem::directory_iterator;
+    dit_t end;
+    for (dit_t it(record_dir_or_record_full_path); it != end; ++it) {
+      if (IsRecordFile(it->path().string())) {
+        records_path.push_back(it->path().string());
+      }
+    }
+  }
+  return records_path;
+}
+
+bool ChannelVerify::IsRecordChecked(const std::string& record_path) {
+  return !checked_records_.insert(record_path).second;
+}
+
+std::shared_ptr<CyberRecordInfo> ChannelVerify::GetRecordInfo(
+    const std::string& record_path) const {
+  if (!IsRecordFile(record_path)) {
+    AINFO << "get_record_info failed.[" << record_path
+          << "] is not record file";
+    return nullptr;
+  }
+  std::shared_ptr<CyberRecordInfo> sp_record_info(new CyberRecordInfo);
+  std::shared_ptr<apollo::cyber::record::RecordReader> sp_reader =
+      std::make_shared<apollo::cyber::record::RecordReader>(record_path);
+  if (sp_reader == nullptr || !sp_reader->IsValid()) {
+    AINFO << "open record [" << record_path << "] failed";
+    return nullptr;
+  }
+  std::shared_ptr<apollo::cyber::record::RecordViewer> sp_viewer(
+      new apollo::cyber::record::RecordViewer(sp_reader));
+  sp_record_info->path = record_path;
+  sp_record_info->start_time = sp_viewer->begin_time();
+  sp_record_info->end_time = sp_viewer->end_time();
+  sp_record_info->duration =
+      static_cast<double>((sp_viewer->end_time() - sp_viewer->begin_time())) /
+      1e9;
+
+  std::set<std::string> channel_list = sp_reader->GetChannelList();
+  for (auto it = channel_list.begin(); it != channel_list.end(); ++it) {
+    const std::string& channel_name = *it;
+    CyberRecordChannel channel;
+    channel.channel_name = channel_name;
+    channel.msgnum = sp_reader->GetMessageNumber(channel_name);
+    channel.msg_type = sp_reader->GetMessageType(channel_name);
+    sp_record_info->channels.push_back(channel);
+  }
+  return sp_record_info;
+}
+
+OneRecordChannelCheckResult ChannelVerify::CheckRecordChannels(
+    const std::string& record_path) {
+  OneRecordChannelCheckResult check_result;
+  std::shared_ptr<CyberRecordInfo> sp_record_info =
+      GetRecordInfo(record_path);
+  if (sp_record_info == nullptr) {
+    return check_result;
+  }
+  std::vector<CyberRecordChannel>& channels = sp_record_info->channels;
+  std::vector<std::pair<std::string, double>>& topic_list =
+      sp_conf_->topic_list;
+  check_result.record_path = record_path;
+  check_result.start_time = sp_record_info->start_time;
+  for (size_t i = 0; i < topic_list.size(); ++i) {
+    std::string& channel_in_list = topic_list[i].first;
+    double channel_expected_rate = topic_list[i].second;
+    bool channel_in_list_found = false;
+    size_t j = 0;
+    for (j = 0; j < channels.size(); ++j) {
+      std::string& channel_in_record = channels[j].channel_name;
+      if (channel_in_record == channel_in_list) {
+        channel_in_list_found = true;
+        break;
+      }
+    }
+    if (!channel_in_list_found) {  // topic
+      AINFO << record_path << " lacks [" << channel_in_list << "]";
+      check_result.lack_channels.push_back(channel_in_list);
+    } else {  // rate
+      double actual_rate =
+          static_cast<double>((channels[j].msgnum)) / sp_record_info->duration;
+      if (actual_rate < 1e-8) {
+        actual_rate = 0.0;
+        AINFO << "msgnum:" << channels[j].msgnum
+              << ",duration:" << sp_record_info->duration;
+      }
+      AINFO << record_path << " [" << channel_in_list
+            << "] expected rate: " << channel_expected_rate
+            << ", actual rate: " << actual_rate;
+      if (actual_rate <
+          channel_expected_rate * sp_conf_->topic_rate_tolerance) {
+        check_result.inadequate_rate[channel_in_list] =
+            std::make_pair(channel_expected_rate, actual_rate);
+      }
+    }
+  }
+  return check_result;
+}
+
+ErrorCode ChannelVerify::GetReturnState() const { return return_state_; }
+
+}  // namespace hdmap
+}  // namespace apollo

modules/map/tools/map_datachecker/server/channel_verify.h

+/******************************************************************************
+ * Copyright 2019 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 <map>
+#include <memory>
+#include <set>
+#include <string>
+#include <utility>
+#include <vector>
+
+#include "modules/map/tools/map_datachecker/proto/collection_error_code.pb.h"
+#include "modules/map/tools/map_datachecker/server/common.h"
+
+namespace apollo {
+namespace hdmap {
+
+struct CyberRecordChannel {
+  std::string channel_name;
+  uint64_t msgnum;
+  std::string msg_type;
+};
+
+struct CyberRecordInfo {
+  std::string path;
+  double duration;  // the unit is seconds
+  uint64_t start_time;
+  uint64_t end_time;
+  std::vector<CyberRecordChannel> channels;
+};
+
+struct OneRecordChannelCheckResult {
+  std::string record_path;
+  uint64_t start_time;
+  std::vector<std::string> lack_channels;
+  // inadequate_rate: channel_name <---> (expected_rate, actual_rate)
+  std::map<std::string, std::pair<double, double>> inadequate_rate;
+};
+
+typedef std::shared_ptr<std::vector<OneRecordChannelCheckResult>> CheckedResult;
+typedef std::vector<OneRecordChannelCheckResult>::iterator CheckResultIterator;
+
+class ChannelVerify {
+ public:
+  explicit ChannelVerify(std::shared_ptr<JSonConf> sp_conf);
+  ErrorCode Check(const std::string& record_dir_or_record_full_path);
+  std::shared_ptr<std::vector<OneRecordChannelCheckResult>> get_check_result()
+      const;
+  ErrorCode GetReturnState() const;
+
+ private:
+  bool IsRecordFile(const std::string& path) const;
+  std::shared_ptr<CyberRecordInfo> GetRecordInfo(
+      const std::string& record_path) const;
+  int IncrementalCheck(const std::vector<std::string>& records_path);
+  std::vector<std::string> GetRecordsPath(
+      const std::string& record_dir_or_record_full_path) const;
+  bool IsRecordChecked(const std::string& record_path);
+  OneRecordChannelCheckResult CheckRecordChannels(
+      const std::string& record_path);
+  void Reset();
+
+ private:
+  std::shared_ptr<JSonConf> sp_conf_ = nullptr;
+  CheckedResult sp_vec_check_result_ = nullptr;
+  ErrorCode return_state_;
+  std::set<std::string> checked_records_;
+};
+
+}  // namespace hdmap
+}  // namespace apollo

modules/map/tools/map_datachecker/server/channel_verify_agent.cc

+/******************************************************************************
+ * Copyright 2019 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.
+ *****************************************************************************/
+#include "modules/map/tools/map_datachecker/server/channel_verify_agent.h"
+
+#include <chrono>
+#include <map>
+#include <string>
+#include <thread>
+#include <utility>
+#include <vector>
+
+namespace apollo {
+namespace hdmap {
+
+ChannelVerifyAgent::ChannelVerifyAgent(std::shared_ptr<JSonConf> sp_conf) {
+  sp_conf_ = sp_conf;
+  sp_channel_checker_ = nullptr;
+  sp_check_result_ = nullptr;
+}
+
+void ChannelVerifyAgent::Reset() {
+  std::lock_guard<std::mutex> guard(stop_mutex_);
+  need_stop_ = false;
+  stopped_ = false;
+  sp_channel_checker_ = std::make_shared<ChannelVerify>(sp_conf_);
+  sp_check_result_ = nullptr;
+  SetState(ChannelVerifyAgentState::IDLE);
+}
+
+grpc::Status ChannelVerifyAgent::ProcessGrpcRequest(
+    grpc::ServerContext *context, ChannelVerifyRequest *request,
+    ChannelVerifyResponse *response) {
+  AINFO << "ChannelVerifyAgent Request: " << request->DebugString();
+  switch (request->cmd()) {
+    case CmdType::START:
+      AINFO << "ChannelVerifyAgent start";
+      StartCheck(request, response);
+      break;
+    case CmdType::CHECK:
+      AINFO << "ChannelVerifyAgent check";
+      CheckResult(request, response);
+      break;
+    case CmdType::STOP:
+      AINFO << "ChannelVerifyAgent stop";
+      StopCheck(request, response);
+      break;
+    default:
+      response->set_code(ErrorCode::ERROR_REQUEST);
+      AERROR << "command error";
+  }
+  AINFO << "ChannelVerifyAgent progress: " << response->DebugString();
+  return grpc::Status::OK;
+}
+
+void ChannelVerifyAgent::StartCheck(ChannelVerifyRequest *request,
+                                    ChannelVerifyResponse *response) {
+  if (GetState() == ChannelVerifyAgentState::RUNNING) {
+    AINFO << "ChannelVerify is RUNNING, do not need start again";
+    response->set_code(ErrorCode::ERROR_REPEATED_START);
+    return;
+  }
+  Reset();
+  std::string records_path = request->path();
+  AsyncCheck(records_path);
+  response->set_code(ErrorCode::SUCCESS);
+}
+
+void ChannelVerifyAgent::AsyncCheck(const std::string &records_path) {
+  SetState(ChannelVerifyAgentState::RUNNING);
+  std::thread doctor_strange([=]() {
+    check_thread_id_ = std::this_thread::get_id();
+    int wait_sec = sp_conf_->channel_check_trigger_gap;
+    while (true) {
+      {
+        std::lock_guard<std::mutex> guard(stop_mutex_);
+        if (need_stop_) {
+          break;
+        }
+        DoCheck(records_path);
+        AINFO << "thread check done";
+      }
+      std::this_thread::sleep_for(std::chrono::seconds(wait_sec));
+      if (std::this_thread::get_id() != check_thread_id_) {
+        break;
+      }
+    }
+    stopped_ = true;
+  });
+  doctor_strange.detach();
+  AINFO << "ChannelVerifyAgent::async_check exit";
+}
+
+void ChannelVerifyAgent::DoCheck(const std::string &records_path) {
+  if (sp_channel_checker_ == nullptr) {
+    sp_channel_checker_ = std::make_shared<ChannelVerify>(sp_conf_);
+  }
+  SetState(ChannelVerifyAgentState::RUNNING);
+  sp_channel_checker_->Check(records_path);
+  sp_check_result_ = sp_channel_checker_->get_check_result();
+}
+
+int ChannelVerifyAgent::AddTopicLack(
+    VerifyResult *result, const std::string &record_path,
+    std::vector<std::string> const &lack_channels) {
+  TopicResult *topics = result->mutable_topics();
+  for (const std::string &channel : lack_channels) {
+    topics->add_topic_lack(channel);
+    AINFO << record_path << " lack topic: " << channel;
+  }
+  return static_cast<int>(lack_channels.size());
+}
+
+FrameRate *ChannelVerifyAgent::FindRates(VerifyResult *result,
+                                         const std::string &channel) {
+  int rates_size = result->rates_size();
+  for (const FrameRate &rates : result) {
+    if (rates.topic() == channel) {
+      return result->mutable_rates(i);
+    }
+  }
+  return nullptr;
+}
+
+int ChannelVerifyAgent::AddInadequateRate(
+    VerifyResult *result, std::string const &record_path,
+    std::map<std::string, std::pair<double, double>> const &inadequate_rate) {
+  for (auto it = inadequate_rate.begin(); it != inadequate_rate.end(); ++it) {
+    const std::string &channel = it->first;
+    double expected_rate = it->second.first;
+    double current_rate = it->second.second;
+    FrameRate *rate = FindRates(result, channel);
+    if (rate == nullptr) {
+      rate = result->add_rates();
+      rate->add_bad_record_name(record_path);
+      rate->set_topic(channel);
+      rate->set_expected_rate(expected_rate);
+      rate->set_current_rate(current_rate);
+    } else {
+      rate->set_current_rate(current_rate);
+      rate->add_bad_record_name(record_path);
+    }
+  }
+  return result->rates_size();
+}
+
+void ChannelVerifyAgent::CheckResult(ChannelVerifyRequest *request,
+                                     ChannelVerifyResponse *response) {
+  if (GetState() == ChannelVerifyAgentState::IDLE) {
+    AINFO << "ChannelVerify is not RUNNING, it should start first";
+    response->set_code(ErrorCode::ERROR_CHECK_BEFORE_START);
+    return;
+  }
+
+  if (sp_channel_checker_ == nullptr || sp_check_result_ == nullptr) {
+    AINFO << "check result is null. check later";
+    response->set_code(ErrorCode::SUCCESS);
+    return;
+  }
+
+  if (sp_channel_checker_->GetReturnState() != ErrorCode::SUCCESS) {
+    response->set_code(sp_channel_checker_->GetReturnState());
+    return;
+  }
+
+  response->set_code(ErrorCode::SUCCESS);
+  VerifyResult *result = response->mutable_result();
+  for (CheckResultIterator it = sp_check_result_->begin();
+       it != sp_check_result_->end(); ++it) {
+    int res = 0;
+    // write rate
+    res = AddInadequateRate(result, it->record_path, it->inadequate_rate);
+    if (res > 0) {
+      response->set_code(ErrorCode::ERROR_CHANNEL_VERIFY_RATES_ABNORMAL);
+    }
+    // write topic lack
+    res = AddTopicLack(result, it->record_path, it->lack_channels);
+    if (res > 0) {
+      response->set_code(ErrorCode::ERROR_CHANNEL_VERIFY_TOPIC_LACK);
+    }
+  }
+}
+
+void ChannelVerifyAgent::StopCheck(ChannelVerifyRequest *request,
+                                   ChannelVerifyResponse *response) {
+  std::lock_guard<std::mutex> guard(stop_mutex_);
+  need_stop_ = true;
+  response->set_code(ErrorCode::SUCCESS);
+  SetState(ChannelVerifyAgentState::IDLE);
+  VerifyResult *result = response->mutable_result();
+  if (sp_check_result_ == nullptr) {
+    return;
+  }
+  for (CheckResultIterator it = sp_check_result_->begin();
+       it != sp_check_result_->end(); ++it) {
+    int res = 0;
+    // write rate
+    res = AddInadequateRate(result, it->record_path, it->inadequate_rate);
+    if (res > 0) {
+      response->set_code(ErrorCode::ERROR_CHANNEL_VERIFY_RATES_ABNORMAL);
+    }
+    // write topic lack
+    res = AddTopicLack(result, it->record_path, it->lack_channels);
+    if (res > 0) {
+      response->set_code(ErrorCode::ERROR_CHANNEL_VERIFY_TOPIC_LACK);
+    }
+  }
+}
+
+void ChannelVerifyAgent::SetState(ChannelVerifyAgentState state) {
+  state_ = state;
+}
+
+ChannelVerifyAgentState ChannelVerifyAgent::GetState() const { return state_; }
+
+}  // namespace hdmap
+}  // namespace apollo

modules/map/tools/map_datachecker/server/channel_verify_agent.h

+/******************************************************************************
+ * Copyright 2019 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 <grpc++/grpc++.h>
+#include <map>
+#include <memory>
+#include <mutex>
+#include <string>
+#include <utility>
+#include <vector>
+
+#include "modules/map/tools/map_datachecker/proto/collection_error_code.pb.h"
+#include "modules/map/tools/map_datachecker/proto/collection_service.pb.h"
+#include "modules/map/tools/map_datachecker/server/channel_verify.h"
+
+namespace apollo {
+namespace hdmap {
+
+enum class ChannelVerifyAgentState { IDLE, RUNNING };
+
+class ChannelVerifyAgent {
+ public:
+  explicit ChannelVerifyAgent(std::shared_ptr<JSonConf> sp_conf);
+  grpc::Status ProcessGrpcRequest(grpc::ServerContext *context,
+                                  ChannelVerifyRequest *request,
+                                  ChannelVerifyResponse *response);
+
+ private:
+  void StartCheck(ChannelVerifyRequest *request,
+                  ChannelVerifyResponse *response);
+  void AsyncCheck(const std::string &records_path);
+  void DoCheck(const std::string &records_path);
+  void CheckResult(ChannelVerifyRequest *request,
+                   ChannelVerifyResponse *response);
+  void StopCheck(ChannelVerifyRequest *request,
+                 ChannelVerifyResponse *response);
+  void Reset();
+  void SetState(ChannelVerifyAgentState state);
+  ChannelVerifyAgentState GetState() const;
+  int AddTopicLack(VerifyResult *result, const std::string &record_path,
+                   std::vector<std::string> const &lack_channels);
+  int AddInadequateRate(
+      VerifyResult *result, std::string const &record_path,
+      std::map<std::string, std::pair<double, double>> const &inadequate_rate);
+  FrameRate *FindRates(VerifyResult *result, const std::string &channel);
+
+ private:
+  ChannelVerifyAgentState state_;
+  std::mutex stop_mutex_;
+  bool need_stop_;
+  bool stopped_;
+  std::shared_ptr<JSonConf> sp_conf_ = nullptr;
+  std::shared_ptr<ChannelVerify> sp_channel_checker_ = nullptr;
+  CheckedResult sp_check_result_ = nullptr;
+  std::thread::id check_thread_id_;
+};
+
+}  // namespace hdmap
+}  // namespace apollo

modules/map/tools/map_datachecker/server/eight_route.cc

+/******************************************************************************
+ * Copyright 2019 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.
+ *****************************************************************************/
+#include "modules/map/tools/map_datachecker/server/eight_route.h"
+
+#include <cmath>
+#include <vector>
+
+namespace apollo {
+namespace hdmap {
+
+EightRoute::EightRoute(std::shared_ptr<JSonConf> sp_conf) : Alignment(sp_conf) {
+  Reset();
+}
+
+void EightRoute::Reset() {
+  progress_ = 0.0;
+  last_progress_ = 0;
+}
+
+bool EightRoute::IsEightRoutePose(const std::vector<FramePose>& poses,
+                                     int pose_index) {
+  if (poses.size() == 0 || pose_index <= 0 ||
+      pose_index >= static_cast<int>(poses.size())) {
+    AINFO << "params error, poses size: " << poses.size()
+          << ", pose_index: " << pose_index;
+    return true;
+  }
+
+  double yaw = GetYaw(poses[pose_index - 1].tx, poses[pose_index - 1].ty,
+                       poses[pose_index].tx, poses[pose_index].ty);
+  double yaw_diff = fabs(last_yaw_ - yaw);
+  last_yaw_ = yaw;
+  yaw_diff = yaw_diff < 180 ? yaw_diff : 360 - yaw_diff;
+
+  double xdiff = poses[pose_index].tx - poses[pose_index - 1].tx;
+  double ydiff = poses[pose_index].ty - poses[pose_index - 1].ty;
+  double zdiff = poses[pose_index].tz - poses[pose_index - 1].tz;
+  double dist = std::sqrt(xdiff * xdiff + ydiff * ydiff + zdiff * zdiff);
+  double during =
+      poses[pose_index].time_stamp - poses[pose_index - 1].time_stamp;
+  if (during < 0) {
+    AINFO << "skip back pose is bad pose";
+    return false;
+  }
+  double vel = dist / during;
+  AINFO << std::to_string(poses[pose_index].time_stamp)
+        << ", yaw_diff:" << yaw_diff << ", dist: " << dist
+        << ", during: " << during << ", vel: " << vel;
+  if (yaw_diff > sp_conf_->eight_angle && vel > sp_conf_->eight_vel) {
+    return true;
+  }
+  return false;
+}
+
+double EightRoute::GetGoodPoseDuring() {
+  if (sp_good_pose_info_ == nullptr || sp_good_pose_info_->start_time < 0 ||
+      sp_good_pose_info_->end_time < 0) {
+    return 0.0;
+  }
+  return sp_good_pose_info_->end_time - sp_good_pose_info_->start_time;
+}
+
+double EightRoute::GetEightRouteProgress(
+    const std::vector<FramePose>& poses) {
+  int size = static_cast<int>(poses.size());
+  int start_index = TimeToIndex(poses, start_time_);
+  // select first good pose
+  while (start_index < size) {
+    if (IsGoodPose(poses, start_index) &&
+        IsEightRoutePose(poses, start_index)) {
+      AINFO << "find first good pose.index:" << start_index;
+      break;
+    }
+    ++start_index;
+  }
+  if (start_index >= size) {
+    AINFO << "not find first good pose, start_time: "
+          << std::to_string(start_time_) << ", start_index: " << start_index
+          << ", pose size: " << size;
+    return 0.0;
+  }
+  if (start_index + 1 >= size) {
+    AINFO << "not have enough poses, wait for a moment";
+    return 0.0;
+  }
+  last_yaw_ = GetYaw(poses[start_index].tx, poses[start_index].ty,
+                      poses[start_index + 1].tx, poses[start_index + 1].ty);
+
+  int not_eight_count = 0;
+  for (int i = start_index + 2; i < size; ++i) {
+    if (!IsGoodPose(poses, i)) {
+      AINFO << "not good pose";
+      return 0.0;
+    }
+    if (!IsEightRoutePose(poses, i)) {
+      ++not_eight_count;
+      AINFO << "not eight route pose";
+      if (not_eight_count > sp_conf_->eight_bad_pose_tolerance) {
+        AINFO << "not-eight pose count reached upper limitation";
+        return_state_ = ErrorCode::ERROR_NOT_EIGHT_ROUTE;
+        return 0.0;
+      }
+    } else {
+      not_eight_count = 0;
+    }
+    AINFO << "good pose";
+    UpdateGoodPoseInfo(poses[i]);
+    //  ClearBadPoseInfo();
+  }
+  double eight_route_during = GetGoodPoseDuring();
+  if (eight_route_during < 1e-8) {
+    AINFO << "num of eight route good pose too small, during: "
+          << eight_route_during;
+    return_state_ = ErrorCode::SUCCESS;
+    return 0.0;
+  }
+  return_state_ = ErrorCode::SUCCESS;
+  double progress = eight_route_during / sp_conf_->eight_duration;
+  if (progress >= 1.0) {
+    progress = 1.0;
+  }
+  ClearGoodPoseInfo();
+  return progress;
+}
+
+ErrorCode EightRoute::Process(const std::vector<FramePose>& poses) {
+  AINFO << "[EightRoute::process] begin";
+  size_t size = poses.size();
+  if (size <= 1) {
+    return_state_ = ErrorCode::ERROR_VERIFY_NO_GNSSPOS;
+    return return_state_;
+  }
+
+  progress_ = GetEightRouteProgress(poses);
+  if (return_state_ != ErrorCode::SUCCESS) {
+    AINFO << "get_eight_route_progress failed.";
+    return return_state_;
+  }
+  if (progress_ < last_progress_) {
+    return_state_ = ErrorCode::ERROR_NOT_EIGHT_ROUTE;
+    return return_state_;
+  }
+
+  AINFO << "[EightRoute::process] end, progress:" << progress_;
+  return_state_ = ErrorCode::SUCCESS;
+  return return_state_;
+}
+
+double EightRoute::GetProgress() const { return progress_; }
+
+}  // namespace hdmap
+}  // namespace apollo

modules/map/tools/map_datachecker/server/eight_route.h

+/******************************************************************************
+ * Copyright 2019 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 <grpc++/grpc++.h>
+#include <memory>
+#include <vector>
+
+#include "cyber/cyber.h"
+#include "modules/map/tools/map_datachecker/server/alignment.h"
+#include "modules/map/tools/map_datachecker/server/common.h"
+#include "modules/map/tools/map_datachecker/server/worker_gflags.h"
+
+namespace apollo {
+namespace hdmap {
+// TODO(yuanyijun): change EightRoute to FigureEight
+class EightRoute : public Alignment {
+ public:
+  explicit EightRoute(std::shared_ptr<JSonConf> sp_conf);
+  ErrorCode Process(const std::vector<FramePose>& poses);
+  double GetProgress() const;
+
+ private:
+  void Reset();
+  bool IsEightRoutePose(const std::vector<FramePose>& poses, int pose_index);
+  double GetGoodPoseDuring();
+  double GetEightRouteProgress(const std::vector<FramePose>& poses);
+
+ private:
+  double progress_;
+  double last_yaw_;
+};
+
+}  // namespace hdmap
+}  // namespace apollo

modules/map/tools/map_datachecker/server/laps_checker.cc

+/******************************************************************************
+ * Copyright 2019 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.
+ *****************************************************************************/
+#include "modules/map/tools/map_datachecker/server/laps_checker.h"
+
+#include <boost/property_tree/json_parser.hpp>
+#include <boost/property_tree/ptree.hpp>
+#include <algorithm>
+#include <cmath>
+#include <fstream>
+#include <iostream>
+#include <limits>
+
+namespace apollo {
+namespace hdmap {
+
+LapsChecker::LapsChecker(const std::vector<FramePose> &poses, int laps_to_check,
+                         std::shared_ptr<JSonConf> sp_conf)
+    : poses_(poses), sp_conf_(sp_conf) {
+  laps_to_check_ = laps_to_check;
+  maxx_ = 0.0;
+  maxy_ = 0.0;
+  minx_ = 0.0;
+  miny_ = 0.0;
+  possible_max_laps_ = (laps_to_check_ + 1) * 10;
+  confidence_.resize(possible_max_laps_ + 1, 0.0);
+  finished_ = false;
+  return_state_ = ErrorCode::SUCCESS;
+  AINFO << "instance has " << poses.size() << " poses";
+  AINFO << "confidence size: " << possible_max_laps_ + 1;
+}
+
+int LapsChecker::SetProgress(double p) {
+  progress_ = p;
+  return 0;
+}
+
+double LapsChecker::GetProgress() const { return progress_; }
+
+size_t LapsChecker::GetLap() const { return lap_; }
+
+double LapsChecker::GetConfidence() {
+  double res = 0.0;
+  lap_ = laps_to_check_;
+  for (size_t i = 0; i < confidence_.size(); ++i) {
+    AINFO << "confidence[" << i << "]: " << std::to_string(confidence_[i]);
+  }
+  AINFO << "laps to check: " << laps_to_check_;
+  for (size_t i = laps_to_check_; i < confidence_.size(); ++i) {
+    res += confidence_[i];
+  }
+  AINFO << "current confidence: " << res
+        << ", confidence thresh:" << sp_conf_->laps_rate_thresh;
+  if (res < sp_conf_->laps_rate_thresh) {
+    if (confidence_.size() == 0) {
+      AINFO << "some problems induce lap problem";
+      return 0.0;
+    }
+    res = confidence_[0];
+    lap_ = 0;
+    for (size_t i = 1; i < confidence_.size(); ++i) {
+      if (i == laps_to_check_) {
+        continue;
+      }
+      if (confidence_[i] > res) {
+        lap_ = i;
+        res = confidence_[i];
+      }
+    }
+  }
+  return res;
+}
+
+ErrorCode LapsChecker::Check() {
+  if (poses_.size() == 0) {
+    return_state_ = ErrorCode::ERROR_VERIFY_NO_GNSSPOS;
+    return return_state_;
+  }
+  DoCheck();
+  finished_ = true;
+  return return_state_;
+}
+
+void LapsChecker::DoCheck() {
+  AINFO << "do_check";
+  SetProgress(0.0);
+  int ret = 0;
+  AINFO << "check->check_params";
+  ret = CheckParams();
+  if (ret < 0) {
+    AINFO << "check_params failed";
+  }
+  SetProgress(0.1);
+
+  AINFO << "check->setup_grids_map";
+  ret = SetupGridsMap();
+  if (ret < 0) {
+    AINFO << "setup_grids_map failed";
+  }
+  SetProgress(0.5);
+  AINFO << "check->setup_grids_map done";
+
+  AINFO << "check->check_laps";
+  ret = CheckLaps();
+  if (ret < 0) {
+    AINFO << "check_laps failed";
+  }
+  SetProgress(1.0);
+  AINFO << "check->check_laps done";
+
+  AINFO << "do_check done";
+}
+
+int LapsChecker::CheckParams() {
+  int n_pose = static_cast<int>(poses_.size());
+  if (n_pose < sp_conf_->laps_frames_thresh) {
+    return -1;
+  }
+  return 0;
+}
+
+int LapsChecker::SetupGridsMap() {
+  AINFO << "setup_grids_map->get_min_max";
+  GetMinMax();
+  AINFO << "setup_grids_map->do_setup_grids_map";
+  int ret = DoSetupGridsMap();
+  if (ret < 0) {
+    AINFO << "do_setup_grids_map failed";
+    return -1;
+  }
+  AINFO << "setup_grids_map done";
+  return 0;
+}
+
+int LapsChecker::CheckLaps() {
+  int height = static_cast<int>(grids_map_.size());
+  if (height <= 2 || height > 1000000) {
+    AINFO << "grids_map_ size error. height = " << height;
+    return -1;
+  }
+
+  int width = static_cast<int>(grids_map_[0].size());
+  if (width <= 2 || width >= 1000000) {
+    AINFO << "grids_map_ size error. width = " << width;
+    return -1;
+  }
+
+  for (int y = 0; y < height; y++) {
+    for (int x = 0; x < width; x++) {
+      Grid &grid = grids_map_[y][x];
+      size_t t_size = grid.size();
+      if (t_size == 0) {
+        continue;
+      }
+      for (size_t i = 0; i < t_size; ++i) {
+        std::vector<double> stamps;
+        GatherTimestamps(&stamps, grid[i].alpha, x, y);
+        if (stamps.size() == 0) {
+          continue;
+        }
+        std::sort(stamps.begin(), stamps.end());
+        double thresh_in_sec = sp_conf_->laps_time_err_thresh * 60;
+        size_t segment = 1;
+        for (size_t j = 1; j < stamps.size(); ++j) {
+          if (stamps[j] - stamps[j - 1] > thresh_in_sec) {
+            segment++;
+          }
+        }
+        if (segment <= possible_max_laps_) {
+          confidence_[segment] += 1;
+        }
+      }
+    }
+  }
+
+  double all = 0.0;
+  for (size_t i = 0; i < confidence_.size(); ++i) {
+    all += confidence_[i];
+  }
+  if (all == 0.0) {
+    AINFO << "there seems no poses";
+    return -1;
+  }
+  AINFO << "confidence size: " << confidence_.size();
+  for (size_t i = 0; i < confidence_.size(); ++i) {
+    confidence_[i] /= all;
+  }
+
+  return 0;
+}
+
+int LapsChecker::GatherTimestamps(std::vector<double> *sp_stamps, double alpha,
+                                   int center_x, int center_y) {
+  int search_d = sp_conf_->laps_search_diameter;
+  if ((search_d & 1) == 0) {
+    AINFO << "laps_search_diameter should be an odd";
+    return -1;
+  }
+  int search_r = (search_d >> 1);
+  size_t height = grids_map_.size(), width = grids_map_[0].size();
+  std::vector<double> &stamps = *sp_stamps;
+  stamps.clear();
+  const size_t start_y = std::max(0, center_y - search_r);
+  const size_t end_y =
+      std::min(static_cast<int>(height) - 1, center_y + search_r);
+  const size_t start_x = std::max(0, center_x - search_r);
+  const size_t end_x =
+      std::min(static_cast<int>(width) - 1, center_x + search_r);
+  for (size_t y = start_y; y <= end_y; y++) {
+    for (size_t x = start_x; x <= end_x; x++) {
+      Grid &grid = grids_map_[y][x];
+      for (size_t i = 0; i < grid.size(); ++i) {
+        if (std::abs(alpha - grid[i].alpha) < sp_conf_->laps_alpha_err_thresh) {
+          std::vector<int> &idxs = grid[i].idxs;
+          for (size_t j = 0; j < idxs.size(); ++j) {
+            if (idxs[j] >= static_cast<int>(poses_.size())) {
+              AINFO << "index error, index: " << idxs[j]
+                    << ", pose size: " << poses_.size();
+            } else {
+              stamps.push_back(poses_[idxs[j]].time_stamp);
+            }
+          }
+        }
+      }
+    }
+  }
+
+  return 0;
+}
+
+int LapsChecker::GetMinMax() {
+  minx_ = std::numeric_limits<double>::max();
+  miny_ = std::numeric_limits<double>::max();
+  maxx_ = std::numeric_limits<double>::min();
+  maxy_ = std::numeric_limits<double>::min();
+  size_t size = poses_.size();
+  AINFO << "get_min_max pose size: " << size;
+  for (size_t i = 0; i < size; ++i) {
+    double tx = poses_[i].tx, ty = poses_[i].ty;
+    if (tx < minx_) {
+      minx_ = tx;
+    }
+    if (tx > maxx_) {
+      maxx_ = tx;
+    }
+    if (ty < miny_) {
+      miny_ = ty;
+    }
+    if (ty > maxy_) {
+      maxy_ = ty;
+    }
+  }
+  return 0;
+}
+
+int LapsChecker::DoSetupGridsMap() {
+  size_t width = size_t(maxx_ - minx_ + 1);
+  size_t height = size_t(maxy_ - miny_ + 1);
+  AINFO << "grid map width: " << width << ", height: " << height;
+  size_t size = poses_.size();
+  if (1 >= size || 0 == height || 0 == width || height > 1000000 ||
+      width > 1000000) {
+    AINFO << "pose size: " << size << ", height: " << height
+          << ", width: " << width;
+    AINFO << "pose size error or grid map size error";
+    return -1;
+  }
+
+  grids_map_.resize(height);
+  for (size_t i = 0; i < height; ++i) {
+    grids_map_[i].resize(width);
+  }
+  // first pose can not be used
+  for (size_t i = 1; i < size; ++i) {
+    int x = static_cast<int>(poses_[i].tx - minx_);
+    int y = static_cast<int>(poses_[i].ty - miny_);
+    PutPoseToGrid(static_cast<int>(i), y, x);
+    PutPoseToNeighborGrid(static_cast<int>(i));
+  }
+  return 0;
+}
+
+double LapsChecker::CalcAlpha(int pose_index) {
+  double vecx = poses_[pose_index].tx - poses_[pose_index - 1].tx;
+  double vecy = poses_[pose_index].ty - poses_[pose_index - 1].ty;
+  double alpha = acos(vecx / sqrt(vecx * vecx + vecy * vecy)) * 180 / M_PI;
+  if (alpha > 0) {
+    return alpha;
+  }
+  return 360 + alpha;
+}
+
+int LapsChecker::PutPoseToGrid(int pose_index, int grid_y, int grid_x) {
+  if (pose_index <= 0) {
+    return 0;
+  }
+  double alpha = CalcAlpha(pose_index);
+  if (std::isnan(alpha)) {
+    AERROR << "ignore static pose " << pose_index;
+    return 0;
+  }
+
+  Grid &grid = grids_map_[grid_y][grid_x];
+  size_t t_size = grid.size();
+  for (size_t j = 0; j < t_size; ++j) {
+    if (std::abs(alpha - grid[j].alpha) < sp_conf_->laps_alpha_err_thresh) {
+      grid[j].idxs.push_back(pose_index);
+      grid[j].alpha = (grid[j].alpha + alpha) / 2;
+      return 0;
+    }
+  }
+
+  GridMeta gm;
+  gm.alpha = alpha;
+  gm.idxs = {pose_index};
+  grid.push_back(gm);
+  return 0;
+}
+
+int LapsChecker::PutPoseToNeighborGrid(int pose_index) {
+  if (pose_index <= 0) {
+    return 0;
+  }
+  std::vector<int> x, y;
+  GetPassedGrid(pose_index, &x, &y);
+  for (size_t i = 0; i < x.size(); ++i) {
+    PutPoseToGrid(pose_index, y[i], x[i]);
+  }
+  return 0;
+}
+
+int LapsChecker::GetPassedGrid(int pose_index, std::vector<int> *sp_grid_x,
+                                 std::vector<int> *sp_grid_y) {
+  if (pose_index <= 0) {
+    return 0;
+  }
+  std::vector<int> &grid_x = *sp_grid_x;
+  std::vector<int> &grid_y = *sp_grid_y;
+  grid_x.clear();
+  grid_y.clear();
+  double x = poses_[pose_index].tx - minx_;
+  double y = poses_[pose_index].ty - miny_;
+  double last_x = poses_[pose_index - 1].tx - minx_;
+  double last_y = poses_[pose_index - 1].ty - miny_;
+  if (std::abs(x - last_x) < 1e-6) {  // current trace is vertical
+    int start_y = static_cast<int>(std::min(y, last_y)) + 1;
+    int end_y = static_cast<int>(std::max(y, last_y));
+    int start_x = static_cast<int>(x);
+    while ((start_y++) < end_y) {
+      grid_x.push_back(start_x);
+      grid_y.push_back(start_y);
+    }
+  } else if (std::abs(y - last_y) < 1e-6) {  // current trace is horizontal
+    int start_x = static_cast<int>(std::min(x, last_x));
+    int end_x = static_cast<int>(std::max(x, last_x));
+    int start_y = static_cast<int>(y);
+    while ((++start_x) < end_x) {
+      grid_x.push_back(start_x);
+      grid_y.push_back(start_y);
+    }
+  } else {
+    double k = Slope(last_x, last_y, x, y);
+    if (k > 99999999.0) {
+      AERROR << "slope error";
+      return -1;
+    }
+    int steps = static_cast<int>(std::abs(last_x - x));
+    int step = 0;
+    double xx = 0.0, yy = 0.0;
+    if (x < last_x) {
+      xx = x, yy = y;
+    } else {
+      xx = last_x, yy = last_y;
+    }
+    while ((step++) < steps) {
+      xx = xx + 1;
+      yy = yy + k;
+      grid_x.push_back(static_cast<int>(xx));
+      grid_y.push_back(static_cast<int>(yy));
+    }
+  }
+  return 0;
+}
+
+double LapsChecker::Slope(double x1, double y1, double x2, double y2) {
+  if (std::abs(x1 - x2) < 1e-6) {
+    return std::numeric_limits<double>::max();
+  }
+  if (std::abs(y1 - y2) < 1e-6) {
+    return 0.0;
+  }
+  return (y2 - y1) / (x2 - x1);
+}
+
+ErrorCode LapsChecker::GetReturnState() { return return_state_; }
+
+}  // namespace hdmap
+}  // namespace apollo

modules/map/tools/map_datachecker/server/laps_checker.h

+/******************************************************************************
+ * Copyright 2019 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 <map>
+#include <memory>
+#include <utility>
+#include <vector>
+
+#include "modules/map/tools/map_datachecker/proto/collection_error_code.pb.h"
+#include "modules/map/tools/map_datachecker/server/common.h"
+
+namespace apollo {
+namespace hdmap {
+
+struct GridMeta {
+  double alpha;
+  std::vector<int> idxs;
+};
+typedef std::vector<GridMeta> Grid;
+
+class LapsChecker {
+ public:
+  explicit LapsChecker(const std::vector<FramePose>& poses, int laps_to_check,
+                       std::shared_ptr<JSonConf> sp_conf);
+  ErrorCode Check();
+  double GetProgress() const;
+  double GetConfidence();
+  size_t GetLap() const;
+  ErrorCode GetReturnState();
+
+ private:
+  void DoCheck();
+  int SetupGridsMap();
+  int CheckLaps();
+  int CheckParams();
+  int GetMinMax();
+  int DoSetupGridsMap();
+  double CalcAlpha(int pose_index);
+  int PutPoseToGrid(int pose_index, int grid_y, int grid_x);
+  int PutPoseToNeighborGrid(int pose_index);
+  int GetPassedGrid(int pose_index, std::vector<int>* sp_grid_x,
+                      std::vector<int>* sp_grid_y);
+  double Slope(double x1, double y1, double x2, double y2);
+  int GatherTimestamps(std::vector<double>* sp_stamps, double alpha,
+                        int center_x, int center_y);
+  inline int SetProgress(double p);
+
+ public:
+  const std::vector<FramePose>& poses_;
+  double maxx_, maxy_, minx_, miny_;
+  std::vector<std::vector<Grid>> grids_map_;
+  bool finished_;
+
+ private:
+  std::vector<double> confidence_;
+  double progress_;
+  size_t laps_to_check_;
+  size_t possible_max_laps_;
+  size_t lap_;
+  std::shared_ptr<JSonConf> sp_conf_;
+  ErrorCode return_state_;
+};
+
+}  // namespace hdmap
+}  // namespace apollo