slam功能包详解

base_control(底盘)

launch文件

将机器人本身硬件名字和发布订阅的话题更名并设置串口通信参数波特率和端口
启动base_control功能包下的base_control节点来控制机器人底盘
默认更名：
baudrate:——————115200
port:————————/dev/move_base
base_id:——————-base_footprint
odom_id:——————-odom
imu_id:———————imu
odom_topic:—————odom
imu_topic:—————-imu
battery_topic:———-battery
cmd_vel_topic:———-cmd_vel(ros自带的终端控制电机话题-不停的发布)
当需要控制机器人移动时可以向这个话题里写数据http://wiki.ros.org/Robots/TIAGo/Tutorials/motions/cmd_vel
ackermann_cmd_topic:-ackermann_cmd
imu和雷达不发布话题,不订阅ackermann话题
launch文件输入robot_name参数后以上除了波特率和端口，其余改为robot_name/()
eg: robot_name/base_footprint
base_control.py文件

打开串口,
编写定时器回调函数timerOdomC—timerBatteryCB—timerSonarCB—timerIMUCB当定时器时间到了就执行这四个函数(经过定时器回调函数timerCommunicationCB解析)分别从串口获取odom,电池,超声波,imu信息数据并发布话题，同时发布base_link-odom,Sonar-base_link,imu-base_link坐标关系
当终端的键盘控制节点启动就会自动发布cmd_vel,编写订阅函数当里面有东西时执行回调函数cmdCB,复制vel_cmd里存的信息，根据串口通讯进行转换再通过串口写入底盘

package.xml文件

package.xml用来做功能包的描述，它内容包含有功能包名，版本号，功能包功能描述，作者联系邮箱，开源协议等等。

//指明xml语法的版本
<?xml version="1.0"?>
<package>
//指明xml对应的功能包名
  <name>base_control</name>
  //维护的版本-自定义，每次维护一次可以改一下
  <version>1.3.0</version>
  //对功能包的描述
  <description>The base_control package</description>

  <!-- One maintainer tag required, multiple allowed, one person per tag --> 
  <!-- Example:  -->
  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
  //包的作者和联系方式
  <maintainer email="liu.fuzhi@bingda-robot.com">FuZhi, Liu</maintainer>


  <!-- One license tag required, multiple allowed, one license per tag -->
  <!-- Commonly used license strings: -->
  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
  //开源证书
  <license>Apache 2.0</license>


  <!-- Url tags are optional, but multiple are allowed, one per tag -->
  <!-- Optional attribute type can be: website, bugtracker, or repository -->
  <!-- Example: -->
  <!-- <url type="website">http://wiki.ros.org/base_control</url> -->


  <!-- Author tags are optional, multiple are allowed, one per tag -->
  <!-- Authors do not have to be maintainers, but could be -->
  <!-- Example: -->
  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->


  <!-- The *_depend tags are used to specify dependencies -->
  <!-- Dependencies can be catkin packages or system dependencies -->
  <!-- Examples: -->
  <!-- Use build_depend for packages you need at compile time: -->
  <!--   <build_depend>message_generation</build_depend> -->
  <!-- Use buildtool_depend for build tool packages: -->
  <!--   <buildtool_depend>catkin</buildtool_depend> -->
  <!-- Use run_depend for packages you need at runtime: -->
  <!--   <run_depend>message_runtime</run_depend> -->
  <!-- Use test_depend for packages you need only for testing: -->
  <!--   <test_depend>gtest</test_depend> -->
  //run_depend是用来帮助rosdep这个工具解决依赖项的问题
  <run_depend>rospy</run_depend> 
  <run_depend>tf</run_depend> 
  <run_depend>geometry_msgs</run_depend> 
  <run_depend>nav_msgs</run_depend> 
  <run_depend>sensor_msgs</run_depend> 
  <run_depend>ackermann_msgs</run_depend> 
  <run_depend>teleop_twist_keyboard</run_depend> 
  

  <buildtool_depend>catkin</buildtool_depend>


  <!-- The export tag contains other, unspecified, tags -->
  <export>
    <!-- Other tools can request additional information be placed here -->

  </export>
</package>

base_control.launch

arg标签

arg标签是用来在launch文件中声明参数的，在launch中声明过的参数都可以在启动launch文件时传入已声明过的参数。我们在终端输入roslaunch base_control base_control.launch然后按TAB键补全就能看到可传入的参数列表。default的内容则是对参数赋值。如无参数传入则使用launch文件中定义的值。也可以在启动launch文件时传入参数的值。例如传入pub_imu的值为true：
roslaunch base_control base_control.launch pub_imu:=ture
group标签

group标签会将group里的内容划分成一个组，在launch中使用if unless作为判断语句。判断符合第一个条件则执行第一个group中的内容，否则则执行第二个group的内容。
node标签

node标签包括了要执行的节点的名称name、该节点所在的包名pkg，节点的可执行文件type以及输出方式output，“screen”表示输出在当前终端屏幕。
param标签

节点中有param标签，它的作用也是声明参数。param和arg的区别在于，param声明的变量是节点所接收的，而arg的参数是launch文件所接收的。这里利用param标签设置了传入节点的参数，这些参数的值是通过value直接传入的固定值。例如：
1
2
<param name="baudrate" value="115200"/>
<param name="port" value='/dev/move_base'/>
也可以是通过value传入的一个参数
1
2
<param name="base_id" value="$(arg base_frame)"/>
//这里base_id这个参数的值就是launch文件中arg标签声明的参数base_frame
ns

ns是ros下的命名空间机制。使用命名空间后，比如会在话题名前加上robot_name这个变量所对应的参数值。以odom话题为例如果传入的robot_name是robot1，查看话题列表显示的就会是/robot1/odom。这样是为例便于支持多机器人，防止话题名重名。

代码注释：

//指明xml版本
<?xml version="1.0"?>

<launch>
//传入机器人名字，机器人坐标系名称：imu和Odom类似都用来测位姿变化本次只用了odom
    <!-- support multi robot -->
    <arg name="robot_name" default=""/>   <!-- support multi robot -->
    <!-- robot frame -->
    <arg name="base_frame"       default="base_footprint" />    
    <arg name="odom_frame"       default="odom" />
    <arg name="imu_frame"       default="imu" /> 
 //给机器人硬件odom ,imu,电池，发布的话题名字改为odom,imu,battery  
    <!-- pub topic -->
    <arg name="odom_topic"       default="odom" /><!-- do NOT use '/' in topic name-->
    <arg name="imu_topic"       default="imu" />
    <arg name="battery_topic"       default="battery" />
    <!-- sub topic -->
//给终端发布的控制速度话题,ackermann机器人控制话题命名为cmd_vel,ackermann_cmd
    <arg name="cmd_vel_topic"       default="cmd_vel" /> <!-- do NOT use '/' in topic name-->
    <arg name="ackermann_cmd_topic"       default="ackermann_cmd" />
    <!-- config param -->
    <arg name="pub_imu"       default="False" />
    <arg name="sub_ackermann"       default="False" />
    <arg name="pub_sonar"       default="False" />
    <!-- base control node -->
//因为默认机器人名字为空，所以执行第一个group里的内容
    <!--ros namespace name can NOT be empty,so need evaluate use multi robot or not-->
    <group if="$(eval robot_name == '')">
    //启动base_control功能包下的base_control节点来控制机器人底盘
        <node name="base_control"   pkg="base_control"  type="base_control.py" output="screen">
    //给base_control节点传入参数
    //波特率，端口--通信用的

            <param name="baudrate"   value="115200"/>  
            <param name="port"  value='/dev/move_base'/>
    //底盘、odom、imu这些硬件命名为某某坐标系--引用了上面的base_footprint，odom，imu
            <param name="base_id"    value="$(arg base_frame)"/> <!-- base_link name -->
            <param name="odom_id"    value="$(arg odom_frame)"/>      <!-- odom link name -->
            <param name="imu_id"    value="$(arg imu_frame)"/>      <!-- imu link name -->
    //odom、imu、电池这些硬件发布的话题命名为--引用了上面的odom，imu,battery
            <param name="odom_topic" value="$(arg odom_topic)"/> <!-- topic name -->
            <param name="imu_topic" value="$(arg imu_topic)"/> <!-- topic name -->
            <param name="battery_topic" value="$(arg battery_topic)"/> <!-- topic name -->
    //终端发布的控制速度话题命名为--引用了上面的cmd_vel,ackermann_cmd
            <param name="cmd_vel_topic" value="$(arg cmd_vel_topic)"/> 
            <param name="ackermann_cmd_topic" value="$(arg ackermann_cmd_topic)"/> <!-- topic name -->
    //imu和雷达硬件参数（发送话题）改false
            <param name="pub_imu" value="$(arg pub_imu)"/> <!-- pub imu topic or not -->
            <param name="pub_sonar" value="$(arg pub_sonar)"/> <!-- pub sonar topic or not -->
    //不订阅ackermann话题       
            <param name="sub_ackermann" value="$(arg sub_ackermann)"/> <!-- sub ackermann topic or not -->
        </node>
    </group>-->
    //如果给该launch文件robot_name参数赋值不为空则执行下列
    <group unless="$(eval robot_name == '')">
    //命名空间后会在话题名前加上robot_name这个变量所对应的参数值。以odom话题为例如果传入的robot_name是robot1，查看话题列表显示的就会是/robot1/odom
        <group ns="$(arg robot_name)">
    //启动base_control功能包下的base_control节点来控制机器人底盘
            <node name="base_control"   pkg="base_control"  type="base_control.py" output="screen">
    //给base_control节点传入参数
    //波特率，端口--通信用的 
                <param name="baudrate"   value="115200"/>  
                <param name="port"  value='/dev/move_base'/>
    //底盘、odom、imu这些硬件命名为机器人名字/某某坐标系--引用了上面的robot_name/base_footprint，robot_name/odom，robot_name/imu
                <param name="base_id"    value="$(arg robot_name)/$(arg base_frame)"/> <!-- base_link name -->
                <param name="odom_id"    value="$(arg robot_name)/$(arg odom_frame)"/>      <!-- odom link name -->
                <param name="imu_id"    value="$(arg robot_name)/$(arg imu_frame)"/>      <!-- imu link name -->
    //odom、imu、电池这些硬件发布的话题命名为--引用了上面的robot_name/odom，robot_name/imu,robot_name/battery
                <param name="odom_topic" value="$(arg odom_topic)"/> <!-- topic name -->
                <param name="imu_topic" value="$(arg imu_topic)"/> <!-- topic name -->
                <param name="battery_topic" value="$(arg battery_topic)"/> <!-- topic name -->
    //终端发布的控制速度话题命名为--引用了上面的robot_name/cmd_vel,robot_name/ackermann_cmd
                <param name="cmd_vel_topic" value="$(arg cmd_vel_topic)"/> 
                <param name="ackermann_cmd_topic" value="$(arg robot_name)$(arg ackermann_cmd_topic)"/> <!-- topic name -->
    //imu和雷达硬件参数（发送话题）改false
                <param name="pub_imu" value="$(arg pub_imu)"/> <!-- pub imu topic or not -->
                <param name="pub_sonar" value="$(arg pub_sonar)"/> <!-- pub sonar topic or not -->
    //不订阅ackermann话题  
                <param name="sub_ackermann" value="$(arg sub_ackermann)"/> <!-- sub ackermann topic or not -->
            </node>
        </group>
    </group>

