navigation中Global_planner进阶，ROS添加自己的全局路径算法

正在测试阶段，暂时可以实现导航，第一次导航是正确的，后面的很大几率出现问题，发送了路径，但是无法进行导航，正在调试......

前言：如何选择Dijkstra算法和A*算法

（1）为什么navigation包里有两个全局算法的包?

navigation包里有global_planner和navfn两个全局算法的包

https://blog.csdn.net/heyijia0327/article/details/45030929

机器人全局路径规划使用的是navfn包，这在move_base的默认参数中可以找到 base_global_planner  (string, default: "navfn/NavfnROS")。而在navigation的源代码中还有一个global_planner的包，该包的源文件夹(navigation-hydro-devel\global_planner\src )下已经有了A*,Dijkstra等算法的实现。可是navfn的源程序中也有这两个算法的实现，貌似根本就没用到global_planner这个文件夹下的源程序。因此最开始直接看用于move_base全局导航的程序时有点一头雾水，为什么有两个用于全局导航的包在ROS里面？到底这两个包navfn和global_planner是什么关系？
 

可以将navfn包和global_planner包理解成一个并列关系，因为他们两个都是用来做全局规划的，两个包里面也都实现了A*,Dijkstra算法。那是不是意味着这两者中的一个包就是多余的呢？其实不是，早期的开发中是用navfn包做导航的，那时候并没有global_planner这个包，并且在navfn的源代码里可以看到这个包默认是使用Dijkstra做全局路径规划，并且有A*的代码，那为什么没有使用A*呢？幸好有人在ROS answers里问了这个问题，也引来了众开发者回答：

      根据12年bhaskara的回答，意思是navfn里的A*算法存在bug，没人有时间去弄（在ros的各种答案里经常可以看到开发者说没时间弄，他们确实也相当的忙），直到13年David Lu 才完成了这部分工作，重新发布了global_planner包，修改好的代码封装性更强，更清晰明了。因此，也可以认为global_planner是navfn的替代者。也有人问David Lu为什么没用global_planner替代掉navfn？他的回答是为了和以前兼容。因此可以看到源代码中两个包都在，并且move_base的那个全局变量参数默认的是navfn，也就是说没用global_planner。
 

现在用的都是global_planner包，那为什么不删去呢？我任认为应该是为了给我们重新编写全局算法的一个参考，因为里面的插件的编写是我们编写插件的标准。

我们编写其他的插件都要都要遵循作者的标准，所以这是给我们多了一个参考

（2）如何使用global_planner

rbx1中的fake_move_base_amcl.launch:

这里使用的是navfn包。

https://blog.csdn.net/shixiaolu63/article/details/84544499

如果想要使用global_planner包，则需要在launch中加入：

move_base_prams.yaml

shutdown_costmaps: false

controller_frequency: 4.0
controller_patience: 3.0 # 3.0

# planner_frequency:全局规划操作的执行频率.如果设置为0.0,则全局规划器仅
# 在接收到新的目标点或者局部规划器报告路径堵塞时才会重新执行规划操作.
# planner_patience:在空间清理操作执行前,留给规划器多长时间来找出一条有效规划.
# planner_frequency: 1.0
# planner_patience: 3.0
planner_frequency: 1.0
planner_patience: 3.0

oscillation_timeout: 5.0
oscillation_distance: 0.2

# Planner selection
base_global_planner: "global_planner/GlobalPlanner"  
base_local_planner: "teb_local_planner/TebLocalPlannerROS"

max_planning_retries: 1

recovery_behavior_enabled: true
clearing_rotation_allowed: true

recovery_behaviors:
  - name: 'conservative_reset'
    type: 'clear_costmap_recovery/ClearCostmapRecovery'
  #- name: 'aggressive_reset'
  #  type: 'clear_costmap_recovery/ClearCostmapRecovery'
  #- name: 'super_reset'
  #  type: 'clear_costmap_recovery/ClearCostmapRecovery'
  - name: 'clearing_rotation'
    type: 'rotate_recovery/RotateRecovery'
  #- name: 'move_slow_and_clear'
    #type: 'move_slow_and_clear/MoveSlowAndClear'
    
conservative_reset:
  reset_distance: 1.0
  #layer_names: [static_layer, obstacle_layer, inflation_layer]
  layer_names: [obstacle_layer]

aggressive_reset:
  reset_distance: 3.0
  #layer_names: [static_layer, obstacle_layer, inflation_layer]
  layer_names: [obstacle_layer]

super_reset:
  reset_distance: 5.0
  #layer_names: [static_layer, obstacle_layer, inflation_layer]
  layer_names: [obstacle_layer]

move_slow_and_clear:
  clearing_distance: 0.5
  limited_trans_speed: 0.1
  limited_rot_speed: 0.4
  limited_distance: 0.3

