bool ObservationBuffer::isCurrent() const
{
  if (expected_update_rate_ == ros::Duration(0.0))
    	return true;
  // last_updated_ 没有赋值
  bool current = (ros::Time::now() - last_updated_).toSec() <= expected_update_rate_.toSec();
  if (!current)
  {
    ROS_WARN(
        "The %s observation buffer has not been updated for %.2f seconds, and it should be updated every %.2f seconds.",
        topic_name_.c_str(), (ros::Time::now() - last_updated_).toSec(), expected_update_rate_.toSec());
  }
  return current;
}

if(!controller_costmap_ros_->isCurrent())
{
  ROS_WARN("[%s]:Sensor data is out of date, we're not going to allow commanding of the base for safety",ros::this_node::getName().c_str());
  publishZeroVelocity();
  return false;
}

bool LayeredCostmap::isCurrent()
{
  current_ = true;
  for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins_.begin(); plugin != plugins_.end();
      ++plugin)
  {
    current_ = current_ && (*plugin)->isCurrent();
  }
}

#ifndef PACKAGE_PATH_FILE_H
#define PACKAGE_PATH_FILE_H
	......
#endif

#if DEBUG
        temporary_debugger_break();
#endif

namespace Choices
{
  enum Choice
  {
     Choice1,
     Choice2,
     Choice3
  };
}
typedef Choices::Choice Choice;

enum class Choice
{
    Choice1,
    Choice2,
    Choice3
};
Choice c = Choice::Choice1;

#include<iostream>
  
int main()
{
  #if defined __linux__
      std::cout<<"linux system"<<std::endl;
  #elif defined _WIN32
      std::cout<<"windows system"<<std::endl;
  #endif
   
  #if defined  __aarch64__
      std::cout<<"this is arm cpu"<<std::endl;
  #elif defined __x86_64__
      std::cout<<"this id x86 cpu"<<std::endl;
  #endif
      return 0;
}

//判断是不是C++环境，需要注意的是ROS环境中这里为否
#ifdef _cplusplus
  printf("C++\n");
#endif
//判断是不是C环境
#ifdef __STDC__
  printf("C\n");
#endif
//输出语句所在函数，行，文件等参数
printf("function %s: Line 25\n",__func__);   //或者用__FUNCTION__
printf("pretty function %s: Line 25\n",__PRETTY_FUNCTION__);   //函数声明，包括了参数
printf("line: %d\n",__LINE__);
printf("current file: %s\n",__FILE__);
printf("date: %s\n",__DATE__);
printf("time: %s\n",__TIME__);

bool Costmap2DROS::getRobotPose(tf::Stamped<tf::Pose>& global_pose) const
{
  global_pose.setIdentity();
  tf::Stamped < tf::Pose > robot_pose;
  robot_pose.setIdentity();
  robot_pose.frame_id_ = robot_base_frame_;
  robot_pose.stamp_ = ros::Time();
  ros::Time current_time = ros::Time::now();  // save time for checking tf delay later
  try
  {
    tf_.transformPose(global_frame_, robot_pose, global_pose);
  }
  catch (tf::LookupException& ex)
  {
    ROS_ERROR_THROTTLE(1.0, "No Transform available Error looking up robot pose: %s\n", ex.what());
    return false;
  }
  catch (tf::ConnectivityException& ex)
  {
    ROS_ERROR_THROTTLE(1.0, "Connectivity Error looking up robot pose: %s\n", ex.what());
    return false;
  }
  catch (tf::ExtrapolationException& ex)
  {
    ROS_ERROR_THROTTLE(1.0, "Extrapolation Error looking up robot pose: %s\n", ex.what());
    return false;
  }
  // check global_pose timeout
  if (current_time.toSec() - global_pose.stamp_.toSec() > transform_tolerance_)
  {
    ROS_WARN_THROTTLE(1.0,
                      "Costmap2DROS transform timeout. Current time: %.4f, global_pose stamp: %.4f, tolerance: %.4f",
                      current_time.toSec(), global_pose.stamp_.toSec(), transform_tolerance_);
    return false;
  }
  return true;
}

void Transformer::transformPose(const std::string&   target_frame,
                                const Stamped< tf::Pose >&   stamped_in,
                                Stamped< tf::Pose >&   stamped_out 
) const

void Transformer::transformPose(const std::string&   target_frame,
                                const ros::Time&   target_time,
                                const Stamped< tf::Pose >&   stamped_in,
                                const std::string&   fixed_frame,
                                Stamped< tf::Pose >&   stamped_out 
) const

