YAML::Node gps_root;
try {
    gps_root = YAML::LoadFile("/home/user/111.yaml");
} catch (YAML::ParserException &ex) {
    std::cerr << "gps.yaml parse failed: " << ex.what() << std::endl;
   exit(-1);
} catch (YAML::BadFile &ex) {
    std::cerr << "gps.yaml load failed: " << ex.what() << std::endl;
   exit(-1);
}

Node LoadFile(const std::string& filename) {
  std::ifstream fin(filename.c_str());
  if (!fin) {
    throw BadFile();
  }
  return Load(fin);
}

Node Load(std::istream& input)
{
	Parser parser(input);
	NodeBuilder builder;
	if (!parser.HandleNextDocument(builder)) {
		return Node();
	}

	return builder.Root();
}

double planner::distance(double x1,double y1,double x2,double y2)
{
    return sqrt( (x1-x2) * (x1-x2) + (y1-y2) * (y1-y2) );
}

bool planner::isAroundFree(unsigned int mx, unsigned int my)
{
    if(mx <= 1 || my <= 1 || mx >= this->costmap_->getSizeInCellsX()-1 || my >= this->costmap_->getSizeInCellsY()-1)
      return false;
    int x,y;
    for(int i=-1;i<=1;i++)
    {
      for(int j=-1;j<=1;j++)
      {
        x = static_cast<int>(mx) + i;
        y = static_cast<int>(my) + j;
        if(this->costmap_->getCost(static_cast<unsigned int>(x),static_cast<unsigned int>(y)) != costmap_2d::FREE_SPACE)
          return false;
      }
    }
    return true;
}

// tolerance：搜索半径,单位米。
// in：输入的点
// out：搜索到的可行点
void planner::getNearFreePoint(const geometry_msgs::PoseStamped in,
                                       geometry_msgs::PoseStamped& out,
                                       double tolerance)
{
    out = in;
    unsigned int grid_size = static_cast<unsigned int>(tolerance/costmap_->getResolution() + 0.5);
    if(grid_size<1)
    {
      out = in;
      return;
    }

    unsigned int mx0,my0;
    if(costmap_->worldToMap(in.pose.position.x,in.pose.position.y,mx0,my0))
    {
      if(this->isAroundFree(mx0,my0))
        return;
      unsigned int minx,maxx,miny,maxy;
      double wx = 0.0,wy = 0.0;
      double min_move_cost = 10000000.0;
      minx = mx0-grid_size>0?mx0-grid_size:0;
      maxx = mx0+grid_size<costmap_->getSizeInCellsX()?mx0+grid_size:costmap_->getSizeInCellsX();
      miny = my0-grid_size>0?my0-grid_size:0;
      maxy = my0+grid_size<costmap_->getSizeInCellsY()?my0+grid_size:costmap_->getSizeInCellsY();
      for(unsigned int i=minx;i<=maxx;i++)
      {
        for(unsigned int j=miny;j<=maxy;j++)
        {
          costmap_->mapToWorld(i,j,wx,wy);
          double current_move_cost = this->distance(in.pose.position.x,in.pose.position.y,wx,wy);
          if(!this->isAroundFree(i,j) || current_move_cost > tolerance)
            continue;
          if(min_move_cost > current_move_cost)
          {
            min_move_cost = current_move_cost;
            out.pose.position.x = wx;
            out.pose.position.y = wy;
          }
        }
      }
    }
}

#include <stdio.h>
#include <zmq.h>
// 显示当前的zeromq版本
int main()
{
    int major, minor, patch;
    zmq_version(&major, &minor, &patch);
    printf("Current ZeroMQ version is %d.%d.%d\n", major, minor, patch);
}

syntax = "proto3";
// 包名:在生成对应的C++文件时，将被替换为名称空间，在代码中会有体现
package Test; 

message Pose {
    float x = 1;
    float y = 2;
    float yaw = 3;
}

Test::Pose*  pose_msg = new Test::Pose();
pose_msg->set_x(1.5);
pose_msg->set_y(2.7);
pose_msg->set_yaw(0);

int main()
{
    int num = 0;
    int count = 0;
    std::chrono::steady_clock::time_point  start, end;
    double duration;

    while(1)
    {
        std::srand(time(0));
        count = rand() % 1000000;
        if(count > 680000)
        {
            if(num==0)
                start = std::chrono::steady_clock::now();

            num++;
            end = std::chrono::steady_clock::now();
            duration = std::chrono::duration<double>(end - start).count();
            cout << "count: " << count << endl;
            cout << "time elapsed " << static_cast<int>(duration) << endl;
        }
        else
        {
            duration = 0;
            num = 0;
            cout << endl;
        }
    }
    return 0;
}

/*
 * @tparam T point type
 * @param first Given the starting point of a line segment
 * @param last The endpoint of a given line segment
 * @param third Given point
 * @return T Distance from point to line
 */
template <typename T>
static T PointToLineDistance(const Point<T>& first, const Point<T>& last, const Point<T>& third) {
    float dis_suqare =
        ((first.y - last.y) * third.x + (last.x - first.x) * third.y +
         (first.x * last.y - last.x * first.y)) *
        ((first.y - last.y) * third.x + (last.x - first.x) * third.y +
         (first.x * last.y - last.x * first.y)) /
        ((last.x - first.x) * (last.x - first.x) + (last.y - first.y) * (last.y - first.y));
    return std::sqrt(dis_suqare);
}

int  a=8, b=3;

//运行时需要实际执行a+b，哪怕编译时就能推导出类型
auto c = a+b;  

//编译期类型推导不可以用auto  c; 直接声明变量，必须同时初始化。
decltype(a+b)  d;