ROS naviagtion analysis: costmap_2d--Costmap2DROS

博客轉載自：https://blog.csdn.net/u013158492/article/details/50485418

在上一篇文章中moveBase就有關於costmap_2d的使用： 
planner_costmap_ros_是用於全局導航的地圖，controller_costmap_ros_是局部導航用的地圖，地圖類型為經過ROS封裝的costmap_2d::Costmap2DROS*。 

　　

ROS Costmap2DROS類的UML

看看這個類的成員變量：LayeredCostmap* layered_costmap_; pluginlib::ClassLoader<Layer> plugin_loader_; 這兩個最重要的成員變量，而LayeredCostmap類又包含了Costmap2D costmap_; 這個數據成員。 

類之間的關系

綠色的是核心代碼，從ROS用戶的角度，只需要調用Costmap2DROS這個類，因為這個類已經把所有關於地圖的操作都封裝好了。不過我這里是分析底層算法實現，就不得不寫得很長很長。 

所以還是先回到對Costmap2DROS這個類的分析，然后再進一步一層一層的分析其他的類。這些類完成了對機器人地圖的表示和操作，因此其數據結構和算法都很有分析的價值。首先是構造函數Costmap2DROS::Costmap2DROS(std::string name, tf::TransformListener& tf), 因此必須提供一個tf參數。tf參數需要提供以下兩個坐標系的關系：

// get two frames
  private_nh.param("global_frame", global_frame_, std::string("/map"));
  private_nh.param("robot_base_frame", robot_base_frame_, std::string("base_link"));

如果沒有找到這兩個坐標系的關系或者超時，則構造函數會一直阻塞在這里：

while (ros::ok()
&& !tf_.waitForTransform(global_frame_,robot_base_frame_,ros::Time(), ros::Duration(0.1), ros::Duration(0.01),&tf_error))
  {
    ros::spinOnce();
    if (last_error + ros::Duration(5.0) < ros::Time::now())
    {
      last_error = ros::Time::now();
    }
    tf_error.clear();
  }

然后加入各個層次的地圖：

if (private_nh.hasParam("plugins"))
  {
    XmlRpc::XmlRpcValue my_list;
    private_nh.getParam("plugins", my_list);
    for (int32_t i = 0; i < my_list.size(); ++i)
    {
      std::string pname = static_cast<std::string>(my_list[i]["name"]);
      std::string type = static_cast<std::string>(my_list[i]["type"]);
      ROS_INFO("Using plugin \"%s\"", pname.c_str());

      boost::shared_ptr<Layer> plugin =lugin_loader_.createInstance(type);
      layered_costmap_->addPlugin(plugin);
      plugin->initialize(layered_costmap_, name + "/" + pname, &tf_);
    }
  }

boost::shared_ptr<Layer> plugin = plugin_loader_.createInstance(type); 這行會創建一個以 type為類類型的實例變量，然后讓plugin這個指針指向這個實例。

layered_costmap_->addPlugin(plugin);

然后layered_costmap_ 將這些類型的地圖都加入，addPlugin 實現：

void addPlugin(boost::shared_ptr<Layer> plugin)
{
  plugins_.push_back(plugin);
}

這里的關系是：Costmap2DROS 有一個layered_costmap_ 數據成員，然后layered_costmap_ 又有一個std::vector<boost::shared_ptr<Layer> > plugins_; 成員，因此可以將各個子類的實例化對象的指針交給父類Layer 指針plugins_ 管理。

plugin->initialize(layered_costmap_, name + "/" + pname, &tf_);

這行將會對實例初始化，實際執行是plugin調用的父類Layer的方法void Layer::initialize(LayeredCostmap* parent, std::string name, tf::TransformListener *tf) 。 

實際上父類Layer有一個成員變量為LayeredCostmap* layered_costmap_的指針，因此通過LayeredCostmap* layered_costmap_指針指向了具體的子類，比如ObstacleLayer StaticLayer InflationLayer 等。

然后設置footprint：footprint_sub_ = private_nh.subscribe(topic, 1, &Costmap2DROS::setUnpaddedRobotFootprintPolygon, this) ，回調函數setUnpaddedRobotFootprintPolygon 實際調用的是成員函數：

void Costmap2DROS::setUnpaddedRobotFootprint(const std::vector<geometry_msgs::Point>& points)
{
  unpadded_footprint_ = points;
  padded_footprint_ = points;
  padFootprint(padded_footprint_, footprint_padding_);//這個函數是在padded_footprint_基礎上膨脹，增加了footprint_padding_

  layered_costmap_->setFootprint(padded_footprint_);
}
//以下是在footprint.cpp中的定義：
void padFootprint(std::vector<geometry_msgs::Point>& footprint, double padding)
{
  // pad footprint in place
  for (unsigned int i = 0; i < footprint.size(); i++)
  {
    geometry_msgs::Point& pt = footprint[ i ];
    pt.x += sign0(pt.x) * padding;
    pt.y += sign0(pt.y) * padding;
  }
}

