系列文章目录

第一章 ROS空间创建、helloworld的实现、开启多个节点
第二章 话题通信
第三章 服务通信
第四章 参数服务器
第五章 常用指令
第六章 通信机制实操
第七章 ROS通信机制进阶（常用API、Python模块的导入)
第八章 元功能包、节点运行管理launch文件（teleop_twist安装方法）
第九章 重名问题、分布式通信
第十章-第一节 TF坐标变换（内含PyKDL 和PyInit__tf2功能缺失等解决）
第十章-第二节 TF坐标变换实操
第十章-第三节 rosbag、rqt工具箱
第十一章-第一节 机器人系统仿真（URDF相关）
第十一章-第二节 机器人系统仿真（Gazebo相关）
第十二章 机器人导航(仿真)

---

前言

现在大二，之前大一有幸参加了2021的国赛，很壮烈的拿了个江苏赛区的二等奖。但发现无人机这个题，真的是往堆钱上走了。不上ROS不行，现在来记录一下一个纯小白学习ROS的过程和遇到的问题。防止学弟、学妹们再走我走过的弯路。板子用的是学长给的Jetson Nano（4GB），版本是Ubuntu18.04（已配置好基础ROS所需配置）

导航是机器人系统中最重要的模块之一，比如现在较为流行的服务型室内机器人，就是依赖于机器人导航来实现室内自主移动的，本章主要就是介绍仿真环境下的导航实现，主要内容有:
导航相关概念
导航实现:机器人建图(SLAM)、地图服务、定位、路径规划…以可视化操作为主。
导航消息:了解地图、里程计、雷达、摄像头等相关消息格式。

预期达成的学习目标:

• 了解导航模块中的组成部分以及相关概念
• 能够在仿真环境下独立完成机器人导航

一、概述

概念
在ROS中机器人导航(Navigation)由多个功能包组合实现，ROS 中又称之为导航功能包集，关于导航模块，官方介绍如下:
一个二维导航堆栈，它接收来自里程计、传感器流和目标姿态的信息，并输出发送到移动底盘的安全速度命令。
更通俗的讲: 导航其实就是机器人自主的从 A 点移动到 B 点的过程。

ROS 的导航功能包集优势如下:

• 安全: 由专业团队开发和维护
• 功能: 功能更稳定且全面
• 高效: 解放开发者，让开发者更专注于上层功能实现

1.导航模块简介

总结下来，涉及的关键技术有如下五点:

• 全局地图
• 自身定位
• 路径规划
• 运动控制
• 环境感知

机器人导航实现与无人驾驶类似，关键技术也是由上述五点组成，只是无人驾驶是基于室外的，而我们当前介绍的机器人导航更多是基于室内的。

1. 全局地图——全局概览图：定位+路径规划
   在机器人导航中地图是一个重要的组成元素，当然如果要使用地图，首先需要绘制地图。关于地图建模技术不断涌现，这其中有一门称之为 SLAM 的理论脱颖而出:
   SLAM(地图构建和即时定位)可以描述为: 机器人在未知环境中从一个未知位置开始移动,在移动过程中根据位置估计和地图进行自身定位，同时在自身定位的基础上建造增量式地图，以绘制出外部环境的完全地图。
2. 自身定位——确定在地图中的位置
   SLAM 就可以实现自身定位，除此之外，ROS 中还提供了一个用于定位的功能包: amcl
   amcl——自适应的蒙特卡洛定位,是用于2D移动机器人的概率定位系统。它实现了自适应（或KLD采样）蒙特卡洛定位方法，该方法使用粒子过滤器根据已知地图跟踪机器人的姿态。——用传感器实时感知到的消息，来匹配地图。
3. 路径规划——全局路径+局部路径规划
   导航就是机器人从A点运动至B点的过程，在这一过程中，机器人需要根据目标位置计算全局运动路线，并且在运动过程中，还需要时时根据出现的一些动态障碍物调整运动路线，直至到达目标点，该过程就称之为路径规划。在 ROS 中提供了 move_base 包来实现路径规则,该功能包主要由两大规划器组成:
   （1）.全局路径规划(gloable_planner)
   根据给定的目标点和全局地图实现总体的路径规划，使用 Dijkstra 或 A* 算法进行全局路径规划，计算最优路线，作为全局路线
   （2）.本地时时规划(local_planner)
   在实际导航过程中，机器人可能无法按照给定的全局最优路线运行，比如:机器人在运行中，可能会随时出现一定的障碍物… 本地规划的作用就是使用一定算法(Dynamic Window Approaches) 来实现障碍物的规避，并选取当前最优路径以尽量符合全局最优路径
   全局路径规划与本地路径规划是相对的，全局路径规划侧重于全局、宏观实现，而本地路径规划侧重与当前、微观实现。
4. 运动控制——控制速度和方向
   导航功能包集假定它可以通过话题"cmd_vel"发布geometry_msgs/Twist类型的消息，这个消息基于机器人的基座坐标系，它传递的是运动命令。这意味着必须有一个节点订阅"cmd_vel"话题， 将该话题上的速度命令转换为电机命令并发送。
5. 环境感知——感知周围环境
   感知周围环境信息，比如: 摄像头、激光雷达、编码器…，摄像头、激光雷达可以用于感知外界环境的深度信息，编码器可以感知电机的转速信息，进而可以获取速度信息并生成里程计信息。
   在导航功能包集中，环境感知也是一重要模块实现，它为其他模块提供了支持。其他模块诸如: SLAM、amcl、move_base 都需要依赖于环境感知。

2.导航之坐标系

1. 简介
   定位实现需要依赖于机器人自身，机器人需要逆向推导参考系原点并计算坐标系相对关系，该过程实现常用方式有两种:
   通过里程计定位:时时收集机器人的速度信息计算并发布机器人坐标系与父级参考系的相对关系。
   通过传感器定位:通过传感器收集外界环境信息通过匹配计算并发布机器人坐标系与父级参考系的相对关系。
   两种方式在导航中都会经常使用。
2. 特点
   里程计定位:
   优点:里程计定位信息是连续的，没有离散的跳跃。
   缺点:里程计存在累计误差，不利于长距离或长期定位。
   传感器定位:
   优点:比里程计定位更精准；
   缺点:传感器定位会出现跳变的情况，且传感器定位在标志物较少的环境下，其定位精度会大打折扣。
   两种定位方式优缺点互补，应用时一般二者结合使用。
3. 坐标系变换
   上述两种定位实现中，机器人坐标系一般使用机器人模型中的根坐标系(base_link 或 base_footprint)，里程计定位时，父级坐标系一般称之为 odom，如果通过传感器定位，父级参考系一般称之为 map。当二者结合使用时，map 和 odom 都是机器人模型根坐标系的父级，这是不符合坐标变换中"单继承"的原则的，所以，一般会将转换关系设置为: map -> odom -> base_link 或base_footprint。

3.导航条件说明

导航实现，在硬件和软件方面是由一定要求的，需要提前准备。

1. 硬件
   虽然导航功能包集被设计成尽可能的通用，在使用时仍然有三个主要的硬件限制：
   它是为差速驱动的轮式机器人设计的。它假设底盘受到理想的运动命令的控制并可实现预期的结果，命令的格式为：x速度分量，y速度分量，角速度(theta)分量。
   它需要在底盘上安装一个单线激光雷达。这个激光雷达用于构建地图和定位。
   导航功能包集是为正方形的机器人开发的，所以方形或圆形的机器人将是性能最好的。 它也可以工作在任意形状和大小的机器人上，但是较大的机器人将很难通过狭窄的空间。