下面来依次解释下各参数的意义：

• shutdown_costmaps:当move_base在不活动状态时,是否关掉costmap.

• controller_frequency:向底盘控制移动话题cmd_vel发送命令的频率.

• controller_patience:在空间清理操作执行前,控制器花多长时间等有效控制下发.

• planner_frequency:全局规划操作的执行频率.如果设置为0.0,则全局规划器仅在接收到新的目标点或者局部规划器报告路径堵塞时才会重新执行规划操作.

• planner_patience:在空间清理操作执行前,留给规划器多长时间来找出一条有效规划.

• oscillation_timeout:执行修复机制前,允许振荡的时长.

• oscillation_distance:来回运动在多大距离以上不会被认为是振荡.

• base_local_planner:指定用于move_base的局部规划器名称.

• base_global_planner:指定用于move_base的全局规划器插件名称.

global_planner_params.yaml

GlobalPlanner:
  allow_unknown: false
  default_tolerance: 0.2
  visualize_potential: false
  use_dijkstra: true
  use_quadratic: true
  use_grid_path: false
  old_navfn_behavior: false

  lethal_cost: 253
  neutral_cost: 50
  cost_factor: 3.0
  publish_potential: true
  orientation_mode: 0
  orientation_window_size: 1

下面来依次解释下各参数意义：

• allow_unknown:是否允许规划器规划穿过未知区域的路径,只设计该参数为true还不行,还要在costmap_commons_params.yaml中设置track_unknown_space参数也为true才行。

• default_tolerance：当设置的目的地被障碍物占据时,需要以该参数为半径寻找到最近的点作为新目的地点.

• visualize_potential：是否显示从PointCloud2计算得到的势区域.

• use_dijkstra：设置为true,将使用dijkstra算法,否则使用A*算法.

• use_quadratic：设置为true,将使用二次函数近似函数,否则使用更加简单的计算方式,这样节省硬件计算资源.

• use_grid_path：如果设置为true,则会规划一条沿着网格边界的路径,偏向于直线穿越网格,否则将使用梯度下降算法,路径更为光滑点.

• old_navfn_behavior：若在某些情况下,想让global_planner完全复制navfn的功能,那就设置为true,但是需要注意navfn是非常旧的ROS系统中使用的,现在已经都用global_planner代替navfn了,所以不建议设置为true.

• lethal_cost：致命代价值,默认是设置为253,可以动态来配置该参数.

• neutral_cost：中等代价值,默认设置是50,可以动态配置该参数.

• cost_factor：代价地图与每个代价值相乘的因子.

• publish_potential：是否发布costmap的势函数.

• orientation_mode：如何设置每个点的方向（None = 0,Forward = 1,Interpolate = 2,ForwardThenInterpolate = 3,Backward = 4,Leftward = 5,Rightward = 6）（可动态重新配置）

• orientation_window_size：根据orientation_mode指定的位置积分来得到使用窗口的方向.默认值1,可以动态重新配置.

teb_local_planner_params.yaml

TebLocalPlannerROS:

 odom_topic: odom
 #odom_topic: /robot_pose_ekf/odom_combined
 map_frame: /odom
 #map_frame: /odom_combined

 # Trajectory

 teb_autosize: True
 dt_ref: 0.45
 dt_hysteresis: 0.1
 global_plan_overwrite_orientation: True
 max_global_plan_lookahead_dist: 3.0
 feasibility_check_no_poses: 5

 # Robot

 max_vel_x: 0.3
 max_vel_x_backwards: 0.15
 max_vel_theta: 0.3
 acc_lim_x: 0.15
 acc_lim_theta: 0.15
 min_turning_radius: 0.0
 footprint_model: # types: "point", "circular", "two_circles", "line", "polygon"
   #radius: 0.23 # for type "circular"
   #vertices: [[-0.1, -0.2], [-0.1, 0.2],[0.3, 0.2], [0.3, -0.2]]
   vertices: [[-0.1643, -0.243], [-0.1643, 0.243],[0.3207, 0.243], [0.3207, -0.243]]

 # GoalTolerance

 xy_goal_tolerance: 0.2
 yaw_goal_tolerance: 0.5
 free_goal_vel: False

 # Obstacles

 min_obstacle_dist: 0.24
 include_costmap_obstacles: True
 costmap_obstacles_behind_robot_dist: 1.0
 obstacle_poses_affected: 7
 costmap_converter_plugin: ""
 costmap_converter_spin_thread: True
 costmap_converter_rate: 5

 # Optimization

 no_inner_iterations: 5
 no_outer_iterations: 4
 optimization_activate: True
 optimization_verbose: False
 penalty_epsilon: 0.1
 weight_max_vel_x: 1
 weight_max_vel_theta: 1
 weight_acc_lim_x: 1
 weight_acc_lim_theta: 1
 weight_kinematics_nh: 1000
 weight_kinematics_forward_drive: 60
 weight_kinematics_turning_radius: 1
 weight_optimaltime: 1
 weight_obstacle: 50
 weight_dynamic_obstacle: 10 # not in use yet
 selection_alternative_time_cost: False # not in use yet

 # Homotopy Class Planner

 enable_homotopy_class_planning: False
 enable_multithreading: True
 simple_exploration: False
 max_number_classes: 4
 roadmap_graph_no_samples: 15
 roadmap_graph_area_width: 5
 h_signature_prescaler: 0.5
 h_signature_threshold: 0.1
 obstacle_keypoint_offset: 0.1
 obstacle_heading_threshold: 0.45
 visualize_hc_graph: False

