struct Timespan {
	common::Time start;
	common::Time end;
};
Timespan表征扫描的时间范围。

struct Floor {
	std::vector< Timespan> timespans;
	double z; //z轴的中值
};

int main(int argc, char** argv)
{
    ros::init(argc, argv, "launch_subscribers");
    ros::NodeHandle nh;
    Node node;
    node.LaunchSubscribers("base_scan", 1, "/base_odometry/odom", 1, "/torso_lift_imu/data", 1);
    ros::spin();
}

void Node::LaunchSubscribers(const std::string& scan_topic, const int scan_num,
                             const std::string& odom_topic, const int use_odom,
                             const std::string& imu_topic,  const int use_imu
                            )
{
	if(scan_num)
        LaunchLaserScanSub(scan_topic, scan_num);
    if(use_odom)
        LaunchOdomSub(odom_topic, use_odom);
    if(use_imu)
        LaunchImuSub(imu_topic, use_imu);
}

void Node::LaunchLaserScanSub(const std::string& scan_topic, const int scan_num)
{
    subscribers_.push_back(
      { SubscribeWithHandler<sensor_msgs::LaserScan>(
           &Node::HandleLaserScanMessage, 
           scan_num, scan_topic, &node_handle_, this),
       scan_topic}  );
}

// 这里也可以把 int 改为 bool
template <typename MessageType>
::ros::Subscriber SubscribeWithHandler(
    void (Node::*handler)(int, const std::string&,
                          const typename MessageType::ConstPtr&),
    const int num, const std::string& topic,
    ::ros::NodeHandle* const node_handle, Node* const node)
{
  return node_handle->subscribe<MessageType>(
      topic, 1,
      boost::function<void(const typename MessageType::ConstPtr&)>(
          [node, handler, num,
           topic](const typename MessageType::ConstPtr& msg) {
            (node->*handler)(num, topic, msg);
          })  );
}

// 根据上面的void (Node::*handler)部分，参数依次为int,  string&,  MessageType::ConstPtr&
// scan_num 就是main函数里的1
void Node::HandleLaserScanMessage(int scan_num, const std::string& sensor_id,
                                  const sensor_msgs::LaserScan::ConstPtr& msg)
{
    ROS_INFO("range_min: %f, scan_num: %d", msg->range_min, scan_num );
}

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);

int b = 3, c = 12;
qDebug() << std::greater<int>()(b, c);
qDebug() << std::less<int>()(b, c);

int a[]={3,4,1,12,0,8,4};
// 默认从小到大排列
std::sort(a, a+7);
// 从大到小
std::sort(a, a+7, std::greater<int>());
for(int i=0; i < 7; i++)
    cout << a[i] <<"  ";
// 0  1  3  4  4  8  12

struct Frontier
{
    Frontier(double c):
        cost(c){}

    double cost;
};

bool compareFrontier(Frontier f1, Frontier f2)
{
    return (f1.cost < f2.cost);
}
void showFrontiers(const vector<Frontier>& v)
{
    for(auto f: v)
        cout << f.cost << "  ";
    cout << endl;
}

vector<Frontier> frontiers;
frontiers.push_back(Frontier(1.2) );
frontiers.push_back(Frontier(4.9) );
frontiers.push_back(Frontier(12.7) );
frontiers.push_back(Frontier(0.3) );
frontiers.push_back(Frontier(3.6) );
showFrontiers(frontiers);

std::sort(frontiers.begin(), frontiers.end(),
      [](const Frontier& f1, const Frontier& f2) { return f1.cost < f2.cost; });
// std::sort(frontiers.begin(), frontiers.end(), compareFrontier);

showFrontiers(frontiers);

1.2  4.9  12.7  0.3  3.6  
0.3  1.2  3.6  4.9  12.7  

#include <vector>
#include <iostream>
#include <iterator>
#include <algorithm>
 
int main()
{
    std::vector<int> v({1,2,3});
    std::reverse(std::begin(v), std::end(v));
    std::cout << v[0] << v[1] << v[2] << '\n';
 
    int a[] = {4, 5, 6, 7};
    std::reverse(std::begin(a), std::end(a));
    std::cout << a[0] << a[1] << a[2] << a[3] << '\n';
}