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

typedef tf::Vector3 	tf::Point
typedef tf::Transform 	tf::Pose

#include "tf2_geometry_msgs/tf2_geometry_msgs.h"

tf2::Quaternion quat_tf;
quat_tf.setRPY(1.0, 0.0, 0.0);
geometry_msgs::Quaternion quat_msg;
tf2::convert(quat_tf, quat_msg);

geometry_msgs::PoseStamped  p;
// 对 p 赋值
// ......
tf2::Quaternion  q;
tf2::convert(p.pose.orientation,  q);

geometry_msgs::TransformStamped   transform;
tf::StampedTransform  tfTransform;
transformStampedTFToMsg(tfTransform , transform);

tf2::Transform  tf2_transform;
tf2::convert(transform.transform, tf2_transform);

// pgm的像素值，或者说保存方式和OpenCV是一样的
costmap_->saveMap("/home/user/costmap.pgm");

// 范围 0~255
cv::Mat map_mat = cv::Mat(m_y, m_x, CV_8U);

for (int x = 0; x < map_mat.cols; x++)
    for (int y = 0; y < map_mat.rows; y++)
    {
        map_mat.at<uint8_t>(map_mat.rows -1 - y, x) = 255 - costmap_->getCharMap()[ y * map_mat.cols + x];
    }

template<class ActionSpec>
bool   actionlib::SimpleActionClient< ActionSpec >::waitForServer (const ros::Duration &  timeout = ros::Duration(0,0) )  [inline]

while( ! ac->isServerConnected() )
{
    ROS_INFO("Waiting for move_base action server to start");
    ac->waitForServer(ros::Duration(1) );
}

ac = new actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction>("move_base", true);
// 注意是 MoveBaseGoal, 成员只有  geometry_msgs/PoseStamped  target_pose
move_base_msgs::MoveBaseGoal  goal;
// 一系列赋值 ......
ac->sendGoal(goal);
// ac->sendGoal(goal, &doneCb, &activeCb, &feedbackCb);

bool finished_before_timeout = ac->waitForResult(ros::Duration(30.0)); 
if (finished_before_timeout)
{
    actionlib::SimpleClientGoalState state = ac->getState();
    ROS_INFO("MoveBase Action finished: %s", state.toString().c_str());
}

// Allows users to register a callback to be invoked when a new goal is available.
void    registerGoalCallback (boost::function< void()> cb)

// Allows users to register a callback to be invoked when a new preempt request is available.
void    registerPreemptCallback (boost::function< void()> cb)