agv 路径规划move_base

重要资源：这个大神竟然写了rosjava的教程，值得一看
http://www.iroboapp.org/index.php?title=ROSJAVA_Tutorials_and_References

这个叉车的视频不错
https://www.youtube.com/watch?v=lgf6AF1sFyY

如果能够做到这个视频当中的路径规划该有多好。

---

https://www.youtube.com/watch?v=E-IUAL-D9SY

---

上海思岚的大神
http://v.youku.com/v_show/id_XMTY3ODA2MjEzNg==.html?from=s1.8-1-1.2&spm=a2h0k.8191407.0.0

---

navguide阅读心得
设置minimum的值。线速度是角速度最好都设置成负值。角度的话，那么就可以设置旋转成任意的角度。在DWA当中要取得是旋转值的绝对值。X方面的速度，就是运动方向的速度。Y轴方向的速度，叫做扫射速度。通常情况下设置为0.
全局路径
在move_base当中有两个，一个是全局的路径规划和局部的路径规划。全局的规划的规划，我们可以有三种方式：carrt_planner, navfn和global_planner。
carrot_planner
这是最简单的一种方式。carrot允许机器人尽可能的靠近用户指定的目标点。不需要任何全局的路径规划。但是只是针对短距离的范围。在一些的复杂的室内环境，这种方式并不是很适合。
navfn和global_planner
navfn使用的是Dijkstra算法，找到一条全局的路径，在启点和终点之间。global_planner是一种更加灵活的方式，来替代navfn. global_planner支持A*算法。触发二次方式的逼近。触发网格路径。
全局的规划参数
我们可以通过rqt_reconfigure来看。我们将
将这些参数的设置。

use_dijkstra (ture)
use_quadratic(ture)
use_grid_path(false)
old_navfn_behavior(false)

cost=CONST_NEUTRAL+COST_FACTOR*const_cost_value.
输入的地图代价地图的cost的值是0~252.当COST_NEUTRAL 是50 这个COST_FACTOR大约在0.8来确认这个输入的值，所以就是50~253.如果 这个COST_FACTOR高的话，那么cost，将会很高，在遇到障碍物的时候，总之就是不好。通过实验观测，我们发现：当cost_factor=0.55 neutral_cost=66. lethal_cost=253,这个时候的全局的路径规划效果是最好的。
局部路径规划
动态窗口的算法的步骤
1、对机器人的速度进行离散采样。
2、对于每个采样后的速度，用当前的位置信息去模拟一段时间后小车的速度
3、从向前的运动过程当中，评估每条运动的轨迹。使用不完整的度量，例如，接近障碍物，接近目标，接近全局规划的路径和速度。抛弃原有的存在问题的路径。
4、选择一条得分较高的路径，并且给底盘发布速度。
5、清除和重复。
DWA算法，就是说，当你需要障碍物的时候，给你画一个圆，然后让机器人按照这个圆走。
算法步骤：
1、在速度空间上进行采样。
2、对于特殊情况（例如有障碍）立即将速度置为0
3、评估目标 函数进行评估速度。
4、执行目前最好的速度值
5、重复计算
DWA通过局部地图提供的障碍物的信息进行速度的规划，
图11是模拟1.5秒 图12是模拟4秒

通过DWA进行前进运动的模拟：
重要的参数sim_time
当机器人通过窄门的时候，或者通过两个很近的障碍物的时候，将sim_time的值设置成小于2.0.因为没有充足的时间来计算这个路径。当设置sim_time>5.0，的时候，将会使一个很大很大的圆弧。所以这个sim_time设置成4.0是比较靠谱的。
速度采样 vx_sample vy_sample vth_sample。采样的大小由你的计算机的计算能力决定。大部分情况下，我们将vth_sample的值大于 vx_sample和vy_smaple的值，通常我们设置vx_sample=20,vth_samples=40
simulation granularity粒子仿真。sim granularity 两个轨迹之间步长的轨迹的采样频率。如何这个值设置的很低，那么意味着一个很高的采样频率。同时这需要很高的采样频率，默认的取值是0.025
DWA当中计算Trajactory score计算轨迹的得分
这个分数依赖于 想要到达的目标，从障碍物中清除和前进的速度。

    cost=path_distance_bias*(distance(m) to path from the endpoint of the trajectory)路径到终点的距离
      + goal_distance_bias*(distance(m) to path from the local of the trajectory)局部目标点到终点的距离
    +occdist_scale*(maxmium obstacle cost along the trajectory in obstacle cost(0-254))这个值通常取0-254中间。

