使用turtlebot来实现多点导航跟踪的问题

摘要：经历了五六天的摸索，终于搞出了使用机器人来实现多个位置目标的顺序导航，对于自己来说也是一个小小的成功，接下来我给大家进行分享如何实现这个功能如有疑问可以给我发我邮箱对于如何启动的那些指令我就不再一一叙述，这个是最最基本的。

经历了五六天的摸索，终于搞出了使用turtlebot机器人来实现多个位置目标的顺序导航，对于自己来说也是一个小小的成功，接下来我给大家进行分享如何实现这个功能：如有疑问可以给我发我邮箱：liushengkai008@163.com
对于如何启动tutlebot的那些指令我就不再一一叙述，这个是最最基本的。roscore别忘了，因为有的时候不打上这个指令会有程序出现问题
第一，我们需要构建好一个地图，将我们室内的地图构建好了之后保存下来，这里我要提醒大家一下，构建的地图默认保存的路径是在计算机下的tmp文件里，这个文件下的自己保存的地图map，在关机后系统会默认的把它给删除，所以我们最好把保存的地图更换一个路径保存下来，这样就方便我们去读取地图数据，防止误删除辛苦构建的地图
第二，地图构建完毕，1、正常启动turtlebot 2、载入要读取的地图，然后将turtlebot_navgation的包放在catkin_ws空间下，这个时候我们需要有一些小的对于安装包的调整，这个自己捉摸一下很快就会搞定哈哈，不会的可以问我哦，因为是一些小细节，根据出现的问题或百度或其他的资源绝对能够解决
3、在网上我找到有个随机导航的算法程序，这个是随机的路径的导航不能满足我们最短时间内的解决我们的问题，所以我对该程序进行了修改，修改后的程序如下：

#-*- 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文件就可以了，最后调出rviz的screen 确定当前的位置就可以自己进行导航了
注意：我们需要通过事先构建好的地图，来获取我们想要机器人到达的地方，用rqt_console就可以获取节点的坐标信息。如有任何问题可以来找我哦

以上的实现了多点导航的问题，但是也有一些的不足之处，比如坐标需要我们事先的输入进去，不能直接使用鼠标，这个很费力很耗时间，我后期会把这个做一个界面出来，能够方面操作，大家有任何想法可以跟我交流哦