std::vector<int> score{ 10, 20, 30, 40 };

auto it_1 = std::find(score.begin(), score.end(), 30 );
if(it_1 != score.end() )
    qDebug()<< "found !";
else qDebug()<< "no found !";

auto it_2 = std::find_if(score.begin(), score.end(), [](int& m){return m>20;}  );
if(it_2 != score.end() )
    qDebug()<< "found: " << *it_2;
else qDebug()<< "no found !";

// boost::atomic<int> i(0);
int i=0;
boost::mutex mut;
void thread_1(int n);

int main()
{
    int n=100000;   //  n不够大时，i不容易出现不同的情况
    boost::thread th1 = boost::thread(boost::bind(&thread_1,n));
    boost::thread th2 = boost::thread(boost::bind(&thread_1,n));
    th1.join();
    th2.join();
    cout<< i<<endl;
    return 0;
}

void thread_1(int n)
{
    while(n--)
    {
       mut.lock();
        i++;
       mut.unlock();
    }
}

atomic<int> a(10);
assert(a==10);      //安全函数，若表达式不成立结束程序


atomic<long> L;
cout << L <<endl;    //初始值不确定

boost::atomic<bool> b(1);
assert(b != 0);
std::cout << b << std::endl;
b.store(0);//存值
std::cout << b << std::endl;

boost::atomic<int> n1(100);
std::cout << n1.exchange(200) << std::endl;  //交换两个值，并且返回原值100
std::cout << n1 << std::endl;

//int v = 0;
std::atomic<int> v(0);

void Test()
{
    for(int i=0; i<10000; i++)
        ++v;
}

int main(int argc, char *argv[])
{
    std::thread t1(Test);
    std::thread t2(Test);
    t1.join();
    t2.join();

    std::cout << "v: " << v << std::endl;
    return 0;
}

#include <iostream>     // std::cout
#include <iterator>     // std::next
#include <list>         // std::list
using namespace std;
int main() {
    //创建并初始化一个 list 容器
    std::list<int> mylist{ 1,2,3,4,5 };
    std::list<int>::iterator it = mylist.end();
    //获取一个距离 it 迭代器 2 个元素的迭代器，由于 2 为正数，newit 位于 it 左侧
    auto newit = prev(it, 2);
    cout << "prev(it, 2) = " << *newit << endl;   // 4

    //n为负数，newit 位于 it 右侧
    it = mylist.begin();
    newit = prev(it, -2);
    cout << "prev(it, -2) = " << *newit;   // 3
    return 0;
}

int main(int argc, char** argv)
{
  google::InitGoogleLogging(argv[0]);

  FLAGS_logtostderr = true;
  const std::string usage_message =
      "Swiss Army knife for pbstreams.\n\n"
      "Currently supported subcommands are:\n"
      "\tinfo    - Prints summary of pbstream.\n"
      "\tmigrate - Migrates old pbstream (w/o header) to new pbstream format.";
  google::ParseCommandLineFlags(&argc, &argv, true);

  if (argc < 2) {
    google::SetUsageMessage(usage_message);
    google::ShowUsageWithFlagsRestrict(argv[0], "pbstream_info_main");
    return EXIT_FAILURE;
  } else if (std::string(argv[1]) == "info") {
    return ::cartographer::io::pbstream_info(argc, argv);
  } else if (std::string(argv[1]) == "migrate") {
    return ::cartographer::io::pbstream_migrate(argc, argv);
  }
  // 其他情况会报错
}

#include "cartographer/mapping/map_builder.h"
#include "cartographer_ros/offline_node.h"
#include "cartographer_ros/ros_log_sink.h"
#include "gflags/gflags.h"
#include "ros/ros.h"

