使用Gmapping算法建图与A*路径规划算法导航的智能小车，GUI界面显示_路径规划gazebo

一、基本技术介绍

在Gazebo中建立了一个仿真场景，加入了一个搭载激光雷达，使用四轮差速控制的智能小车。
使用ROS系统通过键盘实现对小车的控制，采用Gmapping算法对仿真场景进行二维激光建图，获得仿真场景的栅格地图。
建立一个GUI界面程序，该界面中显示了建立的地图，并设置了在地图上之间选点的功能。在地图上选择目标点后，使用A*路径规划算法进行路径规划，并控制Gazebo中的智能小车沿着规划的路径前进。

二、效果展示

2.1.展示视频

【ROS】使用Gmapping算法建图与A*路径规划算法导航的智能小车，GUI界面显示

2.2.Gazebo仿真场景

### 2.3.Gmapping建图 ### 2.4.获取的地图

2.5.启动GUI程序

在程序界面中选择目标点，程序自动使用A*算法进行路径规划，程序界面显示规划结果，依据规划结果控制Gazebo中的小车移动。
路劲规划GUI




窗口自适应

三、部分代码展示

界面GUI

class MainWindow(QMainWindow):
    drawPathUpdate = pyqtSignal() 
    def __init__(self):
        super().__init__()
        global resources_directory
        self.resources_directory = resources_directory
        self.image = self.load_img()
        self.start_label_pos = [0, 0]
        self.end_label_pos = [30, 40]
        self.path_label_list = []
        self.pathPatch = QPixmap(self.resources_directory + "red_patch.png")
        self.pathPatch = self.pathPatch.scaled(8, 8)
        self.inPathPlanningProcess = False
        self.setWindowTitle('Path Planning')
        self.setGeometry(0, 0, 800, 600)
        self.mdi_area = QMdiArea()
        self.setCentralWidget(self.mdi_area)
        self.subwindow1 = QMdiSubWindow()
        self.subwindow1.setWindowTitle('SubWindow1')
        self.subwindow1.setWindowFlags(Qt.FramelessWindowHint)
        widget1 = QWidget()
        layout1 = QVBoxLayout(widget1)
        self.subwindow1.setWidget(widget1)
        self.mdi_area.addSubWindow(self.subwindow1)
        self.subwindow1.resize(600, 600)
        self.subwindow1.setObjectName("SubWindow1")
        self.backgroundImgPath = self.resources_directory + "testWorld1_1000_1000.png"
        self.subwindow1.setStyleSheet("#SubWindow1{border-image:url(%s)}" %(self.backgroundImgPath))
        self.subwindow1.mousePressEvent = self.handle_mouse_press
        self.subwindow2 = QMdiSubWindow()
        self.subwindow2.setWindowFlags(Qt.FramelessWindowHint)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31

A*算法

            while end_mark == 0:
                f_list = []
                for i in list(range(0,len(open_list))):
                    f_list.append(open_list[i][2])
                min_f = min(f_list)
                save_point = open_list[f_list.index(min_f)]
                close_list.append(open_list[f_list.index(min_f)])
                open_list.pop(f_list.index(min_f))
                for i in list(range(0,8)):
                    temp_point = copy.deepcopy(save_point)
                    temp_point[3][0] = save_point[0]
                    temp_point[3][1] = save_point[1]
                    temp_point[0] = save_point[0] + operate_list[i][0]
                    temp_point[1] = save_point[1] + operate_list[i][1]
                    mark = 0
                    if (temp_point[0] < 0 or temp_point[1] <0 or 
                        temp_point[0] > x_max or temp_point[1] > y_max):
                        mark = 1
                    if mark != 1:
                        for q in list(range(0,len(open_list))):
                            if open_list[q][0] == temp_point[0] and open_list[q][1] == temp_point[1]:
                                mark = 1
                                break
                        if mark != 1:
                            for p in list(range(0,len(close_list))):
                                if close_list[p][0] == temp_point[0] and close_list[p][1] == temp_point[1]:
                                    mark = 1
                                    break
                            if mark != 1:
                                for r in list(range(0,len(obstacle_list))):
                                    if obstacle_list[r][0] == temp_point[0] and obstacle_list[r][1] == temp_point[1]:
                                        mark = 1
                                        break
                    if mark == 0:
                        temp_point[2] = Chebyshev_mode(i, save_point[2], temp_point[0], temp_point[1])
                        open_list.append(temp_point)
                        calculate_times += 1
                    if temp_point[0] == end_point[0] and temp_point[1] == end_point[1]:
                        end_mark = 1
                        close_list.append(temp_point)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40

机器人控制

    while (ros::ok())
    {
        if(route_list.size() > 0 && (!done_move || new_route_list_get))
        {
            if(new_route_list_get)
            {
                move_mark = 0;
                new_route_list_get = false;
                done_move = false;
            }
            else
            {
                float target_x = route_list[move_mark][0];
                float target_y = route_list[move_mark][1];
                float res = pow(
                    pow(target_x - robotPosition.pose.pose.position.x * 5, 2) +  
                    pow(target_y - robotPosition.pose.pose.position.y * 5, 2),
                    0.5);
                robot_vel.linear.x = calLinear_vel(res);
                robot_vel.angular.z = calTurn_vel(
                    robot_yaw, 
                    atan2(target_y - robotPosition.pose.pose.position.y * 5, 
                        target_x - robotPosition.pose.pose.position.x * 5)
                );
                if(move_mark < route_list.size() - 1)
                {
                    if(abs(target_x - robotPosition.pose.pose.position.x * 5) < 1.5 &&
                    abs(target_y - robotPosition.pose.pose.position.y * 5) < 1.5)
                    {
                        move_mark ++;
                    }
                }
                else
                {
                    if(abs(target_x - robotPosition.pose.pose.position.x * 5) < 0.5 &&
                    abs(target_y - robotPosition.pose.pose.position.y * 5) < 0.5)
                    {
                        done_move = true;
                        robot_vel.linear.x = 0;
                        robot_vel.angular.z = 0;
                    }
                }
            }
            robot_vel_cmd_pub.publish(robot_vel);
        }
        ros::spinOnce();
        rate.sleep();
    }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48

四、总结

该项目采用python和C++编写，通过pyQT5库建立了一个界面显示程序，采用Gmapping算法和A*路径规划算法进行了一个完整的建图和导航的过程。源码中提供了项目的复刻文档，能够容易复刻项目，并对项目进行二次开发，应用于更多方面。

五、源码获取

扫码关注公众号，私信“小车建图与导航”获取。