自主导航nav_test.py代码分析

#这段代码的作用是实现在地图中随机导航。在ros下需要先加载机器人和相关配置，详请参阅 古-月 的博客http://blog.csdn.net/hcx25909/article/details/12110959
#这里主要是用于自己来理解这段代码

#!/usr/bin/env python

import rospy
import actionlib
from actionlib_msgs.msg import *
from geometry_msgs.msg import Pose, PoseWithCovarianceStamped, Point, Quaternion, Twist
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from random import sample
from math import pow, sqrt

class NavTest():
    def __init__(self):
        rospy.init_node('nav_test', anOnymous=True)
        
        rospy.on_shutdown(self.shutdown)#rospyAPI：on_shutdown()
        
        # How long in seconds should the robot pause at each location?
        #在每个目标点停留的时间
        self.rest_time = rospy.get_param("~rest_time", 10)#API get_param
        
        # Are we running in the fake simulator?
        #是否是在模拟环境中运行
        self.fake_test = rospy.get_param("~fake_test", False)
        
        # Goal state return values
        #目标状态返回值
        goal_states = ['PENDING', 'ACTIVE', 'PREEMPTED', 
                       'SUCCEEDED', 'ABORTED', 'REJECTED',
                       'PREEMPTING', 'RECALLING', 'RECALLED',
                       'LOST']
        
        # Set up the goal locations. Poses are defined in the map frame.  
        #设置目标位置，在地图坐标系中的姿态
        # An easy way to find the pose coordinates is to point-and-click
        #找到某一姿态的坐标，选中然后单击
        # Nav Goals in RViz when running in the simulator.
        #
        # Pose coordinates are then displayed in the terminal
        # that was used to launch RViz.
        locatiOns= dict()
        #这里定义了六个定位点的姿态
        locations['hall_foyer'] = Pose(Point(0.643, 4.720, 0.000), Quaternion(0.000, 0.000, 0.223, 0.975))
        locations['hall_kitchen'] = Pose(Point(-1.994, 4.382, 0.000), Quaternion(0.000, 0.000, -0.670, 0.743))
        locations['hall_bedroom'] = Pose(Point(-3.719, 4.401, 0.000), Quaternion(0.000, 0.000, 0.733, 0.680))
        locations['living_room_1'] = Pose(Point(0.720, 2.229, 0.000), Quaternion(0.000, 0.000, 0.786, 0.618))
        locations['living_room_2'] = Pose(Point(1.471, 1.007, 0.000), Quaternion(0.000, 0.000, 0.480, 0.877))
        locations['dining_room_1'] = Pose(Point(-0.861, -0.019, 0.000), Quaternion(0.000, 0.000, 0.892, -0.451))
        
        # Publisher to manually control the robot (e.g. to stop it)
        #发布控制机器人的消息
        self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist)
        
        # Subscribe to the move_base action server
        #订阅move_base动作服务
        self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
        
        rospy.loginfo("Waiting for move_base action server...")
        
        # Wait 60 seconds for the action server to become available
        #等60s
        self.move_base.wait_for_server(rospy.Duration(60))
        
        rospy.loginfo("Connected to move base server")
        
        # A variable to hold the initial pose of the robot to be set by 
        #用户在rviz中设置机器人的初始位姿的变量
        # the user in RViz
        initial_pose = PoseWithCovarianceStamped()
        
        # Variables to keep track of success rate, running time,
        #保存成功率、运行时间、运动距离的变量们
        # and distance traveled
        n_locatiOns= len(locations)
        n_goals = 0
        n_successes = 0
        i = n_locations
        distance_traveled = 0
        start_time = rospy.Time.now()
        running_time = 0
        location = ""
        last_location = ""
        
        # Get the initial pose from the user
        #由用户处获取初始位姿
        rospy.loginfo("*** Click the 2D Pose Estimate button in RViz to set the robot's initial pose...")
        rospy.wait_for_message('initialpose', PoseWithCovarianceStamped)
        self.last_location = Pose()
        rospy.Subscriber('initialpose', PoseWithCovarianceStamped, self.update_initial_pose)
        
        # Make sure we have the initial pose
        #确保有初始位姿
        while initial_pose.header.stamp == "":
            rospy.sleep(1)
            
        rospy.loginfo("Starting navigation test")
        
        # Begin the main loop and run through a sequence of locations
        #开始主循环，跑一个定位的序列
        while not rospy.is_shutdown():
            # If we've gone through the current sequence,
            #如果结束了当前序列，开始一个新的序列
            # start with a new random sequence
            if i == n_locations:
                i = 0
                sequence = sample(locations, n_locations)####
                # Skip over first location if it is the same as
                # the last location
                #如果最后一个位置和第一个位置一样，则跳过第一个位置
                if sequence[0] == last_location:
                    i = 1
            
            # Get the next location in the current sequence
            #获取当前序列中的下一个位置
            location = sequence[i]
                        
            # Keep track of the distance traveled.
            #跟踪行驶的距离
            # Use updated initial pose if available.
            #使用更新的初始位姿
            if initial_pose.header.stamp == "":
                distance = sqrt(pow(locations[location].position.x - 
                                    locations[last_location].position.x, 2) +
                                pow(locations[location].position.y - 
                                    locations[last_location].position.y, 2))
            else:
                rospy.loginfo("Updating current pose.")
                distance = sqrt(pow(locations[location].position.x - 
                                    initial_pose.pose.pose.position.x, 2) +
                                pow(locations[location].position.y - 
                                    initial_pose.pose.pose.position.y, 2))
                initial_pose.header.stamp = ""
            
            # Store the last location for distance calculations
            #存储上一个位置来计算距离
            last_location = location
            
            # Increment the counters
            #计数器增加1
            i += 1
            n_goals += 1
        
            # Set up the next goal location
            #设置下一个目标点
            self.goal = MoveBaseGoal()###
            self.goal.target_pose.pose = locations[location]
            self.goal.target_pose.header.frame_id = 'map'
            self.goal.target_pose.header.stamp = rospy.Time.now()
            
            # Let the user know where the robot is going next
            #告诉用户下一个位置
            rospy.loginfo("Going to: " + str(location))
            
            # Start the robot toward the next location
            #开始向下一个位置前进
            self.move_base.send_goal(self.goal)#move_base.send_goal()
            
            # Allow 5 minutes to get there
            #设置时间，5分钟之内到达
            finished_within_time = self.move_base.wait_for_result(rospy.Duration(300)) 
            
            # Check for success or failure
            #检查是否成功到达
            if not finished_within_time:
                self.move_base.cancel_goal()#move_base.cancle_goal()
                rospy.loginfo("Timed out achieving goal")
            else:
                state = self.move_base.get_state()#move_base.get_state()
                if state == GoalStatus.SUCCEEDED:
                    rospy.loginfo("Goal succeeded!")
                    n_successes += 1
                    distance_traveled += distance
                    rospy.loginfo("State:" + str(state))
                else:
                  rospy.loginfo("Goal failed with error code: " + str(goal_states[state]))
            
            # How long have we been running?
            #已经运行了多长时间
            running_time = rospy.Time.now() - start_time
            running_time = running_time.secs / 60.0
            
            # Print a summary success/failure, distance traveled and time elapsed
            #打印一个总结信息，成功与否，形驶的距离和消耗的时间
            rospy.loginfo("Success so far: " + str(n_successes) + "/" + 
                          str(n_goals) + " = " + 
                          str(100 * n_successes/n_goals) + "%")
            rospy.loginfo("Running time: " + str(trunc(running_time, 1)) + 
                          " min Distance: " + str(trunc(distance_traveled, 1)) + " m")
            rospy.sleep(self.rest_time)
    
    #更新初始位姿的函数        
    def update_initial_pose(self, initial_pose):
        self.initial_pose = initial_pose

    #关机处理函数
    def shutdown(self):
        rospy.loginfo("Stopping the robot...")
        self.move_base.cancel_goal()
        rospy.sleep(2)
        self.cmd_vel_pub.publish(Twist())
        rospy.sleep(1)
      
def trunc(f, n):
    # Truncates/pads a float f to n decimal places without rounding
    #拉长/缩短一个浮点数f到一个n位小数
    slen = len('%.*f' % (n, f))
    return float(str(f)[:slen])

if __name__ == '__main__':
    try:
        NavTest()
        rospy.spin()
    except rospy.ROSInterruptException:
        rospy.loginfo("AMCL navigation test finished.")