</launch>

base_control.py文件

指明脚本解释器，编码模式，开源证书，载入依赖包

#!/usr/bin/python
# coding=gbk

# Copyright 2019 Wechange Tech.
# Developer: FuZhi, Liu (liu.fuzhi@wechangetech.com)
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

import os
import rospy
import tf
import time
import sys
import math
import serial
import string
from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry
from sensor_msgs.msg import BatteryState
from sensor_msgs.msg import Imu
from sensor_msgs.msg import Range

import ctypes

读取环境变量BASE_TYPE，SONAR_NUM的值。根据车型和超声波数量的不同执行不同指令

base_type = os.getenv('BASE_TYPE')
if os.getenv('SONAR_NUM') is None:
    sonar_num = 0
else:
    sonar_num = int(os.getenv('SONAR_NUM'))

设计环形队列缓存串口数据防止数据丢失

#class queue is design for uart receive data cache
class queue:
    def __init__(self, capacity = 1024*4):
        self.capacity = capacity
        self.size = 0
        self.front = 0
        self.rear = 0
        self.array = [0]*capacity
 
    def is_empty(self):
        return 0 == self.size
 
    def is_full(self):
        return self.size == self.capacity
 
    def enqueue(self, element):
        if self.is_full():
            raise Exception('queue is full')
        self.array[self.rear] = element
        self.size += 1
        self.rear = (self.rear + 1) % self.capacity
 
    def dequeue(self):
        if self.is_empty():
            raise Exception('queue is empty')
        self.size -= 1
        self.front = (self.front + 1) % self.capacity
 
    def get_front(self):
        return self.array[self.front]
    
    def get_front_second(self):
        return self.array[((self.front + 1) % self.capacity)]

    def get_queue_length(self):
        return (self.rear - self.front + self.capacity) % self.capacity

    def show_queue(self):
        for i in range(self.capacity):
            print self.array[i],
        print(' ')

设计BaseControl类实现底盘硬件的基本控制

1.获取基本参数值(baseId,odomId,device_port等都是launch定义好的，没有的赋值最后面的默认值)

#class BaseControl is design for hardware base relative control
class BaseControl:
    def __init__(self):
        #Get params
        self.baseId = rospy.get_param('~base_id','base_footprint')
        self.odomId = rospy.get_param('~odom_id','odom')
        self.device_port = rospy.get_param('~port','/dev/ttyUSB0')
        self.baudrate = int(rospy.get_param('~baudrate','115200'))
        self.odom_freq = int(rospy.get_param('~odom_freq','50'))
        self.odom_topic = rospy.get_param('~odom_topic','/odom')
        self.battery_topic = rospy.get_param('~battery_topic','battery')
        self.battery_freq = float(rospy.get_param('~battery_freq','1'))
        self.cmd_vel_topic= rospy.get_param('~cmd_vel_topic','/cmd_vel')
        self.ackermann_cmd_topic = rospy.get_param('~ackermann_cmd_topic','/ackermann_cmd_topic')
            self.pub_imu = bool(rospy.get_param('~pub_imu',False))
        if(self.pub_imu == True):
            self.imuId = rospy.get_param('~imu_id','imu')
            self.imu_topic = rospy.get_param('~imu_topic','imu')
            self.imu_freq = float(rospy.get_param('~imu_freq','50'))
            if self.imu_freq > 100:
                self.imu_freq = 100

        self.pub_sonar = bool(rospy.get_param('~pub_sonar',False))
        self.sub_ackermann = bool(rospy.get_param('~sub_ackermann',False))

2.定义一些接下来用的变量

#define param
self.current_time = rospy.Time.now()//当前时间
self.previous_time = self.current_time//之前时间
self.pose_x = 0.0 //odom数据通过计算显示的x坐标
self.pose_y = 0.0 //odom数据通过计算显示的y坐标
self.pose_yaw = 0.0 //odom数据通过计算显示的绕z轴转的角度
self.serialIDLE_flag = 0
self.trans_x = 0.0 //复制vel_cmd里存的信息-x方向的线速度
self.trans_y = 0.0 //复制vel_cmd里存的信息-y方向的线速度
self.rotat_z = 0.0 //复制vel_cmd里存的信息-z方向的角速度
self.speed = 0.0  // 复制ackermann_cmd里存的信息-转向速度
self.steering_angle = 0.0//复制ackermann_cmd里存的信息-转向角度
self.sendcounter = 0
self.ImuErrFlag = False
self.EncoderFlag = False
self.BatteryFlag = False
self.OdomTimeCounter = 0 //odom定时器计数器
self.BatteryTimeCounter = 0 //电池定时器计时器
self.Circleloop = queue(capacity = 1024*4) //环形队列
self.Vx = 0//x轴方向速度
self.Vy = 0//y轴方向速度
self.Vyaw = 0//沿y轴转动的速度
self.Yawz = 0//imu姿态-沿z轴转动的角度
self.Vvoltage = 0//电池的电压
self.Icurrent = 0//电池的电流
self.Gyro = [0,0,0]//陀螺仪-姿态
self.Accel = [0,0,0]//xyz方向线加速度
self.Quat = [0,0,0,0]//四元数-姿态
self.Sonar = [0,0,0,0]//超声波测距信息
self.movebase_firmware_version = [0,0,0]//底盘软件版本号
self.movebase_hardware_version = [0,0,0]//底盘硬件版本号
//底盘类型
self.movebase_type = ["NanoCar","NanoRobot","4WD_OMNI","4WD","RC_ACKERMAN"]
//电机型号
self.motor_type = ["25GA370","37GB520","TT48","RS365","RS540"]
self.last_cmd_vel_time = rospy.Time.now()
self.last_ackermann_cmd_time = rospy.Time.now()

3.打开串口

# Serial Communication
try:
    self.serial = serial.Serial(self.device_port,self.baudrate,timeout=10)
    rospy.loginfo("Opening Serial")
    try:
        if self.serial.in_waiting:
            self.serial.readall()
    except:
        rospy.loginfo("Opening Serial Try Faild")
        pass
except:
    rospy.logerr("Can not open Serial"+self.device_port)
    self.serial.close
    sys.exit(0)
rospy.loginfo("Serial Open Succeed")

4.判断小车类型如果是不能原地打转的，订阅电机转向移动话题ackermann topic否则订阅cmd_vel topic
终端控制转向移动的话题信息有两种：ackermann_cmd和cmd_vel
当终端的键盘控制节点启动就会自动发布cmd_vel，键盘控制时随着键盘变化cmd_vel里的速度信息也变化(比如直行 linear.x=10,其余为0 )
也可以通过rostopic pub主动发布话题cmd_vel
ackermann_cmd_topic就是ackermann_cmd(launch里定义的)
订阅终端控制话题,当里面有东西时执行回调函数cmdCB

#if move base type is ackermann car like robot and use ackermann msg ,sud ackermann topic,else sub cmd_vel topic
if(('NanoCar' in base_type) & (self.sub_ackermann == True)):
    from ackermann_msgs.msg import AckermannDriveStamped
    self.sub = rospy.Subscriber(self.ackermann_cmd_topic,AckermannDriveStamped,self.ackermannCmdCB,queue_size=20)
else:
    self.sub = rospy.Subscriber(self.cmd_vel_topic,Twist,self.cmdCB,queue_size=20)

5.定义发布器(发布odom,电池，超声波话题)没发布呢

self.pub = rospy.Publisher(self.odom_topic,Odometry,queue_size=10)
self.battery_pub = rospy.Publisher(self.battery_topic,BatteryState,queue_size=3)
if self.pub_sonar:
    if sonar_num > 0:
        self.range_pub1 = rospy.Publisher('sonar_1',Range,queue_size=3)
    if sonar_num > 1:    
        self.range_pub2 = rospy.Publisher('sonar_2',Range,queue_size=3)
    if sonar_num > 2:
        self.range_pub3 = rospy.Publisher('sonar_3',Range,queue_size=3)
    if sonar_num > 3:
        self.range_pub4 = rospy.Publisher('sonar_4',Range,queue_size=3)
    if sonar_num > 0:
        self.timer_sonar = rospy.Timer(rospy.Duration(100.0/1000),self.timerSonarCB)

6.注册tf坐标发布器，定义定时器控制odom,电池发布周期以及串口通信多久一次
定时器到了分别执行回调函数timerOdomCB，timerBatteryCB，timerCommunicationCB

self.tf_broadcaster = tf.TransformBroadcaster()
self.timer_odom = rospy.Timer(rospy.Duration(1.0/self.odom_freq),self.timerOdomCB)
self.timer_battery = rospy.Timer(rospy.Duration(1.0/self.battery_freq),self.timerBatteryCB)  
self.timer_communication = rospy.Timer(rospy.Duration(1.0/500),self.timerCommunicationCB)

7.定义发布器(发布imu)没发布呢,定义定时器控制imu发布周期,定时器时间到了执行回调函数timerIMUCB

#inorder to compatibility old version firmware,imu topic is NOT pud in default
if(self.pub_imu):
    self.imu_pub = rospy.Publisher(self.imu_topic,Imu,queue_size=10)
    self.timer_imu = rospy.Timer(rospy.Duration(1.0/self.imu_freq),self.timerIMUCB)

8.获取底盘版本（自写）,发了出去但没有收到地盘回复