int main(int argc, char** argv)
{
  google::InitGoogleLogging(argv[0]);
  google::ParseCommandLineFlags(&argc, &argv, true);

  ::ros::init(argc, argv, "cartographer_offline_node");
  ::ros::start();
  cartographer_ros::ScopedRosLogSink ros_log_sink;

  const cartographer_ros::MapBuilderFactory   map_builder_factory =
      [](const ::cartographer::mapping::proto::MapBuilderOptions& 
             map_builder_options) {
        return absl::make_unique< ::cartographer::mapping::MapBuilder>(
            const_cast<::cartographer::mapping::proto::MapBuilderOptions&>(map_builder_options));
      };
  cartographer_ros::RunOfflineNode(map_builder_factory);
  ::ros::shutdown();
}

constexpr char kClockTopic[] = "clock";
constexpr char kTfStaticTopic[] = "/tf_static";
constexpr char kTfTopic[] = "tf";
constexpr double kClockPublishFrequencySec = 1. / 30.;
constexpr int kSingleThreaded = 1;
// We publish tf messages one second earlier than other messages. Under
// the assumption of higher frequency tf this should ensure that tf can
// always interpolate.
const ::ros::Duration kDelay = ::ros::Duration(1.0);

::ros::spin();
// 终止所有轨迹
node.FinishAllTrajectories();
node.RunFinalOptimization();

void Node::FinishAllTrajectories()
{
  carto::common::MutexLocker lock(&mutex_);
  for (auto& entry : is_active_trajectory_) {
    const int trajectory_id = entry.first;
    // 已经lock了，所以叫做 FinishTrajectoryUnderLock
    if (entry.second) {
      CHECK_EQ(FinishTrajectoryUnderLock(trajectory_id).code,
               cartographer_ros_msgs::StatusCode::OK);
    }
  }
}

cartographer_ros_msgs::StatusResponse Node::FinishTrajectoryUnderLock(
    const int trajectory_id)
{
  cartographer_ros_msgs::StatusResponse status_response;

  // First, check if we can actually finish the trajectory
  // trajectory_id 是否属于正在结束的轨迹集合，如果是 FROZEN 则返回
  if (map_builder_bridge_.GetFrozenTrajectoryIds().count(trajectory_id))
  {
    const std::string error =
        "Trajectory " + std::to_string(trajectory_id) + " is frozen.";
    LOG(ERROR) << error;
    status_response.code = cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT;
    status_response.message = error;
    return status_response;
  }
  // trajectory_id 是否是 ACTIVE状态
  if (is_active_trajectory_.count(trajectory_id) == 0)
  {
    const std::string error =
        "Trajectory " + std::to_string(trajectory_id) + " is not created yet.";
    LOG(ERROR) << error;
    status_response.code = cartographer_ros_msgs::StatusCode::NOT_FOUND;
    status_response.message = error;
    return status_response;
  }
  // trajectory_id 是否已经 finish
  if (!is_active_trajectory_[trajectory_id])
  {
    const std::string error = "Trajectory " + std::to_string(trajectory_id) +
                              " has already been finished.";
    LOG(ERROR) << error;
    status_response.code =
        cartographer_ros_msgs::StatusCode::RESOURCE_EXHAUSTED;
    status_response.message = error;
    return status_response;
  }
  // Shutdown the subscribers of this trajectory.
  for (auto& entry : subscribers_[trajectory_id])
  {
    entry.subscriber.shutdown();
    subscribed_topics_.erase(entry.topic);
    LOG(INFO) << "Shutdown the subscriber of [" << entry.topic << "]";
  }
  CHECK_EQ(subscribers_.erase(trajectory_id), 1);
  CHECK(is_active_trajectory_.at(trajectory_id));
  // 主要是这里
  map_builder_bridge_.FinishTrajectory(trajectory_id);

  is_active_trajectory_[trajectory_id] = false;
  const std::string message =
      "Finished trajectory " + std::to_string(trajectory_id) + ".";
  status_response.code = cartographer_ros_msgs::StatusCode::OK;
  status_response.message = message;
  return status_response;
}

