基于melodic、turtlebot2、Rplidar下的主动建图（active slam）

基于melodic、turtlebot2的自主建图

一、仿真环境下实验

实验流程

roslaunch turtlebot3_gazebo turtlebot3_stage_4.launch

其它地图启动原点不是（0，0），是否有影响还未知？

roslaunch turtlebot3_slam turtlebot3_slam.launch slam_methods:=karto

melodic 下 gmapping安装有点麻烦，便直接使用karto，karto可直接源码安装>

roslaunch obrobot_navigation move_base.launch

启动一个move_base节点。

roslaunch obrobot_navigation auto_gmapping.launch

启动自动建图节点

遇到问题

1. Extrapolation Error: Lookup would require extrapolation into the future. Requested time 139.214000000 but the latest data is at time 139.209000000, when looking up transform from frame [odom] to frame [map]
2. 栅格地图的表示方式？

问题分析

1. tf转换问题

https://www.cnblogs.com/havain/p/11737363.html
是版本问题？
https://blog.csdn.net/shixiaolu63/article/details/84544499
跟tolerance有关？
https://www.ncnynl.com/archives/201611/1079.html
transform传输有延时，应该等待几秒？

出现这个问题一般是因为小车目前无法成功规划出一条正确的局部路径，导致获取的tf关系阻塞，一般不用管，让小车重新规划路径即可。
【更新】后来又遇到这个问题，换kinect版本真的有效！实际上是melodic的move_base包内没有加上listener.waitForTransform("/map", “/odom”,now, ros::Duration(3.0)); ros安装的move_base包我没有找到源码，从网上下一个自己修改也可。

//base_local_planner/goal_functions.cpp  105-111行
      tf::StampedTransform plan_to_global_transform;
      tf.waitForTransform(global_frame, ros::Time::now(),
                          plan_pose.header.frame_id, plan_pose.header.stamp,
                          plan_pose.header.frame_id, ros::Duration(0.5));
      tf.lookupTransform(global_frame, ros::Time(),
                         plan_pose.header.frame_id, plan_pose.header.stamp, 
                         plan_pose.header.frame_id, plan_to_global_transform);

2. ROS中map、costmap数据格式

map与costmap都是以nav_msgs::OccupancyGrid类型发布其topic。

其中整张地图的障碍物信息存放在data数据成员中，data是一个int8类型的vector向量，即一维数组。假设一张pgm的map地图，宽：width，高：height，单位为像素，分辨率为resolution，左下角像素点在世界坐标系下的位置为：（origin_x，origin_y）,单位米，那么世界坐标系下一点（x,y）单位米，假设其在地图中，那么该点对应的data中的索引index为：

$$index=(int)((x-origi{n}_x)/resolution)+((int)((y-origi{n}_y)/resolution))\ast width;$$

那么该点在地图中的信息即为data[index]
即：data是按照那张地图图片的自底向上，自左至右逐个像素点存储的

map中data存储的格式如下：
0:空白区域
100:障碍物
-1:未知
1-99:根据像素的灰度值转换后的值，代表有障碍物的概率
costmap中的data 由0-255转换至0-100。

3. costmap计算规则图例

参考：https://blog.csdn.net/mataiyuan/article/details/81912801

成本函数的计算方法如下：exp(-1.0 * cost_scaling_factor * (distance_from_obstacle – inscribed_radius)) * (costmap_2d::INSCRIBED_INFLATED_OBSTACLE – 1)，公式中costmap_2d::INSCRIBED_INFLATED_OBSTACLE目前指定为254。

收获

1. 在rviz下显示/move_base/DWAPlannerROS/global_plan、/move_base/GlobalPlanner/plan、/move_base/DWAPlannerROS/local_plan这三条路径，方便观察是全局规划还是局部规划出现问题。
2. /move_base/global_costmap/footprint可以显示move_base中为机器人设定的碰撞体积。
3. /move_base/local_costmap/costmap、/move_base/global_costmap/costmap可以显示膨胀后的地图情况，以便调整move_base参数。
4. move_base中的关键参数：
   global_planner_params.yaml:

   allow_unknown: true   # Allow planner to plan through unknown space, default true Needs to have track_unknown_space: true in the obstacle / voxel layer (in costmap_commons_param) to work
   default_tolerance: 5.0 # If goal in obstacle, plan to the closest point in radius default_tolerance, default 0.0
   lethal_cost: 150 # default 253
   neutral_cost: 50 # default 50
   cost_factor: 0.55 # Factor to multiply each cost from costmap by, default 3.0
   

   dwa_local_planner_params.yaml:

   # Trajectory Scoring Parameters
     path_distance_bias: 32.0 # change->64.0      # 32.0   - weighting for how much it should stick to the global path plan
     goal_distance_bias: 64.0 #-------- change 50.0      # 24.0   - wighting for how much it should attempt to reach its goal
     occdist_scale: 0.01 # change 0.5            # 0.01   - weighting for how much the controller should avoid obstacles
   

   costmap_common_params.yaml:

   footprint: [[-0.105, -0.105], [-0.105, 0.105], [0.041, 0.105], [0.041, -0.105]]
   # distance a circular robot should be clear of the obstacle (kobuki: 0.18)
   # cost_scaling_factor and inflation_radius were now moved to the inflation_layer ns
   inflation_layer:
     enabled:  true
     cost_scaling_factor:  8.0 # exponential rate at which the obstacle cost drops off (default: 10)
     inflation_radius:     0.1  # max. distance from an obstacle at which costs are incurred for planning paths.
   

二、真实环境下实验

现实场景没遇到啥不一样的问题，略。