self.getVersion()
#move base imu initialization need about 2s,during initialization,move base system is blocked
#so need this gap
while self.movebase_hardware_version[0] == 0:
    pass
# if self.movebase_hardware_version[0] < 2:
#     print self.movebase_hardware_version
#     time.sleep(2.0)

9.获取底盘SN（自写）,发了出去但没有收到地盘回复
1
2
3
self.getSN()
time.sleep(0.01)
self.getInfo()

10.CRC校验

#CRC-8 Calculate
    def crc_1byte(self,data):
        crc_1byte = 0
        for i in range(0,8):
            if((crc_1byte^data)&0x01):
                crc_1byte^=0x18
                crc_1byte>>=1
                crc_1byte|=0x80
            else:
                crc_1byte>>=1
            data>>=1
        return crc_1byte
    def crc_byte(self,data,length):
        ret = 0
        for i in range(length):
            ret = self.crc_1byte(ret^data[i])
        return ret

11.定义收到vel_cmd话题后的回调函数并记录回调时的时间,复制vel_cmd里存的信息，根据串口通讯进行转换再通过串口写入底盘

#Subscribe vel_cmd call this to send vel cmd to move base
    def cmdCB(self,data):
        self.trans_x = data.linear.x
        self.trans_y = data.linear.y
        self.rotat_z = data.angular.z
        self.last_cmd_vel_time = rospy.Time.now()
        output = chr(0x5a) + chr(12) + chr(0x01) + chr(0x01) + \
            chr((int(self.trans_x*1000.0)>>8)&0xff) + chr(int(self.trans_x*1000.0)&0xff) + \
            chr((int(self.trans_y*1000.0)>>8)&0xff) + chr(int(self.trans_y*1000.0)&0xff) + \
            chr((int(self.rotat_z*1000.0)>>8)&0xff) + chr(int(self.rotat_z*1000.0)&0xff) + \
            chr(0x00)
        outputdata = [0x5a,0x0c,0x01,0x01,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00]
        outputdata[4] = (int(self.trans_x*1000.0)>>8)&0xff
        outputdata[5] = int(self.trans_x*1000.0)&0xff
        outputdata[6] = (int(self.trans_y*1000.0)>>8)&0xff
        outputdata[7] = int(self.trans_y*1000.0)&0xff
        outputdata[8] = (int(self.rotat_z*1000.0)>>8)&0xff
        outputdata[9] = int(self.rotat_z*1000.0)&0xff
        crc_8 = self.crc_byte(outputdata,len(outputdata)-1)
        output += chr(crc_8)
        while self.serialIDLE_flag:
            time.sleep(0.01)
        self.serialIDLE_flag = 4
        try:
            while self.serial.out_waiting:
                pass
            self.serial.write(output)
        except:
            rospy.logerr("Vel Command Send Faild")
        self.serialIDLE_flag = 0

12.定义收到ackermann_cmd话题后的回调函数并记录回调时的时间,复制ackermann_cmd里存的信息，根据串口通讯进行转换再通过串口写入底盘

#Subscribe ackermann Cmd call this to send vel cmd to move base
def ackermannCmdCB(self,data):
    self.speed = data.drive.speed
    self.steering_angle = data.drive.steering_angle
    self.last_ackermann_cmd_time = rospy.Time.now()
    output = chr(0x5a) + chr(12) + chr(0x01) + chr(0x15) + \
        chr((int(self.speed*1000.0)>>8)&0xff) + chr(int(self.speed*1000.0)&0xff) + \
        chr(0x00) + chr(0x00) + \
        chr((int(self.steering_angle*1000.0)>>8)&0xff) + chr(int(self.steering_angle*1000.0)&0xff) + \
        chr(0x00)
    outputdata = [0x5a,0x0c,0x01,0x15,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00]
    outputdata[4] = (int(self.speed*1000.0)>>8)&0xff
    outputdata[5] = int(self.speed*1000.0)&0xff
    outputdata[8] = (int(self.steering_angle*1000.0)>>8)&0xff
    outputdata[9] = int(self.steering_angle*1000.0)&0xff
    crc_8 = self.crc_byte(outputdata,len(outputdata)-1)
    output += chr(crc_8)
    while self.serialIDLE_flag:
        time.sleep(0.01)
    self.serialIDLE_flag = 4
    try:
        while self.serial.out_waiting:
            pass
        self.serial.write(output)
    except:
        rospy.logerr("Vel Command Send Faild")
    self.serialIDLE_flag = 0

13.编写定时器回调函数timerCommunicationCB-检查串口中有无缓存数据,有的话存放到编写好的消息环形队列,如果消息环形队列有数据把数据取出来放到我们定义的变量




#Communication Timer callback to handle receive data
#depend on communication protocol
def timerCommunicationCB(self,event):
    length = self.serial.in_waiting
    if length:
        reading = self.serial.read_all()
        if len(reading)!=0:
            for i in range(0,len(reading)):
                data = (int(reading[i].encode('hex'),16)) 
                try:
                    self.Circleloop.enqueue(data)
                except:
                    pass
    else:
        pass
    if self.Circleloop.is_empty()==False:
        data = self.Circleloop.get_front()
        if data == 0x5a:
            length = self.Circleloop.get_front_second()
            if length > 1 :
                if self.Circleloop.get_front_second() <= self.Circleloop.get_queue_length():
                    databuf = []
                    for i in range(length):
                        databuf.append(self.Circleloop.get_front())
                        self.Circleloop.dequeue()
                    
                    if (databuf[length-1]) == self.crc_byte(databuf,length-1):
                        pass
                    else:
                        pass
                    #parse receive data
                    if(databuf[3] == 0x04):
                        self.Vx =    databuf[4]*256
                        self.Vx +=   databuf[5]
                        self.Vy =    databuf[6]*256
                        self.Vy +=   databuf[7]
                        self.Vyaw =  databuf[8]*256
                        self.Vyaw += databuf[9]
                    elif(databuf[3] == 0x06):
                        self.Yawz =  databuf[8]*256
                        self.Yawz += databuf[9]
                    elif (databuf[3] == 0x08):
                        self.Vvoltage = databuf[4]*256
                        self.Vvoltage += databuf[5]
                        self.Icurrent = databuf[6]*256
                        self.Icurrent += databuf[7]
                    elif (databuf[3] == 0x0a):
                        self.Vx =    databuf[4]*256
                        self.Vx +=   databuf[5]
                        self.Yawz =  databuf[6]*256
                        self.Yawz += databuf[7]
                        self.Vyaw =  databuf[8]*256
                        self.Vyaw += databuf[9]
                    elif (databuf[3] == 0x12):
                        self.Vx =    databuf[4]*256
                        self.Vx +=   databuf[5]
                        self.Vy =  databuf[6]*256
                        self.Vy += databuf[7]
                        self.Yawz =  databuf[8]*256
                        self.Yawz += databuf[9]    
                        self.Vyaw =  databuf[10]*256
                        self.Vyaw += databuf[11]                          
                    elif (databuf[3] == 0x14):
                        self.Gyro[0] = int(((databuf[4]&0xff)<<24)|((databuf[5]&0xff)<<16)|((databuf[6]&0xff)<<8)|(databuf[7]&0xff))
                        self.Gyro[1] = int(((databuf[8]&0xff)<<24)|((databuf[9]&0xff)<<16)|((databuf[10]&0xff)<<8)|(databuf[11]&0xff))
                        self.Gyro[2] = int(((databuf[12]&0xff)<<24)|((databuf[13]&0xff)<<16)|((databuf[14]&0xff)<<8)|(databuf[15]&0xff))

                        self.Accel[0] = int(((databuf[16]&0xff)<<24)|((databuf[17]&0xff)<<16)|((databuf[18]&0xff)<<8)|(databuf[19]&0xff))
                        self.Accel[1] = int(((databuf[20]&0xff)<<24)|((databuf[21]&0xff)<<16)|((databuf[22]&0xff)<<8)|(databuf[23]&0xff))
                        self.Accel[2] = int(((databuf[24]&0xff)<<24)|((databuf[25]&0xff)<<16)|((databuf[26]&0xff)<<8)|(databuf[27]&0xff))

                        self.Quat[0] = int((databuf[28]&0xff)<<8|databuf[29])
                        self.Quat[1] = int((databuf[30]&0xff)<<8|databuf[31])
                        self.Quat[2] = int((databuf[32]&0xff)<<8|databuf[33])
                        self.Quat[3] = int((databuf[34]&0xff)<<8|databuf[35])
                    elif (databuf[3] == 0x1a):
                        self.Sonar[0] = databuf[4]
                        self.Sonar[1] = databuf[5]
                        self.Sonar[2] = databuf[6]
                        self.Sonar[3] = databuf[7]    
                    elif(databuf[3] == 0xf2):
                        self.movebase_hardware_version[0] = databuf[4]
                        self.movebase_hardware_version[1] = databuf[5]
                        self.movebase_hardware_version[2] = databuf[6]
                        self.movebase_firmware_version[0] = databuf[7]
                        self.movebase_firmware_version[1] = databuf[8]
                        self.movebase_firmware_version[2] = databuf[9]
                        version_string = "Move Base Hardware Ver %d.%d.%d,Firmware Ver %d.%d.%d"\
                            %(self.movebase_hardware_version[0],self.movebase_hardware_version[1],self.movebase_hardware_version[2],\
                            self.movebase_firmware_version[0],self.movebase_firmware_version[1],self.movebase_firmware_version[2])
                        rospy.loginfo(version_string)
                    elif(databuf[3] == 0xf4):
                        sn_string = "SN:"
                        for i in range(4,16):
                            sn_string = "%s%02x"%(sn_string,databuf[i])
                        rospy.loginfo(sn_string)                            

                    elif(databuf[3] == 0x22):
                        fRatio = float(databuf[6]<<8|databuf[7])/10
                        fDiameter = float(databuf[8]<<8|databuf[9])/10
                        info_string = "Type:%s Motor:%s Ratio:%.01f WheelDiameter:%.01f"\
                            %(self.movebase_type[databuf[4]-1],self.motor_type[databuf[5]-1],fRatio,fDiameter)
                        rospy.loginfo(info_string)
                    else:
                        pass
            else:
                pass
        else:
            self.Circleloop.dequeue()
    else:
        # rospy.loginfo("Circle is Empty")
        pass

