前言

紧张刺激的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竞赛群群文件里,大家自提~~