move_base导航的源码心得

最新推荐文章于 2024-03-18 20:08:22 发布

一朵云008

最新推荐文章于 2024-03-18 20:08:22 发布

阅读量9.3k

点赞数 6

分类专栏：【ROS探索】

本文链接：https://blog.csdn.net/David_Han008/article/details/72032898

版权

【ROS探索】专栏收录该内容

19 篇文章 4 订阅

订阅专栏

参考move_base当中的代码的学习。
http://www.cnblogs.com/shhu1993/p/6323699.html
关于planner主要关注的就是：

nav_core::BaseGlobalPlanner
nav_core::BaseLocalPlanner
nav_core::RecoveryBehavior

上面的这个三个东西都是插件的形式进行存储，十分是可以更换的。
关于costmap要关注的就是：

global planner costmap
local planner costmap

这里是关于action这个PKG，我看的云里雾里的。
http://wiki.ros.org/actionlib
但是我就是觉得这玩意挺重要

我们在实现的过程中，就是要写一些自己的plugIn。
参考链接：
http://blog.csdn.net/u013158492/article/details/50502266
在costmap_2d中，大量使用了plugin技术，来配置layer和planner. plugin不需要重新编译就可以实现

最近在做一个多点导航的事情，在创客智造上面找到turtlebot的自主导航

内容参考创客智造

先补充一些基础

rospack depends package

这里写图片描述
可以查询package的依赖项

书写package.xml
这里写图片描述
定义简单的消息发布机制

rosed packagename msgname 其实就是进入了VIM界面。
1、需要修改package.xml
添加这两条

<build_depend>message_generation</build_depend>
<run_depend>message_runtime</run_depend>

2、然后在cmakelist当中添加

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
 message_generation
)

add_message_files(
  FILES
  Num.msg
)

 generate_messages(
   DEPENDENCIES
   std_msgs
 )

catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES beginner_tutorials
   CATKIN_DEPENDS roscpp rospy std_msgs message_runtime
#  DEPENDS system_lib
)

3、然后编译一下
4、检查服务命令
rosmsg show multinav/Num
输出int64 num

python编写简单的服务器和客户端

$ roscd beginner_tutorials
$ mkdir srv
$ cd srv
$ touch AddTwoInts.srv
$ rosed beginner_tutorials AddTwoInts.srv

将srv稳中分成请求部分和响应部分通过—进行分隔

int64 A
int64 B
---
int64 Sum

修改package.xml，添加下面两条依赖项

<build_depend>message_generation</build_depend>
<run_depend>message_runtime</run_depend>

CMakeLists.txt,增加依赖,

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
 message_generation
)

catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES beginner_tutorials
   CATKIN_DEPENDS roscpp rospy std_msgs message_runtime
#  DEPENDS system_lib
)

然后编译一下
用rossrv show beginner_tutorials/AddTwoInts

int64 A
int64 B
---
int64 sum

由于前面是大A和大B 那么后面写代码的地方也应该是大A和大B

#!/usr/bin/env python

NAME='add_two_ints_server'

from multinav.srv import *
import rospy

def add_two_ints(req):
    print("add %s + %s = %s"%(req.A,req.B,(req.A+req.B)))
    sum=req.A+req.B
    return AddTwoIntsResponse(sum)

def add_two_ints_server():
    rospy.init_node(NAME)
    s=rospy.Service('add_two_ints',AddTwoInts,add_two_ints)
    print "read to add two ints"
    rospy.spin()

if __name__=="__main__":
    add_two_ints_server()

from multinav.srv import *导入自定义的服务

这是定义的服务的节点名称，服务的类型，和处理函数，基本上就是这三个。

s = rospy.Service('add_two_ints', AddTwoInts, add_two_ints)

rosservice call /add_two_inits 1 2
sum :3

rosrun beginner_tutorials add_two_ints_server.py 
Ready to add Two Ints
Returning [1 + 2 = 3]
Returning [1 + 4 = 5]
Returning [1 + 3 = 4]

上面的这是一个服务器，与服务器对应的就是客户端。

#!/usr/bin/env python

import sys
import os

import rospy

# imports the AddTwoInts service 
from rospy_tutorials.srv import *