然后聲明了一個timer，定時檢測機器人是否在移動：

// Create a time r to check if the robot is moving
robot_stopped_ = false;
timer_ = private_nh.createTimer(ros::Duration(.1),&Costmap2DROS::movementCB, this);

這里回調函數movementCB 實現，是通過比較前后兩個pose的差，判斷機器人是否在移動：

void Costmap2DROS::movementCB(const ros::TimerEvent &event)
{
  tf::Stamped < tf::Pose > new_pose;
  if (!getRobotPose(new_pose))
  {
    robot_stopped_ = false;
  }
  else if (fabs((old_pose_.getOrigin() - new_pose.getOrigin()).length()) < 1e-3
      && fabs(old_pose_.getRotation().angle(new_pose.getRotation())) < 1e-3)
  {
    old_pose_ = new_pose;
    robot_stopped_ = true;
  }
  else
  {
    old_pose_ = new_pose;
    robot_stopped_ = false;
  }
}

在構造函數末尾，開啟參數動態配置：

dsrv_ = new dynamic_reconfigure::Server<Costmap2DConfig>(ros::NodeHandle("~/" + name));
dynamic_reconfigure::Server<Costmap2DConfig>::CallbackType cb = boost::bind(&Costmap2DROS::reconfigureCB, this, _1, _2);
dsrv_->setCallback(cb);

回調函數reconfigureCB 除了對一些類成員的配置值做賦值以外，還會開啟一個更新map的線程 

map_update_thread_ = new boost::thread(boost::bind(&Costmap2DROS::mapUpdateLoop, this, map_update_frequency));

更新map的線程函數定義：

void Costmap2DROS::mapUpdateLoop(double frequency)
{
  if (frequency == 0.0)    return;
  ros::NodeHandle nh;
  ros::Rate r(frequency);
  while (nh.ok() && !map_update_thread_shutdown_)
  {
    struct timeval start, end;
    double start_t, end_t, t_diff;
    gettimeofday(&start, NULL);

    updateMap();

    gettimeofday(&end, NULL);
    start_t = start.tv_sec + double(start.tv_usec) / 1e6;
    end_t = end.tv_sec + double(end.tv_usec) / 1e6;
    t_diff = end_t - start_t;
    ROS_DEBUG("Map update time: %.9f", t_diff);
    if (publish_cycle.toSec() > 0 && layered_costmap_->isInitialized())
    {
      unsigned int x0, y0, xn, yn;
      layered_costmap_->getBounds(&x0, &xn, &y0, &yn);
      publisher_->updateBounds(x0, xn, y0, yn);

      ros::Time now = ros::Time::now();
      if (last_publish_ + publish_cycle < now)
      {
        publisher_->publishCostmap();
        last_publish_ = now;
      }
    }
    r.sleep();
    // make sure to sleep for the remainder of our cycle time
    if (r.cycleTime() > ros::Duration(1 / frequency))
      ROS_WARN("Map update loop missed its desired rate of %.4fHz... the loop actually took %.4f seconds", frequency,
               r.cycleTime().toSec());
  }
}

核心功能在於調用updateMap()

void Costmap2DROS::updateMap()
{
  if (!stop_updates_)
  {
    // get global pose
    tf::Stamped < tf::Pose > pose;
    if (getRobotPose (pose))
    {
      double x = pose.getOrigin().x(),
             y = pose.getOrigin().y(),
             yaw = tf::getYaw(pose.getRotation());
      layered_costmap_->updateMap(x, y, yaw);
      initialized_ = true;
    }
  }
}

函數layered_costmap_->updateMap(x, y, yaw)定義

void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw)
{
  // if we're using a rolling buffer costmap... we need to update the origin using the robot's position
  if (rolling_window_)
  {
    double new_origin_x = robot_x - costmap_.getSizeInMetersX() / 2;
    double new_origin_y = robot_y - costmap_.getSizeInMetersY() / 2;
    costmap_.updateOrigin(new_origin_x, new_origin_y);
  }

  if (plugins_.size() == 0)
    return;

  minx_ = miny_ = 1e30;
  maxx_ = maxy_ = -1e30;

  for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins_.begin(); plugin != plugins_.end();
      ++plugin)
  {
    (*plugin)->updateBounds(robot_x, robot_y, robot_yaw, &minx_, &miny_, &maxx_, &maxy_);
  }

  int x0, xn, y0, yn;
  costmap_.worldToMapEnforceBounds(minx_, miny_, x0, y0);
  costmap_.worldToMapEnforceBounds(maxx_, maxy_, xn, yn);

  x0 = std::max(0, x0);
  xn = std::min(int(costmap_.getSizeInCellsX()), xn + 1);
  y0 = std::max(0, y0);
  yn = std::min(int(costmap_.getSizeInCellsY()), yn + 1);

  ROS_DEBUG("Updating area x: [%d, %d] y: [%d, %d]", x0, xn, y0, yn);

  if (xn < x0 || yn < y0)
    return;

  {
    // Clear and update costmap under a single lock
    boost::unique_lock<Costmap2D::mutex_t> lock(*(costmap_.getMutex()));
    costmap_.resetMap(x0, y0, xn, yn);
    for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins_.begin(); plugin != plugins_.end();
        ++plugin)
    {
      (*plugin)->updateCosts(costmap_, x0, y0, xn, yn);
    }
  }

  bx0_ = x0;
  bxn_ = xn;
  by0_ = y0;
  byn_ = yn;

  initialized_ = true;
}

