一、基本技术介绍
在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)
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)
机器人控制
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();
}
四、总结
该项目采用python和C++编写,通过pyQT5库建立了一个界面显示程序,采用Gmapping算法和A*路径规划算法进行了一个完整的建图和导航的过程。源码中提供了项目的复刻文档,能够容易复刻项目,并对项目进行二次开发,应用于更多方面。
五、源码获取
扫码关注公众号,私信“小车建图与导航”获取。