ROS naviagtion analysis: costmap

写在最前：

尊重原创，尊重他人劳动成果。原文地址：https://blog.csdn.net/u013158492/article/details/50485418

在上一篇文章中moveBase就有关于costmap_2d的使用： planner_costmap_ros_是用于全局导航的地图, controller_costmap_ros_是局部导航用的地图，地图类型为经过ROS封装的costmap_2d::Costmap2DROS*。

move_base.cpp中planner_costmap_ros_的代码如下：

//create the ros wrapper for the planner's costmap... and initializer a pointer we'll use with the underlying map
planner_costmap_ros_ = new costmap_2d::Costmap2DROS("global_costmap", tf_);
planner_costmap_ros_->pause();

move_base.cpp中controller_costmap_ros_代码如下：

//create the ros wrapper for the controller's costmap... and initializer a pointer we'll use with the underlying map
controller_costmap_ros_ = new costmap_2d::Costmap2DROS("local_costmap", tf_);
controller_costmap_ros_->pause();

以下是这个ROS类的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 global and robot base frame names
private_nh.param("global_frame", global_frame_, std::string("map"));
private_nh.param("robot_base_frame", robot_base_frame_, std::string("base_link"));

如果没有找到这两个坐标系的关系或者超时，则构造函数会一直阻塞在这里：

  // we need to make sure that the transform between the robot base frame and the global frame is availablewhile (ros::ok()&& !tf_.canTransform(global_frame_, robot_base_frame_, ros::Time(), ros::Duration(0.1), &tf_error)){ros::spinOnce();if (last_error + ros::Duration(5.0) < ros::Time::now()){ROS_WARN("Timed out waiting for transform from %s to %s to become available before running costmap, tf error: %s",robot_base_frame_.c_str(), global_frame_.c_str(), tf_error.c_str());last_error = ros::Time::now();}// The error string will accumulate and errors will typically be the same, so the last// will do for the warning above. Reset the string here to avoid accumulation.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("%s: Using plugin \"%s\"", name_.c_str(), pname.c_str());copyParentParameters(pname, type, private_nh);boost::shared_ptr<Layer> plugin = plugin_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_);layered_costmap_->setFootprint(padded_footprint_);
}

以下是在footprint.cpp中的定义：

