ROS导航仿真和多点导航3——实现机器人巡检功能

  从本地文件读取巡检点在map的位置，发布任务使机器人循环的在巡检点移动。

0.代码分享

链接：https://pan.baidu.com/s/1Fsi2sAUWse_Jb5sqy8ypUw
提取码：p9pe

1.主要结构梳理

class Task:
    def __init__(self):
        self.taskPoints = []
        self.currentIndex = 0
        self.robot_transfer = None
        self.src_ind = None
        self.des_ind = None
        self.package_path = None
        self.mark_pub = None
        self.task_pose_pub = None
        self.ntask = 0
	#找到当前功能包的路径package_path
    def init_rospack(self):
        #get an instance of RosPack with the default search paths
        rospack = rospkg.RosPack()
        #list all packages,,equivalent to rospack list
        rospack.list()
        try:
            self.package_path = rospack.get_path('auto_nav2d_point')
        except:
            print("Could not find package auto_nav2d_point")
            exit(1)
        print(self.package_path)
            
     #初始化任务，，并将机器人由当前位置移动到距离最近的巡检点
    def init(self):
        self.mark_pub = rospy.Publisher('/robot_path',MarkerArray,queue_size = 100)
        self.task_pose_pub = rospy.Publisher('/robot_path_pose',PoseArray,queue_size = 100)
        task_init = TaskInit()                                                  #任务初始化类
        self.init_rospack()													 #找到当前功能包的路径package_path
        self.robot_transfer = TaskTransfer()						#创建TaskTransfer对象，实现机器人从一个路径点到另一个路径点的功能
        self.loadTaskPoints()												#从当前文件中下载所有巡检点信息

        #plot all task points
        self.plot_marker(self.taskPoints)								#将巡检点从RVIZ中的map中标记出来

        best_ind,initial_point = task_init.getBestTaskInd(self.taskPoints)								#找出距离机器人当前位置最小的巡检点
        #First to transfer to best index point  'self.taskPoints[best_index]'
        self.robot_transfer.task_transfer(initial_point,self.taskPoints[best_ind])						#将机器人从当前位置移到到最近的巡检点

        self.taskPoints[best_ind].runTask()																				#到达巡检点后，执行事件
        self.ntask = self.taskPoints.__len__()																			#循环到达下一个巡检点
        self.src_ind = best_ind
        self.des_ind = (best_ind + 1) % self.ntask


if __name__=="__main__":
    rospy.init_node("auto_2d_nav_task")
    task = Task()												#class Task		任务类，用于发布巡检指令
    task.init()														#初始化任务，并将机器人由当前位置移动到距离最近的巡检点
    task.run()														#循环的在不同巡检点间顺序移动

2.任务初始化类class TaskInit

class TaskInit():
    def __init__(self,pose_topic="/ndt/current_pose"):
       self.initialPose = None
       self.pose_topic = pose_topic
       self.tf_listener = tf.TransformListener()
              
	#通过base_link与map坐标的tf，得到机器人实时位置
    def listen_tf(self):
        try:
            (pos,ori) = self.tf_listener.lookupTransform("/map","base_link",rospy.Duration(0.0))
            print "pos:",pos
            print "ori:",ori
            msg_list = [
                pos[0],</