2. 软件
   导航功能实现之前，需要搭建一些软件环境:
   毋庸置疑的，必须先要安装 ROS
   当前导航基于仿真环境，先保证上一章的机器人系统仿真可以正常执行
   在仿真环境下，机器人可以正常接收 /cmd_vel 消息，并发布里程计消息，传感器消息发布也正常，也即导航模块中的运动控制和环境感知实现完毕

后续导航实现中，我们主要关注于: 使用 SLAM 绘制地图、地图服务、自身定位与路径规划。

二、导航实现

本节内容主要介绍导航的完整性实现，旨在掌握机器人导航的基本流程，该章涉及的主要内容如下:

• SLAM建图(选用较为常见的gmapping)
• 地图服务(可以保存和重现地图)
• 机器人定位
• 路径规划

上述流程介绍完毕，还会对功能进一步集成实现探索式的SLAM建图。

请先安装相关的ROS功能包:（如果是Ubuntu18.04，ros版本为melodic）
安装 gmapping 包(用于构建地图):sudo apt install ros-<ROS版本>-gmapping
安装地图服务包(用于保存与读取地图):sudo apt install ros-<ROS版本>-map-server
安装 navigation 包(用于定位以及路径规划):sudo apt install ros-<ROS版本>-navigation
这里安装有问题的可以去看我的博客https://blog.csdn/layra_liu/article/details/125627739

新建功能包，并导入依赖: gmapping map_server amcl move_base

mkdir -p daohang/src
cd daohang/
catkin_make
cd src
catkin_create_pkg nav_demo gmapping map_server amcl move_base
cd ..
catkin_make

1.导航实现01_SLAM建图

1. gmapping简介

gmapping 是较为常用且比较成熟的SLAM算法之一，gmapping可以根据移动机器人里程计数据和激光雷达数据来绘制二维的栅格地图，对应的，gmapping对硬件也有一定的要求:
1.该移动机器人可以发布里程计消息
2.机器人需要发布雷达消息(该消息可以通过水平固定安装的雷达发布，或者也可以将深度相机消息转换成雷达消息)

2. gmapping节点说明
   gmapping 功能包中的核心节点是:slam_gmapping。为了方便调用，需要先了解该节点订阅的话题、发布的话题、服务以及相关参数。
   
   
3. gmapping使用
   3.1编写gmapping节点相关launch文件
   launch文件编写可以参考 github 的演示 launch文件：https://github/ros-perception/slam_gmapping/blob/melodic-devel/gmapping/launch/slam_gmapping_pr2.launch

复制并修改如下:

<!-- 设置gmapping相关节点 -->
<launch>
    <!-- 是否使用仿真，是的话就是true -->
    <param name="use_sim_time" value="true"/>
    <!-- gmapping的节点 -->
    <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">
        <!-- 设置雷达话题 -->
        <!-- to指向的是你的scan名字 -->
        <remap from="scan" to="scan"/>

        <!-- 关键参数：坐标系 -->
        <!-- 
            ~base_frame(string, default:"base_link")机器人基坐标系。
            ~map_frame(string, default:"map")地图坐标系。
            ~odom_frame(string, default:"odom")里程计坐标系。
         -->
        <param name="base_frame" value="base_footprint" />
        <param name="map_frame" value="map" />
        <param name="odom" value="odom" />

        <!-- 地图更新间隔 -->
        <param name="map_update_interval" value="5.0"/>
        <!-- 雷达长度阈值，根据实际的进行修改 -->
        <param name="maxUrange" value="16.0"/>
        <param name="sigma" value="0.05"/>
        <param name="kernelSize" value="1"/>
        <param name="lstep" value="0.05"/>
        <param name="astep" value="0.05"/>
        <param name="iterations" value="5"/>
        <param name="lsigma" value="0.075"/>
        <param name="ogain" value="3.0"/>
        <param name="lskip" value="0"/>
        <param name="srr" value="0.1"/>
        <param name="srt" value="0.2"/>
        <param name="str" value="0.1"/>
        <param name="stt" value="0.2"/>
        <param name="linearUpdate" value="1.0"/>
        <param name="angularUpdate" value="0.5"/>
        <param name="temporalUpdate" value="3.0"/>
        <param name="resampleThreshold" value="0.5"/>
        <param name="particles" value="30"/>
        <param name="xmin" value="-50.0"/>
        <param name="ymin" value="-50.0"/>
        <param name="xmax" value="50.0"/>
        <param name="ymax" value="50.0"/>
        <param name="delta" value="0.05"/>
        <param name="llsamplerange" value="0.01"/>
        <param name="llsamplestep" value="0.01"/>
        <param name="lasamplerange" value="0.005"/>
        <param name="lasamplestep" value="0.005"/>
    </node>
    <!-- 
        所需的坐标变换
        雷达坐标系→基坐标系
        一般由 robot_state_publisher 或 static_transform_publisher 发布。

        基坐标系→里程计坐标系
        一般由里程计节点发布。
     -->
    <!--
        rviz中显示urdf时，必须发布不同部件之间的 坐标系 关系
        解决策略：ROS中提供了关于机器人模型显示的坐标发布相关节点（两个）
        rosrun joint_state_publisher joint_state_publisher  #发布关节相关节点
        rosrun robot_state_publisher robot_state_publisher  #发布机器人相关节点
    -->
    <node pkg="joint_state_publisher" name="joint_state_publisher" type="joint_state_publisher" />
    <node pkg="robot_state_publisher" name="robot_state_publisher" type="robot_state_publisher" />
    <!--启动rviz-->
    <node pkg="rviz" type="rviz" name="rviz" />
</launch>

3.2执行
1.先启动 Gazebo 仿真环境

cd workspace
source ./devel/setup.bash
roslaunch urdf02_gazebo demo03_env.launch

2.然后再启动地图绘制的 launch 文件:

cd workspace
source ./devel/setup.bash
roslaunch nav_demo nav01_slam.launch

3.启动键盘键盘控制节点，用于控制机器人运动建图

rosrun teleop_twist_keyboard teleop_twist_keyboard.py

4.在 rviz 中添加组件，显示栅格地图

2.导航实现02_地图服务

我们需要将栅格地图序列化到的磁盘以持久化存储，后期还要通过反序列化读取磁盘的地图数据再执行后续操作。在ROS中，地图数据的序列化与反序列化可以通过 map_server 功能包实现。

1. map_server简介
   map_server功能包中提供了两个节点: map_saver 和 map_server，前者用于将栅格地图保存到磁盘，后者读取磁盘的栅格地图并以服务的方式提供出去。
2. map_server使用之地图保存节点(map_saver)

2.1map_saver节点说明
订阅的topic:map(nav_msgs/OccupancyGrid)
订阅此话题用于生成地图文件。

2.2地图保存launch文件
地图保存的语法比较简单，编写一个launch文件，内容如下:

<launch>
    <arg name="filename" value="$(find nav_demo)/map/nav" />
    <node name="map_save" pkg="map_server" type="map_saver" args="-f $(arg filename)" />
</launch>

1.先启动 Gazebo 仿真环境

cd workspace
source ./devel/setup.bash
roslaunch urdf02_gazebo demo03_env.launch

