问题:
现在已有基于视觉slam建立的map地图,但在该地图基础上想进行导航规划时,发现map和odom并没有建立正确的tf关系。
参考:
ROS_DEBUG("New pose: %6.3f %6.3f %6.3f",
hyps[max_weight_hyp].pf_pose_mean.v[0],
hyps[max_weight_hyp].pf_pose_mean.v[1],
hyps[max_weight_hyp].pf_pose_mean.v[2]);
// hyps[max_weight_hyp].pf_pose_mean.v[0], [1], [2] 就代表了Mp 也就是机器人的位姿那么位姿的格式是(x,y,theta)最后一个是yaw偏航角,
// subtracting base to odom from map to base and send map to odom instead
tf::Stamped<tf::Pose> odom_to_map;
try
{
tf::Transform tmp_tf(tf::createQuaternionFromYaw(hyps[max_weight_hyp].pf_pose_mean.v[2]), tf::Vector3(hyps[max_weight_hyp].pf_pose_mean.v[0],
hyps[max_weight_hyp].pf_pose_mean.v[1],
0.0));
// tmp_tf = 从map原点看base_link的位置 为yaw生成四元数,最后的0.0是(x,y,z)的Z的值为0 因为这是在二维平面中。
tf::Stamped<tf::Pose> tmp_tf_stamped (tmp_tf.inverse(),
laser_scan->header.stamp,
base_frame_id_);
//tmp_tf.inverse() = 以为Mp为坐标的原点,地图map原点相对于Mp的位置,就是在base_link坐标系下map 原点的位置
this->tf_->transformPose(odom_frame_id_,
tmp_tf_stamped,
odom_to_map);
//进行 base_link坐标系下的点转换到odom坐标系,也就是把map原点转换到odom坐标系下,等于从odom原点看map原点的位置。放到latest_tf_变量里面*******因为odom到base_link的tf变换是已知的(从里程计发布),所以可以通过上面的函数tranformpose进行转换。这一步是重点,然后后面就开始发送tf变换了
}
catch(tf::TransformException)
{
ROS_DEBUG("Failed to subtract base to odom transform");
return;
}
- gmapping中map->odom的转换
gmapping 通过“粒子滤波"实现定位。具体通过粒子与已经产生的地图进行scanMatch,矫正里程计误差实现。 在定位的同时,每次经过map_update_interval_时间,进行地图更新updateMap(*scan)。相对cartographer,缺少闭环,所以计算量很小,实测,局部地图比carto清晰,质量较好。
//gmapping/src/slam_gmapping.cpp
void SlamGMapping::startLiveSlam()
{
... // 订阅激光数据
scan_filter_->registerCallback(boost::bind(&SlamGMapping::laserCallback, this, _1));
}
void SlamGMapping::laserCallback(const sensor_msgsList item::LaserScan::ConstPtr& scan)
{
// We can't initialize the mapper until we've got the first scan
if(!got_first_scan_)
{
if(!initMapper(*scan))
return;
}
GMapping::OrientedPoint odom_pose;
if(addScan(*scan, odom_pose))// addscan:对新激光,结合里程计数据进行粒子滤波矫正得出最新位置
{ // 以下通过getBestParticle的位置(即,当前激光位置)和里程计最终,算出map-to-odom,发布出去
GMapping::OrientedPoint mpose = gsp_->getBestParticleIndex().pose;
tf::Transform laser_to_map = tf::Transform(mpose...);
tf::Transform odom_to_laser = tf::Transform(odom_pose...);
map_to_odom_ = (odom_to_laser * laser_to_map).inverse();
// 当满足 一定时间间隔 更新地图
if(!got_map_ || (scan->header.stamp - last_map_update) > map_update_interval_)
{
updateMap(*scan);
last_map_update = scan->header.stamp;
}
}
- tf数据格式类型 http://wiki.ros.org/tf/Overview/Data%20Types
分析
AMCL和gmapping中对于map和odom的tf转换都是先得到map->base_link和odom->base_link的关系后,结合这两个关系计算出map->odom的tf关系并进行发布,所以在视觉slam中首先也要得到map->base_link的tf关系,这里可以通过orb的定位模块得到,odom->base_link的tf关系机器人底盘驱动会给出。
- 这样的话odom->base_link的tf关系有什么用?最终不就是要map->base_link的tf关系吗?(待解决)
实践
-
出现问题:Lookup would require extrapolation into the past. Requested time 1593002364.911686420 but the earliest data is at time 1593002364.944419791, when looking up transform from frame [odom] to frame [base_link]
解决:时间戳的问题,转换时统一时间戳,都用ros::Time(0)。tf 缓冲和时间延迟是相关联的。
Time(0) : 是 tf 缓存里的第一个 tf 信息。
now() : 是当前这个时间的 tf 信息。 -
[ERROR] [1385945596.417775629]: Extrapolation Error: Lookup would require extrapolation into the future. Requested time 1385945596.400712013 but the latest data is at time 1385945596.309387000, when looking up transform from frame [odom] to frame [map]
解决:move_base过程中出现这个有可能是版本问题,参考我上一篇博客
https://blog.csdn.net/jUst3Doit/article/details/110601277?spm=1001.2014.3001.5501 -
tf树不稳定,某一时间戳有,下一时间戳又没了(rviz中tf树一会亮一会暗)
执行rosrun tf tf_echo /map /odom
时发现map->odom的转换时几秒才发出一次
一开始以为是sleep的问题,后来发现不是,sleep用来控制发布时间间隔。
我发布是放在一个函数内的,while(ros::ok())时便执行这个函数,后来重写一个节点单独用来发布就解决了,可能是进程阻塞的问题?(或者把ros::Time::now()放到函数外?) -
move_base
Costmap2DROS transform timeout. Current time: 1607404370.1614, global_pose stamp: 1607404368.5138, tolerance: 0.5000
Could not get robot pose, cancelling reconfiguration
1.修改global_costmap_params.yaml中的transform_tolerance参数,调大点。
2.查看map和base_link的发布频率,调大些 -
[ WARN] [1607443346.863876207]: Timed out waiting for transform from base_footprint to map to become available before running costmap, tf error: canTransform: target_frame map does not exist. canTransform: source_frame base_footprint does not exist… canTransform returned after 0.100114 timeout was 0.1.
参考:
https://blog.csdn.net/qq_41906592/article/details/89327900?utm_medium=distribute.pc_relevant.none-task-blog-title-2&spm=1001.2101.3001.4242
https://blog.csdn.net/qq_41906592/article/details/89327900解决:是因为tf转换关系出错,我的问题原因比较蠢,是因为开了机器人master节点在服务器上,但我的roscore没有注册过去。
-
开始规划后机器人不移动,recover行为也没用。
发现发布cmd_vel话题也无法控制小车移动,主要原因是控制的话题被重映射为/cmd_vel_mux/input/navisubscribers:
- name: “Safe reactive controller”
topic: “input/safety_controller”
timeout: 0.2
priority: 10
- name: “Teleoperation”
topic: “input/teleop”
timeout: 1.0
priority: 7
- name: “Switch”
topic: “input/switch”
timeout: 1.0
priority: 6
- name: “Navigation”
topic: “input/navi”
timeout: 1.0
priority: 5 -
[ WARN] [1609858085.267486997]: Map update loop missed its desired rate of 3.0000Hz… the loop actually took 0.3682 seconds
1.加快map和base_link的发布频率,因为在更新地图前要订阅该转换关系。
2.减小参数local_costmap_params.yaml/update_frequency -
[ WARN] [1609858086.276164066]: Control loop missed its desired rate of 2.0000Hz… the loop actually took 1.0089 seconds
1.减小参数move_base_params.yaml/controller_frequency.
2.减小参数costmap_common_params/obstacle_range或costmap_common_params/raytrace_range
https://answers.ros.org/question/328653/cartographer-move_basecontrol-loop-missed-its-desired-rate/
3.修改local_planner为base_local_planner.
附录
- ROS右手坐标系图
- 四元数旋转矩阵
(上图的旋转以xyz为准) - TF inverse of a pose
tf::Transform.inverse()表示取反转换(map->odom —> odom->map)