path_planner.py 文件源码

def visualize(self):
        if self.circle_pub.get_num_connections() > 0 and self.circle_path:
            print "CIRCLE PUB"
            # root = self.space_explorer.tree_root
            # markers = [utils.marker_clear_all("/map")]
            # explored = self.non_overlapping_paths(root)
            # if type(explored[0]) == list:
            #   explored = reduce(lambda x,y: x+y, explored)
            # print "explored: ", len(explored)
            # markers += [utils.marker_from_circle(circle, index=i, linewidth=0.01, color=ColorRGBA(0, 1, 0, 0.1), lifetime=1.0) \
            #             for i, circle in enumerate(explored)]

            # marker_array = MarkerArray(markers=markers)
            # self.circle_pub.publish(marker_array)
            # print len(self.circle_path.states)
            # marker = utils.marker_from_circles(self.circle_path.states, 1, "Markers_NS2", "/map", 3.0, [1.,0.,0.])
            # self.circle_pub.publish(marker)

            markers = [utils.marker_clear_all("/map")]
            markers += [utils.marker_from_circle(circle, index=i, linewidth=0.05, color=ColorRGBA(1, 0, 0, 0.4), \
                        lifetime=4.0)
                        for i, circle in enumerate(self.circle_path.states)]
            # print len(markers)
            marker_array = MarkerArray(markers=markers)
            self.circle_pub.publish(marker_array)

        if self.should_refine_trajectory and self.fast_circle_pub.get_num_connections() > 0 and self.fast_path:
            print "FAST CIRCLE PUB"
            # root = self.path_planner.tree_root
            # markers = [utils.marker_clear_all("/map")]
            # explored = self.non_overlapping_paths(root)
            # if type(explored[0]) == list:
            #   explored = reduce(lambda x,y: x+y, explored)
            # print "explored: ", len(explored)
            # markers += [utils.marker_from_circle(circle, index=i, linewidth=0.01, color=ColorRGBA(0, 0, 1, 0.1), lifetime=1.0) \
            #             for i, circle in enumerate(explored)]
            markers = [utils.marker_clear_all("/map")]
            markers += [utils.marker_from_circle(circle, index=i, linewidth=0.05, color=ColorRGBA(1, 0, 0, 0.4), \
                        lifetime=4.0)
                        for i, circle in enumerate(self.fast_path.states)]

            marker_array = MarkerArray(markers=markers)
            self.fast_circle_pub.publish(marker_array)

        if self.show_exploration_buffer and self.exploration_pub.get_num_connections() > 0:
            print "Publishing exploration grid"
            self.exploration_pub.publish(self.map.get_exploration_occupancy_grid())
        # plt.imshow(self.map.exploration_buffer)
        # plt.imshow(self.map.permissible_region)
        # plt.show()
        print "visualize"