void padFootprint(std::vector<geometry_msgs::Point>& footprint, double padding)
{// pad footprint in placefor (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 movingrobot_stopped_ = false;timer_ = private_nh.createTimer(ros::Duration(.1), &Costmap2DROS::movementCB, this);

这里回调函数movementCB 实现，是通过比较前后两个pose的差，判断机器人是否在移动：

void Costmap2DROS::movementCB(const ros::TimerEvent &event)
{// don't allow configuration to happen while this check occurs// boost::recursive_mutex::scoped_lock mcl(configuration_mutex_);geometry_msgs::PoseStamped new_pose;if (!getRobotPose(new_pose)){ROS_WARN_THROTTLE(1.0, "Could not get robot pose, cancelling reconfiguration");robot_stopped_ = false;}// make sure that the robot is not movingelse{old_pose_ = new_pose;robot_stopped_ = (tf2::Vector3(old_pose_.pose.position.x, old_pose_.pose.position.y,old_pose_.pose.position.z).distance(tf2::Vector3(new_pose.pose.position.x,new_pose.pose.position.y, new_pose.pose.position.z)) < 1e-3) &&(tf2::Quaternion(old_pose_.pose.orientation.x,old_pose_.pose.orientation.y,old_pose_.pose.orientation.z,old_pose_.pose.orientation.w).angle(tf2::Quaternion(new_pose.pose.orientation.x,new_pose.pose.orientation.y,new_pose.pose.orientation.z,new_pose.pose.orientation.w)) < 1e-3);}
}

在构造函数末尾，开启参数动态配置：

  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的线程

void Costmap2DROS::reconfigureCB(costmap_2d::Costmap2DConfig &config, uint32_t level)
{transform_tolerance_ = config.transform_tolerance;if (map_update_thread_ != NULL){map_update_thread_shutdown_ = true;map_update_thread_->join();delete map_update_thread_;}map_update_thread_shutdown_ = false;double map_update_frequency = config.update_frequency;double map_publish_frequency = config.publish_frequency;if (map_publish_frequency > 0)publish_cycle = ros::Duration(1 / map_publish_frequency);elsepublish_cycle = ros::Duration(-1);// find size parametersdouble map_width_meters = config.width, map_height_meters = config.height, resolution = config.resolution, origin_x =config.origin_x,origin_y = config.origin_y;if (!layered_costmap_->isSizeLocked()){layered_costmap_->resizeMap((unsigned int)(map_width_meters / resolution),(unsigned int)(map_height_meters / resolution), resolution, origin_x, origin_y);}// If the padding has changed, call setUnpaddedRobotFootprint() to// re-apply the padding.if (footprint_padding_ != config.footprint_padding){footprint_padding_ = config.footprint_padding;setUnpaddedRobotFootprint(unpadded_footprint_);}readFootprintFromConfig(config, old_config_);old_config_ = config;map_update_thread_ = new boost::thread(boost::bind(&Costmap2DROS::mapUpdateLoop, this, map_update_frequency));
}

mapUpdateLoop() 的线程函数定义：

void Costmap2DROS::mapUpdateLoop(double frequency)
{// the user might not want to run the loop every cycleif (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 timeif (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 posegeometry_msgs::PoseStamped pose;if (getRobotPose (pose)){double x = pose.pose.position.x,y = pose.pose.position.y,yaw = tf2::getYaw(pose.pose.orientation);layered_costmap_->updateMap(x, y, yaw);geometry_msgs::PolygonStamped footprint;footprint.header.frame_id = global_frame_;footprint.header.stamp = ros::Time::now();transformFootprint(x, y, yaw, padded_footprint_, footprint);footprint_pub_.publish(footprint);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 positionif (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 lockboost::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：这个阶段会更新每个Layer的更新区域，这样在每个运行周期内减少了数据拷贝的操作时间。

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

第二个阶段是 ` UpdateCosts ：

  for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins_.begin(); plugin != plugins_.end();++plugin){(*plugin)->updateCosts(costmap_, x0, y0, xn, yn);}

这个阶段将逐一拷贝数据到Master Map，关于Master Map是如何得到的，见下图，图来源于David Lu的Paper《Layered Costmaps for Context-Sensitive Navigation》：

函数

void Costmap2DROS::start(),这里通过成员变量layered_costmap_拿到类LayeredCostmap的数据成员std::vector<boost::shared_ptr<Layer> > plugins_; ，然后调用没个Layer 的子类的方法(*plugin)->activate();

void Costmap2DROS::start()
{std::vector < boost::shared_ptr<Layer> > *plugins = layered_costmap_->getPlugins();// check if we're stopped or just pausedif (stopped_){// if we're stopped we need to re-subscribe to topicsfor (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 runros::Rate r(100.0);while (ros::ok() && !initialized_)r.sleep();
}

函数 void Costmap2DROS::stop()

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

函数 void 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();}
}

函数 bool Costmap2DROS::getRobotPose，这里只需要指定global_pose 和 robot_pose 各自的frame_id_ 就可以通过tf_.transformPose(global_frame_, robot_pose, global_pose); 获得机器人的 global_pose 。

bool Costmap2DROS::getRobotPose(geometry_msgs::PoseStamped& global_pose) const
{tf2::toMsg(tf2::Transform::getIdentity(), global_pose.pose);geometry_msgs::PoseStamped robot_pose;tf2::toMsg(tf2::Transform::getIdentity(), robot_pose.pose);robot_pose.header.frame_id = robot_base_frame_;robot_pose.header.stamp = ros::Time();ros::Time current_time = ros::Time::now();  // save time for checking tf delay later// get the global pose of the robottry{tf_.transform(robot_pose, global_pose, global_frame_);}catch (tf2::LookupException& ex){ROS_ERROR_THROTTLE(1.0, "No Transform available Error looking up robot pose: %s\n", ex.what());return false;}catch (tf2::ConnectivityException& ex){ROS_ERROR_THROTTLE(1.0, "Connectivity Error looking up robot pose: %s\n", ex.what());return false;}catch (tf2::ExtrapolationException& ex){ROS_ERROR_THROTTLE(1.0, "Extrapolation Error looking up robot pose: %s\n", ex.what());return false;}// check global_pose timeoutif (current_time.toSec() - global_pose.header.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.header.stamp.toSec(), transform_tolerance_);return false;}return true;
}

函数 void Costmap2DROS::getOrientedFootprint 完成将机器人坐标系下的机器人轮廓点的坐标转化为机器人在当前全局坐标系下的轮廓点的值。具体定义如下：

void Costmap2DROS::getOrientedFootprint(std::vector<geometry_msgs::Point>& oriented_footprint) const
{geometry_msgs::PoseStamped global_pose;if (!getRobotPose(global_pose))return;double yaw = tf2::getYaw(global_pose.pose.orientation);transformFootprint(global_pose.pose.position.x, global_pose.pose.position.y, yaw,padded_footprint_, oriented_footprint);
}

到此，基本上 Costmap2DROS 的定义就这么多了。不过其中类和类之间的调用关系依然还是很复杂，因此需要需要分析plugin原理，才能真正知道这些类的关系是如何实现的。

ROS naviagtion analysis: costmap_2d--Costmap2DROS相关推荐

ROS 教程2 机器人雷达建图蒙特卡洛定位导航路径规划仿真
ros 机器人雷达建图蒙特卡洛定位导航路径规划仿真 move_base gmapping acml 博文github 一.安装 turtlebot 移动机器人底座进行导航 1.安装系统依赖 ...
ROS Navigation-----DWA
DWA 简介: 该包使用DWA(Dynamic Window Approach)算法实现了平面上移动机器人局部导航功能. 输入全局规划和代价地图后,局部规划器将会生成发送给移动底座的命令.该包适用于 ...
costmap_2d（1）
costmap_2d 1.简介这个包提供了一个 2D 代价地图的实现,它从世界上获取传感器数据,构建数据的 2D 或 3D 占用网格(取决于是否使用基于体素的实现),并在基于占用网格和用户指定的膨胀 ...
dwa的区别 teb_ROS与navigation教程-dwa_local_planner（DWA）
ROS与navigation教程-dwa_local_planner(DWA) 说明: 介绍dwa_local_planner(DWA)的概念和相关知识. 代码库概要该包使用DWA(Dynamic ...
ROS与navigation教程——概述
navigation是ROS的二维导航功能包,简单来说,就是根据输入的里程计等传感器的信息流和机器人的全局位置,通过导航算法,计算得出安全可靠的机器人速度控制指令. 代码库:https://githu ...
第一章：costmap_2d代价地图生成原理
系列文章目录提示:这里可以添加系列文章的所有文章的目录,目录需要自己手动添加 TODO:写完再整理文章目录系列文章目录前言 (1)[代价地图]costmap_2d功能包源码解读 (1)代价地图 ...
ROS源代码阅读（9）——DWA算法
2021SC@SDUSC ROS源代码阅读(9) 后面主要看dwa算法 DWAPlannerROS是封装类,提供了与move_base的接口,而DWAPlanner是具体实现类,它非常依赖costma ...
costmap_2d 代价地图
导航功能包用代价地图来存储障碍物的信息,代价地图自动获取传感器的信息来进行自我更新.传感器被用来在地图中标记障碍物信息/清除障碍物信息.标记障碍物信息的过程其实是更改单元值的过程. costmap_2 ...
ROS Wiki教程归纳
-1,CMakeLists:http://wiki.ros.org/catkin/CMakeLists.txt 0,ROS基础教程:http://wiki.ros.org/ROS/Tutorials ...

ROS naviagtion analysis: costmap_2d--Costmap2DROS

函数

ROS naviagtion analysis: costmap_2d--Costmap2DROS相关推荐

最新文章

热门文章