14.编写获取底盘版本函数,发了出去但没有收到底盘回复

#get move base hardware & firmware version    
def getVersion(self):
    #Get version info
    output = chr(0x5a) + chr(0x06) + chr(0x01) + chr(0xf1) + chr(0x00) + chr(0xd7) #0xd7 is CRC-8 value
    while(self.serialIDLE_flag):
        time.sleep(0.01)
    self.serialIDLE_flag = 1
    try:
        while self.serial.out_waiting:
            pass
        self.serial.write(output)
    except:
        rospy.logerr("Get Version Command Send Faild")
    self.serialIDLE_flag = 0

15.编写获取底盘SN函数,发了出去但没有收到底盘回复


#get move base SN
def getSN(self):
    #Get version info
    output = chr(0x5a) + chr(0x06) + chr(0x01) + chr(0xf3) + chr(0x00) + chr(0x46) #0x46 is CRC-8 value
    while(self.serialIDLE_flag):
        time.sleep(0.01)
    self.serialIDLE_flag = 1
    try:
        while self.serial.out_waiting:
            pass
        self.serial.write(output)
    except:
        rospy.logerr("Get SN Command Send Faild")
    self.serialIDLE_flag = 0

16.编写获取底盘信息函数(车型，轮距)发了出去但没有收到底盘回复


#get move base info
def getInfo(self):
    #Get version info
    output = chr(0x5a) + chr(0x06) + chr(0x01) + chr(0x21) + chr(0x00) + chr(0x8f) #0x8f is CRC-8 value
    while(self.serialIDLE_flag):
        time.sleep(0.01)
    self.serialIDLE_flag = 1
    try:
        while self.serial.out_waiting:
            pass
        self.serial.write(output)
    except:
        rospy.logerr("Get info Command Send Faild")
    self.serialIDLE_flag = 0

17.编写定时器回调函数timerOdomCB-将串口发送的odom信息经过处理得到位姿信息，填入odom话题并发布,同时发布tf坐标广播(底盘坐标系在odom坐标系中的位姿)

#Odom Timer call this to get velocity and imu info and convert to odom topic
def timerOdomCB(self,event):
    #Get move base velocity data
    if self.movebase_firmware_version[1] == 0: 
        #old version firmware have no version info and not support new command below
        output = chr(0x5a) + chr(0x06) + chr(0x01) + chr(0x09) + chr(0x00) + chr(0x38) #0x38 is CRC-8 value
    else:
        #in firmware version new than v1.1.0,support this command      
        output = chr(0x5a) + chr(0x06) + chr(0x01) + chr(0x11) + chr(0x00) + chr(0xa2) 
    while(self.serialIDLE_flag):
        time.sleep(0.01)
    self.serialIDLE_flag = 1
    try:
        while self.serial.out_waiting:
            pass
        self.serial.write(output)
    except:
        rospy.logerr("Odom Command Send Faild")
    self.serialIDLE_flag = 0   
    #calculate odom data
    Vx = float(ctypes.c_int16(self.Vx).value/1000.0)
    Vy = float(ctypes.c_int16(self.Vy).value/1000.0)
    Vyaw = float(ctypes.c_int16(self.Vyaw).value/1000.0)

    self.pose_yaw = float(ctypes.c_int16(self.Yawz).value/100.0)
    self.pose_yaw = self.pose_yaw*math.pi/180.0
  
    self.current_time = rospy.Time.now()
    dt = (self.current_time - self.previous_time).to_sec()
    self.previous_time = self.current_time
    self.pose_x = self.pose_x + Vx * (math.cos(self.pose_yaw))*dt - Vy * (math.sin(self.pose_yaw))*dt
    self.pose_y = self.pose_y + Vx * (math.sin(self.pose_yaw))*dt + Vy * (math.cos(self.pose_yaw))*dt

    pose_quat = tf.transformations.quaternion_from_euler(0,0,self.pose_yaw)        
    msg = Odometry()
    msg.header.stamp = self.current_time
    msg.header.frame_id = self.odomId
    msg.child_frame_id =self.baseId
    msg.pose.pose.position.x = self.pose_x
    msg.pose.pose.position.y = self.pose_y
    msg.pose.pose.position.z = 0
    msg.pose.pose.orientation.x = pose_quat[0]
    msg.pose.pose.orientation.y = pose_quat[1]
    msg.pose.pose.orientation.z = pose_quat[2]
    msg.pose.pose.orientation.w = pose_quat[3]
    msg.twist.twist.linear.x = Vx
    msg.twist.twist.linear.y = Vy
    msg.twist.twist.angular.z = Vyaw
    self.pub.publish(msg)
    self.tf_broadcaster.sendTransform((self.pose_x,self.pose_y,0.0),pose_quat,self.current_time,self.baseId,self.odomId)

18.编写定时器回调函数timerBatteryCB-将串口发送的电池信息经过处理填入电池话题并发布

#Battery Timer callback function to get battery info
    def timerBatteryCB(self,event):
        output = chr(0x5a) + chr(0x06) + chr(0x01) + chr(0x07) + chr(0x00) + chr(0xe4) #0xe4 is CRC-8 value
        while(self.serialIDLE_flag):
            time.sleep(0.01)
        self.serialIDLE_flag = 3
        try:
            while self.serial.out_waiting:
                pass
            self.serial.write(output)
        except:
            rospy.logerr("Battery Command Send Faild")
        self.serialIDLE_flag = 0
        msg = BatteryState()
        msg.header.stamp = self.current_time
        msg.header.frame_id = self.baseId
        msg.voltage = float(self.Vvoltage/1000.0)
        msg.current = float(self.Icurrent/1000.0)
        self.battery_pub.publish(msg)

19.编写定时器回调函数timerSonarCB-将串口发送的超声波信息经过处理填入超声波话题并发布,同时发布tf坐标广播(超声波仪器和底盘)

#Sonar Timer callback function to get battery info
    def timerSonarCB(self,event):
        output = chr(0x5a) + chr(0x06) + chr(0x01) + chr(0x19) + chr(0x00) + chr(0xff) #0xff is CRC-8 value
        while(self.serialIDLE_flag):
            time.sleep(0.01)
        self.serialIDLE_flag = 3
        try:
            while self.serial.out_waiting:
                pass
            self.serial.write(output)
        except:
            rospy.logerr("Sonar Command Send Faild")
        self.serialIDLE_flag = 0
        msg = Range()
        msg.header.stamp = self.current_time
        msg.field_of_view = 0.26 #about 15 degree
        msg.max_range = 2.5
        msg.min_range = 0.01
        # Sonar 1
        msg.header.frame_id = 'Sonar_1'
        if self.Sonar[0] == 0xff:
            msg.range = float('inf') 
        else:
            msg.range = self.Sonar[0] / 100.0
        self.range_pub1.publish(msg)
         
        # TF value calculate from mechanical structure
        if('NanoRobot' in base_type ):
            self.tf_broadcaster.sendTransform((0.0, 0.0, 0.0 ),(0.0, 0.0, 0.0, 1.0),self.current_time,'Sonar_1',self.baseId)
        elif('NanoCar' in base_type):
            self.tf_broadcaster.sendTransform((0.18, 0.0, 0.0 ),(0.0, 0.0, 0.0, 1.0),self.current_time,'Sonar_1',self.baseId)
        elif('4WD' in base_type):
            self.tf_broadcaster.sendTransform((0.0, 0.0, 0.0 ),(0.0, 0.0, 0.0, 1.0),self.current_time,'Sonar_1',self.baseId)
        elif('Race182' in base_type):
            self.tf_broadcaster.sendTransform((0.18, 0.0, 0.0 ),(0.0, 0.0, 0.0, 1.0),self.current_time,'Sonar_1',self.baseId)   
        elif('NanoOmni' in base_type):
            self.tf_broadcaster.sendTransform((0.11, 0.0, 0.0 ),(0.0, 0.0, 0.0, 1.0),self.current_time,'Sonar_1',self.baseId)              
        else:
            pass

        # Sonar 2
        if sonar_num > 1:   
            if self.Sonar[1] == 0xff:
                msg.range = float('inf') 
            else:
                msg.range = self.Sonar[1] / 100.0
            msg.header.frame_id = 'Sonar_2'
            self.range_pub2.publish(msg)
            
            if('NanoRobot' in base_type):
                self.tf_broadcaster.sendTransform((0.0, 0.0 ,0.0 ),(0.0,0.0,-1.0,0),self.current_time,'Sonar_2',self.baseId) 
            elif('NanoCar' in base_type):
                self.tf_broadcaster.sendTransform((-0.035, 0.0 ,0.0 ),(0.0,0.0,-1.0,0),self.current_time,'Sonar_2',self.baseId) 
            elif('4WD' in base_type):
                self.tf_broadcaster.sendTransform((0.0, 0.0 ,0.0 ),(0.0,0.0,-1.0,0),self.current_time,'Sonar_2',self.baseId) 
            elif('Race182' in base_type):
                self.tf_broadcaster.sendTransform((-0.08, 0.0 ,0.0 ),(0.0,0.0,-1.0,0),self.current_time,'Sonar_2',self.baseId)     
            elif('NanoOmni' in base_type):
                self.tf_broadcaster.sendTransform((-0.11, 0.0, 0.0 ),(0.0, 0.0, -1.0, 0.0),self.current_time,'Sonar_2',self.baseId)  
            else:
                pass
        if sonar_num > 2:   
        # Sonar 3
            msg.header.frame_id = 'Sonar_3'
            if self.Sonar[2] == 0xff:
                msg.range = float('inf') 
            else:
                msg.range = self.Sonar[2] / 100.0
            self.range_pub3.publish(msg)
            if('Race182' in base_type):
                self.tf_broadcaster.sendTransform((0.0, 0.06 ,0.0 ),(0.0,0.0,0.707,0.707),self.current_time,'Sonar_3',self.baseId) 
            elif('NanoOmni' in base_type):   
                self.tf_broadcaster.sendTransform((0.0, 0.07 ,0.0 ),(0.0,0.0,0.707,0.707),self.current_time,'Sonar_3',self.baseId)          
        if sonar_num > 3:   
        # Sonar 4
            if self.Sonar[3] == 0xff:
                msg.range = float('inf') 
            else:
                msg.range = self.Sonar[3] / 100.0
            msg.header.frame_id = 'Sonar_4'
            self.range_pub4.publish(msg)
            if('Race182' in base_type):
                self.tf_broadcaster.sendTransform((0.0, -0.06 ,0.0 ),(0.0,0.0,-0.707,0.707),self.current_time,'Sonar_4',self.baseId) 
            elif('NanoOmni' in base_type):
                self.tf_broadcaster.sendTransform((0.0, -0.07 ,0.0 ),(0.0,0.0,-0.707,0.707),self.current_time,'Sonar_4',self.baseId)