## add two numbers using the add_two_ints service
## @param x int: first number to add
## @param y int: second number to add
def add_two_ints_client(x, y):

    # NOTE: you don't have to call rospy.init_node() to make calls against
    # a service. This is because service clients do not have to be
    # nodes.

    # block until the add_two_ints service is available
    # you can optionally specify a timeout
    rospy.wait_for_service('add_two_ints')

    try:
        # create a handle to the add_two_ints service
        add_two_ints = rospy.ServiceProxy('add_two_ints', AddTwoInts)

        print "Requesting %s+%s"%(x, y)

        # simplified style
        resp1 = add_two_ints(x, y)

        # formal style
        resp2 = add_two_ints.call(AddTwoIntsRequest(x, y))

        if not resp1.sum == (x + y):
            raise Exception("test failure, returned sum was %s"%resp1.sum)
        if not resp2.sum == (x + y):
            raise Exception("test failure, returned sum was %s"%resp2.sum)
        return resp1.sum
    except rospy.ServiceException, e:
        print "Service call failed: %s"%e

def usage():
    return "%s [x y]"%sys.argv[0]

if __name__ == "__main__":

    argv = rospy.myargv()
    if len(argv) == 1:
        import random
        x = random.randint(-50000, 50000)
        y = random.randint(-50000, 50000)
    elif len(argv) == 3:
        try:
            x = int(argv[1])
            y = int(argv[2])
        except:
            print usage()
            sys.exit(1)
    else:
        print usage()
        sys.exit(1)
    print "%s + %s = %s"%(x, y, add_two_ints_client(x, y))

rospy.wait_for_service('add_two_ints')

表示接入服务节点
add_two_ints=rospy.ServiceProxy(‘add_two_ints’,AddTwoInts)创建一个实例化的句柄
进行测试的rosrun beginner_tutorials add_two_ints_client.py 4 5

获取全局参数
rospy.get_param('/global_param_name')

获取目前命名空间的参数
rospy.get_param('param_name')

获取私有命名空间的参数
rospy.get_param('~private_param_name')

获取参数，如果没有，那么使用默认的值
rospy.get_parpam('foo','default_value')

设置参数.使用使用rospy.set_param(param_name,param_value)

rospy.set_param('some_number',[1,2,3,4])
rospy.set_param('truth',True)
rospy.set_param('~private_bar',1+2)

删除参数

rospy.delete_param('to_delete')

判断参数是否存在

if rospy.has_param('to_delete'):
    rospy.delete_param('to_delete')

解释参数名
rospy.resolve_name(name)用来获取一个节点的最初的名字。因为在remap可以将节点的名称进行各种映射。
搜索参数
rospy.search_param(‘参数名’)
rospy当中的日志的API

rospy.logdebug(msg,*args)
rospy.logwarn(msg,*args)
rospy.loginfo(msg,*args)
rospy.logerr(msg,*args)
rospy.logfatal(msg,*args)

实际使用

#!/usr/bin/env python
import rospy
from std_msgs.msg import String

def log_level():
    debug="This is debug"
    rospy.logdebug("success to debug %s",debug)

    info="This is info"
    rospy.loginfo("success to info %s",info)

    warn="This is warn"
    rospy.logwarn("success to warn %s",warn)

    error="This is error"
    rospy.logerr("success to err %s",error)

    fatal="This is fatal"
    rospy.logfatal("success to fatal %s", fatal)

if __name__=='__main__':
    try:
        rospy.init_node('log_level',log_level=rospy.DEBUG)
        log_level()
    except rospy.ROSException:
        pass

这里写图片描述

可以重用一个已经在你工作区间的包
这里写图片描述

未使用numpy的listener的节点
这里写图片描述

#!/usr/bin/env python

import rospy
from rospy_tutorials.msg import Floats

def callback(data):
    print rospy.get_name(), "I heard %s"%str(data.data)

def listener():
    rospy.init_node('listener')
    rospy.Subscriber("floats", Floats, callback)
    rospy.spin()

if __name__ == '__main__':
    listener()

这里写图片描述

#!/usr/bin/env python

import rospy
from rospy.numpy_msg import numpy_msg
from rospy_tutorials.msg import Floats