加了这三个就可以实现global_planner包的实现，至于局部路径规划，我们使用的是teb，还有一个是dwa，后面再介绍

注意：不要用teb，还是用dwa吧

dwa_local_planner_params.yaml

DWAPlannerROS:

# Robot Configuration Parameters - stdr robot

  acc_lim_x: 0.3  # maximum is theoretically 2.0

  acc_lim_y: 0.0  # diff drive robot

  acc_lim_th: 0.3

 

  max_trans_vel: 0.3 #choose slightly less than the base's capability

  min_trans_vel: 0.1 #this is the min trans velocity when there is negligible rotational velocity

 

  max_vel_x: 0.3

  min_vel_x: -0.1

  max_vel_y: 0.0  #diff drive robot,don't need set vel_y

  min_vel_y: 0.0

 

  max_rot_vel: 0.5  #choose slightly less than the base's capability

  min_rot_vel: 0.1  #this is the min angular velocity when there is negligible translational velocity

 

# Goal Tolerance Parameters

  yaw_goal_tolerance: 0.1  # 0.1 rad = 5.7 degree

  xy_goal_tolerance: 0.12

  latch_xy_goal_tolerance: false

 

# Forward Simulation Parameters

  sim_time: 2.0    # 1.7

  sim_granularity: 0.025

  vx_samples: 6    # default 3

  vy_samples: 1    # diff drive robot, there is only one sample

  vth_samples: 20  # 20

  controller_frequency: 5.0

 

# Trajectory Scoring Parameters

  path_distance_bias: 90.0      # 32.0

  goal_distance_bias: 24.0      # 24.0

  occdist_scale: 0.3            # 0.01

  forward_point_distance: 0.325 # 0.325

  stop_time_buffer: 0.2         # 0.2

  scaling_speed: 0.20           # 0.25

  max_scaling_factor: 0.2       # 0.2

  publish_cost_grid: false

 

# Oscillation Prevention Parameters

  oscillation_reset_dist: 0.05  # default 0.05

 

# Global Plan Parameters

  prune_plan: false

 

1.获得地图信息

static const unsigned char NO_INFORMATION = 255;

static const unsigned char LETHAL_OBSTACLE = 254;

static const unsigned char INSCRIBED_INFLATED_OBSTACLE = 253;

static const unsigned char FREE_SPACE = 0;

继承自Layer类，实现了其定义的虚函数接口。

总的来说，该类干了这样一件事情，读取父层LayeredCostmap上costmap的障碍物信息，并对范围内的每一个点更新其cost值，依据的是点离其最近障碍物点的距离来指定损失值。

损失值确定方式如下：

（1）若距离为0，则COST为LETHAL_OBSTACLE（254）

（2）若距离小于机器人内半径，则COST为INSCRIBED_INFLATED_OBSTACLE（253）

（3）若距离比内半径远， 则按照距离越远COST越低取值

（4） 距离障碍物越远应该cost越低，所以factor应该根据距离递减，weight_由外部的cost_scaling_factor决定，默认为10,所以要是想要离inflation近的损失更大，应该减小cost_scaling_factor的值252/(e^(dist*weight))。

结论：获得的地图（障碍物为0，空为1，可以看自己算法是怎么实现的从而选择设为0或者1）

int map_i = toIndex(i,j);
int c;
if (costs[map_i]>=lethal_cost_ && !(unknown_ && costs[map_i]==costmap_2d::NO_INFORMATION))
{
    c = 0;
}
else
{
    c = 1;
}
mapData[i*m_width + j] = c;         
                        
                                

2.获得计算后的grid

 if(publish_potential_)
        publishPotential(potential_array_);

https://blog.csdn.net/start_from_scratch/article/details/78268522

其中整张地图的障碍物信息存放在data数据成员中，data是一个int8类型的vector向量，即一维数组。