我们的目标是将cost最低。path_distance_bias表示的是，局部路径规划应该跟随全局路径规划的比重。这个值越高，那么久会让局部的路径规划的轨迹和全局规划的轨迹越接近。goal_distance_bias，表示的是，机器人到底目标距离的偏差的容忍程度。实验经验表明：goal_distance_bias的值越大，那么机器人距离全局的路径规划就越远。occdist_scale表示机器人距离障碍物的多远的时候，还是规划路径来避开障碍物。这个数据太发哦，将导致，机器人在这个空间中没有办法做出决定。目前，我们设置的是path_distance_bias=32 goal_distance_bias=20.0 occdist_scale=0.02效果是比较理想的。

代价地图的参数
代价地图的参数十分重要对于局部路径规划而言，costmap是由静态地图，障碍物物地图层和膨胀区地图组成，静态地图层直接理解为，从navigation stack当中给的静态SLAM地图。障碍物地图层包括2维的障碍物和3维的障碍物（像素层），膨胀区是障碍物的膨胀层，用来计算每个2维的costmap cell的的代价。
另外还有一个global cost map和一个local costmap。global costmap通过地图中的障碍物产生，局部的通过激光雷达实时的数据产生。
运动的轨迹
膨胀层由cost从0-255，没有像素的都三种状态，占有，空间，未知。下面这个图表示的膨胀递减曲线是如何计算的。我们通过inflation_radius 和 cost_scaling_factor的参数来决定膨胀区。inflation_radius来决定一些东西，我现在还不确定。cost_scaling_factor与单元格的cost成反比。 这个数值设置的越高，将会使得decay曲线变得更加陡峭。
costmap的衰减曲线是一个低倾面。
你应该查看你的激光雷达的扫描的分辨率，如果你想要的得到的地图的分辨率大于激光雷达的分辨率，那么就是出现一些 UNKNOW的点

障碍物层是像素层
他们在costmap当中负责标注障碍物。他们统称为障碍物层。障碍物的标记和清除都是通过传感器（也就是激光雷达）的数据进行标记。
kinect他们会有那种3D的体积块。（其实我觉得就是8叉树地图）
voxel_grid是ROS的一个package
max obstacle height 插入的障碍物的最大米，这个参数应该设置的略微比你的机器人高。
obstacle range 离障碍物的距离
raytrace range 光线追踪的范围
AMCL
AMCL使用了贝叶斯估计
运动恢复
ROS当中有两种恢复运动的方式，第一种，clear_costmap_recovery。第二种，rotate_recovery。清除costmap当中的基本恢复状态，他会旋转360度在原来的位置。

---

叉车的资料：
这个地方主要叉车的型号牌子
http://b2b.hc360.com/supplyself/316166838.html

猫哥那些有一些关于自动回充的和能够实现自主导航的模块，自己还是需要在继续学习一下。

---

下面我就要更行一些全局的全局的路径规到底如何进行插件的控制
下面的大神的YouTube的视频链接
https://www.youtube.com/channel/UCLfA6l3ldcXcfZaxOW_ygrw
首先创建包：

catkin_create_pkg relaxed_astar nav_core roscpp rospy std_msgs

这条语句是

 unsigned int cost=static_cast<int>(costmap_->getCost(ix,iy))

