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

ROS导航仿真和多点导航3——实现机器人巡检功能
0.代码分享
1.主要结构梳理
2.任务初始化类class TaskInit
3.创建TaskTransfer对象，实现机器人从一个路径点到另一个路径点的功能
4.从当前文件中下载所有巡检点信息
5.将巡检点从RVIZ中的map中标记出来
5.巡检点TaskPoint类

  从本地文件读取巡检点在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],pos[1],pos[2],
                ori[0],ori[1],ori[2],ori[3]
            ]
            self.initialPose = msg_list
            return True
        except tf.Exception as e:
            print "listen to tf failed"
            print e
            print e.message
            return False
            
    #刷新位置，返回当前位置
    def refreshInitialPose(self):
        self.initialPose = None
        RATE = 50
        while not self.initialPose:
            self.listen_tf()				#对tf进行监听
            rospy.sleep(1.0 / RATE)
	#获得距离当前点最近的巡检点位置
    def getBestTaskInd(self,task_points):
        self.refreshInitialPose()
        fake_task = TaskPoint()
        fake_task.setRobotPose(self.initialPose)
        dist_list = [fake_task.calDistance(task_point) for task_point in task_points]
        return np.argmin(np.array(dist_list)),fake_task

3.创建TaskTransfer对象，实现机器人从一个路径点到另一个路径点的功能

#!/usr/bin/python

import rospy
import actionlib
from move_base_msgs.msg import MoveBaseAction,MoveBaseGoal
from TaskPoint import TaskPoint
from geometry_msgs.msg import PoseStamped

class TaskTransfer:

    def __init__(self):
        self.moveToClient = actionlib.SimpleActionClient('move_base',MoveBaseAction)
        self.moveToClient.wait_for_server()
        rospy.loginfo("Action 'MoveTo' is up !")
        
	#将机器人从当前位置移到到最近的巡检点
    def task_transfer(self,src_point,des_point):
        '''
            Transfer robot to next TaskPoint.
        '''
        des_point.setPreTaskPoint(src_point)
        goal_msg = MoveBaseGoal()

        goal_pose = PoseStamped()
        goal_pose.header.stamp = rospy.Time.now()
        goal_pose.header.frame_id = "map"
        poseList = des_point.getPoseList()
        goal_pose.pose.position.x = poseList[0]
        goal_pose.pose.position.y = poseList[1]
        goal_pose.pose.position.z = poseList[2]
        goal_pose.pose.orientation.x =poseList[3]
        goal_pose.pose.orientation.y = poseList[4]
        goal_pose.pose.orientation.z = poseList[5]
        goal_pose.pose.orientation.w = poseList[6]

        goal_msg.target_pose = goal_pose

        print "-----------------------------------------------"
        print "goal_msg.target_pose.pose.position:",goal_msg.target_pose.pose.position
        print "goal_msg.target_pose.pose.orientation:",goal_msg.target_pose.pose.orientation
        print "--------------------------------------------------"

        not_done = True
        while not rospy.is_shutdown() and not_done:
            self.moveToClient.send_goal(goal_msg)
            rospy.logwarn("Transfer from [%s] to [%s]"%(src_point.name,des_point.name))
            done = self.moveToClient.wait_for_result(timeout = rospy.Duration(300.0))
            #Make sure the action succeed
            not_done = (not done) or (self.moveToClient.gh.get_goal_status() != actionlib.GoalStatus.SUCCEEDED)
            print "Goal status:" ,self.moveToClient.gh.get_goal_status == actionlib.GoalStatus.SUCCEEDED
            if not_done:
                print "Goal not done,Resent goal."

4.从当前文件中下载所有巡检点信息

  从当前功能包的data目录下，读取机器人巡检点信息

    def loadTaskPoints(self):
        '''
                Construct TaskPoint list from json files in data folder.
        '''
        folder = '{}/data'.format(self.package_path)
        task_json = None
        if os.path.exists(folder):
            task_json=os.listdir(folder)
        
        if not task_json:
            raise Exception("No valid task point to tranverse!!")

        task_list = []
        for i,file_name in enumerate(task_json):
            with open(folder + '/' + file_name,'r') as json_fp:
                waypoint_record = json.load(json_fp)
                task_list.append(waypoint_record)
        task_list = sorted(task_list,key = lambda s : s['order'])
        for waypoint_record in task_list:
            self.taskPoints.append(
                TaskPoint(waypoint_record)
            )