20.编写定时器回调函数timerIMUCB-将串口发送的imu信息经过处理填入imu话题并发布,同时发布tf坐标广播(imu和底盘)

#IMU Timer callback function to get raw imu info
def timerIMUCB(self,event):
    output = chr(0x5a) + chr(0x06) + chr(0x01) + chr(0x13) + chr(0x00) + chr(0x33) #0x33 is CRC-8 value
    while(self.serialIDLE_flag):
        time.sleep(0.01)
    self.serialIDLE_flag = 3
    try:
        while self.serial.out_waiting:
            pass
        self.serial.write(output)
    except:
        rospy.logerr("Imu Command Send Faild")

    self.serialIDLE_flag = 0
    msg = Imu()
    msg.header.stamp = self.current_time
    msg.header.frame_id = self.imuId

    msg.angular_velocity.x = float(ctypes.c_int32(self.Gyro[0]).value/100000.0)
    msg.angular_velocity.y = float(ctypes.c_int32(self.Gyro[1]).value/100000.0)
    msg.angular_velocity.z = float(ctypes.c_int32(self.Gyro[2]).value/100000.0)

    msg.linear_acceleration.x = float(ctypes.c_int32(self.Accel[0]).value/100000.0)
    msg.linear_acceleration.y = float(ctypes.c_int32(self.Accel[1]).value/100000.0)
    msg.linear_acceleration.z = float(ctypes.c_int32(self.Accel[2]).value/100000.0)

    msg.orientation.w = float(ctypes.c_int16(self.Quat[0]).value/10000.0)
    msg.orientation.x = float(ctypes.c_int16(self.Quat[1]).value/10000.0)
    msg.orientation.y = float(ctypes.c_int16(self.Quat[2]).value/10000.0)
    msg.orientation.z = float(ctypes.c_int16(self.Quat[3]).value/10000.0)

    self.imu_pub.publish(msg)  
    # TF value calculate from mechanical structure
    if('NanoRobot' in base_type):
        self.tf_broadcaster.sendTransform((-0.062,-0.007,0.08),(0.0,0.0,0.0,1.0),self.current_time,self.imuId,self.baseId)   
    elif('NanoCar' in base_type):
        self.tf_broadcaster.sendTransform((0.0,0.0,0.09),(0.0,0.0,0.0,1.0),self.current_time,self.imuId,self.baseId) 
    elif('4WD' in base_type):
        self.tf_broadcaster.sendTransform((-0.065,0.0167,0.02),(0.0,0.0,0.0,1.0),self.current_time,self.imuId,self.baseId)     
    else:
        self.tf_broadcaster.sendTransform((0.0,0.0,0.0),(0.0,0.0,0.0,1.0),self.current_time,self.imuId,self.baseId)

21.main循环执行BaseControl()

#main function
if __name__=="__main__":
    try:
        rospy.init_node('base_control',anonymous=True)
        if base_type != None:
            rospy.loginfo('%s base control ...'%base_type)
        else:
            rospy.loginfo('base control ...')
            rospy.logerr('PLEASE SET BASE_TYPE ENV FIRST')

        bc = BaseControl()
        rospy.spin()
    except KeyboardInterrupt:
        bc.serial.close
        print("Shutting down")

sh文件

setbase.sh

定义底盘的类型作为环境变量并写入配置文件

rpi4initsetup.sh

将move_base创建符号链接到端口并将端口权限更改为所有用户可读写(方便移植)

这里的文件夹厂家自带，不需要自己编写。

lidar是多个常用的雷达功能包的合集。
其下包含hls_lfcd_lds_driver，iiiroboticslidar2_ros，rplidar_ros，sc_mini，ydlidar五款雷达的功能包。（里面有发布节点和客户端节点）
此外lidar文件夹下还包含一个用于udev规则的生成的脚本initenv.sh它的作用是将设备进行udev规则转换让所有设备拥有统一的名称。

launch文件夹

includes文件夹

存放的是ros自带的建图算法的launch文件

gmapping.launch、hector.launch、 karto.launch、 cartographer.launch

lidar文件夹

lidar文件夹存放的是雷达的launch文件是重新编写的不是自带的

启动雷达硬件
雷达的基坐标系名字使用lidar_frame参数（lidar）

雷达的端口改为/dev/rplidar

<launch>
    <arg name="lidar_frame" default="lidar"/>   

    <node name="rplidarNode"          pkg="rplidar_ros"  type="rplidarNode" output="screen">
        <param name="serial_port"         type="string" value="/dev/rplidar"/>
        <param name="serial_baudrate"     type="int"    value="115200"/><!--A1/A2 -->
        <!--param name="serial_baudrate"     type="int"    value="256000"--><!--A3 -->
        <param name="frame_id"            type="string" value="$(arg lidar_frame)"/>
        <param name="inverted"            type="bool"   value="false"/>
        <param name="angle_compensate"    type="bool"   value="true"/>
    </node>
</launch>

rviz文件夹

rviz存放的是rviz相关的launch文件用来打开对应的rviz界面--打开功能包robot_navigation下的rviz文件(可以用rviz界面导出)

lidar_rviz.launch—打开雷达的rviz界面(lidar.rviz)
slam_rviz.launch—打开slam建图的rviz界面(slam.rviz)
navigation_rviz.launch—打开单点导航的rviz界面(navigation.rviz)
multi_navigation.launch—打开多点导航的rviz界面(multi_navigation.rviz)

robot_slam_laser.launch文件

激光SLAM建图

简介

slam_methods———gmapping
open_rviz————-false
simulation————false
planner—————-“”
指定导航算法，指定是否打开rviz，指定是否使用simulation(仿真机器人)，指定局部路径规划算法(dwa和teb)-
如果simulation为true就启动机器人模型robot_simulation/launch/simulation_one_robot.launch
如果simulation为false就启动robot_navigation/launch/robot_lidar.launch(雷达和底盘控制)
这个launch文件启动了底盘控制节点base_control/launch/base_control.launch和雷达节点robot_navigation/launch/lidar.launch
雷达节点robot_navigation/launch/lidar.launch功能是根据雷达环境变量的不同启动不同的雷达robot_navigation/launch/lidar/$(arg lidar_type).launch，根据底盘类型的不同发布不同的tf静态坐标-雷达到底盘，名字为base_footprint_to_laser
robot_navigation/launch/lidar/$(arg lidar_type).launch实现了以下功能
1.启动雷达硬件
2.雷达的基坐标系名字使用lidar_frame参数（lidar）
3.雷达的端口改为/dev/rplidar
启动建图算法的launch文件robot_navigation/launch/includes/$(arg slam_methods).launch
启动导航堆栈move_base.launch(ros提供)—-如果启动则可以边导航边建图

代码详解

<launch>
  <!-- Arguments -->
  //指定导航算法
  <arg name="slam_methods" default="gmapping" doc="slam type [gmapping, hector, karto, cartographer]"/>
  //指定是否打开rviz
  <arg name="open_rviz" default="false"/>
  //指定是否使用simulation(仿真机器人)
  <arg name="simulation" default= "false"/> 
  //指定局部路径规划算法(dwa和teb)--teb适合阿克曼的转向，不能原地旋转
  <arg name="planner"  default="" doc="opt: dwa, teb"/> 

  <param name="/use_sim_time" value="$(arg simulation)" />  

  <!-- simulation robot with lidar and map-->
  //如果simulation为true就启动机器人模型robot_simulation/launch/simulation_one_robot.launch
  <group if="$(arg simulation)">
    <include file="$(find robot_simulation)/launch/simulation_one_robot.launch"/>
  </group>
  //如果simulation为false就启动robot_navigation/launch/robot_lidar.launch(雷达和底盘控制)
  <!-- robot with lidar -->
  <group unless="$(arg simulation)">
    <include file="$(find robot_navigation)/launch/robot_lidar.launch"/>
  </group>
  //启动建图算法的launch文件robot_navigation/launch/includes/$(arg slam_methods).launch
  <!-- SLAM: Gmapping, Cartographer, Hector, Karto -->
  <include file="$(find robot_navigation)/launch/includes/$(arg slam_methods).launch">
    <arg name="simulation"            value="$(arg simulation)"/>
  </include>
   //启动导航堆栈move_base.launch(ros提供)
  <!-- move_base -->
  <group unless="$(eval planner == '')">
    <include file="$(find robot_navigation)/launch/move_base.launch" unless="$(eval planner == '')">
        <arg name="planner"            value="$(arg planner)"/>
    </include>
  </group>

  <!-- rviz -->
  //如果open_rviz为true打开设置好的rviz配置robot_navigation/rviz/slam.rviz
  <group if="$(arg open_rviz)"> 
    <node pkg="rviz" type="rviz" name="rviz" required="true"
          args="-d $(find robot_navigation)/rviz/slam.rviz"/>
  </group>
  
</launch>

robot_navigation.launch文件

指定单点导航

简介

指定导航的地图路径，启动仿真器机器人的地图
simulation为false就启动robot_navigation/launch/robot_lidar.launch(雷达和底盘控制)
启动地图服务器并读入地图文件
启动定位节点，根据车型载入不同的定位yaml文件
设置机器人在地图的初始位置和航向角
启动导航堆栈robot_navigation/launch/move_base.launch(核心)
这个launch文件主要功能
1.启动导航堆栈move_base(参数为全局规划器和导航规划器),载入车型的所有配置信息
2.接收控制信息话题和odom话题

代码详解