也就是将costmap_->getCost(ix,iy）类型转换成int类型
从这里可以看出，这里输入的是带有&地址的符号，然后在后面的地方是就直接进行使用的

poseStampedMsgToTF

我觉得这条语句的意义是说，将pose的信息输出出来。

        tf::Stamped <tf::Pose> goal_tf;//这里是用来定义的目标的TF
        tf::Stamped <tf::Pose> start_tf;

        poseStampedMsgToTF (goal,goal_tf);
        poseStampedMsgToTF (start,start_tf);

        float startX=start.pose.position.x;
        float statrY=start.pose.position.y;

        float goalX=goal.pose.position.x;
        float goalY=goal.pose.position.y;

        getCorrdinate(startX,startY);//得到这个坐标值
        getCorrdinate(goalX,goalY);

将cell转换成索引。这里的索引的数值是一个int类型。

敲完这个代码给我感受就是：里面使用了很多的容器。我现在还不知道他是如何使用的插件技术，那么接下来我准备好好看看创客制造中关于插件的技术。
这个上面的算法并不是标准的A*算法，而是RA*算法。添加一个nav_core::BaseGlobalPlanner，然后添加到一个插件里面。
首先要添加一个新的类，从nav_core::BaseGlobalPlanner当中，然后我们创建的就是RAstar.h

其实就是这个地方，简单来说就是 定义一个类，然后这个类继承自nav_core::BaseGlobalPlanner.

class RAstarPlannerROS : public nav_core::BaseGlobalPlanner {
public:

 RAstarPlannerROS();  
 RAstarPlannerROS(std::string name, costmap_2d::Costmap2DROS* costmap_ros);

 /** overridden classes from interface nav_core::BaseGlobalPlanner **/
 下面的这两条语句就是重新定义一下这两个类。
 void initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros);
 bool makePlan(const geometry_msgs::PoseStamped& start, 
        const geometry_msgs::PoseStamped& goal, 
        std::vector<geometry_msgs::PoseStamped>& plan
           ); 
 };

添加一些必要的头文件

#include <ros/ros.h>
#include <costmap_2d/costmap_2d_ros.h>
#include <costmap_2d/costmap_2d.h> 
#include <nav_core/base_global_planner.h>
#include <geometry_msgs/PoseStamped.h>
#include <angles/angles.h>
#include <base_local_planner/world_model.h>
#include <base_local_planner/costmap_model.h>

当我们想要使用使用costmap_2d::Costmap2D的时候，就必须添加

#include <costmap_2d/costmap_2d_ros.h>
#include <costmap_2d/costmap_2d.h> 

地图将会通过插件自动导入，因此不需要让costmap获得代价地图。

这就就是使用了命名空间。然后在RAstar_Planner这个命名空间下面，定义了RAstarPlannerROS这个类。

namespace RAstar_lanner {
class RAstarPlannerROS : public nav_core::BaseGlobalPlanner {

其实我们没有必要再这里定义一个命名空间，然后我们在使用的过程中就是用RAstar_lanner::RAstarPlannerROS，因为这个类是继承自 nav_core::BaseGlobalPlanne，所有的这些方法都是overridden（函数的重载，重写）

public:

 RAstarPlannerROS(); 初始化costmap  
 RAstarPlannerROS(std::string name, costmap_2d::Costmap2DROS* costmap_ros);
  /** overriden classes from interface nav_core::BaseGlobalPlanner **/
 void initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros);
 bool makePlan(const geometry_msgs::PoseStamped& start, 
        const geometry_msgs::PoseStamped& goal, 
        std::vector<geometry_msgs::PoseStamped>& plan
          ); 

我们使用下面的这两条语句进行初始化costmap 我们是在costmap上面进行路径规划。

RAstarPlannerROS(); 初始化costmap  
 RAstarPlannerROS(std::string name, costmap_2d::Costmap2DROS* costmap_ros);

下面的这个

 void initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros);

初始化BaseGlobalPlanner. 他可以初始化用于导航的castmap在这个里面确实是实现了了一个ros的句柄。

 bool makePlan(const geometry_msgs::PoseStamped& start, 
        const geometry_msgs::PoseStamped& goal, 
        std::vector<geometry_msgs::PoseStamped>& plan
          ); 

这个bool的方法，是进行函数的重写。最后将存储的参数全部存储到std::vector<geometry_msgs::PoseStamped>& plan当中。这个plan将会自动发布plugin的topic的话题，然后通过这个makeplan将这些话题发布出去。
然后具体是这些类的实现。

#include <pluginlib/class_list_macros.h>
#include "RAstar_ros.h"

将这里的插件技术进行导入。

在使用插件技术，首先要注册这个插件。并且包含上面的两个目录

//register this planner as a BaseGlobalPlanner plugin
PLUGINLIB_EXPORT_CLASS(RAstar_planner::RAstarPlannerROS, nav_core::BaseGlobalPlanner)