import numpy
def talker():
    pub = rospy.Publisher('floats', numpy_msg(Floats),queue_size=10)
    rospy.init_node('talker', anonymous=True)
    r = rospy.Rate(10) # 10hz
    while not rospy.is_shutdown(): 这个地方相当于while(ros::ok())
        a = numpy.array([1.0, 2.1, 3.2, 4.3, 5.4, 6.5], dtype=numpy.float32)
        pub.publish(a)
        r.sleep()

if __name__ == '__main__':
    talker()

通过这个函数来发布经过numpy函数处理过的浮点值内容

pub = rospy.Publisher('floats', numpy_msg(Floats))
a = numpy.array([1.0, 2.1, 3.2, 4.3, 5.4, 6.5], dtype=numpy.float32)

利用numpy转化成numpy的数组结构，指定内容的数据格式，最后说明是float类型。
这里写图片描述
verbose=FALSE，意思就是设置运行的时候不显示详细信息。

从这两句我们就可以看出，单引号里面可以包含多引号，反正就是这么写。

   print "subscribe to /camera/image/compress success"
  print 'received image type ： “%s”'%ros_data.format

导入python的库

# Python libs
import sys, time
# numpy and scipy
import numpy as np
from scipy.ndimage import filters

# OpenCV
import cv2

# Ros libraries
import roslib
import rospy

# Ros Messages
from sensor_msgs.msg import CompressedImage

导入所需要的库文件，Python numpy库，opencv的库，ros相关的消息
time用于测量特征的时间 import time Numpy和SciPy用于处理转换和显示特征

class image_feature:

    def __init__(self): 用于实例化
    ...

    def callback(self, ros_data): 用于处理图片信息

初始化节点

rospy.init_node('my_node_name')
rospy.init_node('my_node_name',anonymous=True)

init_node()函数需要提供一个节点名，必须要是唯一的节点名称。
如果不太关心节点的唯一性情况下，可以设置anonymous=True。
关闭节点

while not rospy.is_shutdown():
    do some work

使用命令行的参数

rospy.myargv(argv=sys.argv)

发布数据和话题

pub=rospy.Publisher('toplic_name',std_msgs.msg.String,queue_size=10)
pub.publish(std_msgs.msg.String("foo"))

rospy.Publisher(toplic_name,msg_class,queue_size)

ros当中有rospy.time 和rospy.Duration
获取当前的时间

now=rospy.Time.now()
now=rospy.get_time()

TF的广播器

#！/usr/bin/env python
import roslib
roslib.load_manifest('learning_tf')
import rospy

import tf
import turtlesim.msg

def handle_turtle_pose(msg,turtlesim):
    br=tf.TransformBroadcaster()
    br.sendTransform((msg.x,msg.y,0),tf.translations.quaternion_from_euler(0,0,msg.theta),rospy.Time.now(),turtlename,"world")

if __name__=="__main__":
    rospy.init_node('turtle_tf_broadcaster')
    turtlename=rospy.get_param('~turtle')
    rospy.Subscriber('%s/pose'%turtlename,turtlesim.msg.Pose,handle_turtle_pose,turtlename)
    rospy.spin()

导入actonlib的库

import actionlib

导入生成的消息

import actionlib_tutorials.msg

self._as = actionlib.SimpleActionServer(self._action_name, actionlib_tutorials.msg.FibonacciAction, execute_cb=self.execute_cb, auto_start = False)

将上面的创客智造的代码分析完毕之后，进入正题，关于多点导航的问题

在这个地方肯定是要
如果在python当中，设置URDF模型的初始的位置？

      # 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()

或者在地方改。

  rospy.loginfo("Click on the map in RViz to set the intial pose...")  
        rospy.wait_for_message('initialpose', PoseWithCovarianceStamped)
rospy.Subscriber('initialpose', PoseWithCovarianceStamped, self.update_initial_pose)

然后这个应该就是问题的所在。改成sorted之后就没问题了
这里写图片描述

 # Wait for the /odom topic to become available  
rospy.wait_for_message('input', PoseWithCovarianceStamped)

这个人在这个地方使用了一个for循环来遍历整个字典当中的点，也是可以了
https://segmentfault.com/a/1190000006262518

机器人卡尔曼滤波器的roswiki官网的教程
http://wiki.ros.org/robot_pose_ekf

这个roswiki值得关注一下
http://wiki.ros.org/pose_publisher
http://www.codeforge.cn/read/236617/set_pose.py__html