<launch>
  <!-- Arguments -->
  //指定导航的地图路径
  <arg name="map_file" default="$(find robot_navigation)/maps/map.yaml"/>
  <arg name="simulation" default= "false"/> 
  <arg name="planner"  default="teb" doc="opt: dwa, teb"/> 
  <arg name="open_rviz" default="false"/>
  <arg name="use_dijkstra" default= "true"/> 
  //启动仿真器机器人的地图
  <group if="$(arg simulation)">
    <!-- simulation robot with lidar and map-->
    <include file="$(find robot_simulation)/launch/simulation_one_robot_with_map.launch"/>
  </group>
   //simulation为false就启动robot_navigation/launch/robot_lidar.launch(雷达和底盘控制)
  <group unless="$(arg simulation)">
    <!-- robot with lidar -->
    <include file="$(find robot_navigation)/launch/robot_lidar.launch">
          <!--<arg name="robot_name"            value="$(arg robot_name)"/>-->
    </include>
    //启动地图服务器并读入地图文件 
    <!-- Map server -->
    <node pkg="map_server" name="map_server" type="map_server" args="$(arg map_file)">
      <param name="frame_id" value="map"/>
    </node>
    //启动定位节点，根据车型载入不同的定位yaml文件
    <!-- AMCL -->
    <node pkg="amcl" type="amcl" name="amcl" output="screen">
      <rosparam file="$(find robot_navigation)/param/$(env BASE_TYPE)/amcl_params.yaml" command="load" />
    //机器人在地图的初始位置和航向角  
      <param name="initial_pose_x"            value="0.0"/>
      <param name="initial_pose_y"            value="0.0"/>
      <param name="initial_pose_a"            value="0.0"/>
    </node>
  </group>
    //启动导航堆栈robot_navigation/launch/move_base.launch
  <!-- move_base -->
  <include file="$(find robot_navigation)/launch/move_base.launch" >
    <arg name="planner"            value="$(arg planner)"/>
    <arg name="simulation"            value="$(arg simulation)"/>
    <arg name="use_dijkstra"     value="$(arg use_dijkstra)"/>

  </include>

  <!-- rviz -->
  <group if="$(arg open_rviz)"> 
    <node pkg="rviz" type="rviz" name="rviz" required="true"
          args="-d $(find robot_navigation)/rviz/navigation.rviz"/>
  </group>

</launch>

multi_points_navigation.launch文件

指定多点导航
只是启动了一个python文件multi_goal_point.py

<launch>

    <!--<node name="robot_pose_publisher" pkg="robot_pose_publisher" type="robot_pose_publisher" />-->
    <node name="multi_mark" pkg="robot_navigation" type="multi_goal_point.py" output="screen" />

</launch>

way_point.launch文件

多点全自动巡航
只是启动了一个python文件way_point.py

<launch>
    <!-- For Simulation -->
    <arg name="sim_mode" default="false" />
    <param name="/use_sim_time" value="$(arg sim_mode)"/>
    <arg name="loopTimes"       default="0" />
    <!-- move base -->
    <node pkg="robot_navigation" type="way_point.py" respawn="false" name="way_point" output="screen">
        <!-- params for move_base -->
        //自动巡航的三个点的坐标以及循环次数，0代表不停止
        <param name="goalListX" value="[4.2, 4.6, 4.4]" />
        <param name="goalListY" value="[4.6, 7.4, 9.2]" />
        <param name="goalListZ" value="[0.0, 0.0, 0.0]" />
        <param name="loopTimes" value="$(arg loopTimes)"/>
        <param name="map_frame" value="map" />
    </node>
    

</launch>

navigation_stack.launch文件

robot_lidar.launch文件

启动底盘控制节点

base_control/launch/base_control.launch

启动雷达节点

robot_navigation/launch/lidar.launch

<launch>
    <!-- config param -->
    <arg name="pub_imu"       default="False" />
    <arg name="sub_ackermann"       default="False" />
    <arg name="lidar_frame" default="base_laser_link"/>  

    <include file="$(find base_control)/launch/base_control.launch">
        <arg name="pub_imu"            value="$(arg pub_imu)"/>  
        <arg name="sub_ackermann"            value="$(arg sub_ackermann)"/>  
    </include>

    <include file="$(find robot_navigation)/launch/lidar.launch">
        <arg name="lidar_frame"            value="$(arg lidar_frame)"/>  
    </include>
    
</launch>

lidar.launch文件

根据雷达环境变量的不同启动不同的雷达

robot_navigation/launch/lidar/$(arg lidar_type).launch
根据底盘类型的不同发布不同的tf静态坐标-雷达到底盘，名字为base_footprint_to_laser

用launch发布静态坐标的方法，名字为base_footprint_to_laser

 <group if="$(eval base_type == 'NanoRobot')">
        <node pkg="tf" type="static_transform_publisher" name="base_footprint_to_laser"
            args="-0.01225 0.0 0.18 3.14159265 0.0 0.0 $(arg base_frame) $(arg lidar_frame) 20">
        </node>
```        
```bash
<launch>
    <!--robot bast type use different tf value-->
    <arg name="base_type"       default="$(env BASE_TYPE)" />
    <!-- robot frame -->
    <arg name="base_frame"       default="/base_footprint" />    
    <arg name="lidar_type"       default="$(env LIDAR_TYPE)" />   
    <arg name="lidar_frame" default="base_laser_link"/>  

    <include file="$(find robot_navigation)/launch/lidar/$(arg lidar_type).launch">
        <arg name="lidar_frame"            value="$(arg lidar_frame)"/>
    </include>

    <group if="$(eval base_type == 'NanoRobot')">
        <node pkg="tf" type="static_transform_publisher" name="base_footprint_to_laser"
            args="-0.01225 0.0 0.18 3.14159265 0.0 0.0 $(arg base_frame) $(arg lidar_frame) 20">
        </node>
    </group>
    <group if="$(eval base_type == 'NanoRobot_Pro')">
        <node pkg="tf" type="static_transform_publisher" name="base_footprint_to_laser"
            args="-0.0515 0.0 0.18 -1.5708 0.0 0.0 $(arg base_frame) $(arg lidar_frame) 20">
        </node>
    </group>
    <group if="$(eval base_type == '4WD')">
        <node pkg="tf" type="static_transform_publisher" name="base_footprint_to_laser"
            args="0.01 0.0 0.25 3.14159265 0.0 0.0 $(arg base_frame) $(arg lidar_frame) 20">
        </node>
    </group>
    <group if="$(eval base_type == '4WD_OMNI')">
        <node pkg="tf" type="static_transform_publisher" name="base_footprint_to_laser"
            args="0.01 0.0 0.25 3.14159265 0.0 0.0 $(arg base_frame) $(arg lidar_frame) 20">
        </node>
    </group>
    <group if="$(eval base_type == 'NanoCar')">
        <node pkg="tf" type="static_transform_publisher" name="base_footprint_to_laser"
            args="0.1037 0.0 0.115 3.14159265 0.0 0.0 $(arg base_frame) $(arg lidar_frame) 20">
        </node>
    </group>
    <group if="$(eval base_type == 'NanoCar_Pro')">
        <node pkg="tf" type="static_transform_publisher" name="base_footprint_to_laser"
            args="0.1037 0.0 0.165 3.14159265 0.0 0.0 $(arg base_frame) $(arg lidar_frame) 20">
        </node>
    </group>
    <group if="$(eval base_type == 'NanoCar_SE')">
        <node pkg="tf" type="static_transform_publisher" name="base_footprint_to_laser"
            args="0.0955 0.0 0.115 1.5708 0.0 0.0 $(arg base_frame) $(arg lidar_frame) 20">
        </node>
    </group>
    <group if="$(eval base_type == 'Race182')">
        <node pkg="tf" type="static_transform_publisher" name="base_footprint_to_laser"
            args="0.1 0.0 0.192 3.14159265 0.0 0.0 $(arg base_frame) $(arg lidar_frame) 20">
        </node>
    </group>
    <group if="$(eval base_type == 'NanoOmni')">
        <node pkg="tf" type="static_transform_publisher" name="base_footprint_to_laser"
            args="0.019 0.0 0.192 3.14159265 0.0 1.0 $(arg base_frame) $(arg lidar_frame) 20">
        </node>
    </group>



</launch>

move_base.launch文件

启动导航堆栈move_base.launch(ros提供)

<launch>
  <!-- Arguments -->
  <arg name="cmd_vel_topic" default="cmd_vel" />
  <arg name="odom_topic" default="odom" />
  <arg name="planner"  default="dwa" doc="opt: dwa, teb"/> 
  <arg name="simulation" default= "false"/> 
  <arg name="use_dijkstra" default= "true"/>  
  

  <arg name="base_type" default= "$(env BASE_TYPE)"/> 
  //雷达建图和导航是360度所以可以用false，用相机导航和建图只能看见前面所以用true
  <arg name="move_forward_only" default="false"/>

  <!-- move_base use DWA planner-->
  <group if="$(eval planner == 'dwa')">
  //启动导航堆栈move_base(参数为全局规划器和导航规划器),载入车型的所有配置信息
    <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
      <!-- use global_planner replace default navfn as global planner ,global_planner support A* and dijkstra algorithm-->
      <param name="base_global_planner" value="global_planner/GlobalPlanner"/>
      <param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS" />
      <!-- <rosparam file="$(find robot_navigation)/param/$(env BASE_TYPE)/global_planner_params.yaml" command="load" />       -->
      <rosparam file="$(find robot_navigation)/param/$(env BASE_TYPE)/costmap_common_params.yaml" command="load" ns="global_costmap" />
      <rosparam file="$(find robot_navigation)/param/$(env BASE_TYPE)/costmap_common_params.yaml" command="load" ns="local_costmap" />
      <rosparam file="$(find robot_navigation)/param/$(env BASE_TYPE)/local_costmap_params.yaml" command="load" />
      <rosparam file="$(find robot_navigation)/param/$(env BASE_TYPE)/global_costmap_params.yaml" command="load" />
      <rosparam file="$(find robot_navigation)/param/$(env BASE_TYPE)/move_base_params.yaml" command="load" />
      <rosparam file="$(find robot_navigation)/param/$(env BASE_TYPE)/dwa_local_planner_params.yaml" command="load" />
      //接收控制信息话题和odom话题
      <remap from="cmd_vel" to="$(arg cmd_vel_topic)"/>
      <remap from="odom" to="$(arg odom_topic)"/>
      当只允许前向运动时设置局部规划最小速度为0，不出现负速度(后向)
      <param name="DWAPlannerROS/min_vel_x" value="0.0" if="$(arg move_forward_only)" />
      <!--default is True,use dijkstra algorithm;set to False,usd A* algorithm-->
      <param name="GlobalPlanner/use_dijkstra " value="$(arg use_dijkstra)" />    
    </node>
  </group>
  <!-- move_base use TEB planner-->
  <group if="$(eval planner == 'teb')">
    <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
      <!-- use global_planner replace default navfn as global planner ,global_planner support A* and dijkstra algorithm-->
      <param name="base_global_planner" value="global_planner/GlobalPlanner"/>
      <param name="base_local_planner" value="teb_local_planner/TebLocalPlannerROS" />
      <!-- <rosparam file="$(find robot_navigation)/param/$(env BASE_TYPE)/global_planner_params.yaml" command="load" /> -->
      <rosparam file="$(find robot_navigation)/param/$(env BASE_TYPE)/costmap_common_params.yaml" command="load" ns="global_costmap" />
      <rosparam file="$(find robot_navigation)/param/$(env BASE_TYPE)/costmap_common_params.yaml" command="load" ns="local_costmap" />
      <rosparam file="$(find robot_navigation)/param/$(env BASE_TYPE)/local_costmap_params.yaml" command="load" />
      <rosparam file="$(find robot_navigation)/param/$(env BASE_TYPE)/global_costmap_params.yaml" command="load" />
      <rosparam file="$(find robot_navigation)/param/$(env BASE_TYPE)/move_base_params.yaml" command="load" />
      <rosparam file="$(find robot_navigation)/param/$(env BASE_TYPE)/teb_local_planner_params.yaml" command="load" />
      <remap from="cmd_vel" to="$(arg cmd_vel_topic)"/>
      <remap from="odom" to="$(arg odom_topic)"/>
      <param name="TebLocalPlannerROS/max_vel_x_backwards" value="0.0" if="$(arg move_forward_only)" />
      <!--default is True,use dijkstra algorithm;set to False,usd A* algorithm-->
      <param name="GlobalPlanner/use_dijkstra " value="$(arg use_dijkstra)" />
      <!--stage simulator takes the angle instead of the rotvel as input (twist message)-->

      
    </node>
  </group>


    <!-- move_base -->