make plan的方法，就是start 和 goal 参数需要初始化 位置和目标的位置。 首先我们将x和Y 的值转换到cell的小格子的索引当中。这个再将这个indices的数值给了RA*的规划器。然后这个规划器开始规划路线。
最后再将那些indices转换到x,y当中，然后给了plan vector当中plan.push_back(pose)在那个for循环当中，然后在将规划的路径给了move_base的global planner的模型当中，然后在通过nav_msgs/path发布出去。
然后的事情就是编译了。（这个就简单了）基本上熟悉套路，和知道为什么要怎么做就可以。

编译就是cmakelist当中添加

然后至直接cakitn_make 就可以了。
在这个包的目录下面创建这个插件的名称

然后指名这个插件所在目录，以及这个插件的数据类型，和这个插件父类。

<library path="lib/librelaxed_astar_lib">
 <class name="RAstar_planner/RAstarPlannerROS" type="RAstar_planner::RAstarPlannerROS" base_class_type="nav_core::BaseGlobalPlanner">
   <description>This is RA* global planner plugin by iroboapp project.</description>
 </class>
</library>

然后修改package.xml当中的配置文件，这样能够插件信息输出出来。

 <export>
   <nav_core plugin="${prefix}/relaxed_astar_planner_plugin.xml" />
 </export>

然后再在上面添加这两条语句

然后输入：rospack plugins --attrib=plugin nav_core
将会输出你所有nav_core的插件的位置和类型

我们可以看出这个时候，已经输出这个relaxedA 的插件。
接下来，我们要运行这个插件

在move_base的节点下面，加入这个插件：

然后在里面你可以也的地图的参数的地图放入

三角测距法，获得激光雷达的数据

其实这个地方我们清楚就是两个ROS之间的通信。他们之间通过wifi进行通信

ROS当中单独编译一个包

出现了一些报错，因为没有安装xcaro这包、

ros-indigo-turtlebot-bringup \
ros-indigo-turtlebot-create-desktop 
ros-indigo-joint-state-publisher

---

上传代码到github上面在ubuntu的环境下面

下面扩展一下，将自己的代码上传的github上面。

davidhan@davidhan-ThinkPad-E460:~$ ssh-keygen -t rsa -b 4096 -C "619409713@qq.com"
Generating public/private rsa key pair.
Enter file in which to save the key (/home/davidhan/.ssh/id_rsa): 
Enter passphrase (empty for no passphrase): 
Enter same passphrase again: 
Your identification has been saved in /home/davidhan/.ssh/id_rsa.
Your public key has been saved in /home/davidhan/.ssh/id_rsa.pub.
The key fingerprint is:
76:d9:ea:12:f6:ff:9b:09:52:b9:a7:02:6b:1d:69:c2 619409713@qq.com
The key's randomart image is:
+--[ RSA 4096]----+
|                 |
|                 |
|                 |
|           o .   |
|       .S o.+    |
|       .E.+o .   |
|       . Oo.o .  |
|        +.+. + o |
|       . ..oo.=. |

eval "$(ssh-agent -s)"
Agent pid 59566

然后添加秘钥

davidhan@davidhan-ThinkPad-E460:~$ ssh-add ~/.ssh/id_rsa
Enter passphrase for /home/davidhan/.ssh/id_rsa: 
Identity added: /home/davidhan/.ssh/id_rsa (/home/davidhan/.ssh/id_rsa)

然后将将你的SSH秘钥保存下来。

sudo apt-get install xclip

这个时候直接到这个txt当中，然后ctrl+v就可以了

然后配置一些信息

然后就可以刷命令了

参考博客：
http://blog.csdn.net/ipatient/article/details/51334153

在ROS里面使用GPS的教程
http://www.jianshu.com/p/68d06e23e19e

找到的关于ROS执行Dlite算法的插件，好激动啊。
https://github.com/palmieri/srl_dstar_lite

但是现在是有一些问题的。暂时我还是不想去解决

---

然后现在又跑了一个ompl算法框架下的东西、
https://github.com/windelbouwman/move-base-ompl
然后这个包也是利用的插件技术，应该会有所改变。
OMPL：（The Open Motion Planning Library）开发的已经很完善，但是我感觉已经过时了，因为最早的相关记录是2013年

---

然后我还是把创客智造当中关于自己如何写一个插件的地方看一遍吧。

---