仔细看了一下这个地方，感觉还是可以的
http://answers.ros.org/question/114631/robot-estimated-pose/
http://answers.ros.org/question/205308/publishing-to-initialpose-programmatically-on-turtlebot-navigation/
还有这个链接

感谢大神无私的贴出源码

#!/usr/bin/env python
import rospy
from geometry_msgs.msg import PoseWithCovarianceStamped

def initial_pos_pub():
    publisher = rospy.Publisher('initialpose', PoseWithCovarianceStamped, queue_size=10)
    rospy.init_node('initial_pos_pub', anonymous=True)
    #Creating the message with the type PoseWithCovarianceStamped
    rospy.loginfo("This node sets the turtlebot's position to the red cross on the floor. It will shudown after publishing to the topic /initialpose")
    start_pos = PoseWithCovarianceStamped()
    #filling header with relevant information
    start_pos.header.frame_id = "map"
    start_pos.header.stamp = rospy.Time.now()
    #filling payload with relevant information gathered from subscribing
    # to initialpose topic published by RVIZ via rostopic echo initialpose
    start_pos.pose.pose.position.x = -0.846684932709
    start_pos.pose.pose.position.y = 0.333061099052
    start_pos.pose.pose.position.z = 0.0

    start_pos.pose.pose.orientation.x = 0.0
    start_pos.pose.pose.orientation.y = 0.0
    start_pos.pose.pose.orientation.z = -0.694837665627
    start_pos.pose.pose.orientation.w = 0.719166613815

    start_pos.pose.covariance[0] = 0.25
    start_pos.pose.covariance[7] = 0.25
    start_pos.pose.covariance[1:7] = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 
    start_pos.pose.covariance[8:34] = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 
    0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 
    start_pos.pose.covariance[35] = 0.06853891945200942

    rospy.loginfo(start_pos)
    publisher.publish(start_pos)

if __name__ == '__main__':
    try:
        initial_pos_pub()
    except rospy.ROSInterruptException:
        pass

在这个里面我们发现，主要就是初始化了一个东西，然后给这个东西进行了赋值。分别对head.frame_id，以及header.stamp这里面每个参数都有。这里关于covariance的东西，我还是不是理解。

start_pos = PoseWithCovarianceStamped()
    #filling header with relevant information
    start_pos.header.frame_id = "map"
    start_pos.header.stamp = rospy.Time.now()
    #filling payload with relevant information gathered from subscribing
    # to initialpose topic published by RVIZ via rostopic echo initialpose
    start_pos.pose.pose.position.x = -0.846684932709
    start_pos.pose.pose.position.y = 0.333061099052
    start_pos.pose.pose.position.z = 0.0

    start_pos.pose.pose.orientation.x = 0.0
    start_pos.pose.pose.orientation.y = 0.0
    start_pos.pose.pose.orientation.z = -0.694837665627
    start_pos.pose.pose.orientation.w = 0.719166613815

    start_pos.pose.covariance[0] = 0.25
    start_pos.pose.covariance[7] = 0.25
    start_pos.pose.covariance[1:7] = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 
    start_pos.pose.covariance[8:34] = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 
    0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 
    start_pos.pose.covariance[35] = 0.06853891945200942

这下面是我要使用的，就是可以不通过做鼠标的点击RVIZ来实现

#!/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  

MAP_POINT_NUM=3

