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

问题：

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

参考：

1. 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;  
    }

2. 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;
    }
 }

3. 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关系吗？（待解决）

实践

1. 出现问题：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 信息。

2. [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://editor.csdn.net/md/?articleId=110601277

3. tf树不稳定，某一时间戳有，下一时间戳又没了（rviz中tf树一会亮一会暗）
   执行rosrun tf tf_echo /map /odom时发现map->odom的转换时几秒才发出一次
   一开始以为是sleep的问题，后来发现不是，sleep用来控制发布时间间隔。
   我发布是放在一个函数内的，while(ros::ok())时便执行这个函数，后来重写一个节点单独用来发布就解决了，可能是进程阻塞的问题？（或者把ros::Time::now()放到函数外？）

4. 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的发布频率，调大些

5. [ 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没有注册过去。

6. 开始规划后机器人不移动，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/

7. [ 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

8. [ 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.

附录

1. ROS右手坐标系图
   
2. 四元数旋转矩阵
   
   
   (上图的旋转以xyz为准)
3. TF inverse of a pose
   
   tf::Transform.inverse()表示取反转换（map->odom —> odom->map）