这个博客要存在下来，然后做上位机的部分。
http://blog.csdn.net/lu199012/article/details/73457209
这个就是rosjava的部分。、

---

看来这么多的算法， 也没有一种算法自己能用的到，然后我想分析一下Dijkstr这种算法

//stdint.h是c99引入的标准C库
#include <stdint.h>

里面包含了这两个头文件

#include <global_planner/planner_core.h>
#include <global_planner/expander.h>

在expander.h的头文件里面
构造函数已经吧 这里全部赋予了初始的数值。

 Expander(PotentialCalculator* p_calc, int nx, int ny) :
                unknown_(true), lethal_cost_(253), neutral_cost_(50), factor_(3.0), p_calc_(p_calc) {
            setSize(nx, ny);
        }

 lethal_cost_(253), neutral_cost_(50)

因为我主要是是让他规划的路线之能规划成直线，那么我要改的就是make plan

   bool makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal,std::vector<geometry_msgs::PoseStamped>& plan);
  bool makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal, double tolerance,  std::vector<geometry_msgs::PoseStamped>& plan);

上面做的这两个的差别就是下面的多个了double tolerance
终于找到我要修改的地方了。

bool GlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal,
                           double tolerance, std::vector<geometry_msgs::PoseStamped>& plan) {
    boost::mutex::scoped_lock lock(mutex_);
    if (!initialized_) {
        ROS_ERROR(
                "This planner has not been initialized yet, but it is being used, please call initialize() before use");
        return false;
    }

    //clear the plan, just in case
    plan.clear();

    ros::NodeHandle n;
    std::string global_frame = frame_id_;

    //until tf can handle transforming things that are way in the past... we'll require the goal to be in our global frame
    if (tf::resolve(tf_prefix_, goal.header.frame_id) != tf::resolve(tf_prefix_, global_frame)) {
        ROS_ERROR(
                "The goal pose passed to this planner must be in the %s frame.  It is instead in the %s frame.", tf::resolve(tf_prefix_, global_frame).c_str(), tf::resolve(tf_prefix_, goal.header.frame_id).c_str());
        return false;
    }

    if (tf::resolve(tf_prefix_, start.header.frame_id) != tf::resolve(tf_prefix_, global_frame)) {
        ROS_ERROR(
                "The start pose passed to this planner must be in the %s frame.  It is instead in the %s frame.", tf::resolve(tf_prefix_, global_frame).c_str(), tf::resolve(tf_prefix_, start.header.frame_id).c_str());
        return false;
    }

    double wx = start.pose.position.x;
    double wy = start.pose.position.y;

    unsigned int start_x_i, start_y_i, goal_x_i, goal_y_i;
    double start_x, start_y, goal_x, goal_y;

    if (!costmap_->worldToMap(wx, wy, start_x_i, start_y_i)) {
        ROS_WARN(
                "The robot's start position is off the global costmap. Planning will always fail, are you sure the robot has been properly localized?");
        return false;
    }
    if(old_navfn_behavior_){
        start_x = start_x_i;
        start_y = start_y_i;
    }else{
        worldToMap(wx, wy, start_x, start_y);
    }

    wx = goal.pose.position.x;
    wy = goal.pose.position.y;

    if (!costmap_->worldToMap(wx, wy, goal_x_i, goal_y_i)) {
        ROS_WARN_THROTTLE(1.0,
                "The goal sent to the global planner is off the global costmap. Planning will always fail to this goal.");
        return false;
    }
    if(old_navfn_behavior_){
        goal_x = goal_x_i;
        goal_y = goal_y_i;
    }else{
        worldToMap(wx, wy, goal_x, goal_y);
    }

    //clear the starting cell within the costmap because we know it can't be an obstacle
    tf::Stamped<tf::Pose> start_pose;
    tf::poseStampedMsgToTF(start, start_pose);
    clearRobotCell(start_pose, start_x_i, start_y_i);

    int nx = costmap_->getSizeInCellsX(), ny = costmap_->getSizeInCellsY();

    //make sure to resize the underlying array that Navfn uses
    p_calc_->setSize(nx, ny);
    planner_->setSize(nx, ny);
    path_maker_->setSize(nx, ny);
    potential_array_ = new float[nx * ny];

    outlineMap(costmap_->getCharMap(), nx, ny, costmap_2d::LETHAL_OBSTACLE);

    bool found_legal = planner_->calculatePotentials(costmap_->getCharMap(), start_x, start_y, goal_x, goal_y,
                                                    nx * ny * 2, potential_array_);

    if(!old_navfn_behavior_)
        planner_->clearEndpoint(costmap_->getCharMap(), potential_array_, goal_x_i, goal_y_i, 2);
    if(publish_potential_)
        publishPotential(potential_array_);

    if (found_legal) {
        //extract the plan
        if (getPlanFromPotential(start_x, start_y, goal_x, goal_y, goal, plan)) {
            //make sure the goal we push on has the same timestamp as the rest of the plan
            geometry_msgs::PoseStamped goal_copy = goal;
            goal_copy.header.stamp = ros::Time::now();
            plan.push_back(goal_copy);
        } else {
            ROS_ERROR("Failed to get a plan from potential when a legal potential was found. This shouldn't happen.");
        }
    }else{
        ROS_ERROR("Failed to get a plan.");
    }

    // add orientations if needed
    orientation_filter_->processPath(start, plan);

    //publish the plan for visualization purposes
    publishPlan(plan);
    delete potential_array_;
    return !plan.empty();
}