5.将巡检点从RVIZ中的map中标记出来

  从当前功能包的data目录下，读取机器人巡检点信息

    def  plot_marker(self,taskPoints):
        markerArray = MarkerArray()
        for point in taskPoints:
            marker = Marker()
            marker.header.frame_id = 'map'
            marker.type = marker.TEXT_VIEW_FACING
            marker.action = marker.ADD
            marker.scale.x = 1
            marker.scale.y = 1
            marker.scale.z = 1
            marker.color.a = 1
            marker.color.r = 1
            marker.color.g = 0
            marker.color.b = 0
            marker.pose.position.x = point.getPosX()
            marker.pose.position.y = point.getPosY()
            marker.pose.position.z = point.getPosZ()
            marker.text = str(point.getOrder())
            markerArray.markers.append(marker)    

        id = 0
        for m in markerArray.markers:
            m.id = id
            id += 1

6.巡检点TaskPoint类

#!/usr/bin/python
'''
Copyright (c) Deep Robotics Inc. - All Rights Reserved
Unauthorized copying of this file, via any medium is strictly prohibited
Proprietary and confidential
Author: Haoyi Han <hanhaoyi@deeprobotics.cn>, Feb, 2020
'''
import time
from tf import transformations
import rospy
import tf
import copy

class TaskPoint:
    """Property and method about a task point.

    Attributes:
        pose: the pose of robot in this wappoint.
        name: waypoint name. 
    """
    def __init__(self,record = None):
        if not record:
            record = {
                "order" : 0,
                "robot_pose" : {
                    "pos_x" : 0.0,
                    "pos_y" : 0.0,
                    "pos_z" : 0.0,
                    "ori_x" : 0.0,
                    "ori_y" : 0.0,
                    "ori_z" : 0.0,
                    "ori_w" : 1.0
                }
            }
        self.record = copy.deepcopy(record)
        self.tf_listener = tf.TransformListener()
        self.update_waypoint_name()
        
    def setPreTaskPoint(self,src_point):
        self.pre_task_point = src_point
    
    def getPreTaskPoint(self):
        return self.pre_task_point

    
    def update_waypoint_name(self):
        self.name = "waypoint_" + str(self.record["order"])

    def setRobotPose(self,robot_pose):
        self.record["robot_pose"]["pos_x"] = robot_pose[0]
        self.record["robot_pose"]["pos_y"] = robot_pose[1]
        self.record["robot_pose"]["pos_z"] = robot_pose[2]

        self.record["robot_pose"]["ori_x"] = robot_pose[3]
        self.record["robot_pose"]["ori_y"] = robot_pose[4]
        self.record["robot_pose"]["ori_z"] = robot_pose[5]
        self.record["robot_pose"]["ori_w"] = robot_pose[6]


    def getPosX(self):
        robot_pose = self.record["robot_pose"]
        return robot_pose["pos_x"]


    def getPosY(self):
        robot_pose = self.record["robot_pose"]
        return robot_pose["pos_y"]

    def getPosZ(self):
        robot_pose = self.record["robot_pose"]
        return robot_pose["pos_z"]

    def getOrder(self):
        return self.record["order"]

    def getYaw(self):
        robot_pose = self.record["robot_pose"]
        pose = []
        pose.append(robot_pose["pos_x"])
        pose.append(robot_pose["pos_y"])
        pose.append(robot_pose["pos_z"])
        pose.append(robot_pose["ori_x"])
        pose.append(robot_pose["ori_y"])
        pose.append(robot_pose["ori_z"])
        pose.append(robot_pose["ori_w"])
        return transformations.euler_from_quaternion(pose[3:])[2]

    def getPoseList(self):
        robot_pose = self.record["robot_pose"]
        pose = []
        pose.append(robot_pose["pos_x"])
        pose.append(robot_pose["pos_y"])
        pose.append(robot_pose["pos_z"])
        pose.append(robot_pose["ori_x"])
        pose.append(robot_pose["ori_y"])
        pose.append(robot_pose["ori_z"])
        pose.append(robot_pose["ori_w"])
        return pose

    def calDistance(self,other):
        '''
            For Simplifacation, using 2D distance as the pose distance.
        '''
        return (
                (self.getPosX()-other.getPosX())**2 + (self.getPosY()-other.getPosY())**2
               )**0.5


    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],pos[1],pos[2],
                ori[0],ori[1],ori[2],ori[3]
            ]
            return msg_list
        except tf.Exception as e:
            print "listen to tf failed"
            print e
            print e.message
            return None

        
	#到达巡检点需要执行的操作定义
    def runTask(self):
        """
            When robot successfully tranfer to a new TaskPoint, Correspoing operation task will be triggered.
            eg. shoot an image. 
        """
        rospy.loginfo("Run task in {}.".format(self.name))
        rospy.sleep(3.0)