自主导航系列5-无先验走迷宫

自主导航系列5-无先验走迷宫

1,编写靠左走的仿真程序

/*****************************************************
 * @author:jianzhuozhu
 * E-mail:sz170320221@163.com
 * Date:2020/7/19
 * Description:
*/
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include "ros/ros.h"
#include "std_msgs/String.h"
#include "std_msgs/Bool.h"
#include "std_msgs/Float32.h"
#include<geometry_msgs/Twist.h>
#include "sensor_msgs/Image.h"
#include "std_msgs/Float64MultiArray.h"          
​
using namespace  std;
geometry_msgs::Twist cmd_red;
​
void obsCallback(const std_msgs::Float64MultiArray obs)
{
    printf("obs.data:  %lf\n",obs.data[0]);
    if((obs.data[0]>0.5))
    {
        cmd_red.linear.x = 0.1;
        cmd_red.linear.y = 0;
        cmd_red.linear.z = 0;
        cmd_red.angular.x = 0;
        cmd_red.angular.y = 0;
        cmd_red.angular.z = 0;
    }
    cmd_red.angular.z = -(obs.data[3]-0.3)*1.9;
    if((obs.data[0]<0.5))
    {
        cmd_red.linear.x = 0;
        cmd_red.linear.y = 0;
        cmd_red.linear.z = 0;
        cmd_red.angular.x = 0;
        cmd_red.angular.y = 0;
        cmd_red.angular.z = 0.5;
    }
}
​
int main(int argc, char **argv)
{
    ROS_WARN("*****START");
    ros::init(argc,argv,"velcontrol");//初始化ROS节点
    ros::NodeHandle n;
    ros::Rate loop_rate(10);//定义速度发布频率
    ros::Publisher pub = n.advertise<geometry_msgs::Twist>("/cmd_vel", 5); //定义速度发布器
    ros::Subscriber origin_scan_data = n.subscribe("/obs", 1, obsCallback);//订阅/obs的信息
​
    while (ros::ok())
    {
        pub.publish(cmd_red);
        ros::spinOnce();
        loop_rate.sleep();
    }
    return 0;
}
​

在仿真环境里面跑效果自然是不咋样的，现在最重要的任务是写好识别路口的代码

2，其他算法

三叉树的算法需要定位信息，我们现在无法拿到定位信息，我们先研究一下一般的走迷宫算法

1，回溯算法，实际上就是我们所说的三叉树算法

2，广度优先算法，简而言之，就是从起点开始，首先遍历起点周围邻近的点，然后再遍历已经遍历过的点邻近的点，逐步的向外扩散，直到找到终点。

但是对于小车说，广度优先算法会导致小车反复跑，会导致时间浪费的问题。

3，A *算法，A *算法通过下面这个函数来计算每个节点的优先级

f(n)是节点n的综合优先级。当我们选择下一个要遍历的节点时，我们总会选取综合优先级最高（值最小）的节点。g(n) 是节点n距离起点的代价。h(n)是节点n距离终点的预计代价，这也就是A*算法的启发函数。

这需要将地图栅格化，对激光雷达探测到的每一个栅格，进行cost函数计算，取出cost最小的那个点，依次的搜寻下去，直到找到终点。从终点开始，每个方格沿着父节点移动直至起点，这就是你的路径。

详见：a星算法

3，move_base仿真

仿真包

 

注：有关于gmapping概念的一些理解