updateMap 分為兩個階段，第一個階段是UpdateBounds

(*plugin)->updateBounds(robot_x, robot_y, robot_yaw, &minx_, &miny_, &maxx_, &maxy_);

這個階段會更新每個Layer的更新區域，這樣在每個運行周期內減少了數據拷貝的操作時間。

第二個階段是 UpdateCosts 

(*plugin)->updateCosts(costmap_, x0, y0, xn, yn);

這個階段將逐一拷貝數據到Master Map，關於Master Map是如何得到的，見下圖，圖來源於David Lu的Paper《Layered Costmaps for Context-Sensitive Navigation》

函數void Costmap2DROS::start()

void Costmap2DROS::start()
{
  std::vector < boost::shared_ptr<Layer> > *plugins = layered_costmap_->getPlugins();
  // check if we're stopped or just paused
  if (stopped_)
  {
    // if we're stopped we need to re-subscribe to topics
    for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins->begin(); plugin != plugins->end();
        ++plugin)
    {
      (*plugin)->activate();
    }
    stopped_ = false;
  }
  stop_updates_ = false;
  // block until the costmap is re-initialized.. meaning one update cycle has run
  ros::Rate r(100.0);
  while (ros::ok() && !initialized_)
    r.sleep();
}

這里通過成員變量layered_costmap_拿到類LayeredCostmap的數據成員std::vector<boost::shared_ptr<Layer> > plugins_; ，然后調用這個Layer 的子類的方法(*plugin)->activate();

函數Costmap2DROS::stop()

void Costmap2DROS::stop()
{
  stop_updates_ = true;
  std::vector < boost::shared_ptr<Layer> > *plugins = layered_costmap_->getPlugins();
  // unsubscribe from topics
  for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins->begin(); plugin != plugins->end();
      ++plugin)
  {
    (*plugin)->deactivate();
  }
  initialized_ = false;
  stopped_ = true;
}

函數Costmap2DROS::resetLayers() 

void Costmap2DROS::resetLayers()
{
  Costmap2D* top = layered_costmap_->getCostmap();
  top->resetMap(0, 0, top->getSizeInCellsX(), top->getSizeInCellsY());
  std::vector < boost::shared_ptr<Layer> > *plugins = layered_costmap_->getPlugins();
  for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins->begin(); plugin != plugins->end();++plugin)
  {
    (*plugin)->reset();
  }
}

函數Costmap2DROS::getRobotPose 

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();
  // get the global pose of the robot
  tf_.transformPose(global_frame_, robot_pose, global_pose);
}

這里只需要指定global_pose 和 robot_pose 各自的frame_id_ 就可以通過tf_.transformPose(global_frame_, robot_pose, global_pose)獲得 global_pose 。

函數 Costmap2DROS::getOrientedFootprint(std::vector<geometry_msgs::Point>& oriented_footprint) const 完成將機器人坐標系下的機器人輪廓點的坐標轉化為機器人在當前全局坐標系下的輪廓點的值。具體定義如下：

void transformFootprint(double x, double y, double theta, const std::vector<geometry_msgs::Point>& footprint_spec,
                        std::vector<geometry_msgs::Point>& oriented_footprint)
{
  // build the oriented footprint at a given location
  oriented_footprint.clear();
  double cos_th = cos(theta);
  double sin_th = sin(theta);
  for (unsigned int i = 0; i < footprint_spec.size(); ++i)
  {
    geometry_msgs::Point new_pt;
    new_pt.x = x + (footprint_spec[i].x * cos_th - footprint_spec[i].y * sin_th);
    new_pt.y = y + (footprint_spec[i].x * sin_th + footprint_spec[i].y * cos_th);
    oriented_footprint.push_back(new_pt);
  }
}

到此，基本上 Costmap2DROS 的定義就這么多了。不過其中類和類之間的調用關系依然還是很復雜，因此需要需要分析plugin原理，才能真正知道這些類的關系是如何實現的。 接下來分析costma_2d的其他類。