</launch>

config文件夹

算法的配置，下载后自动出现

maps文件夹

maps存放的是地图文件和地图配置信息相关的文件

param文件夹

param存放的是yaml文件——一个车型一个文件夹(ros有)
NanoCar：

amcl_params.yaml--设置机器人定位配置

costmap_common_params.yaml--设置机器人的矩形轮廓尺寸

footprint: [ [-0.035,-0.1], [0.18,-0.1], [0.18,0.1], [-0.035,0.1] ]表示以机器人中心为坐标轴原点，这四个坐标点分别对应机器人的四个角。路径规划器获取到机器人轮廓大小从而规划相应的路径

local_costmap_params.yaml--在这个文件中需要指定发布频率和更新频率

为了降低机器人的计算负担设置了一个以机器人所在位置为中心的3*3滑动窗口，这个框设置得越小路径规划效果越差，越大机器人负担的运算就越大。最后设置了一个地图的层，第一个是静态层，也就是建图时的黑色边界以外的灰色可通行区域。第二个是障碍物层，地图中不存在但是实际扫面到的障碍物会被识别在障碍物层。

global_costmap_params.yaml--这个文件也是一样设置一些发布频率等信息

move_base_params.yaml--这个文件也是进行一些频率设置，包括对地盘的控制频率，路径规划频率以及机器人在一个点持续震荡时最大允许震荡时间等等。

最后clearing_rotation_allowed这个参数的作用是，当机器人被困住时会原地360度转一圈重新扫描找到一条出路。这里我们的机器人以NanoCar是为例它不具备原地转向的能力所以需要禁用。

dwa_local_planner_params.yaml--设置机器人导航的最大最小速度，y轴的横向运动速度，转向角度，加速度信息，到达目标所能容忍的误差等等。

当planner参数为teb时，同样启动move_base功能包下的move_base节点加下来载入的参数也基本一致，区别在于最后载入的是teb_local_planner_params.yaml文件

teb_local_planner_params.yaml--和dwa的基本类似，它将参数分开设置成转向速度，x轴速度，x轴后向的速度，y轴速度，y轴后向速度等等。

teb路径规划支持阿克曼结构的机器人，所以需要设置最小转向半径的参数，轮距参数等。在使用仿真器仿真阿克曼车型时需要将cmd_angle_instead_rotvel参数值改为true。这是由于stage仿真器中”Car”车型接受的cmd_vel话题中z轴角速度并非给定机器人角速度，而是指转向结构的转向角度。

rviz文件夹

script文件夹

multi_goal_point.py文件

简介

通过rviz界面的按钮发布/goal话题而不是直接发布/move_base_simple/goal，因为这个话题会直接发布给move_base而直接产生导航目标直接开始了导航。因此通过了markerArray = MarkerArray()把/goal缓存起来作为数组，第一个点通过click_callback回调开始导航然后对每一个导航点的结果触发回调status_callback来启动下一个目标点的导航

#!/usr/bin/python
# coding=gbk

# Copyright 2020 Wechange Tech.
# Developer: FuZhi, Liu (liu.fuzhi@wechangetech.com)
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

from visualization_msgs.msg import Marker
from visualization_msgs.msg import MarkerArray
import rospy
import math
from geometry_msgs.msg import PointStamped,PoseStamped
import actionlib
from move_base_msgs.msg import *
import tf
//如果导航状态为3代表成功到达目标点读取下一个目标点的位置给pose变量
//使用goal_pub发布到/move_base_simple/goal话题(真正触发move_base进行导航的话题)
def status_callback(msg):

    global goal_pub, index,markerArray
    global add_more_point,try_again

    if(msg.status.status == 3):
        try_again = 1
        if add_more_point == 0:
            print 'Goal reached'

        if index < count:

            pose = PoseStamped()
            pose.header.frame_id = "map"
            pose.header.stamp = rospy.Time.now()
            pose.pose.position.x = markerArray.markers[index].pose.position.x
            pose.pose.position.y = markerArray.markers[index].pose.position.y
            pose.pose.orientation.w = 1
            goal_pub.publish(pose)

            index += 1
    //当导航点都执行结束令add_more_point = 1 
    //此时如果设置了新的目标点click_callback会被执行
    //if add_more_point and count > 0得到满足强制发布了move.status.status = 3
    //主函数又发布了/move_base/result话题重新执行status_callback来发布新的目标点     
        elif index == count:
            add_more_point = 1
    else:
    # uint8 PENDING         = 0   # The goal has yet to be processed by the action server
    # uint8 ACTIVE          = 1   # The goal is currently being processed by the action server
    # uint8 PREEMPTED       = 2   # The goal received a cancel request after it started executing
    #                             #   and has since completed its execution (Terminal State)
    # uint8 SUCCEEDED       = 3   # The goal was achieved successfully by the action server (Terminal State)
    # uint8 ABORTED         = 4   # The goal was aborted during execution by the action server due
    #                             #    to some failure (Terminal State)
    # uint8 REJECTED        = 5   # The goal was rejected by the action server without being processed,
    #                             #    because the goal was unattainable or invalid (Terminal State)
    # uint8 PREEMPTING      = 6   # The goal received a cancel request after it started executing
    #                             #    and has not yet completed execution
    # uint8 RECALLING       = 7   # The goal received a cancel request before it started executing,
    #                             #    but the action server has not yet confirmed that the goal is canceled
    # uint8 RECALLED        = 8   # The goal received a cancel request before it started executing
    #                             #    and was successfully cancelled (Terminal State)
    # uint8 LOST            = 9   # An action client can determine that a goal is LOST. This should not be
    #                             #    sent over the wire by an action server
        print 'Goal cannot reached has some error :',msg.status.status," try again!!!!"
        if try_again == 1:
            pose = PoseStamped()
            pose.header.frame_id = "map"
            pose.header.stamp = rospy.Time.now()
            pose.pose.position.x = markerArray.markers[index-1].pose.position.x
            pose.pose.position.y = markerArray.markers[index-1].pose.position.y
            pose.pose.orientation.w = 1
            goal_pub.publish(pose)
            try_again = 0
        else:
            if index < len(markerArray.markers):
                pose = PoseStamped()
                pose.header.frame_id = "map"
                pose.header.stamp = rospy.Time.now()
                pose.pose.position.x = markerArray.markers[index].pose.position.x
                pose.pose.position.y = markerArray.markers[index].pose.position.y
                pose.pose.orientation.w = 1
                goal_pub.publish(pose)
                index += 1

//rviz界面每点击一个多目标点就提取位置信息追加到markerArray数组中并发布出去
def click_callback(msg):
    global markerArray,count
    global goal_pub,index
    global add_more_point

    marker = Marker()
    marker.header.frame_id = "map"
    marker.header.stamp = rospy.Time.now()
    # marker.type = marker.TEXT_VIEW_FACING
    marker.type = marker.CYLINDER
    marker.action = marker.ADD
    marker.scale.x = 0.2
    marker.scale.y = 0.2
    marker.scale.z = 0.5
    marker.color.a = 1.0
    marker.color.r = 0.0
    marker.color.g = 1.0
    marker.color.b = 0.0
    marker.pose.orientation.x = 0.0
    marker.pose.orientation.y = 0.0
    marker.pose.orientation.z = 0.0
    marker.pose.orientation.w = 1.0
    marker.pose.position.x = msg.pose.position.x
    marker.pose.position.y = msg.pose.position.y
    marker.pose.position.z = msg.pose.position.z

    markerArray.markers.append(marker)

    # Renumber the marker IDs
    id = 0
    for m in markerArray.markers:
       m.id = id
       id += 1

    # Publish the MarkerArray
    mark_pub.publish(markerArray)
    //如果rviz只发布了一个目标点就提取位置信息到pose变量
    //使用goal_pub发布到/move_base_simple/goal话题(真正触发move_base进行导航的话题)
    #first goal
    if count==0:
        pose = PoseStamped()
        pose.header.frame_id = "map"
        pose.header.stamp = rospy.Time.now()
        pose.pose.position.x = msg.pose.position.x
        pose.pose.position.y = msg.pose.position.y
        pose.pose.position.z = msg.pose.position.z
        pose.pose.orientation.x = msg.pose.orientation.x
        pose.pose.orientation.y = msg.pose.orientation.y
        pose.pose.orientation.z = msg.pose.orientation.z
        pose.pose.orientation.w = msg.pose.orientation.w
        goal_pub.publish(pose)
        index += 1

    if add_more_point and count > 0:
        add_more_point = 0
        move =MoveBaseActionResult()
        move.status.status = 3
        move.header.stamp = rospy.Time.now()
        goal_status_pub.publish(move)
    quaternion = (msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z, msg.pose.orientation.w)
    theta = tf.transformations.euler_from_quaternion( quaternion)[2]
    count += 1
    # print 'add a path goal point %f %f %f'%(msg.pose.position.x,msg.pose.position.y,theta*180.0/3.14)

