问题：

现在已有基于视觉slam建立的map地图，但在该地图基础上想进行导航规划时，发现map和odom并没有建立正确的tf关系。

参考：

AMCL中map->odom的转换

    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 scanif(!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/navi

subscribers:
- 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

参考：https://answers.ros.org/question/349334/teleop-works-for-turtlebot-but-publishing-to-cmd_vel-does-not-for-turtle-bot/
[ 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）

视觉slam建图导航中建立map-＞odom的tf关系相关推荐

【论文阅读记录】基于视觉SLAM建图的无人机路径规划作者：王海
目录一.论文前瞻问题二.论文内容概述 1.SLAM建图与路径规划综述 2.关键问题 3.SLAM中前后端设计部分 4.三维路径规划三.拓展一.论文前瞻问题智能体在陌生环境中的一次自主导航任务 ...
激光SLAM建图过程中的问题MessageFilter [target=odom_combined ]:Dropped 100.00% of messages so far.，解决方案
最近在和师弟用kobuki底盘进行SLAM建图的过程中遇到一个比较棘手的问题,一直没有解决,直到今天我在仿真环境中也碰到了 [ WARN] [1618301922.711685031, 2351.76 ...
ros 启动建图/导航-- Request for map failed； trying again...
问题描述:在启动ros建图导航时,经常遇到request map类似的报错 [ INFO] [1632648620.520173473]: Requesting the map... [ WARN] ...
视觉SLAM⑫----建图（未完）
目录 12.0 本章内容 12.1 概述 12.2 双目稠密重建 12.2.1 立体视觉 12.2.2 极线搜索与块匹配 12.2.3 高斯分布的深度滤波器 12.2.4 像素梯度的问题 12.2.5 ...
SLAM导航机器人零基础实战系列：（六）SLAM建图与自主避障导航——2.google-cartographer机器人SLAM建图...
SLAM导航机器人零基础实战系列:(六)SLAM建图与自主避障导航--2.google-cartographer机器人SLAM建图摘要通过前面的基础学习,本章进入最为激动的机器人自主导航的学习.在 ...
SLAM导航机器人零基础实战系列：（六）SLAM建图与自主避障导航——3.ros-navigation机器人自主避障导航...
SLAM导航机器人零基础实战系列:(六)SLAM建图与自主避障导航--3.ros-navigation机器人自主避障导航摘要通过前面的基础学习,本章进入最为激动的机器人自主导航的学习.在前面的学习 ...
SLAM导航机器人零基础实战系列：（六）SLAM建图与自主避障导航——1.在机器人上使用传感器...
SLAM导航机器人零基础实战系列:(六)SLAM建图与自主避障导航--1.在机器人上使用传感器摘要通过前面的基础学习,本章进入最为激动的机器人自主导航的学习.在前面的学习铺垫后,终于迎来了最大乐趣 ...
SLAM导航机器人零基础实战系列：（六）SLAM建图与自主避障导航——4.多目标点导航及任务调度...
SLAM导航机器人零基础实战系列:(六)SLAM建图与自主避障导航--4.多目标点导航及任务调度摘要通过前面的基础学习,本章进入最为激动的机器人自主导航的学习.在前面的学习铺垫后,终于迎来了最大乐 ...
【SLAM建图和导航仿真实例】（三）- 使用RTAB-MAP进行SLAM建图和导航
引言在这个-SLAM建图和导航仿真实例-项目中,主要分为三个部分,分别是 (一)模型构建 (二)根据已知地图进行定位和导航 (三)使用RTAB-MAP进行建图和导航该项目的slam_bot已经上传 ...

视觉slam建图导航中建立map-＞odom的tf关系

问题：

参考：

分析

实践

附录

视觉slam建图导航中建立map-＞odom的tf关系相关推荐

最新文章

热门文章