class MultiNav():  
    def __init__(self):  
        rospy.init_node('MultiNav', anonymous=True)  
        rospy.on_shutdown(self.shutdown)  

        # How long in seconds should the robot pause at each location?  
        self.rest_time = rospy.get_param("~rest_time", 2) 

        # 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[0] = Pose(Point(5.088, -0.171, 0.00), Quaternion(0.000, 0.000, 0.721, 0.693))  
        locations[1] = Pose(Point(9.394,-0.562, 0.00), Quaternion(0.000, 0.000, -0.646, 0.763))  
        locations[2] = Pose(Point(19.575, 0.065, 0.00), Quaternion(0.000, 0.000, 0.720, 0.694))  
       # locations['home_refrigerator'] = Pose(Point(-1.00, 6.88, 0.00), Quaternion(0.000, 0.000, 1.000, 0.000)) 
       # locations['home_door'] = Pose(Point(-2.80, 8.00, 0.00), Quaternion(0.000, 0.000, 0.000, 1.000))
       # locations['home_balcony'] = Pose(Point(-2.08, 4.57, 0.00), Quaternion(0.000, 0.000, 0.000, 1.000)) 


        # Publisher to manually control the robot (e.g. to stop it)  
        self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5)  

        # Subscribe to the move_base action server  
        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  
        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 the user in RViz  
        initial_pose = PoseWithCovarianceStamped()  




        initial_pose.header.frame_id='map'
        initial_pose.header.stamp=rospy.Time.now()
        initial_pose.pose.pose.position.x=-0.257
        initial_pose.pose.pose.position.y=0.036
        initial_pose.pose.pose.position.z=0

        initial_pose.pose.pose.orientation.x=0
        initial_pose.pose.pose.orientation.y=0
        initial_pose.pose.pose.orientation.z=-0.01
        initial_pose.pose.pose.orientation.w=1

        initial_pose.pose.covariance[0]=0.25
        initial_pose.pose.covariance[7]=0.25
        initial_pose.pose.covariance[1:7]=[0.0,0.0,0.0,0.0,0.0,0.0]
        initial_pose.pose.covariance[8:34]=[0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0]
        initial_pose.pose.covariance[35]=0.06853891945200942


        # 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 on the map in RViz to set the intial 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 = sorted(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  
            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)  

            # Allow 5 minutes to get there  
            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()  
                rospy.loginfo("Timed out achieving goal")  
            else:  
                state = self.move_base.get_state()  
                if state == GoalStatus.SUCCEEDED:  
                    rospy.loginfo("Goal succeeded!")  
                    n_successes += 1  
                    distance_traveled += distance  
                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)

            #judge stop
            if i==MAP_POINT_NUM: 
                break            

    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:  
        MultiNav()  
        rospy.spin()  
    except rospy.ROSInterruptException:  
        rospy.loginfo("AMCL navigation test finished.")

这个地方是，是顺序排查字典里面的元素

  sequence = sorted(locations)

这个地方是做了一个字典当中数量的判断。

  #judge stop
            if i==MAP_POINT_NUM: 
                break

我觉得将来也可以作一个安卓的APP
http://blog.csdn.net/lu199012/article/details/72649357

我打算花一些时间弄清楚整个move_base的框架，
然后我吧turtlebot_app当中的turtlebot_navigation，然后我出现了一个问题，就是没有turtlebot_msgs。然后我再下面的这个链接当中找到了下载路径。
https://github.com/turtlebot/turtlebot_msgs
随后编译就能过了。
找到这个配置文件。
这里写图片描述
这三个文件当中包含的关于地图的参数：

这里写图片描述
反应到launch文件当中就是

关于这里面参数，如果你是默认的参数的参数的话：
所有的参数都是用的默认的参数的话。
这里写图片描述
如果我们使用的栅格路径的话。也就是说将use_grid_path=True的话。

如果不使用二次方式的话 use_quadratic=False

使用A*算法的话，也就是use_dijkdtra=False

同样的条件下，如果我们使用Dijkstra算法的话：

如果我们只是使用A*算法的话。
这里写图片描述
如果想查看默认的参数，那么点击这个链接：http://wiki.ros.org/global_planner
这个地方是改膨胀区的的透明度，值越大越不透明。

那么下面我们讲解一下参数：

 <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
    <rosparam file="$(find turtlebot_navigation)/param/costmap_common_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find turtlebot_navigation)/param/costmap_common_params.yaml" command="load" ns="local_costmap" />   
    <rosparam file="$(find turtlebot_navigation)/param/local_costmap_params.yaml" command="load" />   
    <rosparam file="$(find turtlebot_navigation)/param/global_costmap_params.yaml" command="load" />
    <rosparam file="$(find turtlebot_navigation)/param/dwa_local_planner_params.yaml" command="load" />
    <rosparam file="$(find turtlebot_navigation)/param/move_base_params.yaml" command="load" />
    <rosparam file="$(find turtlebot_navigation)/param/global_planner_params.yaml" command="load" />
    <rosparam file="$(find turtlebot_navigation)/param/navfn_global_planner_params.yaml" command="load" />

在这些参数里面分成两类：map和planner。我原来的包当中，Planner用了一个base的配置文件。现在就是导入这么多文件。

