<commit_before>/******************************************************************************
 * 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.
 *****************************************************************************/

#include "modules/localization/rtk/rtk_localization_component.h"

#include <limits>

#include "modules/common/math/quaternion.h"
#include "modules/common/time/time.h"

#include "modules/localization/proto/rtk_config.pb.h"

namespace apollo {
namespace localization {

// using apollo::common::monitor::MonitorMessageItem;
using apollo::common::time::Clock;
using ::Eigen::Vector3d;

RTKLocalizationComponent::RTKLocalizationComponent()
    : map_offset_{0.0, 0.0, 0.0} {}
// monitor_logger_(MonitorMessageItem::LOCALIZATION) {}

bool RTKLocalizationComponent::Init() {
  if (InitConfig() != true) {
    AERROR << "Init Config falseed.";
    return false;
  }

  if (InitIO() != true) {
    AERROR << "Init Interval falseed.";
    return false;
  }

  return true;
}

bool RTKLocalizationComponent::InitConfig() {
  rtk_config::Config rtk_config;
  if (!apollo::cybertron::common::GetProtoFromFile(config_file_path_,
                                                   &rtk_config)) {
    return false;
  }
  AINFO << "Rtk localization config: " << rtk_config.DebugString();

  localization_topic_ = rtk_config.localization_topic();
  imu_topic_ = rtk_config.imu_topic();
  gps_topic_ = rtk_config.gps_topic();
  broadcast_tf_frame_id_ = rtk_config.broadcast_tf_frame_id();
  broadcast_tf_child_frame_id_ = rtk_config.broadcast_tf_child_frame_id();
  imu_list_max_size_ = rtk_config.imu_list_max_size();
  gps_imu_time_diff_threshold_ = rtk_config.gps_imu_time_diff_threshold();
  map_offset_[0] = rtk_config.map_offset_x();
  map_offset_[1] = rtk_config.map_offset_y();
  map_offset_[2] = rtk_config.map_offset_z();

  return true;
}

bool RTKLocalizationComponent::InitIO() {
  std::function<void(const std::shared_ptr<localization::CorrectedImu>&)>
      imu_register_call = std::bind(&RTKLocalizationComponent::ImuCallback,
                                    this, std::placeholders::_1);
  corrected_imu_listener_ =
      this->node_->CreateReader<localization::CorrectedImu>(imu_topic_,
                                                            imu_register_call);

  localization_talker_ =
      this->node_->CreateWriter<LocalizationEstimate>(localization_topic_);

  return true;
}

bool RTKLocalizationComponent::Proc(
    const std::shared_ptr<localization::Gps>& gps_msg) {
  // check imu list
  // double time_delay =
  //     common::time::ToSecond(Clock::Now()) - last_received_timestamp_sec_;
  // common::monitor::MonitorLogBuffer buffer(&monitor_logger_);
  // if (enable_gps_timestamp_ && time_delay > gps_time_delay_tolerance_) {
  //   buffer.ERROR() << "GPS message time delay: " << time_delay;
  //   buffer.PrintLog();
  // }

  // {
  //   std::unique_lock<std::mutex> lock(imu_list_mutex_);

  //   if (imu_list_.empty()) {
  //     AERROR << "IMU message buffer is empty.";
  //     if (service_started_) {
  //       buffer.ERROR("IMU message buffer is empty.");
  //     }
  //   }
  // }

  // publish localization messages
  PublishLocalization(*gps_msg);
  service_started_ = true;

  // watch dog
  RunWatchDog(gps_msg->header().timestamp_sec());

  last_received_timestamp_sec_ = common::time::ToSecond(Clock::Now());
  return true;
}

void RTKLocalizationComponent::ImuCallback(
    const std::shared_ptr<localization::CorrectedImu>& imu_msg) {
  std::unique_lock<std::mutex> lock(imu_list_mutex_);
  if (imu_list_.size() < imu_list_max_size_) {
    imu_list_.push_back(*imu_msg);
  } else {
    imu_list_.pop_front();
    imu_list_.push_back(*imu_msg);
  }
  return;
}

void RTKLocalizationComponent::PublishLocalization(
    const localization::Gps& gps_msg) {
  LocalizationEstimate localization;<commit_msg>fix a bug<commit_after>/******************************************************************************
 * 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.
 *****************************************************************************/

#include "modules/localization/rtk/rtk_localization_component.h"

#include <limits>

#include "modules/common/math/quaternion.h"
#include "modules/common/time/time.h"

#include "modules/localization/proto/rtk_config.pb.h"

namespace apollo {
namespace localization {

// using apollo::common::monitor::MonitorMessageItem;
using apollo::common::time::Clock;
using ::Eigen::Vector3d;

RTKLocalizationComponent::RTKLocalizationComponent()
    : map_offset_{0.0, 0.0, 0.0} {}
// monitor_logger_(MonitorMessageItem::LOCALIZATION) {}

bool RTKLocalizationComponent::Init() {
  if (InitConfig() != true) {
    AERROR << "Init Config falseed.";
    return false;
  }

  if (InitIO() != true) {
    AERROR << "Init Interval falseed.";
    return false;
  }

  return true;
}

bool RTKLocalizationComponent::InitConfig() {
  rtk_config::Config rtk_config;
  if (!apollo::cybertron::common::GetProtoFromFile(config_file_path_,
                                                   &rtk_config)) {
    return false;
  }
  AINFO << "Rtk localization config: " << rtk_config.DebugString();

  localization_topic_ = rtk_config.localization_topic();
  imu_topic_ = rtk_config.imu_topic();
  gps_topic_ = rtk_config.gps_topic();
  broadcast_tf_frame_id_ = rtk_config.broadcast_tf_frame_id();
  broadcast_tf_child_frame_id_ = rtk_config.broadcast_tf_child_frame_id();
  imu_list_max_size_ = rtk_config.imu_list_max_size();
  gps_imu_time_diff_threshold_ = rtk_config.gps_imu_time_diff_threshold();
  map_offset_[0] = rtk_config.map_offset_x();
  map_offset_[1] = rtk_config.map_offset_y();
  map_offset_[2] = rtk_config.map_offset_z();

  return true;
}

bool RTKLocalizationComponent::InitIO() {
  std::function<void(const std::shared_ptr<localization::CorrectedImu>&)>
      imu_register_call = std::bind(&RTKLocalizationComponent::ImuCallback,
                                    this, std::placeholders::_1);
  corrected_imu_listener_ =
      this->node_->CreateReader<localization::CorrectedImu>(imu_topic_,
                                                            imu_register_call);

  localization_talker_ =
      this->node_->CreateWriter<LocalizationEstimate>(localization_topic_);

  return true;
}

bool RTKLocalizationComponent::Proc(
    const std::shared_ptr<localization::Gps>& gps_msg) {
  // check imu list
  // double time_delay =
  //     common::time::ToSecond(Clock::Now()) - last_received_timestamp_sec_;
  // common::monitor::MonitorLogBuffer buffer(&monitor_logger_);
  // if (enable_gps_timestamp_ && time_delay > gps_time_delay_tolerance_) {
  //   buffer.ERROR() << "GPS message time delay: " << time_delay;
  //   buffer.PrintLog();
  // }

  {
    std::unique_lock<std::mutex> lock(imu_list_mutex_);

    if (imu_list_.empty()) {
      AERROR << "IMU message buffer is empty.";
      // if (service_started_) {
      //   buffer.ERROR("IMU message buffer is empty.");
      // }
      return false;
    }
  }

  // publish localization messages
  PublishLocalization(*gps_msg);
  service_started_ = true;

  // watch dog
  RunWatchDog(gps_msg->header().timestamp_sec());

  last_received_timestamp_sec_ = common::time::ToSecond(Clock::Now());
  return true;
}

void RTKLocalizationComponent::ImuCallback(
    const std::shared_ptr<localization::CorrectedImu>& imu_msg) {
  std::unique_lock<std::mutex> lock(imu_list_mutex_);
  if (imu_list_.size() < imu_list_max_size_) {
    imu_list_.push_back(*imu_msg);
  } else {
    imu_list_.pop_front();
    imu_list_.push_back(*imu_msg);
  }
  return;
}

void RTKLocalizationComponent::PublishLocalization(
    const localization::Gps& gps_msg) {
  LocalizationEstimate localization;
{'fraction_non_alphanumeric': 0.1516347237880496, 'fraction_numerical': 0.004284103720405863, 'mean_word_length': 4.1786339754816115, 'pattern_counts': {'":': 0, '<': 51, '<?xml version=': 0, '>': 33, 'https://': 0, 'lorem ipsum': 0, 'www.': 2, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 10, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '251816', 'n_tokens_mistral': 2838, 'n_tokens_neox': 2752, 'n_words': 527}