2.然后再启动地图绘制的 launch 文件:

cd workspace
source ./devel/setup.bash
roslaunch nav_demo nav01_slam.launch

3.启动键盘键盘控制节点，用于控制机器人运动建图

rosrun teleop_twist_keyboard teleop_twist_keyboard.py

4.在 rviz 中添加组件，显示栅格地图

5.保存地图

cd workspace
source ./devel/setup.bash
roslaunch nav_demo nav02_map_save.launch

结果：在指定路径下会生成两个文件，xxx.pgm 与 xxx.yaml

2.3 保存结果解释
xxx.pgm 本质是一张图片，直接使用图片查看程序即可打开。

xxx.yaml 保存的是地图的元数据信息，用于描述图片，内容格式如下:

# 1.声明地图图片资源的路径
image: /home/lzl/daohang/src/nav_demo/map/nav.pgm
# 2.地图刻度尺 单位：m/像素
resolution: 0.050000
# 3.地图的位姿信息（按照右手坐标系，地图右下角相对于rviz中的原点的位姿）[x,y,偏航角度]
origin: [-50.000000, -50.000000, 0.000000]

# 4.去反，黑色部分变白色，白变黑
negate: 0

# 地图中的障碍物判断：
# 最终地图结果：白色是可通行区域，黑色是障碍物，蓝灰是未知区域
# 判断规则：
# 1.地图中的每个像素都有取值[0,255] 白色：255 黑色0 像素值设为x
# 2.根据像素值计算一个比例:p=(255-像素值x)/255
# 即白色0,黑色1,灰色是介于0到1的值
# 3.判断是否是障碍物 
# p>occupied_thresh 即为障碍物
# p<free_thresh 视为无物体，可以自由通行

# 下面两个相结合，判断地图上的东西是否是障碍物
# 5.占用阈值
occupied_thresh: 0.65
# 6.空闲阈值
free_thresh: 0.196

3. map_server使用之地图服务(map_server)
   3.1map_server节点说明
   
   3.2地图读取
   通过 map_server 的 map_server 节点可以读取栅格地图数据，编写 launch 文件如下:

<launch>
    <!-- 运行地图服务器，并且加载设置的地图-->
    <node name="map_server" pkg="map_server" type="map_server" args="$(find nav_demo)/map/nav.yaml"/>
</launch>

其中参数是地图描述文件的资源路径，执行该launch文件，该节点会发布话题:map(nav_msgs/OccupancyGrid)
3.3地图显示
打开一个命令行：

cd workspace
source ./devel/setup.bash
roslaunch nav_demo nav03_map_server.launch 

打开一个命令行：rviz

在 rviz 中使用 map 组件可以显示栅格地图：