这个start的数据类型是 geometry_msgs::PoseStamped&, 感觉看完CPP之后看源码很轻松

const geometry_msgs::PoseStamped& start
std::vector<geometry_msgs::PoseStamped>& plan

然后plan的数值都存在这个 向量里面。这个向量其实就是一个模板，往里面传入的参数的类型就是 geometry_msgs::PoseStamped

都到世界坐标系下的x和y

    double wx = start.pose.position.x;
    double wy = start.pose.position.y;

然后找到这个处理processPath的

void OrientationFilter::processPath(const geometry_msgs::PoseStamped& start, 
                                    std::vector<geometry_msgs::PoseStamped>& path)
{
    int n = path.size();
    switch(omode_) {
        case FORWARD:
            for(int i=0;i<n-1;i++){
                pointToNext(path, i);
            }
            break;
        case INTERPOLATE:
            path[0].pose.orientation = start.pose.orientation;
            interpolate(path, 0, n-1);
            break;
        case FORWARDTHENINTERPOLATE:
            for(int i=0;i<n-1;i++){
                pointToNext(path, i);
            }

            int i=n-3;
            double last = getYaw(path[i]);
            while( i>0 ){
                double new_angle = getYaw(path[i-1]);
                double diff = fabs(angles::shortest_angular_distance(new_angle, last));
                if( diff>0.35)
                    break;
                else
                    i--;
            }

            path[0].pose.orientation = start.pose.orientation;
            interpolate(path, i, n-1);
            break;           
    }
}

也就是可以返回这个地方的角度

C 语言里 double atan2(double y,double x) 返回的是原点至点(x,y)的方位角，即与 x 轴的夹角

enum OrientationMode { NONE, FORWARD, INTERPOLATE, FORWARDTHENINTERPOLATE };

但是我觉得这个思路还是断了。
这里的wx,wy表示的在世界坐标系下的x和y mx和my表示的在地图中的x和y

bool GridPath::getPath(float* potential, double start_x, double start_y, double end_x, double end_y, std::vector<std::pair<float, float> >& path) {
    std::pair<float, float> current;
    current.first = end_x;
    current.second = end_y;

    int start_index = getIndex(start_x, start_y);

    path.push_back(current);
    int c = 0;
    int ns = xs_ * ys_;

    while (getIndex(current.first, current.second) != start_index) {
        float min_val = 1e10;
        int min_x = 0, min_y = 0;
        for (int xd = -1; xd <= 1; xd++) {
            for (int yd = -1; yd <= 1; yd++) {
                if (xd == 0 && yd == 0)
                    continue;
                int x = current.first + xd, y = current.second + yd;
                int index = getIndex(x, y);
                if (potential[index] < min_val) {
                    min_val = potential[index];
                    min_x = x;
                    min_y = y;
                }
            }
        }
        if (min_x == 0 && min_y == 0)
            return false;
        current.first = min_x;
        current.second = min_y;
        path.push_back(current);

        if(c++>ns*4){
            return false;
        }

    }
    return true;
}

我还在探索，大神已经吧重要的思路写在git里面了
不得不佩服
https://github.com/stonier/cost_map