rrt_exploration代码解析（三）—— filter.py

rrt_exploration代码解析（一）—— global_rrt_detector.cpp

rrt_exploration代码解析（二）—— local_rrt_detector.cpp

rrt_exploration代码解析（三）—— filter.py

rrt_exploration代码解析（四）—— assigner.py

filter.py

        第三个比较重要的文件是scripts文件夹下的filter.py，该文件的作用是过滤检测到的边界点，并将过滤后的边界点和边界点的聚类中心发布出来。

        回调函数，获取map地图数据、costmap全局代价地图数据、frontiers边界点数据。

def callBack(data, args):
    global frontiers, min_distance
    transformedPoint = args[0].transformPoint(args[1], data)
    x = [array([transformedPoint.point.x, transformedPoint.point.y])]
    if len(frontiers) > 0:
        frontiers = vstack((frontiers, x))
    else:
        frontiers = x


def mapCallBack(data):
    global mapData
    mapData = data


def globalMap(data):
    global global1, globalmaps, litraIndx, namespace_init_count, n_robots
    global1 = data
    if n_robots > 1:
        indx = int(data._connection_header['topic']
                   [litraIndx])-namespace_init_count
    elif n_robots == 1:
        indx = 0
    globalmaps[indx] = data

        节点函数node()，定义了过滤器的主节点。

        参数定义。

global frontiers, mapData, global1, global2, global3, globalmaps, litraIndx, n_robots, namespace_init_count
    rospy.init_node('filter', anonymous=False)

    # fetching all parameters
    map_topic = rospy.get_param('~map_topic', '/map')
    threshold = rospy.get_param('~costmap_clearing_threshold', 70)
    # this can be smaller than the laser scanner range, >> smaller >>less computation time>> too small is not good, info gain won't be accurate
    info_radius = rospy.get_param('~info_radius', 1.0)
    goals_topic = rospy.get_param('~goals_topic', '/detected_points')
    n_robots = rospy.get_param('~n_robots', 1)
    namespace = rospy.get_param('~namespace', '')
    namespace_init_count = rospy.get_param('namespace_init_count', 1)
    rateHz = rospy.get_param('~rate', 100)
    global_costmap_topic = rospy.get_param(
        '~global_costmap_topic', '/move_base_node/global_costmap/costmap')
    robot_frame = rospy.get_param('~robot_frame', 'base_link')

    litraIndx = len(namespace)
    rate = rospy.Rate(rateHz)

        订阅地图话题，获取地图信息。

rospy.Subscriber(map_topic, OccupancyGrid, mapCallBack)

        因为rrt是支持多机建图，因此在进行边界点过滤之前需要将各个robot的雷达数据进行凭借，同时订阅各个robot的globalcostmap数据。

    for i in range(0, n_robots):
        globalmaps.append(OccupancyGrid())

    if len(namespace) > 0:
        for i in range(0, n_robots):
            rospy.Subscriber(namespace+str(i+namespace_init_count) +
                             global_costmap_topic, OccupancyGrid, globalMap)
    elif len(namespace) == 0:
        rospy.Subscriber(global_costmap_topic, OccupancyGrid, globalMap)

        等待地图数据和各个robot的globalcostmap数据。

# wait if map is not received yet
    while (len(mapData.data) < 1):
        rospy.loginfo('Waiting for the map')
        rospy.sleep(0.1)
        pass
# wait if any of robots' global costmap map is not received yet
    for i in range(0, n_robots):
        while (len(globalmaps[i].data) < 1):
            rospy.loginfo('Waiting for the global costmap')
            rospy.sleep(0.1)
            pass

        获取地图数据后进行tf转换。

    tfLisn = tf.TransformListener()
    if len(namespace) > 0:
        for i in range(0, n_robots):
            tfLisn.waitForTransform(global_frame[1:], namespace+str(
                i+namespace_init_count)+'/'+robot_frame, rospy.Time(0), rospy.Duration(10.0))
    elif len(namespace) == 0:
        tfLisn.waitForTransform(
            global_frame[1:], '/'+robot_frame, rospy.Time(0), rospy.Duration(10.0))

        定义话题发布方，边界点frontiers、聚类中心centroids用于在rviz上显示，过滤后的边界点filtered_points用于给规划器分配robot探索。

    pub = rospy.Publisher('frontiers', Marker, queue_size=10)
    pub2 = rospy.Publisher('centroids', Marker, queue_size=10)
    filterpub = rospy.Publisher('filtered_points', PointArray, queue_size=10)

        等待边界点数据。

    while len(frontiers) < 1:
        pass

        对rviz显示用的marker进行初始化。初始化完成后进入主循环，主循环首先利用meanshift方法对边界点进行聚类，得到各个类的中心centroids。当只有一个边界点时聚类中心即是边界点。

        centroids = []
        front = copy(frontiers)
        if len(front) > 1:
            ms = MeanShift(bandwidth=0.3)
            ms.fit(front)
            centroids = ms.cluster_centers_  # centroids array is the centers of each cluster

        # if there is only one frontier no need for clustering, i.e. centroids=frontiers
        if len(front) == 1:
            centroids = front
        frontiers = copy(centroids)

        清除旧的边界点，原因是上一次采样得到的边界点可能已经被探索到了，那么在这一次探索时就需要将之前的边界点清除。(gridValue(globalmaps[i], x) 获取聚类中心当前代价地图上的值（注：栅格地图数据存储的格式一般是 0：无障碍，100：有障碍，-1：未知）判断该边界点的聚类中心是否已经被探索，若被探索到则将其从centroids中删除。

        z = 0
        while z < len(centroids):
            cond = False
            temppoint.point.x = centroids[z][0]
            temppoint.point.y = centroids[z][1]

            for i in range(0, n_robots):

                transformedPoint = tfLisn.transformPoint(
                    globalmaps[i].header.frame_id, temppoint)
                x = array([transformedPoint.point.x, transformedPoint.point.y])
                cond = (gridValue(globalmaps[i], x) > threshold) or cond
            if (cond or (informationGain(mapData, [centroids[z][0], centroids[z][1]], info_radius*0.5)) < 0.2):
                centroids = delete(centroids, (z), axis=0)
                z = z-1
            z += 1

        处理完边界点后将边界点发布出去，第一个发布filterpub发布由下一篇文章所提到的节点接收，用于安排robot对边界进行探索，pub、pub2发布的点由rviz订阅用于显示边界点和聚类中心。

        arraypoints.points = []
        for i in centroids:
            tempPoint.x = i[0]
            tempPoint.y = i[1]
            arraypoints.points.append(copy(tempPoint))
        filterpub.publish(arraypoints)
        pp = []
        for q in range(0, len(frontiers)):
            p.x = frontiers[q][0]
            p.y = frontiers[q][1]
            pp.append(copy(p))
        points.points = pp
        pp = []
        for q in range(0, len(centroids)):
            p.x = centroids[q][0]
            p.y = centroids[q][1]
            pp.append(copy(p))
        points_clust.points = pp
        pub.publish(points)
        pub2.publish(points_clust)
        rate.sleep()

        整个filter.py文件同样只是对边界点和地图信息进行处理，并没有对robot进行控制，下一篇博客将介绍如何给各个robot分配需要探索的边界点。