footprint: [[-0.17, -0.20], [-0.20, -0.10], [-0.225, 0.0], [-0.20, 0.10], [-0.17, 0.20], [0.10, 0.20], [0.17, 0.10], [0.225, 0.0], [0.17, -0.10], [0.10, -0.20]] #机器人的形状
footprint_padding: 0.01
inflation_radius: 0.20 #膨胀区的半径

map_type: voxel #地图的类型是栅格地图


obstacle_layer: 
  enable: true
  max_obstacle_height: 0.6 # 最大障碍物高度
  min_obstacle_height: 0.0 # 最小障碍物高度
  origin_z: 0.0 
  z_resolution: 0.2
  z_voxels: 2
  unknow_threshold: 15 未知区域的阈值
  mark_threshold: 0  
  combination_methold: 1 合并方式，这里应该是有一些方式
  obstacle_range: 2.5 障碍物的半径
  raytrace_range: 3.0 光线跟踪的半径
  min_obstacle_dist: 1.0 最小的障碍物的距离
  costmap_obstacles_behind_robot_dist: 1.2 距离障碍物几米之后，回来。
  weight_obstacle: 50 障碍物的宽度
  observation_sources: scan 观测的来源
  scan: {data_type: LaserScan, topic: /scan, marking: true, clearing: true}

static_layer:
  enable: true

inflation_layer: #关于膨胀区
  enable: true
  cost_scaling_factor: 5.0 花费的尺度 障碍物成本的下降速率
  inflation_radius: 0.5 离障碍物的最大距离

全局的代价地图

global_costmap:
   global_frame: /map
   robot_base_frame: /base_footprint #机器人的参考 这里我的先锋是base_link
   update_frequency: 1.0
   publish_frequency: 0.5
   static_map: true
   transform_tolerance: 0.5
   <!-- global map引入了以下三层，经融合构成了master map，用于global planner-->
   plugins: #插入地图的类型
     - {name: static_layer,            type: "costmap_2d::StaticLayer"}
     - {name: obstacle_layer,          type: "costmap_2d::VoxelLayer"}
     - {name: inflation_layer,         type: "costmap_2d::InflationLayer"}

local_costmap:
   global_frame: odom
   robot_base_frame: /base_footprint #我这里是base_link
   update_frequency: 5.0
   publish_frequency: 2.0
   static_map: false
   rolling_window: true #是否进行滑窗
   width: 4.0
   height: 4.0
   resolution: 0.05
   transform_tolerance: 0.5
   <!-- local map引入了以下两层，经融合构成了master map，用于局部planner-->
   plugins:
    - {name: obstacle_layer,      type: "costmap_2d::VoxelLayer"}
    - {name: inflation_layer,     type: "costmap_2d::InflationLayer"}

地图上面只是做了一些显示的效果。关键的差异还是在planner的差异

然后是导入路径规划的参数：

 /param/move_base_params.yaml" command="load" 
 /param/global_planner_params.yaml" command="load"  
 /param/navfn_global_planner_params.yaml" command="load"  
 /param/dwa_local_planner_params.yaml" command="load"

move_base_params.yaml

shutdown_costmaps: false 不关闭地图

controller_frequency: 5.0 控制器更新的频率
controller_patience: 3.0 控制器容忍


planner_frequency: 1.0 规划的频率
planner_patience: 5.0 5秒没有反应就重新规划

oscillation_timeout: 10.0 震动的时间
oscillation_distance: 0.2 震动的频率

# local planner - default is trajectory rollout
base_local_planner: "dwa_local_planner/DWAPlannerROS"
<!--这里配置了local planner为dwa_local_planner -->用动态窗口法做局部的路径规划
<!--这里配置了global planner为navfn/NavfnROS -->全局的算用navfnROS
base_global_planner: "navfn/NavfnROS" #alternatives: global_planner/GlobalPlanner, carrot_planner/CarrotPlanner

全局的路径规划：

GlobalPlanner:
  old_navfn_behavior: false
  use_quadratic: true
  use_dijsktra: true     #if you want to use A* so use false
  use_grid_path: false

  allow_unknow: true

  planner_windows_x: 0.0
  planner_windows_y: 0.0
  default_tolerance: 0.0

  publish_scale: 100
  planner_costmap_publish_frequency: 0.0

  lethal_cost: 253
  neutral_cost: 50
  cost_factor: 3.0
  publish_potential: true