假设一张pgm的map地图，宽：width，高：height，单位为像素，分辨路为resolution，左下角像素点在世界坐标系下的位置为：（ox,oy）,单位米，那么世界坐标系下一点（x,y）单位米，假设其在地图中，那么该点对应的data中的索引index为：
index = (x-ox)/resolution+(y-oy)/resolution*width

（注：index为int8类型，所以要将结果进行下类型转换），那么该点在地图中的信息即为data[index]
即：data是按照那张地图图片的自底向上，自左至右逐个像素点存储的
map中data存储的格式如下：
0:空白区域
100:障碍物
-1:未知
1-99:根据像素的灰度值转换后的值，代表有障碍物的概率（不确定，猜测是这个含义，实际用的时候把这个值都当作无障碍物）
costmap中data存储格式如下：
0:空白区域
100:障碍物层
99:膨胀层
1-98:距离障碍物层的远近获得的代价值
-1:未知区域

结论：这里的grid和map是不一样的，这里将算法所探测到的点：

potential[next_i] = p_calc_->calculatePotential(potential, costs[next_i] + neutral_cost_, next_i, prev_potential);

这里计算的potential的数组，大小为potential_array_ = new float[nx * ny]

没计算的设为无穷大，计算过得自然就不是无穷大了，下面的就是publishPotential(potential_array_)函数的具体实现：

void GlobalPlanner::publishPotential(float* potential)
{
    int nx = costmap_->getSizeInCellsX(), ny = costmap_->getSizeInCellsY();
    double resolution = costmap_->getResolution();

    //ROS_INFO("nx : %d ny : %d resolution : %f\n",nx,ny,resolution);

    nav_msgs::OccupancyGrid grid;
    // Publish Whole Grid
    grid.header.frame_id = frame_id_;
    grid.header.stamp = ros::Time::now();
    grid.info.resolution = resolution;

    grid.info.width = nx;
    grid.info.height = ny;

    double wx, wy;
    costmap_->mapToWorld(0, 0, wx, wy);
    //地图中左下像素的2-D姿态为（x，y，yaw），偏航为逆时针旋转（yaw = 0表示无旋转）。系统的许多部分目前忽略偏航。
    //wx : -12.175000,wy : -13.775000
    //计算出地图的像素大小
    //printf("wx : %f,wy : %f\n",wx,wy);
    grid.info.origin.position.x = wx - resolution / 2;
    grid.info.origin.position.y = wy - resolution / 2;
    grid.info.origin.position.z = 0.0;
    grid.info.origin.orientation.w = 1.0;

    grid.data.resize(nx * ny);

    float max = 0.0;
    for (unsigned int i = 0; i < grid.data.size(); i++) {
        float potential = potential_array_[i];
        if (potential < POT_HIGH) {
            if (potential > max) {
                //这里什么意思，获得潜在的最大数据
                max = potential;
                
            }
        }
    }
    //printf("max ： %f publish_scale_ : %d\n",max,publish_scale_);
    for (unsigned int i = 0; i < grid.data.size(); i++) {
        if (potential_array_[i] >= POT_HIGH) {
            grid.data[i] = -1;
        } else
        {
            //publish_scale_ = 100
            grid.data[i] = potential_array_[i] * publish_scale_ / max;
        }
    }    
    potential_pub_.publish(grid);
    //现在处理的只是算法探测到的路径上的点，发布的grid
    //0:空白区域,
    //100:障碍物
    //-1:未知
    //1-99:根据像素的灰度值转换后的值，代表有障碍物的概率（不确定，猜测是这个含义，实际用的时候把这个值都当作无障碍物）
}

} //end namespace global_planner

可以看出，当potential_array_[i] >= POT_HIGH的时候说明没有计算。就将此点设为-1，即未知点

其他的就grid.data[i] = potential_array_[i] * publish_scale_ / max，计算的大小就是1-99，全部都是算法探测到的点

上面计算的grid.data[i]数组，再直接进行

ros::Publisher potential_pub_;
potential_pub_.publish(grid);

 将此时计算的grid发布出去。

结论：这个函数发布了计算过的grid.data[i]，就是将我们算法计算的路径全部都放进去了，再发布出去，以便于我们后面路径获得路径。

 

3.获得路径

算法计算出的potential_array_数组，p_calc_->calculatePotential() 采用简单方法计算值为costs[next_i] + neutral_cost_+ prev_potentia 地图代价+单格距离代价(初始化为50)+之前路径代价 为G

getPlanFromPotential(start_x, start_y, goal_x, goal_y, goal, plan)
转到
std::vector > path;
if (!path_maker_->getPath(potential_array_, start_x, start_y, goal_x, goal_y, path))
{
    ROS_ERROR("NO PATH!");
    return false;
}

至于为啥不直接用算法计算出来的路径呢？非得这么麻烦的进行重新计算？

未完待续...