摘要：經(jīng)歷了五六天的摸索，終于搞出了使用機器人來實現(xiàn)多個位置目標的順序?qū)Ш剑瑢τ谧约簛碚f也是一個小小的成功，接下來我給大家進行分享如何實現(xiàn)這個功能如有疑問可以給我發(fā)我郵箱對于如何啟動的那些指令我就不再一一敘述，這個是最最基本的。

經(jīng)歷了五六天的摸索，終于搞出了使用turtlebot機器人來實現(xiàn)多個位置目標的順序?qū)Ш剑瑢τ谧约簛碚f也是一個小小的成功，接下來我給大家進行分享如何實現(xiàn)這個功能：如有疑問可以給我發(fā)我郵箱：liushengkai008@163.com
對于如何啟動tutlebot的那些指令我就不再一一敘述，這個是最最基本的。roscore別忘了，因為有的時候不打上這個指令會有程序出現(xiàn)問題
第一，我們需要構(gòu)建好一個地圖，將我們室內(nèi)的地圖構(gòu)建好了之后保存下來，這里我要提醒大家一下，構(gòu)建的地圖默認保存的路徑是在計算機下的tmp文件里，這個文件下的自己保存的地圖map，在關(guān)機后系統(tǒng)會默認的把它給刪除，所以我們最好把保存的地圖更換一個路徑保存下來，這樣就方便我們?nèi)プx取地圖數(shù)據(jù)，防止誤刪除辛苦構(gòu)建的地圖
第二，地圖構(gòu)建完畢，1、正常啟動turtlebot 2、載入要讀取的地圖，然后將turtlebot_navgation的包放在catkin_ws空間下，這個時候我們需要有一些小的對于安裝包的調(diào)整，這個自己捉摸一下很快就會搞定哈哈，不會的可以問我哦，因為是一些小細節(jié)，根據(jù)出現(xiàn)的問題或百度或其他的資源絕對能夠解決
3、在網(wǎng)上我找到有個隨機導(dǎo)航的算法程序，這個是隨機的路徑的導(dǎo)航不能滿足我們最短時間內(nèi)的解決我們的問題，所以我對該程序進行了修改，修改后的程序如下：

#-*- coding: UTF-8 -*-
#!/usr/bin/env python

import roslib; roslib.load_manifest("rbx1_nav")
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
m = 1
class NavTest():
    def __init__(self):
        rospy.init_node("nav_test", anonymous=True)
        
        rospy.on_shutdown(self.shutdown)
        
        
        self.rest_time = rospy.get_param("~rest_time", 10)
        
        
        self.fake_test = rospy.get_param("~fake_test", False)
        
       
        locations = dict()
        
        #locations["hall_foyer"] = Pose(Point(0.643, 4.720, 0.000), Quaternion(0.000, 0.000, 0.223, 0.975))
        locations["hall_foyer"] = Pose(Point( 8.168, -2.767, 2.669), Quaternion(0.000, 0.000, 0.223, 0.975))
        locations["hall_kitchen"] = Pose(Point(2.531, 0.189, 1.118), Quaternion(0.000, 0.000, -0.670, 0.743))
        locations["hall_bedroom"] = Pose(Point(5.629, 7.590, -0.388), Quaternion(0.000, 0.000, 0.733, 0.680))
        locations["living_room_1"] = Pose(Point(4.720, 9.551, 2.704), Quaternion(0.000, 0.000, 0.786, 0.618))
        locations["living_room_2"] = Pose(Point(3.343, -0.859, -0.864), Quaternion(0.000, 0.000, 0.480, 0.877))
       # locations["dining_room_1"] = Pose(Point(25.687,36.060, 2.990), Quaternion(0.000, 0.000, 0.899, -0.451))
        #locations["dining_room_2"] = Pose(Point(2.341,37.278, 3.309), Quaternion(0.000, 0.000, 0.892, -0.451))
        #locations["dining_room_3"] = Pose(Point(-3.237,30.083, -1.531), Quaternion(0.000, 0.000, 0.896, -0.451))
        #locations["dining_room_4"] = Pose(Point(-2.747,11.047, -1.803), Quaternion(0.000, 0.000, 0.895, -0.451))
        
        
        self.cmd_vel_pub = rospy.Publisher("cmd_vel", Twist)
        
       
        self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
        
        rospy.loginfo("Waiting for move_base action server...")
        
       
        self.move_base.wait_for_server(rospy.Duration(60))
        
        rospy.loginfo("Connected to move base server")
        
        
        initial_pose = PoseWithCovarianceStamped()
        
       
        n_locations = len(locations)    
        
       
        start_time = rospy.Time.now()
       
        
        
        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)
        
       
        while initial_pose.header.stamp == "":
            rospy.sleep(1)
            
        rospy.loginfo("Starting navigation test")
        
        m="true"
        
        while m:
           
            if i == n_locations:
                i = 0
                m=0
                
               
              
           
            mm=locations
            

                        
           
            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 = ""
            
           
            i += 1
            n_goals += 1
           
            for k in mm:
                self.goal = MoveBaseGoal()
                self.goal.target_pose.pose = mm[k]
                self.goal.target_pose.header.frame_id = "map"
                self.goal.target_pose.header.stamp = rospy.Time.now()
                
               
                self.move_base.send_goal(self.goal)
                
                
                finished_within_time = self.move_base.wait_for_result(rospy.Duration(300)) 
                
              
                
                
                
                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
    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.") 

程序就是這些，需要我們建立一個.py的文件，放入turtlebot_navigation的安裝包下，到時候把地圖載進去之后，運行 python XX.py文件就可以了，最后調(diào)出rviz的screen 確定當(dāng)前的位置就可以自己進行導(dǎo)航了
注意：我們需要通過事先構(gòu)建好的地圖，來獲取我們想要機器人到達的地方，用rqt_console就可以獲取節(jié)點的坐標信息。如有任何問題可以來找我哦

以上的實現(xiàn)了多點導(dǎo)航的問題，但是也有一些的不足之處，比如坐標需要我們事先的輸入進去，不能直接使用鼠標，這個很費力很耗時間，我后期會把這個做一個界面出來，能夠方面操作，大家有任何想法可以跟我交流哦