本文介绍了ROS中movebase的global_costmap和local_costmap的差异，包括它们的参数设置如`update_frequency`、`publish_frequency`、`rolling_window`等。local_costmap用于机器人局部避障，具有`rolling_window`属性，而global_costmap则用于全局路径规划。同时，解析了odom和map坐标系转换的过程，以及如何通过tf广播建立这两个坐标系之间的关系。
两份costmap如下：
planner_costmap_ros_ = newcostmap_2d::Costmap2DROS("global_costmap", tf_);
controller_costmap_ros_ = newcostmap_2d::Costmap2DROS("local_costmap", tf_);
看看区别：
他们的参数分别如下；
local_costmap_params.yaml:
local_costmap:
global_frame: odom
robot_base_frame: /base_footprint
update_frequency: 5.0
publish_frequency: 2.0
//注意，其实ros里提供了一种cfg机制，可以建立dynamic_reconfigure::Server并设置callback,当配置变化时，会调用callback。那么那些config会被通知到？需要定义一些cfg文件，config文件制定了关心的config参数。在make时，python处理这些cfg文件生成一些cpp的类，这些类就是具体的config类，他们会从rosparam server获取和设置参数，而rosparam中的参数的源头其实都是这里的yaml文件中的参数。所以我们看到的cfg文件中的和这里同名的参数，那只是说明那些自动生成的类会用到我们在yaml中设置的参数。并且设置了默认，最小，最大值等等。这样以来，我们可以有两条获取config的方法，一是直接访问paramserver,一是访问config。数据源头其实都是yaml文件。
//update frequency控制voidCostmap2DROS::mapUpdateLoop(doublefrequency)这个函数循环执行的频率。每次循环都会调用layeredCostmap的updateMap,将各个layer的数据合并算出结果。publishfrequency用来控制向外publishcostmap的频率。所以增大update_frequency频率会加快内部的地图合成速度。增大publish_frequency会加快发送给rviz的速度。
static_map: false //这个参数貌似是老板本的代码遗留物，目前有了plugins项目后，就不再使用这个了
rolling_window: true //这个costmap是否随着robotroll.默认是false.这里因为是local costmap要随着机器人滚动。而globalcostmap就不需要。当滚动时，layeredcostmap的origin要在每次updatemap时都被更新。
width: 4.0
height: 4.0
resolution: 0.05 //
transform_tolerance: 0.5//这个是Costmap2DROS::getRobotPose函数获取基于globalframe的robotpose所耗时间的一个阈值，超过这个阈值就会有warning.
plugins:
- {name: obstacle_layer, type: "costmap_2d::VoxelLayer"}
- {name: inflation_layer, type: "costmap_2d::InflationLayer"}
global_costmap_params.yaml:
global_costmap:
global_frame: /map
robot_base_frame: /base_footprint
update_frequency: 1.0
publish_frequency: 0.5
static_map: true
transform_tolerance: 0.5
plugins:
- {name: static_layer, type:"costmap_2d::StaticLayer"}
- {name: obstacle_layer, type:"costmap_2d::VoxelLayer"}
- {name: inflation_layer, type:"costmap_2d::InflationLayer"}
看看缺别，看看如何使用这些参数的。
先来看看localcostmap中的odom这个globalframe怎么来的，其实就是坐标系。
kobuki的odometry.cpp中advertise了一个topic叫做”odom”,其实产生的topic叫做/mobile_base/odom,然后kobuki_node的launch文件将其从新remap到了odom上：
./navigation-full/kobuki/kobuki_node/src/library/odometry.cpp:76: odom_publisher = nh.advertise<nav_msgs::Odometry>("odom",50); // topic name and queue size
./navigation-full/kobuki/kobuki_node/launch/minimal.launch:11: <remap from="mobile_base/odom" to="odom"/>
这个是topic,不是frame,他是用来发送里程信息的。
我们可以用rostopicinfo看究竟是谁建立的这个topic.
Odometry.cpp中，向tf不断发送transform信息。这个transform信息的构成：parentframe是odom,childframe是base_footprint.这个transform数据就是告诉系统，基于base_footprint的坐标系的位置相对于odomframe坐标系的关系,transform的origin里面的x,y代表childframe base_footprint原点相对于parentframe odom的原点的偏移，也就是childframe原点在parentframe中的坐标。此外transform里还保存一份旋转角度。这就建立的odom和base_footprint的关联。而base_footprint这个frame是定义在kobuki的urdf里面的。在odometry.cpp中当机器人的位置更新时，会调用不断向tf发送这个transform。因为从kobuki的代码研究发现，odometry.cpp拿到的poseupdate的数据并不是一个累积的相对有某个初始坐标点的数据，而是相对于底座的中心的在很短时间的偏移值，但是，Odometry::update函数将poseupdate叠加到pose里面，所以pose是累积数据（经测试kobuki底座，查看其/odom数据发现其x坐标已知递增，所以pose应该是累积数据），pose作为transform里面的偏移，可以推测出odom坐标系可能就是机器人启动时的开始位置，是固定的，而base_footprint坐标系应该是随着机器人移动的。构造这个transfrom使用这样的数据，并且明确表示该transform的frameid是odom而childframe id是base_footprint,那么就是说这个偏移是base_footprint相对于odomframe的偏移了。base_footprint代表了机器人的当前位置。
特别重要：
看一个transform的例子：
child frame名字在最后。
StampedTransform(consttf::Transform& input, const ros::Time& timestamp, conststd::string & frame_id, const std::string & child_frame_id):
这个transform叫做parentframe到childframe的transform.
也就是说一个transform结构体代表的是从parentframe到childframe的转换，其中的数值x,y代表了childframe坐标原点在parentframe中的坐标。
里面包含的偏移值应该是childframe坐标原点在parentframe坐标系中的坐标。
这个transform发送给tf后，就建立了odom和base_footprint的转换关系。
经验：一般来说，在一个transform结构里，origin的x,y值代表childframe坐标系原点在parentframe坐标系中的位置。
void Odometry::publishTransform(const geometry_msgs::Quaternion&odom_quat)
{
if (publish_tf == false)
return;
//这里的odom_trans的parentframe就是odom,child frame就是base_footprint.
odom_trans.header.stamp = ros::Time::now();
odom_trans.transform.translation.x = pose.x();//这里的pose包含了机器人相对于初始位置的移动位移。
odom_trans.transform.translation.y = pose.y();
odom_trans.transform.translation.z = 0.0;
odom_trans.transform.rotation = odom_quat;
odom_broadcaster.sendTransform(odom_trans);
}
另外，我们看到，在globalcostmap的参数里，globalframe是map.可见localmap和globalmap的基本坐标系都是不一样的。localmap相对于odom,而globalmap相对于map.
我们在slam_gmapping里面找到了对mapframe的使用，并且看到他不断发送mapto odom的transform给tf,这样就能建立map和odom坐标系之间的转换。那么他是如何算出mapto odom的转换的？
在SlamGMapping::laserCallback函数中，当接收到一份激光数据后：
GMapping::OrientedPoint odom_pose;
if(addScan(*scan, odom_pose))//odom_pose应该是laser的中心原点相对于odomframe的坐标。，这个函数将激光数据交给了内部的fastslam算法模块，产生了对应的particle等数据。
{
GMapping::OrientedPoint mpose= gsp_->getParticles()[gsp_->getBestParticleIndex()].pose;
gsp就是gridslam processor,是算法的核心。
mpose是粒子中最好的粒子的地图坐标，可能也就是laser中心相对于map的坐标。
ROS_DEBUG("new best pose: %.3f %.3f %.3f", mpose.x,mpose.y, mpose.theta);
ROS_DEBUG("odom pose: %.3f %.3f %.3f", odom_pose.x,odom_pose.y, odom_pose.theta);
ROS_DEBUG("correction: %.3f %.3f %.3f", mpose.x -odom_pose.x, mpose.y - odom_pose.y, mpose.theta - odom_pose.theta);
至此，我们有了laser中心相对于map,和相对于odom的坐标，那么我们就能够算出map和odom之间的转换。
tf::Transform laser_to_map= tf::Transform(tf::createQuaternionFromRPY(0, 0, mpose.theta),tf::Vector3(mpose.x,mpose.y, 0.0)).inverse();
tf::Transform odom_to_laser =tf::Transform(tf::createQuaternionFromRPY(0, 0, odom_pose.theta),tf::Vector3(odom_pose.x, odom_pose.y, 0.0));
map_to_odom_mutex_.lock();
map_to_odom_ = (odom_to_laser * laser_to_map).inverse();
map_to_odom_mutex_.unlock();
}
void SlamGMapping::publishTransform()
{
map_to_odom_mutex_.lock();
ros::Time tf_expiration = ros::Time::now() +ros::Duration(tf_delay_);
tfB_->sendTransform( tf::StampedTransform (map_to_odom_,tf_expiration,map_frame_,odom_frame_));
map_to_odom_mutex_.unlock();
}
下面继续分析这几个transform是如何算出来的。如何使用的。
map_to_odom，从名字上看就知道这个transform的parentframe是map,child frame是odom.并且transform中的坐标值代表odom坐标系原点在map坐标系中的位置。
map_to_odom可以从(odom_to_laser* laser_to_map).inverse()计算而来。
odom_to_laser，从名字上看出parentframe是odom,child frame是laser.这个transform中包含的坐标值是laser坐标系原点在odom坐标系中的位置。
可以把laser_frame中0,0原点转换成基于odomframe，得到的坐标就是laser原点相对于odomframe的偏移：
//基于laserframe的坐标原点就是centerredlaser pose
centered_laser_pose_ =tf::Stamped<tf::Pose>(tf::Transform(tf::createQuaternionFromRPY(0,0,angle_center),
tf::Vector3(0,0,0)),ros::Time::now(), laser_frame_);
//将基于laserframe的坐标原点转换成基于odomframe的坐标值保存在odom_pose里。这个坐标值在数值上其实等于parentframe是odom,child frame是laserframe，从odom frame到laserframe的transform的数值，所以下面第2行可以直接用这个数值构造一个transform结构。
tf_.transformPose(odom_frame_, centered_laser_pose_, odom_pose);
tf::Transform odom_to_laser= tf::Transform(tf::createQuaternionFromRPY(0, 0, odom_pose.theta),tf::Vector3(odom_pose.x,odom_pose.y, 0.0));
laser_to_map从名字上看，parentframe是laser,child frame是map,比较违反常理，于是可通过计算出map_to_laser后inverse也可以得到。
map_to_laser，从名字上看，就是parentframe是map,child frame是laser坐标原点,其中的坐标值应该代表laserframe原点在map坐标系中的位置。
在建图时，particles中的最好的particle的位置就是基于map坐标的laser原点的坐标：
GMapping::OrientedPoint mpose= gsp_->getParticles()[gsp_->getBestParticleIndex()].pose;
那么以mpose的坐标值作为偏移值构建的transform结构就代表了从parentframe map, 到child frame也就是Laserframe的transform,也就是map_to_laser.因为我们最终要的是lase_to_map的transform，所以需要翻转一下：
tf::Transform laser_to_map =tf::Transform(tf::createQuaternionFromRPY(0, 0, mpose.theta),tf::Vector3(mpose.x, mpose.y, 0.0)).inverse();
需要研究gsp,particles什么东西，这就需要研究gridfast slam算法了。
fastslam算法采用粒子滤波器，这里的particle就是粒子。
Gsp 是gridslam processor,包含了具体的fastslam算法，fastslam算法中采用粒子过滤算法，将移动中的机器人和检测到的数据转换成一系列路径中的粒子，我怀疑每个粒子关联一系列的从该粒子位置观察到的景象，把这些数据融合，过滤，合并得到地图。
扫一扫
3044
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