p3dx_dwa_local_planner_params.yaml

DWAPlanerROS:
  max_vel_x: 0.5
  min_vel_x: 0.1

  max_vel_y: 0.0
  min_vel_y: 0.0

  max_trans_vel: 0.5
  min_trans_vel: 0.1
  trans_stopped_vel: 0.1
  max_rotational_vel: 0.5
  max_rotational_vel: 1.0


  max_rot_vel: 5.0
  min_rot_vel: 0.4
  rot_stopped_vel: 0.4

  acc_lim_theta: 0.75
  acc_lim_x: 0.2
  acc_lim_y: 0.4

  yaw_goal_tolerance: 0.3
  xy_goal_tolerance: 0.15

  sim_time: 1.0
  vx_samples: 6
  vy_samples: 1
  vthea_samples: 20

  path_distance_bias: 64.0
  goal_distance_bias: 24.0
  occdist_scale: 0.5
  forward_point_distance: 0.325
  stop_time_buffer: 0.2
  scaling_speed: 0.25
  max_scaling_factor: 0.2

  oscillation_reset_dist: 0.05

  publish_traj_pc: true
  publish_cost_grid_pc: true 
  global_frame_id: odom

原始的文件：

controller_frequency: 10.0
recovery_behavior_enabled: false # 复原行为使能
clearing_rotation_allowed: false # 清除旋转允许
TrajectoryPlannerROS:
  max_vel_x: 0.50
  min_vel_x: 0.10
  max_rotational_vel: 0.5
  min_in_place_rotational_vel: 1.0
  acc_lim_theta: 0.75
  acc_lim_x: 0.20
  acc_lim_y: 0.50
  pdist_scale: 0.4
  gdist_scale: 0.8
  meter_scoring: true
  backup_vel: -0.2

  holonomic_robot: false
  yaw_goal_tolerance: 0.78
  xy_goal_tolerance: 0.15
  goal_distance_bias: 0.8
  path_distance_bias: 0.6
  sim_time: 1.5
  heading_lookahead: 0.325
  oscillation_reset_dist: 0.05

  vx_samples: 6
  vtheta_samples: 20
  dwa: false

在这个video当中讲解了如何添加导航的puglin（插件）
https://www.youtube.com/watch?v=We1gGDXAO_o
这个video是讲的如何使用你创建的插件
https://www.youtube.com/watch?v=t4A_niNlDdg

这里是关于如何将ROSQT的插件
https://www.youtube.com/watch?v=GqBzI7O9h8A
我感觉合格video对于使用A*的路径规划算法很有帮助。
https://www.youtube.com/watch?v=wf7FvOBaquY

这个hunman的插件让我考虑一下到底要不要去做
https://github.com/marinaKollmitz/human_aware_navigation

在下载movebase源码的过程，遇到的问题

CMake Error at /opt/ros/indigo/share/catkin/cmake/catkin_workspace.cmake:95 (message):
  This workspace contains non-catkin packages in it, and catkin cannot build
  a non-homogeneous workspace without isolation.  Try the
  'catkin_make_isolated' command instead.
Call Stack (most recent call first):
  CMakeLists.txt:63 (catkin_workspace)


-- Configuring incomplete, errors occurred!
See also "/home/davidhan/catkin_ws01/build/CMakeFiles/CMakeOutput.log".
See also "/home/davidhan/catkin_ws01/build/CMakeFiles/CMakeError.log".

这里写图片描述
就是这个这个package里面哟不是catkin能够编译的包，那么我就可以用catkin_make_isolated进行编译。

$ sudo apt-get install ros-indigo-turtlebot-bringup \
ros-indigo-turtlebot-create-desktop ros-indigo-openni-* \
ros-indigo-openni2-* ros-indigo-freenect-* ros-indigo-usb-cam \
ros-indigo-laser-* ros-indigo-hokuyo-node \
ros-indigo-audio-common gstreamer0.10-pocketsphinx \
ros-indigo-pocketsphinx ros-indigo-slam-gmapping \
ros-indigo-joystick-drivers python-rosinstall \
ros-indigo-orocos-kdl ros-indigo-python-orocos-kdl \
python-setuptools ros-indigo-dynamixel-motor-* \
libopencv-dev python-opencv ros-indigo-vision-opencv \
ros-indigo-depthimage-to-laserscan ros-indigo-arbotix-* \
ros-indigo-turtlebot-teleop ros-indigo-move-base \
ros-indigo-map-server ros-indigo-fake-localization \
ros-indigo-amcl git subversion mercurial