注意：以上对launch文件修改后，均需要编译运行！
若没配置过快捷键的话，以下为步骤：
快捷键 ctrl + shift + B 调用编译，选择:catkin_make:build(小齿轮）
可以点击配置设置为默认，修改.vscode/tasks.json 文件

{
// 有关 tasks.json 格式的文档，请参见
    // https://go.microsoft/fwlink/?LinkId=733558
    "version": "2.0.0",
    "tasks": [
        {
            "label": "catkin_make:debug", //代表提示的描述性信息
            "type": "shell",  //可以选择shell或者process,如果是shell代码是在shell里面运行一个命令，如果是process代表作为一个进程来运行
            "command": "catkin_make",//这个是我们需要运行的命令
            "args": [],//如果需要在命令后面加一些后缀，可以写在这里，比如-DCATKIN_WHITELIST_PACKAGES=“pac1;pac2”
            "group": {"kind":"build","isDefault":true},
            "presentation": {
                "reveal": "always"//可选always或者silence，代表是否输出信息
            },
            "problemMatcher": "$msCompile"
        }
    ]
}

3.导航实现03_定位

所谓定位就是推算机器人自身在全局地图中的位置，当然，SLAM中也包含定位算法实现，不过SLAM的定位是用于构建全局地图的，是属于导航开始之前的阶段，而当前定位是用于导航中，导航中，机器人需要按照设定的路线运动，通过定位可以判断机器人的实际轨迹是否符合预期。在ROS的导航功能包集navigation中提供了 amcl 功能包，用于实现导航中的机器人定位。

1. amcl简介
   AMCL(adaptive Monte Carlo Localization) 是用于2D移动机器人的概率定位系统，它实现了自适应（或KLD采样）蒙特卡洛定位方法，可以根据已有地图使用粒子滤波器推算机器人位置。
   amcl已经被集成到了navigation包
2. amcl节点说明
   amcl 功能包中的核心节点是:amcl。为了方便调用，需要先了解该节点订阅的话题、发布的话题、服务以及相关参数。
   
   
   3.6坐标变换
   里程计本身也是可以协助机器人定位的，不过里程计存在累计误差且一些特殊情况时(车轮打滑)会出现定位错误的情况，amcl 则可以通过估算机器人在地图坐标系下的姿态，再结合里程计提高定位准确度。
   里程计定位:只是通过里程计数据实现 /odom_frame 与 /base_frame 之间的坐标变换。
   amcl定位: 可以提供 /map_frame 、/odom_frame 与 /base_frame 之间的坐标变换。
   
3. amcl使用

3.1编写amcl节点相关的launch文件
关于launch文件的实现，在amcl功能包下的example目录已经给出了示例，可以作为参考，具体实现:
打开一个命令行：

roscd amcl
ls examples
gedit examples/amcl_diff.launch 

该目录下会列出两个文件: amcl_diff.launch 和 amcl_omni.launch 文件，前者适用于差分移动机器人，后者适用于全向移动机器人，可以按需选择，此处参考前者，新建 launch 文件，复制 amcl_diff.launch 文件内容并修改如下:

<launch>
    <node pkg="amcl" type="amcl" name="amcl" output="screen">
        <!-- Publish scans from best pose at a max of 10 Hz -->
        <param name="odom_model_type" value="diff"/>
        <param name="odom_alpha5" value="0.1"/>
        <param name="gui_publish_rate" value="10.0"/>
        <param name="laser_max_beams" value="30"/>
        <param name="min_particles" value="500"/>
        <param name="max_particles" value="5000"/>
        <param name="kld_err" value="0.05"/>
        <param name="kld_z" value="0.99"/>
        <param name="odom_alpha1" value="0.2"/>
        <param name="odom_alpha2" value="0.2"/>
        <!-- translation std dev, m -->
        <param name="odom_alpha3" value="0.8"/>
        <param name="odom_alpha4" value="0.2"/>
        <param name="laser_z_hit" value="0.5"/>
        <param name="laser_z_short" value="0.05"/>
        <param name="laser_z_max" value="0.05"/>
        <param name="laser_z_rand" value="0.5"/>
        <param name="laser_sigma_hit" value="0.2"/>
        <param name="laser_lambda_short" value="0.1"/>
        <param name="laser_model_type" value="likelihood_field"/>
        <!-- <param name="laser_model_type" value="beam"/> -->
        <param name="laser_likelihood_max_dist" value="2.0"/>
        <param name="update_min_d" value="0.2"/>
        <param name="update_min_a" value="0.5"/>
        
        <!-- 里程计坐标系 -->
        <param name="odom_frame_id" value="odom"/>
        <!-- 添加机器人基坐标系 -->
        <param name="base_frame_id" value="base_footprint"/>
        <!-- 添加地图坐标系 -->
        <param name="global_frame_id" value="map"/>

        <param name="resample_interval" value="1"/>
        <param name="transform_tolerance" value="0.1"/>
        <param name="recovery_alpha_slow" value="0.0"/>
        <param name="recovery_alpha_fast" value="0.0"/>
    </node>
</launch>

3.2编写测试launch文件
amcl节点是不可以单独运行的，运行 amcl 节点之前，需要先加载全局地图，然后启动 rviz 显示定位结果，上述节点可以集成进launch文件，内容示例如下:

<!-- 测试文件 -->
<launch>
    <!-- 启动rviz -->
    <node pkg="joint_state_publisher" name="joint_state_publisher" type="joint_state_publisher" />
    <node pkg="robot_state_publisher" name="robot_state_publisher" type="robot_state_publisher" />
    <node pkg="rviz" type="rviz" name="rviz" />

    <!-- 加载地图服务 -->
    <include file="$(find nav_demo)/launch/nav03_map_server.launch" />

    <!-- amcl -->
    <include file="$(find nav_demo)/launch/nav04_amcl.launch" />
</launch>

3.3执行
1.先启动 Gazebo 仿真环境

cd workspace
source ./devel/setup.bash
roslaunch urdf02_gazebo demo03_env.launch

2.启动键盘控制节点：

rosrun teleop_twist_keyboard teleop_twist_keyboard.py

3.启动上一步中集成地图服务、amcl 与 rviz 的 launch 文件；

roslaunch nav_demo test_amcl.launch

4.在启动的 rviz 中，添加RobotModel、Map组件，分别显示机器人模型与地图，添加 posearray 插件，设置topic为particlecloud来显示 amcl 预估的当前机器人的位姿，箭头越是密集，说明当前机器人处于此位置的概率越高；

5.通过键盘控制机器人运动，会发现 posearray 也随之而改变。

4.导航实现04_路径规划

毋庸置疑的，路径规划是导航中的核心功能之一，在ROS的导航功能包集navigation中提供了 move_base 功能包，用于实现此功能。

1. move_base简介
   move_base 功能包提供了基于动作(action)的路径规划实现，move_base 可以根据给定的目标点，控制机器人底盘运动至目标位置，并且在运动过程中会连续反馈机器人自身的姿态与目标点的状态信息。如前所述(7.1)move_base主要由全局路径规划与本地路径规划组成。
   move_base已经被集成到了navigation包
2. move_base节点说明
   move_base功能包中的核心节点是:move_base。为了方便调用，需要先了解该节点action、订阅的话题、发布的话题、服务以及相关参数。
   
   
3. move_base与代价地图

3.1概念
机器人导航(尤其是路径规划模块)是依赖于地图的，地图在SLAM时已经有所介绍了，ROS中的地图其实就是一张图片，这张图片有宽度、高度、分辨率等元数据，在图片中使用灰度值来表示障碍物存在的概率。不过SLAM构建的地图在导航中是不可以直接使用的，因为：
SLAM构建的地图是静态地图，而导航过程中，障碍物信息是可变的，可能障碍物被移走了，也可能添加了新的障碍物，导航中需要时时的获取障碍物信息；
在靠近障碍物边缘时，虽然此处是空闲区域，但是机器人在进入该区域后可能由于其他一些因素，比如：惯性、或者不规则形体的机器人转弯时可能会与障碍物产生碰撞，安全起见，最好在地图的障碍物边缘设置警戒区，尽量禁止机器人进入…

所以，静态地图无法直接应用于导航，其基础之上需要添加一些辅助信息的地图，比如时时获取的障碍物数据，基于静态地图添加的膨胀区等数据。

3.2组成
代价地图有两张:global_costmap(全局代价地图) 和 local_costmap(本地代价地图)，前者用于全局路径规划，后者用于本地路径规划。

两张代价地图都可以多层叠加,一般有以下层级:
Static Map Layer：静态地图层，SLAM构建的静态地图。
Obstacle Map Layer：障碍地图层，传感器感知的障碍物信息。
Inflation Layer：膨胀层，在以上两层地图上进行膨胀（向外扩张），以避免机器人的外壳会撞上障碍物。
Other Layers：自定义costmap。根据业务自设置的地图数据，比如模拟一堵墙。

多个layer可以按需自由搭配。

3.3碰撞算法
在ROS中，如何计算代价值呢？请看下图:

上图中，横轴是距离机器人中心的距离，纵轴是代价地图中栅格的灰度值。
致命障碍:栅格值为254，此时障碍物与机器人中心重叠，必然发生碰撞；
内切障碍:栅格值为253，此时障碍物处于机器人的内切圆内，必然发生碰撞；
外切障碍:栅格值为[128,252]，此时障碍物处于其机器人的外切圆内，处于碰撞临界，不一定发生碰撞；
非自由空间:栅格值为(0,127]，此时机器人处于障碍物附近，属于危险警戒区，进入此区域，将来可能会发生碰撞；
自由区域:栅格值为0，此处机器人可以自由通过；
未知区域:栅格值为255，还没探明是否有障碍物。
膨胀空间的设置可以参考非自由空间。

4. move_base使用
   路径规划算法在move_base功能包的move_base节点中已经封装完毕了，但是还不可以直接调用，因为算法虽然已经封装了，但是该功能包面向的是各种类型支持ROS的机器人，不同类型机器人可能大小尺寸不同，传感器不同，速度不同，应用场景不同…最后可能会导致不同的路径规划结果，那么在调用路径规划节点之前，我们还需要配置机器人参数。具体实现如下:

• 先编写launch文件模板
• 编写配置文件
• 集成导航相关的launch文件
• 测试

4.1launch文件
关于move_base节点的调用，模板如下:

<launch>
    <!-- respawn="false"意味着节点关闭之后就直接关闭了，不会再重启 ;clear_params="true"代表开启节点时，会清除历史参数-->
    <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen" clear_params="true">
        <rosparam file="$(find nav_demo)/param/costmap_common_params.yaml" command="load" ns="global_costmap" />
        <rosparam file="$(find nav_demo)/param/costmap_common_params.yaml" command="load" ns="local_costmap" />
        <rosparam file="$(find nav_demo)/param/local_costmap_params.yaml" command="load" />
        <rosparam file="$(find nav_demo)/param/global_costmap_params.yaml" command="load" />
        <rosparam file="$(find nav_demo)/param/base_local_planner_params.yaml" command="load" />
    </node>

</launch>

4.2配置文件
关于配置文件的编写，可以参考一些成熟的机器人的路径规划实现，比如: turtlebot3，github链接：https://github/ROBOTIS-GIT/turtlebot3/tree/master/turtlebot3_navigation/param，先下载这些配置文件备用。

在功能包下新建 param 目录，复制下载的文件到此目录: costmap_common_params_burger.yaml、local_costmap_params.yaml、global_costmap_params.yaml、base_local_planner_params.yaml，并将costmap_common_params_burger.yaml 重命名为:costmap_common_params.yaml。

这里注意：最好将yaml文件里的中文、空行删除

配置文件修改以及解释:
4.2.1costmap_common_params.yaml
该文件是move_base 在全局路径规划与本地路径规划时调用的通用参数，包括:机器人的尺寸、距离障碍物的安全距离、传感器信息等。配置参考如下:

#机器人几何参，如果机器人是圆形，设置 robot_radius,如果是其他形状设置 footprint
# 可能需要修改--------------------------------------------------------------------
robot_radius: 0.12 #圆形 加了车轮宽
# 可能需要修改--------------------------------------------------------------------
# footprint: [[-0.12, -0.12], [-0.12, 0.12], [0.12, 0.12], [0.12, -0.12]] #其他形状

obstacle_range: 3.0 # 用于障碍物探测，比如: 值为 3.0，意味着检测到距离小于 3 米的障碍物时，就会引入代价地图
raytrace_range: 3.5 # 用于清除障碍物，比如：值为 3.5，意味着清除代价地图中 3.5 米以外的障碍物


#膨胀半径，扩展在碰撞区域以外的代价区域，使得机器人规划路径避开障碍物
inflation_radius: 0.2
#代价比例系数，越大则代价值越小
cost_scaling_factor: 3.0

#地图类型
map_type: costmap
#导航包所需要的传感器
observation_sources: scan
#对传感器的坐标系和数据进行配置。这个也会用于代价地图添加和清除障碍物。例如，你可以用激光雷达传感器用于在代价地图添加障碍物，再添加kinect用于导航和清除障碍物。
# sensor_frame: laser这边要设置自己的雷达名字
# 可能需要修改--------------------------------------------------------------------
scan: {sensor_frame: laser, data_type: LaserScan, topic: scan, marking: true, clearing: true}
# 可能需要修改--------------------------------------------------------------------

4.2.2global_costmap_params.yaml
该文件用于全局代价地图参数设置:

global_costmap:
  global_frame: map #地图坐标系
# 可能需要修改--------------------------------------------------------------------
  robot_base_frame: base_footprint #机器人坐标系 基准坐标系
# 可能需要修改--------------------------------------------------------------------
  # 以此实现坐标变换

  update_frequency: 1.0 #代价地图更新频率
  publish_frequency: 1.0 #代价地图的发布频率
  transform_tolerance: 0.5 #等待坐标变换发布信息的超时时间

  static_map: true # 是否使用一个地图或者地图服务器来初始化全局代价地图，如果不使用静态地图，这个参数为false.

4.2.3local_costmap_params.yaml
该文件用于局部代价地图参数设置:

local_costmap:
# 可能需要修改--------------------------------------------------------------------
  global_frame: odom #里程计坐标系
  robot_base_frame: base_footprint #机器人坐标系
# 可能需要修改--------------------------------------------------------------------

  update_frequency: 10.0 #代价地图更新频率
  publish_frequency: 10.0 #代价地图的发布频率
  transform_tolerance: 0.5 #等待坐标变换发布信息的超时时间

  static_map: false  #不需要静态地图，可以提升导航效果
  rolling_window: true #是否使用动态窗口，默认为false，在静态的全局地图中，地图不会变化
  width: 3 # 局部地图宽度 单位是 m
  height: 3 # 局部地图高度 单位是 m
  resolution: 0.05 # 局部地图分辨率 单位是 m，一般与静态地图分辨率保持一致

4.2.4base_local_planner_params
基本的局部规划器参数配置，这个配置文件设定了机器人的最大和最小速度限制值，也设定了加速度的阈值。

TrajectoryPlannerROS:

# Robot Configuration Parameters
  max_vel_x: 0.5 # X 方向最大速度
  min_vel_x: 0.1 # X 方向最小速速

  max_vel_theta:  1.0 # 
  min_vel_theta: -1.0
  min_in_place_vel_theta: 1.0

  acc_lim_x: 1.0 # X 加速限制
  acc_lim_y: 0.0 # Y 加速限制
  acc_lim_theta: 0.6 # 角速度加速限制

# Goal Tolerance Parameters，目标公差
  xy_goal_tolerance: 0.10
  yaw_goal_tolerance: 0.05

# Differential-drive robot configuration
# 是否是全向移动机器人
  holonomic_robot: false

# Forward Simulation Parameters，前进模拟参数
  sim_time: 0.8
  vx_samples: 18
  vtheta_samples: 20
  sim_granularity: 0.05

4.2.5参数配置技巧
以上配置在实操中，可能会出现机器人在本地路径规划时与全局路径规划不符而进入膨胀区域出现假死的情况，如何尽量避免这种情形呢？
全局路径规划与本地路径规划虽然设置的参数是一样的，但是二者路径规划和避障的职能不同，可以采用不同的参数设置策略:
全局代价地图可以将膨胀半径和障碍物系数设置的偏大一些；
本地代价地图可以将膨胀半径和障碍物系数设置的偏小一些。
这样，在全局路径规划时，规划的路径会尽量远离障碍物，而本地路径规划时，机器人即便偏离全局路径也会和障碍物之间保留更大的自由空间，从而避免了陷入“假死”的情形。

4.3launch文件集成
如果要实现导航，需要集成map_server地图服务、amcl定位 、move_base路径规划 与 Rviz 等，集成示例如下:

<launch>

    <!-- map_server地图服务 -->
    <include file="$(find nav_demo)/launch/nav03_map_server.launch" />

    <!-- amcl 定位-->
    <include file="$(find nav_demo)/launch/nav04_amcl.launch" />

    <!-- move_base路径规划 -->
    <include file="$(find nav_demo)/launch/nav05_path.launch" />

    <!-- rviz -->
    <node pkg="joint_state_publisher" name="joint_state_publisher" type="joint_state_publisher" />
    <node pkg="robot_state_publisher" name="robot_state_publisher" type="robot_state_publisher" />
    <node pkg="rviz" type="rviz" name="rviz" />
    
</launch>

4.4测试
1.先启动 Gazebo 仿真环境

cd workspace
source ./devel/setup.bash
roslaunch urdf02_gazebo demo03_env.launch

2.启动导航相关的 launch 文件；

cd workspace
source ./devel/setup.bash
roslaunch nav_demo nav06_test.launch

3.添加Rviz组件(参考演示结果),可以将配置数据保存，后期直接调用；

5.导航与SLAM建图

SLAM建图过程中本身就会时时发布地图信息，所以无需再使用map_server，SLAM已经发布了话题为 /map 的地图消息了，且导航需要定位模块，SLAM本身也是可以实现定位的。

该过程实现比较简单，步骤如下:
编写launch文件，集成SLAM与move_base相关节点；
执行launch文件并测试。

1. 编写launc文件
   当前launch文件实现，无需调用map_server的相关节点，只需要启动SLAM节点与move_base节点，示例内容如下:

<launch>
    <!-- 1.slam实现 -->
    <include file="$(find nav_demo)/launch/nav01_slam.launch" />
    <!-- 2.导航中的move_base -->
    <include file="$(find nav_demo)/launch/nav05_path.launch" />
</launch>

2. 测试

1.首先运行gazebo仿真环境；

source ./devel/setup.bash
roslaunch urdf02_gazebo demo03_env.launch

2.然后执行launch文件；

source ./devel/setup.bash
roslaunch nav_demo nav07_slam_auto.launch 

3.在rviz中通过2D Nav Goal设置目标点，机器人开始自主移动并建图了；

4.最后可以使用 map_server 保存地图。

三、导航相关消息

在导航功能包集中包含了诸多节点，毋庸置疑的，不同节点之间的通信使用到了消息中间件(数据载体)，在上一节的实现中，这些消息已经在rviz中做了可视化处理，比如:地图、雷达、摄像头、里程计、路径规划…的相关消息在rviz中提供了相关组件，本节主要介绍这些消息的具体格式。

1.导航之地图

地图相关的消息主要有两个:
nav_msgs/MapMetaData
地图元数据，包括地图的宽度、高度、分辨率等。
nav_msgs/OccupancyGrid
地图栅格数据，一般会在rviz中以图形化的方式显示。

1. nav_msgs/MapMetaData
   调用rosmsg info nav_msgs/MapMetaData显示消息内容如下:

time map_load_time
float32 resolution #地图分辨率
uint32 width #地图宽度
uint32 height #地图高度
geometry_msgs/Pose origin #地图位姿数据
  geometry_msgs/Point position
    float64 x
    float64 y
    float64 z
  geometry_msgs/Quaternion orientation
    float64 x
    float64 y
    float64 z
    float64 w

2. nav_msgs/OccupancyGrid
   调用 rosmsg info nav_msgs/OccupancyGrid显示消息内容如下:

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
#--- 地图元数据
nav_msgs/MapMetaData info
  time map_load_time
  float32 resolution
  uint32 width
  uint32 height
  geometry_msgs/Pose origin
    geometry_msgs/Point position
      float64 x
      float64 y
      float64 z
    geometry_msgs/Quaternion orientation
      float64 x
      float64 y
      float64 z
      float64 w
#--- 地图内容数据，数组长度 = width * height
int8[] data

2.导航之里程计

里程计相关消息是:nav_msgs/Odometry，调用rosmsg info nav_msgs/Odometry 显示消息内容如下:

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
string child_frame_id
geometry_msgs/PoseWithCovariance pose
  geometry_msgs/Pose pose #里程计位姿
    geometry_msgs/Point position
      float64 x
      float64 y
      float64 z
    geometry_msgs/Quaternion orientation
      float64 x
      float64 y
      float64 z
      float64 w
  float64[36] covariance
geometry_msgs/TwistWithCovariance twist
  geometry_msgs/Twist twist #速度
    geometry_msgs/Vector3 linear
      float64 x
      float64 y
      float64 z
    geometry_msgs/Vector3 angular
      float64 x
      float64 y
      float64 z    
  # 协方差矩阵
  float64[36] covariance

3.导航之坐标变换

坐标变换相关消息是: tf/tfMessage，调用rosmsg info tf/tfMessage 显示消息内容如下:

geometry_msgs/TransformStamped[] transforms #包含了多个坐标系相对关系数据的数组
  std_msgs/Header header
    uint32 seq
    time stamp
    string frame_id
  string child_frame_id
  geometry_msgs/Transform transform
    geometry_msgs/Vector3 translation
      float64 x
      float64 y
      float64 z
    geometry_msgs/Quaternion rotation
      float64 x
      float64 y
      float64 z
      float64 w

4.导航之定位

定位相关消息是:geometry_msgs/PoseArray，调用rosmsg info geometry_msgs/PoseArray显示消息内容如下:

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
geometry_msgs/Pose[] poses #预估的点位姿组成的数组
  geometry_msgs/Point position
    float64 x
    float64 y
    float64 z
  geometry_msgs/Quaternion orientation
    float64 x
    float64 y
    float64 z
    float64 w

5.导航之目标点与路径规划

目标点相关消息是:move_base_msgs/MoveBaseActionGoal，调用rosmsg info move_base_msgs/MoveBaseActionGoal显示消息内容如下:

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
actionlib_msgs/GoalID goal_id
  time stamp
  string id
move_base_msgs/MoveBaseGoal goal
  geometry_msgs/PoseStamped target_pose
    std_msgs/Header header
      uint32 seq
      time stamp
      string frame_id
    geometry_msgs/Pose pose #目标点位姿
      geometry_msgs/Point position
        float64 x
        float64 y
        float64 z
      geometry_msgs/Quaternion orientation
        float64 x
        float64 y
        float64 z
        float64 w

路径规划相关消息是:nav_msgs/Path，调用rosmsg info nav_msgs/Path显示消息内容如下:

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
geometry_msgs/PoseStamped[] poses #由一系列点组成的数组
  std_msgs/Header header
    uint32 seq
    time stamp
    string frame_id
  geometry_msgs/Pose pose
    geometry_msgs/Point position
      float64 x
      float64 y
      float64 z
    geometry_msgs/Quaternion orientation
      float64 x
      float64 y
      float64 z
      float64 w

6.导航之激光雷达

激光雷达相关消息是:sensor_msgs/LaserScan，调用rosmsg info sensor_msgs/LaserScan显示消息内容如下:

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
float32 angle_min #起始扫描角度(rad)
float32 angle_max #终止扫描角度(rad)
float32 angle_increment #测量值之间的角距离(rad)
float32 time_increment #测量间隔时间(s)
float32 scan_time #扫描间隔时间(s)
float32 range_min #最小有效距离值(m)
float32 range_max #最大有效距离值(m)
float32[] ranges #一个周期的扫描数据
float32[] intensities #扫描强度数据，如果设备不支持强度数据，该数组为空

7.导航之相机

深度相机相关消息有:sensor_msgs/Image、sensor_msgs/CompressedImage、sensor_msgs/PointCloud2

sensor_msgs/Image 对应的一般的图像数据，sensor_msgs/CompressedImage 对应压缩后的图像数据，sensor_msgs/PointCloud2 对应的是点云数据(带有深度信息的图像数据)。

调用rosmsg info sensor_msgs/Image显示消息内容如下:

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
uint32 height #高度
uint32 width  #宽度
string encoding #编码格式:RGB、YUV等
uint8 is_bigendian #图像大小端存储模式
uint32 step #一行图像数据的字节数，作为步进参数
uint8[] data #图像数据，长度等于 step * height

调用rosmsg info sensor_msgs/CompressedImage显示消息内容如下:

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
string format #压缩编码格式(jpeg、png、bmp)
uint8[] data #压缩后的数据

调用rosmsg info sensor_msgs/PointCloud2显示消息内容如下:

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
uint32 height #高度
uint32 width  #宽度
sensor_msgs/PointField[] fields #每个点的数据类型
  uint8 INT8=1
  uint8 UINT8=2
  uint8 INT16=3
  uint8 UINT16=4
  uint8 INT32=5
  uint8 UINT32=6
  uint8 FLOAT32=7
  uint8 FLOAT64=8
  string name
  uint32 offset
  uint8 datatype
  uint32 count
bool is_bigendian #图像大小端存储模式
uint32 point_step #单点的数据字节步长
uint32 row_step   #一行数据的字节步长
uint8[] data      #存储点云的数组，总长度为 row_step * height
bool is_dense     #是否有无效点

8. 深度图像转激光数据

本节介绍ROS中的一个功能包:depthimage_to_laserscan，顾名思义，该功能包可以将深度图像信息转换成激光雷达信息，应用场景如下:
那么就需要实现传感器的置换，将深度相机发布的三维的图形信息转换成二维的激光雷达信息，这一功能是通过depthimage_to_laserscan来实现的。

安装
使用之前请先安装,注意版本，命令如下:sudo apt-get install ros-melodic-depthimage-to-laserscan

1. depthimage_to_laserscan节点说明
   depthimage_to_laserscan 功能包的核心节点是:depthimage_to_laserscan ，为了方便调用，需要先了解该节点订阅的话题、发布的话题以及相关参数。
   
2. depthimage_to_laserscan使用
   2.1编写launch文件
   编写launch文件执行，将深度信息转换成雷达信息

<launch>
    <node pkg="depthimage_to_laserscan" type="depthimage_to_laserscan" name="depthimage_to_laserscan">
        <remap from="image" to="/camera/depth/image_raw" />
        <!-- output_frame_id需要与深度相机的坐标系一致。 -->
        <param name="output_frame_id" value="support"  />
    </node>
</launch>

2.2修改URDF文件
经过信息转换之后，深度相机也将发布雷达数据，为了不产生混淆，可以注释掉 xacro 文件中的关于激光雷达的部分内容。
转换后的URDF代码如下：

<?xml version="1.0" encoding="utf-8"?>
<!-- =================================================================================== -->
<!-- |    This document was autogenerated by xacro from car.urdf.xacro                 | -->
<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
<!-- =================================================================================== -->
<robot name="mycar">
  <link name="base_footprint">
    <visual>
      <geometry>
        <sphere radius="0.001"/>
      </geometry>
    </visual>
  </link>
  <!--2-1.link-->
  <link name="base_link">
    <visual>
      <geometry>
        <cylinder length="0.08" radius="0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <material name="baselink_color">
        <color rgba="1 0.5 0.2 0.5"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <cylinder length="0.08" radius="0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </collision>
    <inertial>
      <mass value="2"/>
      <inertia ixx="0.00606666666667" ixy="0" ixz="0" iyy="0.00606666666667" iyz="0" izz="0.01"/>
    </inertial>
  </link>
  <gazebo reference="base_link">
    <material>Gazebo/Yellow</material>
  </gazebo>
  <!--2-2.joint-->
  <joint name="link2footprint" type="fixed">
    <parent link="base_footprint"/>
    <child link="base_link"/>
    <origin rpy="0 0 0" xyz="0 0 0.055"/>
  </joint>
  <link name="left_wheel">
    <visual>
      <geometry>
        <cylinder length="0.015" radius="0.0325"/>
      </geometry>
      <origin rpy="1.57079635 0 0" xyz="0 0 0"/>
      <material name="left_wheel">
        <color rgba="0 0 0 0.3"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <cylinder length="0.015" radius="0.0325"/>
      </geometry>
      <origin rpy="1.57079635 0 0" xyz="0 0 0"/>
    </collision>
    <inertial>
      <mass value="0.05"/>
      <inertia ixx="1.4140625e-05" ixy="0" ixz="0" iyy="1.4140625e-05" iyz="0" izz="2.640625e-05"/>
    </inertial>
  </link>
  <gazebo reference="left_wheel">
    <material>Gazebo/Red</material>
  </gazebo>
  <!--3-2.joint-->
  <joint name="left2link" type="continuous">
    <parent link="base_link"/>
    <child link="left_wheel"/>
    <origin rpy="0 0 0" xyz="0 0.1 -0.0225"/>
    <axis xyz="0 1 0"/>
  </joint>
  <link name="right_wheel">
    <visual>
      <geometry>
        <cylinder length="0.015" radius="0.0325"/>
      </geometry>
      <origin rpy="1.57079635 0 0" xyz="0 0 0"/>
      <material name="right_wheel">
        <color rgba="0 0 0 0.3"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <cylinder length="0.015" radius="0.0325"/>
      </geometry>
      <origin rpy="1.57079635 0 0" xyz="0 0 0"/>
    </collision>
    <inertial>
      <mass value="0.05"/>
      <inertia ixx="1.4140625e-05" ixy="0" ixz="0" iyy="1.4140625e-05" iyz="0" izz="2.640625e-05"/>
    </inertial>
  </link>
  <gazebo reference="right_wheel">
    <material>Gazebo/Red</material>
  </gazebo>
  <!--3-2.joint-->
  <joint name="right2link" type="continuous">
    <parent link="base_link"/>
    <child link="right_wheel"/>
    <origin rpy="0 0 0" xyz="0 -0.1 -0.0225"/>
    <axis xyz="0 1 0"/>
  </joint>
  <!--4-1.link-->
  <link name="front_wheel">
    <visual>
      <geometry>
        <sphere radius="0.0075"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <material name="front_wheel">
        <color rgba="0 0 0 0.3"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <sphere radius="0.0075"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </collision>
    <inertial>
      <mass value="0.01"/>
      <inertia ixx="2.25e-07" ixy="0" ixz="0" iyy="2.25e-07" iyz="0" izz="2.25e-07"/>
    </inertial>
  </link>
  <gazebo reference="front_wheel">
    <material>Gazebo/Red</material>
  </gazebo>
  <!--4-2.joint-->
  <joint name="front2link" type="continuous">
    <parent link="base_link"/>
    <child link="front_wheel"/>
    <origin rpy="0 0 0" xyz="0.08 0 -0.0475"/>
    <axis xyz="0 1 0"/>
  </joint>
  <!--4-1.link-->
  <link name="back_wheel">
    <visual>
      <geometry>
        <sphere radius="0.0075"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <material name="back_wheel">
        <color rgba="0 0 0 0.3"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <sphere radius="0.0075"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </collision>
    <inertial>
      <mass value="0.01"/>
      <inertia ixx="2.25e-07" ixy="0" ixz="0" iyy="2.25e-07" iyz="0" izz="2.25e-07"/>
    </inertial>
  </link>
  <gazebo reference="back_wheel">
    <material>Gazebo/Red</material>
  </gazebo>
  <!--4-2.joint-->
  <joint name="back2link" type="continuous">
    <parent link="base_link"/>
    <child link="back_wheel"/>
    <origin rpy="0 0 0" xyz="-0.08 0 -0.0475"/>
    <axis xyz="0 1 0"/>
  </joint>
  <link name="camera">
    <visual>
      <geometry>
        <box size="0.02 0.05 0.05"/>
      </geometry>
      <material name="black">
        <color rgba="0 0 0 0.8"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <box size="0.02 0.05 0.05"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.01"/>
      <inertia ixx="2.41666666667e-06" ixy="0" ixz="0" iyy="2.41666666667e-06" iyz="0" izz="4.16666666667e-06"/>
    </inertial>
  </link>
  <gazebo reference="camera">
    <material>Gazebo/Blue</material>
  </gazebo>
  <joint name="camera2base" type="fixed">
    <parent link="base_link"/>
    <child link="camera"/>
    <origin rpy="0 0 0" xyz="0.08 0 0.065"/>
  </joint>
  <link name="support">
    <visual>
      <geometry>
        <cylinder length="0.15" radius="0.01"/>
      </geometry>
      <material name="yellow">
        <color rgba="0.8 0.5 0.0 0.5"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <cylinder length="0.15" radius="0.01"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.1"/>
      <inertia ixx="0.00019" ixy="0" ixz="0" iyy="0.00019" iyz="0" izz="5e-06"/>
    </inertial>
  </link>
  <gazebo reference="support">
    <material>Gazebo/Gray</material>
  </gazebo>
  <joint name="support2base" type="fixed">
    <parent link="base_link"/>
    <child link="support"/>
    <origin rpy="0 0 0" xyz="0.0 0.0 0.115"/>
  </joint>
  <!-- <link name="laser">
    <visual>
      <geometry>
        <cylinder length="0.05" radius="0.03"/>
      </geometry>
      <material name="black">
        <color rgba="0 0 0 0.5"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <cylinder length="0.05" radius="0.03"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.15"/>
      <inertia ixx="6.5e-05" ixy="0" ixz="0" iyy="6.5e-05" iyz="0" izz="6.75e-05"/>
    </inertial>
  </link>
  <gazebo reference="laser">
    <material>Gazebo/Black</material>
  </gazebo>
  <joint name="laser2support" type="fixed">
    <parent link="support"/>
    <child link="laser"/>
    <origin rpy="0 0 0" xyz="0.0 0.0 0.1"/>
  </joint> -->
  <!-- Transmission is important to link the joints and the controller -->
  <transmission name="left2link_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="left2link">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
    <actuator name="left2link_motor">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <!-- Transmission is important to link the joints and the controller -->
  <transmission name="right2link_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="right2link">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
    <actuator name="right2link_motor">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <gazebo>
    <plugin filename="libgazebo_ros_diff_drive.so" name="differential_drive_controller">
      <rosDebugLevel>Debug</rosDebugLevel>
      <publishWheelTF>true</publishWheelTF>
      <robotNamespace>/</robotNamespace>
      <publishTf>1</publishTf>
      <publishWheelJointState>true</publishWheelJointState>
      <alwaysOn>true</alwaysOn>
      <updateRate>100.0</updateRate>
      <legacyMode>true</legacyMode>
      <leftJoint>left2link</leftJoint>
      <rightJoint>right2link</rightJoint>
      <wheelSeparation>0.2</wheelSeparation>
      <wheelDiameter>0.065</wheelDiameter>
      <broadcastTF>1</broadcastTF>
      <wheelTorque>30</wheelTorque>
      <wheelAcceleration>1.8</wheelAcceleration>
      <commandTopic>cmd_vel</commandTopic>
      <odometryFrame>odom</odometryFrame>
      <odometryTopic>odom</odometryTopic>
      <robotBaseFrame>base_footprint</robotBaseFrame>
    </plugin>
  </gazebo>
  <!-- <gazebo reference="laser">
    <sensor name="rplidar" type="ray">
      <pose>0 0 0 0 0 0</pose>
      <visualize>true</visualize>
      <update_rate>5.5</update_rate>
      <ray>
        <scan>
          <horizontal>
            <samples>360</samples>
            <resolution>1</resolution>
            <min_angle>-3</min_angle>
            <max_angle>3</max_angle>
          </horizontal>
        </scan>
        <range>
          <min>0.10</min>
          <max>30.0</max>
          <resolution>0.01</resolution>
        </range>
        <noise>
          <type>gaussian</type>
          <mean>0.0</mean>
          <stddev>0.01</stddev>
        </noise>
      </ray>
      <plugin filename="libgazebo_ros_laser.so" name="gazebo_rplidar">
        <topicName>/scan</topicName>
        <frameName>laser</frameName>
      </plugin>
    </sensor>
  </gazebo> -->
  <gazebo reference="camera">
    <sensor name="camera_node" type="camera">
      <update_rate>30.0</update_rate>
      <camera name="head">
        <horizontal_fov>1.3962634</horizontal_fov>
        <image>
          <width>1280</width>
          <height>720</height>
          <format>R8G8B8</format>
        </image>
        <clip>
          <near>0.02</near>
          <far>300</far>
        </clip>
        <noise>
          <type>gaussian</type>
          <mean>0.0</mean>
          <stddev>0.007</stddev>
        </noise>
      </camera>
      <plugin filename="libgazebo_ros_camera.so" name="gazebo_camera">
        <alwaysOn>true</alwaysOn>
        <updateRate>0.0</updateRate>
        <cameraName>/camera</cameraName>
        <imageTopicName>image_raw</imageTopicName>
        <cameraInfoTopicName>camera_info</cameraInfoTopicName>
        <frameName>camera</frameName>
        <hackBaseline>0.07</hackBaseline>
        <distortionK1>0.0</distortionK1>
        <distortionK2>0.0</distortionK2>
        <distortionK3>0.0</distortionK3>
        <distortionT1>0.0</distortionT1>
        <distortionT2>0.0</distortionT2>
      </plugin>
    </sensor>
  </gazebo>
  <gazebo reference="support">
    <sensor name="camera" type="depth">
      <always_on>true</always_on>
      <update_rate>20.0</update_rate>
      <camera>
        <horizontal_fov>1.04719756667</horizontal_fov>
        <image>
          <format>R8G8B8</format>
          <width>640</width>
          <height>480</height>
        </image>
        <clip>
          <near>0.05</near>
          <far>8.0</far>
        </clip>
      </camera>
      <plugin filename="libgazebo_ros_openni_kinect.so" name="kinect_camera_controller">
        <cameraName>camera</cameraName>
        <alwaysOn>true</alwaysOn>
        <updateRate>10</updateRate>
        <imageTopicName>rgb/image_raw</imageTopicName>
        <depthImageTopicName>depth/image_raw</depthImageTopicName>
        <pointCloudTopicName>depth/points</pointCloudTopicName>
        <cameraInfoTopicName>rgb/camera_info</cameraInfoTopicName>
        <depthImageCameraInfoTopicName>depth/camera_info</depthImageCameraInfoTopicName>
        <frameName>support_depth</frameName>
        <baseline>0.1</baseline>
        <distortion_k1>0.0</distortion_k1>
        <distortion_k2>0.0</distortion_k2>
        <distortion_k3>0.0</distortion_k3>
        <distortion_t1>0.0</distortion_t1>
        <distortion_t2>0.0</distortion_t2>
        <pointCloudCutoff>0.4</pointCloudCutoff>
      </plugin>
    </sensor>
  </gazebo>
</robot>

3. SLAM应用
   现在我们已经实现并测试通过深度图像信息转换成激光雷达信息了，接下来是实践阶段，通过深度相机实现SLAM，流程如下:
   1.先启动 Gazebo 仿真环境；

source ./devel/setup.bash
roslaunch urdf02_gazebo demo03_env.launch

2.启动转换节点；

source ./devel/setup.bash
roslaunch nav_demo depth_to_laser.launch 

3.再启动地图绘制的 launch 文件；

source ./devel/setup.bash
roslaunch nav_demo nav07_slam_auto.launch

4.启动键盘键盘控制节点，用于控制机器人运动建图；

rosrun teleop_twist_keyboard teleop_twist_keyboard.py

5.在 rviz 中添加组件，显示栅格地图最后，就可以通过键盘控制gazebo中的机器人运动，同时，在rviz中可以显示gmapping发布的栅格地图数据了，但是，前面也介绍了，由于精度和检测范围的原因，尤其再加之环境的特征点偏少，建图效果可能并不理想，建图中甚至会出现地图偏移的情况。

四、本章小结

本章介绍了在仿真环境下的机器人导航实现，主要内容如下:

• 导航概念以及架构设计
• SLAM概念以及gmapping实现
• 地图的序列化与反序列化
• 定位实现
• 路径规划实现
• 导航中涉及的消息解释

导航整体设计架构中，包含地图、定位、路径规划、感知以及控制等实现，感知与控制模块在上一章机器人系统仿真中已经实现了，因此没有做过多介绍，其他部分，当前也是基于仿真环境实现的，后续，我们将搭建一台实体机器人并实现导航功能。

总结

以上就是今天要讲的内容，本文仅仅简单记录了ROS的机器人导航(仿真)，如果有问题请在博客下留言或者咨询邮箱：layraliu@foxmail。