前言
紧张刺激的ROS组全国总决赛于26号落下了帷幕。我也有幸能够前往现场参赛体验一番。下面给大家带来一些现场的照片，视频，末尾会对路径算法进行开源。
正文
先来感受一下南京信息工程大学的美景吧~
清晨等待体育馆开门的参赛选手们~
志愿者辛苦了一晚搭出来的赛道~
参赛体验
早上的时候五点多起了床，看到群里更新了比赛的地图，赶忙换上了，然后就匆匆赶往赛场了。因为是我四年来第一次进总决赛，也是我最后一次参赛。所以我全程都是非常紧张的，生怕自己忽略了某个环节，错失了获奖的机会。 不过也不是完全没有意外的，由于前一天晚上在宾馆不小心强制断电了，然后重装BIOS电池后测试完功能就装箱了。早上预赛的时候第一遍跑完之后，准备提速时，发现工程没编译成功，工程的时间戳没对上，然后慌张之下也就没有找好对策（其实只要更新一下系统时间就行了），然后十分钟里就用了最慢速跑了三四遍。 这边放上学弟录的我们预赛视频叭
本以为无法参加下午的排位赛了，没想到成绩出来19.47s，排在第八，然后算了算参赛队伍40+，按百分之二十算大概会有八到九个排位赛名额吧，然后心想后面还有一半队伍没比完。心就凉了大半截，准备收拾收拾撤了T.T。中午吃完饭得知排名刚好在第十名，心想着虽然遗憾，但至少也能拿到国二了。没想到下午的排位赛名额又被增加到十二个了。 这心路历程，和坐过山车也没差别了。 下午排位赛前把系统的时间戳校准了之后，便准备去跑决赛了，虽然提速了几次，但都撞上了锥桶，没能完成比赛，果然稳定性还有待提高。不过下午有了成绩也就安心很多了。至少也给自己有了交代。 下面放上排位赛时候记录的视频叭~
进了国赛也是看到了许多大佬的车，不仅稳定，速度还飞快。算法能力确实不是一朝一夕就能提升的，我一直都记得学长教我的一句话：工程一定要建立于理论之上，存在于理论之中。 好啦，这些就是我参赛的感想了，我也算凑齐了四年的证书和参赛证了，嘿嘿嘿嘿，从光电四轮，到三轮车，到双车，再到创意组，也算是一步一个脚印叭。
Open Source
#!/usr/bin/env python
# -*- coding: utf-8 -*-
#-----------------------------#
#南京理工大学紫金学院；永不宕机队#
#-----------------------------#
import copy
import numpy as np
class c_area_sort_status:
"这个类用来表示path中每个节点的状态，是这个py文件内部使用的类"
def __init__(self, is_first_point=False, first_point_head_to=-1, pre_area=-1, current_area=-1):
self.is_first_point = is_first_point
self.first_point_head_to = first_point_head_to
self.pre_area = pre_area
self.current_area = current_area
# 异常参数检查
if self.is_first_point == True:
if first_point_head_to < 0:
raise Exception("first_point_head_to has wrong param")
if first_point_head_to == current_area:
raise Exception("first_point_head_to==current_area")
if self.is_first_point == False:
if pre_area < 0:
raise Exception("pre_area has wrong param")
if pre_area == current_area:
raise Exception("pre_area==current_area")
if self.current_area < 0:
raise Exception("current_area has wrong param")
class c_area_sort_path:
'''
这个类用来表示路径，是这个文件内部使用的类
'''
def __init__(self):
self.is_end = False
self.path = []
class c_area_route:
"用于路径规划的类，是外部文件调用的类"
# 区域中心坐标
pos_area = [[],
[49, 40], # 1
[290, 41], # 2
[519, 95], # 3
[771, 37], # 4
[771, 285], # 5
[765, 473], # 6
[430, 471], # 7
[218, 473], # 8
[48, 469], # 9
[365, 267], # 10
[465, 161]] # 11
# 每个区域相邻的区域
area_connect = [[],
[2, 9], # 1
[1, 4, 10], # 2
[2, 4, 11], # 3
[2, 3, 5], # 4
[4, 6, 11], # 5
[5, 7], # 6
[6, 8, 10], # 7
[7, 9, 10], # 8
[1, 8], # 9
[2, 7, 8, 11], # 10
[3, 5, 10]] # 11
# 返回从last_area到current_area之后，可以选择到达的区域的list
def area_method(self, last_area, current_area, max_angle=105):
if last_area == 2 and current_area == 3:
return [5, 11]
elif last_area == 4 and current_area == 3:
return [11]
elif last_area == 11 and current_area == 3:
return [4]
area_list = []
x_err = self.pos_area[current_area][0] - self.pos_area[last_area][0]
y_err = self.pos_area[current_area][1] - self.pos_area[last_area][1]
angle_in = np.arctan2(y_err, x_err) / 3.1415926535 * 180.0 # 进入角
for i in self.area_connect[current_area]:
x_err = self.pos_area[i][0] - self.pos_area[current_area][0]
y_err = self.pos_area[i][1] - self.pos_area[current_area][1]
angle_out = np.arctan2(y_err, x_err) / 3.1415926535 * 180.0 # 离开角
# 计算离开与进入角度差值
angle_err = angle_out - angle_in
# 转到-180 至 180 内
while angle_err > 180:
angle_err -= 360
while angle_err < -180:
angle_err += 360
if (np.fabs(angle_err) < max_angle): # 需要转过的角度小于max_angle则可行
area_list.append(i)
return area_list
def debug_print_path(self, path): # 将用c_area_sort_path表示的路径用数字序列打印出来
path_print = []
for i in range(0, len(path.path)):
path_print.append(path.path[i].current_area)
print(path_print)
def path_length(self, path): # 用c_area_sort_path表示的路径的长度
length = 0.0
if len(path) < 2: # 点数小于2
return 0.0
for i in range(0, len(path) - 1):
area_num1 = path[i].current_area
area_num2 = path[i + 1].current_area
x_err = self.pos_area[area_num1][0] - self.pos_area[area_num2][0]
y_err = self.pos_area[area_num1][1] - self.pos_area[area_num2][1]
length += np.sqrt(x_err * x_err + y_err * y_err)
return length
def find_best_route(self, start_area, start_head_to, end_area, through_areas, max_steps=16,
print_paths=False, max_angle=105): # 寻找最优路线
'''
寻找最优路线
:param start_area: 车出发的区域
:param start_head_to: 车出发时朝向的区域
:param end_area: 终点区域
:param through_areas: 需要经过区域的列表
:param max_steps: 最大枚举步数（可选）
:param print_paths:打印所有路径
:param max_angle:最大转角
:return:
'''
paths_temp = [] # 当前正在计算的所有路线
# 第一步第二步一定是起始区域和朝向区域
paths_temp.append(c_area_sort_path())
paths_temp[0].path.append(c_area_sort_status(is_first_point=True,
current_area=start_area,
first_point_head_to=start_head_to))
paths_temp[0].path.append(c_area_sort_status(is_first_point=False,
current_area=start_head_to,
pre_area=start_area))
# 枚举
enum_end = False # 枚举结束标志，所有枚举路径均计算到终点后为True
while enum_end == False:
paths_temp_new = [] # paths_temp[]枚举展开一步之后暂存在paths_temp_new中，之后迭代进入paths_temp进行下一步枚举
# 新一步枚举
for i in range(0, len(paths_temp)):
if paths_temp[i].path[-1].current_area == end_area: # 这个路径已经到达终点,单独存下这个路径
# 转存进入paths_temp_new并打上计算完成标志，在下次枚举的时候就不再枚举这个路径
paths_temp_new.append(copy.deepcopy(paths_temp[i]))
paths_temp_new[-1].is_end = True
if len(paths_temp[i].path) < max_steps and paths_temp[i].is_end == False:
# 如果这个路径的步数还不到max_steps，并且未计算完成则继续向下枚举
for j in self.area_method(paths_temp[i].path[-2].current_area, paths_temp[i].path[-1].current_area,
max_angle):
last_area = paths_temp[i].path[-1].current_area
current_area = j
paths_temp_new.append(c_area_sort_path())
paths_temp_new[-1].path = copy.deepcopy(paths_temp[i].path)
paths_temp_new[-1].is_end = copy.deepcopy(paths_temp[i].is_end)
paths_temp_new[-1].path.append(c_area_sort_status(is_first_point=False,
current_area=current_area,
pre_area=last_area)) # 并且新增一个新的节点
# 将paths_temp_new迭代进入paths_temp
paths_temp = []
for i in range(0, len(paths_temp_new)):
paths_temp.append(c_area_sort_path())
paths_temp[i].path = copy.deepcopy(paths_temp_new[i].path)
paths_temp[i].is_end = paths_temp_new[i].is_end
# 删除达到最大步数且最终不是终点的路径
del_end = False # 是否已经全部删除完毕
while del_end == False:
del_end = True
for i in range(0, len(paths_temp)):
if len(paths_temp[i].path) >= max_steps and paths_temp[i].path[-1].current_area != end_area:
del paths_temp[i]
del_end = False
break
# 检查是否所有的路径均已经枚举完毕
enum_end = True
for i in range(0, len(paths_temp)):
if paths_temp[i].is_end == False:
enum_end = False
# 从路线中删除不符合经过区域的路线
del_end = False
while del_end == False:
del_end = True
for i in range(0, len(paths_temp)): # 第i条路径
for j in through_areas: # 遍历需要经过的区域
through_pass = False
for k in range(0, len(paths_temp[i].path)): # 遍历第i条路径中索引为k的区域
if paths_temp[i].path[k].current_area == j: # 如果在第i条路径中发现需要经过的区域j
through_pass = True # 则通过要求
if through_pass == False: # 如果不满足要求
del paths_temp[i] # 删除路径
del_end = False # 还没删完
break;
if del_end == False:
break
if print_paths: # (debug)打印所有符合要求的路径
for i in range(0, len(paths_temp)):
self.debug_print_path(paths_temp[i])
print("共{}条路径".format(len(paths_temp)))
# 从符合要求的路线中选择最短路线返回
if (len(paths_temp) > 0):
shortest_index = 0
shortest_length = self.path_length(paths_temp[0].path)
for i in range(0, len(paths_temp)):
if shortest_length > self.path_length(paths_temp[i].path):
shortest_length = self.path_length(paths_temp[i].path)
shortest_index = i
path = []
for i in range(0, len(paths_temp[shortest_index].path)):
path.append(paths_temp[shortest_index].path[i].current_area)
return path
return []
上面的代码可作为头文件，有接口可调用，下面说明使用教程。
下面是一个调用的例子，调用前要import上面的源码文件。
我们的区域号与其坐标的对应是利用rviz标点来记录的，源码如下：
#!/usr/bin/env python
# -*- coding: utf-8 -*-
'''
坐标存储 文件
'''
import rospy
import io
from nav_msgs.msg import Odometry
from geometry_msgs.msg import PointStamped
import time
import math
import numpy as np
area_num = 0
area_pos_x = []
area_pos_y = []
def click_point_callback(data):
global area_pos_x
global area_pos_y
global area_num
area_pos_x.append(data.point.x)
area_pos_y.append(data.point.y)
area_num += 1
print(" Set area "+str(area_num) + " success !\n")
if area_num == 11:
f = io.open("/home/w/racecar_ZJ/src/multi_goal/src/地图区域记录.txt", mode="wb+")
for i in range(0,11):
f.writelines(str(area_pos_x[i])+'\n')
f.writelines(str(area_pos_y[i])+'\n')
f.flush()
f.close()
print("Input over !")
if __name__ == '__main__':
try:
rospy.init_node('save_area', anonymous = True)#初始化节点
rospy.Subscriber("clicked_point",
PointStamped,
click_point_callback)
rospy.spin()
except rospy.ROSInterruptException:
pass
总结
到此源码结束，这个代码的实现效果在我上一期博客《智能车竞赛——ROS组国赛篇 二》的视频里，我把上期的仿真工程打包成压缩包放在了ROS竞赛群群文件里，大家自提~~
开心洋葱 , 版权所有丨如未注明 , 均为原创丨未经授权请勿修改 , 转载请注明智能车竞赛——ROS组国赛篇 三！