// Get the transform between two frames by frame ID assuming fixed frame.
void Transformer::lookupTransform ( const std::string &   target_frame,
	const ros::Time &   target_time,
	const std::string &   source_frame,
	const ros::Time &   source_time,
	const std::string &   fixed_frame,
	StampedTransform &  transform 
)   const

namespace tf{
  // Pass through exceptions from tf2
  typedef tf2::TransformException TransformException;
  typedef tf2::LookupException LookupException;
  typedef tf2::ConnectivityException ConnectivityException;
  typedef tf2::ExtrapolationException ExtrapolationException;
  typedef tf2::InvalidArgumentException InvalidArgument; 
 }

#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <rosbag/message_instance.h>
#include <boost/foreach.hpp>
#define foreach BOOST_FOREACH
#include <nav_msgs/Path.h>

rosbag::Bag bag;
try{
    bag.open("/home/user/plan.bag", rosbag::bagmode::Write);
}
catch (const rosbag::BagException& ex) {
    ROS_ERROR("open rosbag error: %s", ex.what() );
    return;
}
bag.write("record_plan",ros::Time::now(), coverage_plan);
bag.close();

rosbag::Bag bag;
try{
    bag.open("/home/user/plan.bag", rosbag::bagmode::Read);
}
catch (const rosbag::BagException& ex) {
    ROS_ERROR("open rosbag error: %s", ex.what() );
    return;
}        
std::vector<std::string> topics;
bool bag_read_once = false;

topics.push_back(std::string("record_plan") );
rosbag::View view(bag, rosbag::TopicQuery(topics) );

BOOST_FOREACH(rosbag::MessageInstance const m, view)
{
    nav_msgs::Path::ConstPtr i = m.instantiate<nav_msgs::Path>();
    plan_from_bag.header.frame_id = i->header.frame_id;
    plan_from_bag.header.stamp = i->header.stamp;
    plan_from_bag.header.seq = i->header.seq;

    unsigned int path_size = i->poses.size();
    ROS_INFO("plan from bag size: %zu",i->poses.size() );
    plan_from_bag.poses.reserve(path_size);
    if(bag_read_once)    break;

    plan_from_bag.poses = i->poses;
    bag_read_once = true;
}
bag.close();

/* Create a view and add a query
 * param bag             The bag file on which to run this query
 * param query           The actual query to evaluate which connections to include
 * param start_time      The beginning of the time range for the query
 * param end_time        The end of the time range for the query
 * param reduce_overlap  If multiple views return the same messages, reduce them to a single message
 */
View(Bag const& bag, boost::function<bool(ConnectionInfo const*)> query,
     ros::Time const& start_time = ros::TIME_MIN, ros::Time const& end_time = ros::TIME_MAX, bool const& reduce_overlap = false);

class ROSBAG_DECL TopicQuery
{
public:
    TopicQuery(std::string const& topic);
    TopicQuery(std::vector<std::string> const& topics);

    bool operator()(ConnectionInfo const*) const;

private:
    std::vector<std::string> topics_;
};

// Get the filename of the bag
std::string  getFileName () const

// Get the current size of the bag file (a lower bound) More...
uint64_t  getSize () const
 
bool    isOpen () const

costmap_2d::Costmap2D* global_costmap = planner_costmap_ros_->getCostmap();

// 这个返回类型很不方便
tf::Stamped<tf::Pose> pose;
if(!costmap->getRobotPose(pose))
{
	ROS_ERROR("Cannot clear map because pose cannot be retrieved");
	return;
}
double x = pose.getOrigin().x();
double y = pose.getOrigin().y();
double yaw = tf::getYaw(pose.getRotation());
double yaw_norm = angles::normalize_angle(tf::getYaw(pose.getRotation() ) );

bool Costmap2D::setConvexPolygonCost(const std::vector<geometry_msgs::Point>& polygon, unsigned char cost_value)
{
  // we assume the polygon is given in the global_frame... we need to transform it to map coordinates
  std::vector<MapLocation> map_polygon;
  for (unsigned int i = 0; i < polygon.size(); ++i)
  {
    MapLocation loc;
    // 先将世界系下的多边形顶点转换到地图坐标系，并存放进map_polygon数组
    if (!worldToMap(polygon[i].x, polygon[i].y, loc.x, loc.y))
    {
      // ("Polygon lies outside map bounds, so we can't fill it");
      return false;
    }
    map_polygon.push_back(loc);
  }
  std::vector<MapLocation> polygon_cells;

  // 通过机器人顶点坐标数组map_polygon得到多边形边缘及内部的全部cell，存放在 polygon_cells
  // get the cells that fill the polygon
  convexFillCells(map_polygon, polygon_cells);

  // set the cost of those cells
  // 通过循环对多边形边缘及内部各cell的cost赋值
  for (unsigned int i = 0; i < polygon_cells.size(); ++i)
  {
    unsigned int index = getIndex(polygon_cells[i].x, polygon_cells[i].y);
    costmap_[index] = cost_value;
  }
  return true;
}