//定义一个变量名字为markerArray类型为MarkerArray
//rviz界面每增加一个目标点就会有MarkerArray类型的标记符号
markerArray = MarkerArray()
//发布的目标点数量
count = 0       #total goal num
//统计当前执行到了哪一个目标点
index = 0       #current goal point index
//统计到达所有目标点后有无新增加的目标点
add_more_point = 0 # after all goal arrive, if add some more goal
//没有到达目标点重试一次
try_again = 1  # try the fail goal once again
//创建节点名字为multi_goal_point_demo
rospy.init_node('multi_goal_point_demo')
//创建发布器发布/path_point_array话题发布类型为MarkerArray
mark_pub = rospy.Publisher('/path_point_array', MarkerArray,queue_size=100)
//创建订阅器订阅/goal话题--rviz界面多目标点发布的话题，回调函数click_callback
click_goal_sub = rospy.Subscriber('/goal',PoseStamped,click_callback)
//创建发布器发布/move_base_simple/goal话题(真正触发move_base进行导航的话题)发布类型为PoseStamped
goal_pub = rospy.Publisher('/move_base_simple/goal',PoseStamped,queue_size=1)
//创建订阅器订阅/move_base/result--move_base导航的结果，回调函数status_callback
goal_status_sub = rospy.Subscriber('/move_base/result',MoveBaseActionResult,status_callback)
#after all goal arrive, if add some more goal
#we deleberate pub a topic to trig the goal sent
//所有导航点结束/move_base/result话题不再发布，手动发布一下导航结果话题方便继续发布导航点
goal_status_pub = rospy.Publisher('/move_base/result',MoveBaseActionResult,queue_size=1)
rospy.spin()

way_point.py文件

简介

获取自动巡航的三个点的坐标以及循环次数，0代表不停止(launch文件可传)如果没有值则默认后面的数据
对三个点处理成数组形式
判断三个数组是不是相等
执行MultiGoals函数功能如下:
创建订阅器订阅导航结果move_base/result回调函数为statusCB
创建发布器发布move_base_simple/goal话题直接启动move_base进行导航
读取巡航点的坐标和循环次数，设置巡航结束标志wayPointFinished为False
设置目标点id为0
发布第一个目标点的位置信息到move_base_simple/goal话题
目标点id加1，到达第一个目标点后move_base/result话题会被发布执行回调函数为statusCB
回调函数statusCB功能如下:
如果循环次数达到，令wayPointFinished = True
如果成功到达且巡航结束标志wayPointFinished = false继续发布下一个目标点位置信息
如果没有执行完所有目标点就令目标点id加1，
执行完了所有目标点就令目标点id为0并且循环次数标志加1

#!/usr/bin/python
# coding=gbk

# Copyright 2020 Wechange Tech.
# Developer: FuZhi, Liu (liu.fuzhi@wechangetech.com)
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

import rospy
import string
import math
import time
import sys

from std_msgs.msg import String
from move_base_msgs.msg import MoveBaseActionResult
from actionlib_msgs.msg import GoalStatusArray
from geometry_msgs.msg import PoseStamped
import tf
//创建订阅器订阅导航结果move_base/result回调函数为statusCB
//创建发布器发布move_base_simple/goal话题直接启动move_base进行导航
class MultiGoals:
    def __init__(self, goalListX, goalListY, goalListZ,loopTimes, map_frame):
        self.sub = rospy.Subscriber('move_base/result', MoveBaseActionResult, self.statusCB, queue_size=10)
        self.pub = rospy.Publisher('move_base_simple/goal', PoseStamped, queue_size=10)   
        # params & variables
        //读取巡航点的坐标和循环次数，设置巡航结束标志wayPointFinished为False
        self.goalListX = goalListX
        self.goalListY = goalListY
        self.goalListZ = goalListZ
        self.loopTimes = loopTimes
        self.loop = 1
        self.wayPointFinished = False 
        //设置目标点id为0
        self.goalId = 0
        self.goalMsg = PoseStamped()
        self.goalMsg.header.frame_id = map_frame
        self.goalMsg.pose.orientation.z = 0.0
        self.goalMsg.pose.orientation.w = 1.0
        # Publish the first goal
        //发布第一个目标点的位置信息
        time.sleep(1)
        self.goalMsg.header.stamp = rospy.Time.now()
        self.goalMsg.pose.position.x = self.goalListX[self.goalId]
        self.goalMsg.pose.position.y = self.goalListY[self.goalId]
        self.goalMsg.pose.orientation.x = 0.0
        self.goalMsg.pose.orientation.y = 0.0
        if abs(self.goalListZ[self.goalId]) > 1.0:
            self.goalMsg.pose.orientation.z = 0.0
            self.goalMsg.pose.orientation.w = 1.0
        else:
            w = math.sqrt(1 - (self.goalListZ[self.goalId]) ** 2)
            self.goalMsg.pose.orientation.z = self.goalListZ[self.goalId]
            self.goalMsg.pose.orientation.w = w
        self.pub.publish(self.goalMsg) 
        rospy.loginfo("Current Goal ID is: %d", self.goalId)
        //目标点id加1，到达第一个目标点后move_base/result话题会被发布执行回调函数为statusCB
        self.goalId = self.goalId + 1 

    def statusCB(self, data):
    //如果循环次数达到，令wayPointFinished = True
        if self.loopTimes and (self.loop > self.loopTimes):
            rospy.loginfo("Loop: %d Times Finshed", self.loopTimes)
            self.wayPointFinished = True
     //如果成功到达且巡航结束标志wayPointFinished = false继续发布下一个目标点位置信息       
        if data.status.status == 3 and (not self.wayPointFinished): # reached
            self.goalMsg.header.stamp = rospy.Time.now()                
            self.goalMsg.pose.position.x = self.goalListX[self.goalId]
            self.goalMsg.pose.position.y = self.goalListY[self.goalId]
            if abs(self.goalListZ[self.goalId]) > 1.0:
                self.goalMsg.pose.orientation.z = 0.0
                self.goalMsg.pose.orientation.w = 1.0
            else:
                w = math.sqrt(1 - (self.goalListZ[self.goalId]) ** 2)
                self.goalMsg.pose.orientation.z = self.goalListZ[self.goalId]
                self.goalMsg.pose.orientation.w = w
            self.pub.publish(self.goalMsg)  
            rospy.loginfo("Current Goal ID is: %d", self.goalId)  
            //如果没有执行完所有目标点就令目标点id加1，
            //执行完了所有目标点就令目标点id为0并且循环次数标志加1            
            if self.goalId < (len(self.goalListX)-1):
                self.goalId = self.goalId + 1
            else:
                self.goalId = 0
                self.loop += 1

            
                


if __name__ == "__main__":
    try:    
        # ROS Init    
        rospy.init_node('way_point', anonymous=True)
    //获取自动巡航的三个点的坐标以及循环次数，0代表不停止(launch文件可传)如果没有值则默认后面的数据
        # Get params
        goalListX = rospy.get_param('~goalListX', '[2.0, 2.0]')
        goalListY = rospy.get_param('~goalListY', '[2.0, 4.0]')
        goalListZ = rospy.get_param('~goalListZ', '[0.0, 0.0]')
        map_frame = rospy.get_param('~map_frame', 'map' )
        loopTimes = int(rospy.get_param('~loopTimes', '0')) 
    //对三个点处理成数组形式
        goalListX = goalListX.replace("[","").replace("]","")
        goalListY = goalListY.replace("[","").replace("]","")
        goalListZ = goalListZ.replace("[","").replace("]","")
        goalListX = [float(x) for x in goalListX.split(",")]
        goalListY = [float(y) for y in goalListY.split(",")]
        goalListZ = [float(z) for z in goalListZ.split(",")]
    //判断三个数组是不是相等    
        if len(goalListX) == len(goalListY) == len(goalListZ) & len(goalListY) >=2:          
            # Constract MultiGoals Obj

            rospy.loginfo("Multi Goals Executing...")
            mg = MultiGoals(goalListX, goalListY, goalListZ, loopTimes, map_frame) 
    //执行MultiGoals函数                 
            rospy.spin()
        else:
            rospy.errinfo("Lengths of goal lists are not the same")
    except KeyboardInterrupt:
        print("shutting down")

initenv.sh文件

setlidar.sh文件

package.xml文件

运行依赖，编译依赖

simulation_one_robot.launch：
这个launch文件是启动机器人的仿真时用到的launch，它启动了stage_ros这个节点。这个节点运行了一个仿真器的环境，它需要一个仿真器的配置作为参数传入。我们以NanoCar为例看一下它的配置文件内容。
首先它包含了一个robot.inc文件，这个文件是用于描述我们所要仿真的机器人的信息。它定义了一个雷达传感器包括雷达扫描最远距离，扫描角度，雷达采样点数量以及雷达的尺寸。然后定义了机器人的位置，里程计的误差，机器人的尺寸，机器人的基座标与仿真器仿真出来的机器人立方体模型的偏移，机器人的颜色，机器人的运动学特性，机器人轴距，激光雷达到车身的变换等信息
回到maze.world文件，这里需要设置仿真器启动后窗口的大小中心点等信息。
配置仿真地图，地图存放在功能包的maps文件下。size参数设置地图的实际大小。地图的规格为200200我们设置的实际尺寸是10米10米也就是每个像素点5厘米。然后设置中心为55
最后把机器人加载进地图当中，在地图的11位置放置机器人，机器人名设为robot。这里注意机器人位置如果设为0*0会把机器人放到边界上导致出错
simulation_one_robot_with_map.launch：
这个launch的前半部分和simulation_one_robot.launch是一样的，后半部分它分别启动了一个map_server来载入地图和amcl的节点用于定位机器人。amcl的节点有初始位置作为参数传入节点。
map_server载入的地图文件就是maze.yaml文件，它读取的图为maze.png。设置分辨率为0.05也就是一个像素对应5厘米，这与前面设置的实际地图大小是一致的。