void MapBuilderBridge::FinishTrajectory(const int trajectory_id)
{
  LOG(INFO) << "Finishing trajectory with ID '" << trajectory_id << "'...";
  // Make sure there is a trajectory with 'trajectory_id'.
  CHECK_EQ(sensor_bridges_.count(trajectory_id), 1);
  map_builder_->FinishTrajectory(trajectory_id);
  sensor_bridges_.erase(trajectory_id);
}

// 再看这个函数
void MapBuilder::FinishTrajectory(const int trajectory_id) {
  sensor_collator_->FinishTrajectory(trajectory_id);
  pose_graph_->FinishTrajectory(trajectory_id);
}

void PoseGraph2D::FinishTrajectory(const int trajectory_id)
{
  AddWorkItem([this, trajectory_id]() LOCKS_EXCLUDED(mutex_)
  {
    absl::MutexLock  locker(&mutex_);
    CHECK(!IsTrajectoryFinished(trajectory_id));   // 不能是已结束
    data_.trajectories_state[trajectory_id].state = TrajectoryState::FINISHED;

    for (const auto& submap : data_.submap_data.trajectory(trajectory_id)) {
      data_.submap_data.at(submap.id).state = SubmapState::kFinished;
    }
    return WorkItem::Result::kRunOptimization;
  });
}

server_builder.RegisterHandler<handlers::GetLocalToGlobalTransformHandler>();

// get_local_to_global_transform_handler.cc 文件
void GetLocalToGlobalTransformHandler::OnRequest(
    const proto::GetLocalToGlobalTransformRequest& request)
{
  auto response = absl::make_unique<proto::GetLocalToGlobalTransformResponse>();
  auto local_to_global =
      GetContext<MapBuilderContextInterface>()
          ->map_builder()
          .pose_graph()
          ->GetLocalToGlobalTransform(request.trajectory_id());
  *response->mutable_local_to_global() = transform::ToProto(local_to_global);
  Send(std::move(response));
}

transform::Rigid3d PoseGraph2D::GetLocalToGlobalTransform(
    const int trajectory_id) const
{
  absl::MutexLock locker(&mutex_);
  return ComputeLocalToGlobalTransform(data_.global_submap_poses_2d,
                                       trajectory_id);
}

// node.cc中
if (collect_metrics)
{
  metrics_registry_ = absl::make_unique<metrics::FamilyFactory>();
  carto::metrics::RegisterAllMetrics(metrics_registry_.get());
}


// register.cc 中定义
void RegisterAllMetrics(FamilyFactory* registry) {
  mapping::constraints::ConstraintBuilder2D::RegisterMetrics(registry);
  mapping::GlobalTrajectoryBuilderRegisterMetrics(registry);
  mapping::LocalTrajectoryBuilder2D::RegisterMetrics(registry);
  mapping::PoseGraph2D::RegisterMetrics(registry);
  sensor::TrajectoryCollator::RegisterMetrics(registry);
  // 不需要可以注释掉
  mapping::constraints::ConstraintBuilder3D::RegisterMetrics(registry);
  mapping::LocalTrajectoryBuilder3D::RegisterMetrics(registry);
  mapping::PoseGraph3D::RegisterMetrics(registry);
}

static auto* kLocalSlamCpuRealTimeRatio = metrics::Gauge::Null();`

auto* cpu_real_time_ratio = family_factory->NewGaugeFamily(
      "mapping_2d_local_trajectory_builder_cpu_real_time_ratio",
      "sensor duration / cpu duration.");

kLocalSlamCpuRealTimeRatio = cpu_real_time_ratio->Add({});

if (last_thread_cpu_time_seconds_.has_value()) {
    const double thread_cpu_duration_seconds =
        thread_cpu_time_seconds - last_thread_cpu_time_seconds_.value();
    if (sensor_duration.has_value()) {
      kLocalSlamCpuRealTimeRatio->Set(
          common::ToSeconds(sensor_duration.value()) / thread_cpu_duration_seconds );
    }
  }

kGlobalConstraintsFoundMetric->Increment();
kGlobalConstraintScoresMetric->Observe(score);