void Costmap2D::convexFillCells(const std::vector<MapLocation>& polygon, std::vector<MapLocation>& polygon_cells)
{
  // we need a minimum polygon of a triangle
  if (polygon.size() < 3)
    return;

  // first get the cells that make up the outline of the polygon
   //首先获得轮廓点之间连线的列表，存放在polygon_cells中
  polygonOutlineCells(polygon, polygon_cells);

  // quick bubble sort to sort points by x
  MapLocation swap;
  unsigned int i = 0;
  // 对边上的cell点的x做排序，使其按x坐标升序排列
  while (i < polygon_cells.size() - 1)
  {
    if (polygon_cells[i].x > polygon_cells[i + 1].x)
    {
      swap = polygon_cells[i];
      polygon_cells[i] = polygon_cells[i + 1];
      polygon_cells[i + 1] = swap;

      if (i > 0)
        --i;
    }
    else
      ++i;
  }
   /*  遍历所有x，对每个相同的x，检查y，获得y最大和最小的polygon cell，
   将范围内的所有cell填充进polygon_cells，获得多边形边缘及内部的所有cell   */
  i = 0;
  MapLocation min_pt;
  MapLocation max_pt;
  unsigned int min_x = polygon_cells[0].x;
  unsigned int max_x = polygon_cells[polygon_cells.size() - 1].x;

  // walk through each column and mark cells inside the polygon
  for (unsigned int x = min_x; x <= max_x; ++x)
  {
    if (i >= polygon_cells.size() - 1)
      break;

    if (polygon_cells[i].y < polygon_cells[i + 1].y)
    {
      min_pt = polygon_cells[i];
      max_pt = polygon_cells[i + 1];
    }
    else
    {
      min_pt = polygon_cells[i + 1];
      max_pt = polygon_cells[i];
    }

    i += 2;
    while (i < polygon_cells.size() && polygon_cells[i].x == x)
    {
      if (polygon_cells[i].y < min_pt.y)
        min_pt = polygon_cells[i];
      else if (polygon_cells[i].y > max_pt.y)
        max_pt = polygon_cells[i];
      ++i;
    }

    MapLocation pt;
    // loop though cells in the column
    for (unsigned int y = min_pt.y; y < max_pt.y; ++y)
    {
      pt.x = x;
      pt.y = y;
      polygon_cells.push_back(pt);
    }
  }
}

void Costmap2D::polygonOutlineCells(const std::vector<MapLocation>& polygon, std::vector<MapLocation>& polygon_cells)
{
  PolygonOutlineCells cell_gatherer(*this, costmap_, polygon_cells);
  for (unsigned int i = 0; i < polygon.size() - 1; ++i)
  {
    raytraceLine(cell_gatherer, polygon[i].x, polygon[i].y, polygon[i + 1].x, polygon[i + 1].y);
  }
  if (!polygon.empty())
  {
    unsigned int last_index = polygon.size() - 1;
    // we also need to close the polygon by going from the last point to the first
    raytraceLine(cell_gatherer, polygon[last_index].x, polygon[last_index].y, polygon[0].x, polygon[0].y);
  }
}

template<class ActionType>  inline void raytraceLine(ActionType at, 
				unsigned int x0, unsigned int y0, 
				unsigned int x1, unsigned int y1,
				unsigned int max_length = UINT_MAX )
{
  int dx = x1 - x0;
  int dy = y1 - y0;

  unsigned int abs_dx = abs(dx);
  unsigned int abs_dy = abs(dy);

  int offset_dx = sign(dx);
  int offset_dy = sign(dy) * size_x_;

  unsigned int offset = y0 * size_x_ + x0;

  // we need to chose how much to scale our dominant dimension, based on the maximum length of the line
  double dist = hypot(dx, dy);
  double scale = (dist == 0.0) ? 1.0 : std::min(1.0, max_length / dist);

  // if x is dominant
  if (abs_dx >= abs_dy)
  {
    int error_y = abs_dx / 2;
    bresenham2D(at, abs_dx, abs_dy, error_y, offset_dx, offset_dy, offset, (unsigned int)(scale * abs_dx));
    return;
  }
  // otherwise y is dominant
  int error_x = abs_dy / 2;
  bresenham2D(at, abs_dy, abs_dx, error_x, offset_dy, offset_dx, offset, (unsigned int)(scale * abs_dy));
}