总之就是各种编译不过，然后我尝试着暗转一些启动的一些东，能不能把编译通过了

sudo apt-get install ros-indigo-orocos-kdl
ros-indigo-fake-localization
ros-indigo-laser-*
ros-indigo-dynamixel-motor-*

sudo apt-get install ros-indigo-dynamic-reconfigure

如果这个时候还是有问题的话，呢么你就把build 和devel这两个文件都删除掉，然后在进行catkin_make，吐槽一句，其实出现这个问题原因是因为一些ROS当中，因为为了增加开发的效率，会保存一些上次编译的东西在build和devel当中，所有为了能够然让你这次的编译通过，那么直接将原来的这个两个文件删除之后，重新编译就可以通过了。

感觉还是白巧克力博客比较好
http://blog.csdn.net/heyijia0327/article/details/41823809/
PWM值，就是脉冲宽度调制。通过在一个周期中，开关导通的时间开控制转速。
整个move_base的框架必须要输入的是：
goal
tf
odom
LaserScan
输出
cmd_vel
move_base接收到goal之后，将目标goal通过actionlib的client（客户端）想服务器发送数据，服务器根据你的tf关系以及发布odom消息不断反馈机器人的状态到客户端，然后让move_base当中的客户端进行控制转向twist
这里写图片描述
其中linear 的x就是代表前进方向的速度，单位为m/s。angular 的z就代表机器人的绕中心旋转的角速度，单位为弧度/s (rad/s)。

如何将左右轮的差速，转化成转过的角度，就是用下面这个公式：
yaw_rate = (Rwheelspeed - Lwheelspeed) / d .其中d为两轮间的间距，得到的转速单位rad/s。
move_base实现了一个actionlib用于设定目标的位置。
链接global planner和local planner用于导航。可以通过重新实现nav_core::BaseGlobalPlanner是nav_core::BaseLocalPlanner修改global和local planner的导航策略。

因为我要修改的local planner那么我先把这个地方做了，然后将来有机会的话，我觉得，就是那个人的想法，使用多个雷达，或者一个激光雷达进行路径规划和测距，其实我觉得在代码里面也是能够实现，到时候看情况吧，目前我还是觉得不管怎么样子，在local planner 这个地方还是需要将壁障给弄好，然后弄好之后，接下来就是该是做全局的路径规划，然后就是能够通过ROS制作一个界面出来，之后这部分就不在花时间做做了，将来的这个地方，地方就是能够将机械活动的部分在加上，基本上就可以了。
然后自己现在开始看的定地方技术就是这个地方，然后我尽快找到这个函数的实现地方
但是感觉这个点放只是指定做单个的点的位置，还是不能够满足用户用的需要，如果我能够指定多个在输入的时候多个目标点，那会怎么样呢？

我也不知道该怎么分析代码，那就一个文件一个文件的看吧，或者在网上找了一个大牛的博客，然后边看博客，边对照源码进行参看。

<library path="lib/libglobal_planner">
  <class name="global_planner/GlobalPlanner" type="global_planner::GlobalPlanner" base_class_type="nav_core::BaseGlobalPlanner">
    <description>
      A implementation of a grid based planner using Dijkstras or A*
    </description>
  </class>
</library>

这里写图片描述
然后将这个插件的细节都提取出来。

感觉这个cfg很多东西都是python的东西

其实不管怎么样，我觉得现在我还是需要先把按个关于如何设置插件的这个过程熟悉一下，然后开始写代码。

MoveBase::executeCb这个函数就用来启动线程的

一朵云008

关注

6
点赞
踩
45

收藏

觉得还不错? 一键收藏
3
评论
move_base导航的源码心得

参考move_base当中的代码的学习。 http://www.cnblogs.com/shhu1993/p/6323699.html 关于planner主要关注的就是：nav_core::BaseGlobalPlannernav_core::BaseLocalPlannernav_core::RecoveryBehavior关于costmap要关注的就是：global planner co
复制链接

扫一扫