技术标签: navigation ROS costmap_2d ROS Navigation
写在最前:
尊重原创,尊重他人劳动成果。原文地址: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 available
while (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 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)
{
// 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 moving
else
{
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);
else
publish_cycle = ros::Duration(-1);
// find size parameters
double 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 cycle
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
geometry_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 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
:这个阶段会更新每个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 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();
}
函数
void 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;
}
函数 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 robot
try
{
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 timeout
if (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原理,才能真正知道这些类的关系是如何实现的。
文章浏览阅读2.3w次,点赞21次,收藏71次。一. 下载Unity HubUnity官网下载 Unity Hub :https://unity.cn/releases先注册,登录点击如下图的链接下好安装后快捷方式如下图:运行Unity Hub,获得个人使用的授权,再依次点击左侧菜单栏的安装->右上方的安装,然后勾选需要的版本,本文选的是2019.4.12f1(LTS)版本,下滑勾选语言包->下一步。LTS即 Long Term Support就是字面意思长期支持版本,也就是长期支持的版本,会定期更新,可以理解为稳定版。_unity hub官网
文章浏览阅读303次。/proc/sys/net/ipv4/icmp_timeexceed_rate这个在traceroute时导致著名的“Solaris middle star”。这个文件控制发送ICMP Time Exceeded消息的比率。/proc/sys/net/ipv4/igmp_max_memberships主机上最多有多少个igmp (多播)套接字进行监听。/proc/sys/net/ipv4/inet_peer__/proc/sys/net/ipv4/icmp_timeexceed_rate
文章浏览阅读1k次。用AD绘制PCB板之覆铜一、开始覆铜(1)进行如下操作(2)设置网路(3)相关处理a.修改铜与线之间的间距一、开始覆铜(1)进行如下操作(2)设置网路这里在为顶层覆铜,后面处理底层勾选去除死铜并apply为底层覆铜:ctrl+c 选择参考点然后ctrl+v,然后把顶层改为底层就可以了做到这覆铜就完成了,板子基本也完成了,但还要进行一些处理(3)相关处理a.修改铜与线之间的间距..._ad顶层铺铜是什么文件
文章浏览阅读1.1w次,点赞6次,收藏22次。一、编写接口所需几样工具或软件(均是win7+64位):1.phpStudy、SQLyog和编码工具(sublime text/webStorm/vs code均可,按自己习惯来);2.安装好phpStudy之后,打开软件,点击启动;如果Apache和MySQL右边的显示都是绿色的,那么说明服务启动成功;另外注意一下开始的PHP服务版本,因为不同的版本对应不同node.js版本或SQLyo..._php 写api
文章浏览阅读812次。平时做图和海报的时候,通常有一件事让我们十分头疼,就是找背景图,特别是想要选取合适的纹理背景的时候,尤其头疼。优图网 收集了非常棒的纹理背景图。主要是大理石纹理及玫瑰色金箔纹理背景图,使用场景非常多,也特别能够凸显高级感,真的是每一款都让人疯狂心动。珠光金纹理背景珠光金纹,单从她那浪漫的名字,就能引起人们无数美妙的联想。色调柔和迷人的玫瑰金以她特有的风格与文化,演绎出又一片崭新天地。这个颜色浪漫而又典雅大气,很适合严肃端庄的大型场合的海报、邀请函及其他材料。波点圆点珠光亮片无边背景圣_海报纹理有哪些类型
文章浏览阅读1k次。本系列文章由@浅墨_毛星云 出品,转载请注明出处。 文章链接: http://blog.csdn.net/poem_qianmo/article/details/49719247作者:毛星云(浅墨) 微博:http://weibo.com/u/1723155442本文工程使用的Unity3D版本: 5.2.1 _unity indirect multiple
文章浏览阅读1k次。SAP与OBS通讯接口架构及技术说明_obs接口文档
文章浏览阅读221次。事件处理直接绑定指定事件,事件类型即方法名,支持click、focus、blur、submit等。$("#button").click(function(){//script goes here});用on来绑定事件,off来解绑事件,第一个参数为事件名,第二个参数为回调函数。1.7.2版本开始支持。$("#button").on('click',function(){_jquery disable combotree
文章浏览阅读2.5k次,点赞5次,收藏26次。F280049C是TI公司最新的DSP芯片,100pin非常好用,日常测试可选用lauchpad作为测试开发平台,官方开发板可以放心用。F280049C官方给的资料已经很全面了,不过日常使用还是需要看下TI的英文官方论坛。F280049C的主要官方支持为C2000各种软件包,通常C2000Ware软件包就已经很够用了。里面内容如下,资料很全这是相关文件及其路径:C2000Ware的打开目录C2000Ware_3_01_00_00\device_support\f28004x\docs建立工程需_lauchxl f280049c 脉冲
文章浏览阅读3.8k次,点赞6次,收藏3次。MAC安装composer全局时遇到/usr/local/bin/composer: No such file or directory的问题解决分析原因解决办法分析原因No such file or directory这就表明在/usr/local下不存在bin这个文件夹然后有的人想着直接自己在local下创建一个bin文件夹,在尝试全局安装的指令:sudo mv composer.phar /usr/local/bin/composer结果再进行镜像的安装:composer config _zsh: command not found: composer
文章浏览阅读744次。计算机一级证书丢了能补吗,补办流程是什么,小编整理了相关信息,希望会对大家有所帮助!计算机一级证书丢了能不能补办计算机一级证书丢了可以补办,在教育部考试中心网站进行补办。补办步骤如下:登陆教育部考试中心网,在左边导航栏选择补办合格证明书。2.登录系统后,选择考试项目,申请补办考试合格证明书。3.点击申请。系统在用户提交申请后将对提交资料进行审查,审查合格后,方可办理证书发放手续。全国计算机等级考试..._网络高级管理员证书补办
文章浏览阅读216次。计算机专业本科美国大学排名 - 美国圣路易斯华盛顿大学计算机科学专业怎么样是最次的专业吗,1. 美国圣路易斯华盛顿大学计算机科学专业怎么样是最次的专业吗华盛顿大学圣路易斯分校,成立于 1853 年,是一所综合大学。 学校下设文理学院、建筑学院、艺术学院、商业管理学院、工程及应用科学学院。 强势专业:医药、法律、建筑、工商管理硕士(MBA/EMBA)。计算机科学专业一般般。2. 麻省大学阿姆赫斯...