每天都在努力学习SLAM的小黑

SLAM小白日记——ros基础入门

根据慕课网机器人入门基础课程笔记摘抄

1.ROS

ros是一个适用于机器人编程的框架，这个框架把原本松散的零部件耦合在了一起，为他们提供了通信架构。ROS虽然叫做操作系统，但并非Windows、Mac那样通常意义的操作系统，它只是连接了操作系统和你开发的ROS应用程序，所以它也算是一个中间件，基于ROS的应用程序之间建立起了沟通的桥梁，所以也是运行在Linux上的运行时环境

ROS具有一个庞大的社区ROS WIKI(http://wiki.ros.org/ )

2.1Catkin编译系统

一个Catkin的软件包（package）必须要包括两个文件：

package.xml: 包括了package的描述信息
- name, description, version, maintainer(s), license
- opt. authors, url’s, dependencies, plugins, etc…
CMakeLists.txt: 构建package所需的CMake文件
- 调用Catkin的函数/宏
- 解析package.xml
- 找到其他依赖的catkin软件包
- 将本软件包添加到环境变量

catkin_make命令也有一些可选参数，例如：

catkin_make [args]
  -h, --help            帮助信息
  -C DIRECTORY, --directory DIRECTORY
                        工作空间的路径 (默认为 '.')
  --source SOURCE       src的路径 (默认为'workspace_base/src')
  --build BUILD         build的路径 (默认为'workspace_base/build')
  --use-ninja           用ninja取代make
  --use-nmake           用nmake取'make
  --force-cmake         强制cmake，即使已经cmake过
  --no-color            禁止彩色输出(只对catkin_make和CMake生效)
  --pkg PKG [PKG ...]   只对某个PKG进行make
  --only-pkg-with-deps  ONLY_PKG_WITH_DEPS [ONLY_PKG_WITH_DEPS ...]
                        将指定的package列入白名单CATKIN_WHITELIST_PACKAGES，
                        之编译白名单里的package。该环境变量存在于CMakeCache.txt。
  --cmake-args [CMAKE_ARGS [CMAKE_ARGS ...]]
                        传给CMake的参数
  --make-args [MAKE_ARGS [MAKE_ARGS ...]]
                        传给Make的参数
  --override-build-tool-check
                        用来覆盖由于不同编译工具产生的错误

catkin工作空间的结构,包括了src、build、devel三个路径，在有些编译选项下也可能包括其他。但这三个文件夹是catkin编译系统默认的。它们的具体作用如下：

src/: ROS的catkin软件包（源代码包）
build/: catkin（CMake）的缓存信息和中间文件
devel/: 生成的目标文件（包括头文件，动态链接库，静态链接库，可执行文件等）、环境变量

2.2package软件包

package是catkin工作空间的基本单元

一个package下常见的文件、路径有：

  ├── CMakeLists.txt    #package的编译规则(必须)
  ├── package.xml       #package的描述信息(必须)
  ├── src/              #源代码文件
  ├── include/          #C++头文件
  ├── scripts/          #可执行脚本
  ├── msg/              #自定义消息
  ├── srv/              #自定义服务
  ├── models/           #3D模型文件
  ├── urdf/             #urdf文件
  ├── launch/           #launch文件

其中定义package的是CMakeLists.txt和package.xml，这两个文件是package中必不可少的。catkin编译系统在编译前，首先就要解析这两个文件。这两个文件就定义了一个package。

CMakeLists.txt: 定义package的包名、依赖、源文件、目标文件等编译规则，是package不可少的成分
package.xml: 描述package的包名、版本号、作者、依赖等信息，是package不可少的成分
src/: 存放ROS的源代码，包括C++的源码和(.cpp)以及Python的module(.py)
include/: 存放C++源码对应的头文件
scripts/: 存放可执行脚本，例如shell脚本(.sh)、Python脚本(.py)
msg/: 存放自定义格式的消息(.msg)
srv/: 存放自定义格式的服务(.srv)
models/: 存放机器人或仿真场景的3D模型(.sda, .stl, .dae等)
urdf/: 存放机器人的模型描述(.urdf或.xacro)
launch/: 存放launch文件(.launch或.xml)

package相关命令

rospack

rospack是对package管理的工具，命令的用法如下：

rostopic命令	作用
`rospack help`	显示rospack的用法
`rospack list`	列出本机所有package
`rospack depends [package]`	显示package的依赖包
`rospack find [package]`	定位某个package
`rospack profile`	刷新所有package的位置记录

以上命令如果package缺省，则默认为当前目录(如果当前目录包含package.xml)

roscd

roscd命令类似与Linux系统的cd，改进之处在于roscd可以直接cd到ROS的软件包。

rostopic命令	作用
`roscd [pacakge]`	cd到ROS package所在路径

rosls

rosls也可以视为Linux指令ls的改进版，可以直接lsROS软件包的内容。

rosls命令	作用
`rosls [pacakge]`	列出pacakge下的文件

rosdep

rosdep是用于管理ROS package依赖项的命令行工具，用法如下：

rosdep命令	作用
`rosdep check [pacakge]`	检查package的依赖是否满足
`rosdep install [pacakge]`	安装pacakge的依赖
`rosdep db`	生成和显示依赖数据库
`rosdep init`	初始化/etc/ros/rosdep中的源
`rosdep keys`	检查package的依赖是否满足
`rosdep update`	更新本地的rosdep数据库

一个较常使用的命令是rosdep install --from-paths src --ignore-src --rosdistro=kinetic -y,用于安装工作空间中src路径下所有package的依赖项（由pacakge.xml文件指定）。

2.3 CMakeLists.txt

这个文件直接规定了这个package要依赖哪些package，要编译生成哪些目标，如何编译等等流程。所以CMakeLists.txt非常重要，它指定了由源码到目标文件的规则，catkin编译系统在工作时首先会找到每个package下的CMakeLists.txt，然后按照规则来编译构建。

CMakeLists.txt写法

CMakeLists.txt的基本语法都还是按照CMake，而Catkin在其中加入了少量的宏，总体的结构如下：

cmake_minimum_required() #CMake的版本号 
project()                #项目名称 
find_package()           #找到编译需要的其他CMake/Catkin package
catkin_python_setup()    #catkin新加宏，打开catkin的Python Module的支持
add_message_files()      #catkin新加宏，添加自定义Message/Service/Action文件
add_service_files()
add_action_files()
generate_message()       #catkin新加宏，生成不同语言版本的msg/srv/action接口
catkin_package()         #catkin新加宏，生成当前package的cmake配置，供依赖本包的其他软件包调用
add_library()            #生成库
add_executable()         #生成可执行二进制文件
add_dependencies()       #定义目标文件依赖于其他目标文件，确保其他目标已被构建
target_link_libraries()  #链接
catkin_add_gtest()       #catkin新加宏，生成测试
install()                #安装至本机

如果你从未接触过CMake的语法，请阅读《CMake实践》：https://github.com/Akagi201/learning-cmake/blob/master/docs/cmake-practice.pdf 。掌握CMake语法对于理解ROS工程很有帮助。

CMakeLists例子

为了详细的解释CMakeLists.txt的写法，我们以turtlesim小海龟这个pacakge为例，读者可roscd到tuetlesim包下查看，在turtlesim/CMakeLists.txt的写法如下,:

cmake_minimum_required(VERSION 2.8.3)
#CMake至少为2.8.3版

project(turtlesim)
#项目(package)名称为turtlesim，在后续文件中可使用变量${PROJECT_NAME}来引用项目名称turltesim

find_package(catkin REQUIRED COMPONENTS geometry_msgs message_generation rosconsole roscpp roscpp_serialization roslib rostime std_msgs std_srvs)
#cmake宏，指定依赖的其他pacakge，实际是生成了一些环境变量，如_FOUND, _INCLUDE_DIRS, _LIBRARYIS
#此处catkin是必备依赖 其余的geometry_msgs...为组件

find_package(Qt5Widgets REQUIRED)
find_package(Boost REQUIRED COMPONENTS thread)

include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS})
#指定C++的头文件路径
link_directories(${catkin_LIBRARY_DIRS})
#指定链接库的路径

add_message_files(DIRECTORY msg FILES
Color.msg Pose.msg)
#自定义msg文件

add_service_files(DIRECTORY srv FILES
Kill.srv
SetPen.srv
Spawn.srv
TeleportAbsolute.srv
TeleportRelative.srv)
#自定义srv文件

generate_messages(DEPENDENCIES geometry_msgs std_msgs std_srvs)
#在add_message_files、add_service_files宏之后必须加上这句话，用于生成srv msg头文件/module，生成的文件位于devel/include中

catkin_package(CATKIN_DEPENDS geometry_msgs message_runtime std_msgs std_srvs)
# catkin宏命令，用于配置ROS的package配置文件和CMake文件
# 这个命令必须在add_library()或者add_executable()之前调用，该函数有5个可选参数：
# (1) INCLUDE_DIRS - 导出包的include路径
# (2) LIBRARIES - 导出项目中的库
# (3) CATKIN_DEPENDS - 该项目依赖的其他catkin项目
# (4) DEPENDS - 该项目所依赖的非catkin CMake项目。
# (5) CFG_EXTRAS - 其他配置选项

set(turtlesim_node_SRCS
src/turtlesim.cpp

2.4 package.xml

pacakge.xml包含了package的名称、版本号、内容描述、维护人员、软件许可、编译构建工具、编译依赖、运行依赖等信息。
实际上rospack find、rosdep等命令之所以能快速定位和分析出package的依赖项信息，就是直接读取了每一个pacakge中的package.xml文件。它为用户提供了快速了解一个pacakge的渠道。

package.xml写法
在新版本（format2）中，包含的标签为：

<pacakge>               根标记文件  
<name>                  包名  
<version>               版本号  
<description>           内容描述  
<maintainer>            维护者 
<license>               软件许可证  
<buildtool_depend>      编译构建工具，通常为catkin    
<depend>                指定依赖项为编译、导出、运行需要的依赖，最常用
<build_depend>          编译依赖项  
<build_export_depend>   导出依赖项
<exec_depend>           运行依赖项
<test_depend>           测试用例依赖项  
<doc_depend>            文档依赖项

新版本（format2）示例


<package format="2">      
  <name>turtlesimname>
  <version>0.8.1version>
  <description>
    turtlesim is a tool made for teaching ROS and ROS packages.
  description>
  <maintainer email="[email protected]">Dirk Thomasmaintainer>
  <license>BSDlicense>

  <url type="website">http://www.ros.org/wiki/turtlesimurl>
  <url type="bugtracker">https://github.com/ros/ros_tutorials/issuesurl>
  <url type="repository">https://github.com/ros/ros_tutorialsurl>
  <author>Josh Faustauthor>

  
  <buildtool_depend>catkinbuildtool_depend>

    
  <depend>geometry_msgsdepend>
  <depend>rosconsoledepend>
  <depend>roscppdepend>
  <depend>roscpp_serializationdepend>
  <depend>roslibdepend>
  <depend>rostimedepend>
  <depend>std_msgsdepend>
  <depend>std_srvsdepend>

  
  <build_depend>qtbase5-devbuild_depend>
  <build_depend>message_generationbuild_depend>
  <build_depend>qt5-qmakebuild_depend>

  
  <exec_depend>libqt5-coreexec_depend>
  <exec_depend>libqt5-guiexec_depend>
  <exec_depend>message_runtimeexec_depend>
package>

2.5 Metapackage

把一些相近的功能模块、软件包放到一起。
ROS里常见的Metapacakge有：

Metapacakge名称	描述	链接
navigation	导航相关的功能包集	https://github.com/ros-planning/navigation
moveit	运动规划相关的（主要是机械臂）功能包集	https://github.com/ros-planning/moveit
image_pipeline	图像获取、处理相关的功能包集	https://github.com/ros-perception/image_common
vision_opencv	ROS与OpenCV交互的功能包集	https://github.com/ros-perception/vision_opencv
turtlebot	Turtlebot机器人相关的功能包集	https://github.com/turtlebot/turtlebot
pr2_robot	pr2机器人驱动功能包集	https://github.com/PR2/pr2_robot
…	…	…

以上列举了一些常见的功能包集，例如navigation、turtlebot，他们都是用于某一方面的功能，以navigation metapackage（官方介绍里仍然沿用stack的叫法）为例，它包括了以下软件包：

包名	功能
navigation	Metapacakge，依赖以下所有pacakge
amcl	定位
fake_localization	定位
map_server	提供地图
move_base	路径规划节点
nav_core	路径规划的接口类
base_local_planner	局部规划
dwa_local_planner	局部规划
…	…

这个道理并不难理解，比如我们在安装ROS时，用到了sudo apt-get install ros-kinetic-desktop-full命令，由于它依赖了ROS所有的核心组件，我们在安装时也就能够安装整个ROS。

Metapackage写法

我们以ROS-Academy-for-beginners为例介绍meteapckage的写法，在教学包内，有一个ros-academy-for-beginners软件包，该包即为一个metapacakge，其中有且仅有两个文件：CMakeLists.txt和pacakge.xml。

CMakeLists.txt写法如下：

cmake_minimum_required(VERSION 2.8.3)
project(ros_academy_for_beginners)
find_package(catkin REQUIRED)
catkin_metapackage()   #声明本软件包是一个metapacakge

pacakge.xml写法如下：

<package>
    <name>ros_academy_for_beginnersname>
    <version>17.12.4version>
    <description>
        --------------------------------------------------------------------------
        A ROS tutorial for beginner level learners. This metapacakge includes some
        demos of topic, service, parameter server, tf, urdf, navigation, SLAM...
        It tries to explain the basic concepts and usages of ROS.
        --------------------------------------------------------------------------
    description>
    <maintainer email="[email protected]">Chai Changkunmaintainer>
    <author>Chai Changkunauthor>
    <license>BSDlicense>  
    <url>http://http://www.droid.ac.cnurl>

    <buildtool_depend>catkinbuildtool_depend>

    <run_depend>navigation_sim_demorun_depend>  
    <run_depend>param_demorun_depend>
    <run_depend>robot_sim_demorun_depend>
    <run_depend>service_demorun_depend>
    <run_depend>slam_sim_demorun_depend>
    <run_depend>tf_demorun_depend>
    <run_depend>topic_demorun_depend>

    <export>    
        <metapackage/>
    export>
package>

metapacakge中的以上两个文件和普通pacakge不同点是：

CMakeLists.txt:加入了catkin_metapackage()宏，指定本软件包为一个metapacakge。
package.xml:标签将所有软件包列为依赖项，标签中添加标签声明。

metapacakge在我们实际开发一个大工程时可能有用

2.6 其他常见文件类型

在ROS的pacakge中，还有其他许多常见的文件类型，这里做个总结。

launch文件

launch文件一般以.launch或.xml结尾，它对ROS需要运行程序进行了打包，通过一句命令来启动。一般launch文件中会指定要启动哪些package下的哪些可执行程序，指定以什么参数启动，以及一些管理控制的命令。
launch文件通常放在软件包的launch/路径中中。

msg/srv/action文件

ROS程序中有可能有一些自定义的消息/服务/动作文件，为程序的发者所设计的数据结构，这类的文件以.msg,.srv,.action结尾，通常放在package的msg/,srv/,action/路径下。

urdf/xacro文件

urdf/xacro文件是机器人模型的描述文件，以.urdf或.xacro结尾。它定义了机器人的连杆和关节的信息，以及它们之间的位置、角度等信息，通过urdf文件可以将机器人的物理连接信息表示出来。并在可视化调试和仿真中显示。

yaml文件

yaml文件一般存储了ROS需要加载的参数信息，一些属性的配置。通常在launch文件或程序中读取.yaml文件，把参数加载到参数服务器上。通常我们会把yaml文件存放在param/路径下

dae/stl文件

dae或stl文件是3D模型文件，机器人的urdf或仿真环境通常会引用这类文件，它们描述了机器人的三维模型。相比urdf文件简单定义的性状，dae/stl文件可以定义复杂的模型，可以直接从solidworks或其他建模软件导出机器人装配模型，从而显示出更加精确的外形。

rviz文件

rviz文件本质上是固定格式的文本文件，其中存储了RViz窗口的配置（显示哪些控件、视角、参数）。通常rviz文件不需要我们去手动修改，而是直接在RViz工具里保存，下次运行时直接读取。

3.1 Node & Master

Node：在ROS的世界里，最小的进程单元就是节点（node）。一个软件包里可以有多个可执行文件，可执行文件在运行之后就成了一个进程(process)，这个进程在ROS中就叫做节点。

Master： master在整个网络通信架构里相当于管理中心，管理着各个node。node首先在master处进行注册，之后master会将该node纳入整个ROS程序中。node之间的通信也是先由master进行“牵线”，才能两两的进行点对点通信。

rosrun命令的详细用法如下：

$ rosrun [--prefix cmd] [--debug] pkg_name node_name [ARGS]

rosrun将会寻找PACKAGE下的名为EXECUTABLE的可执行程序，将可选参数ARGS传入。
例如在GDB下运行ros程序：

$ rosrun --prefix 'gdb -ex run --args' pkg_name node_name

rosnode命令的详细作用列表如下：

rosnode命令	作用
`rosnode list`	列出当前运行的node信息
`rosnode info node_name`	显示出node的详细信息
`rosnode kill node_name`	结束某个node
`rosnode ping`	测试连接节点
`rosnode machine`	列出在特定机器或列表机器上运行的节点
`rosnode cleanup`	清除不可到达节点的注册信息

以上命令中常用的为前三个，在开发调试时经常会需要查看当前node以及node信息，所以请记住这些常用命令。如果你想不起来，也可以通过rosnode help来查看rosnode命令的用法。

3.2 launch文件

写法与格式
launch文件同样也遵循着xml格式规范，是一种标签文本，它的格式包括以下标签：

<launch>	
<node>	
<include>	
<machine>	
<env-loader>	
<param>	
<rosparam>	
<arg>	
<remap>	
<group>	
launch>

参考链接:http://wiki.ros.org/roslaunch/XML

我们以Ros-Academy-for-Beginners中的robot_sim_demo为例：

<launch>

<arg name="robot" default="xbot2"/>
<arg name="debug" default="false"/>
<arg name="gui" default="true"/>
<arg name="headless" default="false"/>


<include file="$(find gazebo_ros)/launch/empty_world.launch"> 
<arg name="world_name" value="$(find robot_sim_demo)/worlds/ROS-Academy.world"/>
<arg name="debug" value="$(arg debug)" />
<arg name="gui" value="$(arg gui)" />
<arg name="paused" value="false"/>
<arg name="use_sim_time" value="true"/>
<arg name="headless" value="$(arg headless)"/>
include>

 
<include file="$(find robot_sim_demo)/launch/include/$(arg robot).launch.xml" />



launch>

这个launch文件的作用是：启动gazebo模拟器，导入参数内容，加入机器人模型。

3.3 Topic

ROS的通信方式是ROS最为核心的概念，ROS系统的精髓就在于它提供的通信架构。ROS的通信方式有以下四种：

Topic 主题
Service 服务
Parameter Service 参数服务器
Actionlib 动作库

ROS中的通信方式中，topic是常用的一种。对于实时性、周期性的消息，使用topic来传输是最佳的选择

操作命令

在实际应用中，我们应该熟悉topic的几种使用命令，下表详细的列出了各自的命令及其作用。

命令	作用
`rostopic list`	列出当前所有的topic
`rostopic info topic_name`	显示某个topic的属性信息
`rostopic echo topic_name`	显示某个topic的内容
`rostopic pub topic_name ...`	向某个topic发布内容
`rostopic bw topic_name`	查看某个topic的带宽
`rostopic hz topic_name`	查看某个topic的频率
`rostopic find topic_type`	查找某个类型的topic
`rostopic type topic_name`	查看某个topic的类型(msg)

如果你一时忘记了命令的写法，可以通过rostopic help或rostopic command -h查看具体用法。

3.4 Message

Message按照定义解释就是topic内容的数据类型，也称之为topic的格式标准

结构与类型
基本的msg包括bool、int8、int16、int32、int64(以及uint)、float、float64、string、time、duration、header、可变长数组array[]、固定长度数组array[C]。那么具体的一个msg是怎么组成的呢？我们用一个具体的msg来了解，例如上例中的msg sensor_msg/image,位置存放在sensor_msgs/msg/image.msg里,它的结构如下：

std_msg/Header header
uint32	 seq
time	 stamp
string	 frame_id
uint32	 height
uint32	 width
string	 encoding
uint8	 is_bigendian
uint32	 step
uint8[]	 data

操作命令
rosmsg的命令相比topic就比较少了，只有两个如下：

rosmsg命令	作用
`rosmsg list`	列出系统上所有的msg
`rosmsg show msg_name`	显示某个msg的内容

4.1 Service

一种请求-查询式的通信模型

Service通信是双向的，它不仅可以发送消息，同时还会有反馈。所以service包括两部分，一部分是请求方（Clinet），另一部分是应答方/服务提供方（Server）。这时请求方（Client）就会发送一个request，要等待server处理，反馈回一个reply，这样通过类似“请求-应答”的机制完成整个服务通信。

topic VS service

名称	Topic	Service
通信方式	异步通信	同步通信
实现原理	TCP/IP	TCP/IP
通信模型	Publish-Subscribe	Request-Reply
映射关系	Publish-Subscribe(多对多)	Request-Reply（多对一）
特点	接受者收到数据会回调（Callback）	远程过程调用（RPC）服务器端的服务
应用场景	连续、高频的数据发布	偶尔使用的功能/具体的任务
举例	激光雷达、里程计发布数据	开关传感器、拍照、逆解计算

操作命令

在实际应用中，service通信方式的命令时rosservice，具体的命令参数如下表：

rosservice 命令	作用
`rosservice list`	显示服务列表
`rosservice info`	打印服务信息
`rosservice type`	打印服务类型
`rosservice uri`	打印服务ROSRPC uri
`rosservice find`	按服务类型查找服务
`rosservice call`	使用所提供的args调用服务
`rosservice args`	打印服务参数

4.2 Srv

srv文件格式很固定，第一行是请求的格式，中间用**- - -**隔开，第三行是应答的格式。所以srv可以嵌套msg在其中，但它不能嵌套srv。

操作命令

具体的操作指令如下表：

rossrv 命令	作用
`rossrv show`	显示服务描述
`rossrv list`	列出所有服务
`rossrv md5`	显示服务md5sum
`rossrv package`	列出包中的服务
`rossrv packages`	列出包含服务的包

修改部分文件

定义完了msg、srv文件，还有重要的一个步骤就是修改package.xml和修改CMakeList.txt。这些文件需要添加一些必要的依赖等，例如：

** message_generation **
** message_runtime **

上述文本中“**”所引就是新添加的依赖。又例如：

find_package(...roscpp rospy std_msgs ** message_generation **)
catkin_package(
...
CATJIN_DEPENDS ** message_runtime ** ...
...)

add_message_file(
FILES
** DetectHuman.srv **
** HumanPose.msg **
** JointPos.msg **)

** generate_messages(DEPENDENCIES std_msgs) **

添加的这些内容指定了srv或者msg在编译或者运行中需要的依赖。具体的作用我们初学者可不深究，我们需要了解的是，无论我们自定义了srv,还是msg，修改上述部分添加依赖都是必不可少的一步。

4.3 Parameter server

与前两种通信方式不同，参数服务器也可以说是特殊的“通信方式”。特殊点在于参数服务器是节点存储参数的地方、用于配置参数，全局共享参数。参数服务器使用互联网传输，在节点管理器中运行，实现整个通信过程。

维护方式
参数服务器的维护方式非常的简单灵活，总的来讲有三种方式：

命令行维护
launch文件内读写
node源码

下面我们来一一介绍这三种维护方式。

1.命令行维护
使用命令行来维护参数服务器，主要使用rosparam语句来进行操作的各种命令，如下表：

rosparam 命令	作用
`rosparam set param_key param_value`	设置参数
`rosparam get param_key`	显示参数
`rosparam load file_name`	从文件加载参数
`rosparam dump file_name`	保存参数到文件
`rosparam delete`	删除参数
`rosparam list`	列出参数名称

2.load&&dump文件

load和dump文件需要遵守YAML格式，YAML格式具体示例如下：

name:'Zhangsan'
age:20
gender:'M'
score{Chinese:80,Math:90}
score_history:[85,82,88,90]

简明解释。就是“名称+：+值”这样一种常用的解释方式。一般格式如下：

key : value

遵循格式进行定义参数。其实就可以把YAML文件的内容理解为字典，因为它也是键值对的形式。

3. launch文件内读写
launch文件中有很多标签，而与参数服务器相关的标签只有两个，一个是，另一个是。这两个标签功能比较相近，但一般只设置一个参数，请看下例：

  <launch>
    <!--读取机器人参数模型-->

    （1）<param name="robot_description" command="$(find xacro)/xacro.py $(find robot_sim_demo)/urdf/robot.xacro" />
    <!--在Gazebo中启动机器人模型-->
    <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen" 
        args="-urdf -x $(arg x) -y $(arg y) -z $(arg z) -Y $(arg yaw) -model xbot2 -param robot_description"/>
        
    <!--把关节控制的配置信息读到参数服务器-->
    （2）<rosparam file="$(find robot_sim_demo)/config/xbot2_control.yaml" command="load"/>

    <!--启动关节控制器-->
    <node name="spawner" pkg="controller_manager" type="spawner" respawn="false"
    output="screen" ns="/xbot2" args="joint_state_controller
    yaw_platform_position_controller
    pitch_platform_position_controller
    "/>  <!--mobile_base_controller-->

      <!-- 将关节状态转换为TF变-->
      <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" ns="/xbot2" respawn="false" output="screen">
    （3）<param name="publish_frequency" value="100.0"/>
      </node-->
    </launch>

观察上例比如序号3的param就定义了一个key和一个value，交给了参数服务器维护。而序号1的param只给出了key，没有直接给出value，这里的value是由后没的脚本运行结果作为value进行定义的。序号(2)就是rosparam的典型用法，先指定一个YAML文件，然后施加command,其效果等于rosparam load file_name 。

4.4 Action

Actionlib是ROS中一个很重要的库，类似service通信机制，actionlib也是一种请求响应机制的通信方式，actionlib主要弥补了service通信的一个不足，就是当机器人执行一个长时间的任务时，假如利用service通信方式，那么publisher会很长时间接受不到反馈的reply，致使通信受阻。当service通信不能很好的完成任务时候，actionlib则可以比较适合实现长时间的通信过程，actionlib通信过程可以随时被查看过程进度，也可以终止请求，这样的一个特性，使得它在一些特别的机制中拥有很高的效率。

Action 规范
利用动作库进行请求响应，动作的内容格式应包含三个部分，目标、反馈、结果。

目标

机器人执行一个动作，应该有明确的移动目标信息，包括一些参数的设定，方向、角度、速度等等。从而使机器人完成动作任务。

反馈

在动作进行的过程中，应该有实时的状态信息反馈给服务器的实施者，告诉实施者动作完成的状态，可以使实施者作出准确的判断去修正命令。

结果

当运动完成时，动作服务器把本次运动的结果数据发送给客户端，使客户端得到本次动作的全部信息，例如可能包含机器人的运动时长，最终姿势等等。

Action规范文件格式

Action规范文件的后缀名是.action，它的内容格式如下：

# Define the goal
uint32 dishwasher_id  # Specify which dishwasher we want to use
---
# Define the result
uint32 total_dishes_cleaned
---
# Define a feedback message
float32 percent_complete

Action实例详解

Actionlib是一个用来实现action的一个功能包集。我们在demo中设置一个场景，执行一个搬运的action，搬运过程中客户端会不断的发回反馈信息，最终完成整个搬运过程．

首先写handling.action文件,类比如上的格式.包括三个部分,目标,结果,反馈.如下:

# Define the goal
uint32 handling_id 
---
# Define the result
uint32 Handling_completed
---
# Define a feedback message
float32 percent_complete

写完之后修改文件夹里CmakeLists.txt如下内容:

find_package(catkin REQUIRED genmsg actionlib_msgs actionlib)
add_action_files(DIRECTORY action FILES DoDishes.action) generate_messages(DEPENDENCIES actionlib_msgs)
add_action_files(DIRECTORY action FILES Handling.action)
generate_messages(
DEPENDENCIES
actionlib_msgs)

修改package.xml,添加所需要的依赖如下:

actionlib
actionlib_msgs
actionlib
actionlib_msgs

然后回到工作空间 catkin_ws进行编译.

5.1 rqt

命令

rqt_graph :显示通信架构
rqt_plot ：绘制曲线
rqt_console ：查看日志

rqt_graph
rqt_graph是来显示通信架构，也就是我们上一章所讲的内容节点、主题等等，当前有哪些Node和topic在运行，消息的流向是怎样，都能通过这个语句显示出来。此命令由于能显示系统的全貌，所以非常的常用。

rqt_plot
rqt_plot将一些参数，尤其是动态参数以曲线的形式绘制出来。当我们在开发时查看机器人的原始数据，我们就能利用rqt_plot将这些原始数据用曲线绘制出来，非常的直观，利于我们分析数据。

rqt_console
rqt_console里存在一些过滤器，我们可以利用它方便的查到我们需要的日志。

实例测试

首先打开我们教材的模拟场景，输入roslaunch robot_sim_demo robot_spawn_launch
输入命令语句rqt_graph,显示出了当前环境下运行的Node和topic，十分直观的看到通信结构以及消息流向。注意在椭圆形的代表节点，矩形代表topic。
输入命令语句rqt_plot,显示出曲线坐标窗口，在上方输入框里添加或者删除topic，比如我们查看速度，可以在框里设置好topic后，移动机器人，就可以看到自动绘制的线速度或者角速度曲线。
输入命令语句rqt_console，显示日志的输出，配合rqt_logger_level查看日志的级别。

5.2 Rosbag

rosbag是一个这是一套用于记录和回放ROS主题的工具。它旨在提高性能，并避免消息的反序列化和重新排序。rosbag package提供了命令行工具和代码API，可以用C++或者python来编写包。而且rosbag命令行工具和代码API是稳定的，始终保持向后的兼容性。

命令
rosbag对软件包来操作，一个包是ROS用于存储ROS消息数据的文件格式，rosbag命令可以记录、回放和操作包。指令列表如下：

命令	作用
`cheak`	确定一个包是否可以在当前系统中进行，或者是否可以迁移。
`decompress`	压缩一个或多个包文件。
`filter`	解压一个或多个包文件。
`fix`	在包文件中修复消息，以便在当前系统中播放。
`help`	获取相关命令指示帮助信息
`info`	总结一个或多个包文件的内容。
`play`	以一种时间同步的方式回放一个或多个包文件的内容。
`record`	用指定主题的内容记录一个包文件。
`reindex`	重新索引一个或多个包文件。

5.3 Rosbridge

简介

Rosbridge是一个用在ROS系统和其他系统之间的一个功能包,就像是它的名字一样,起到一个"桥梁"的作用,使得ros系统和其他系统能够进行交互.Rosbridge为非ROS程序提供了一个JSON API,有许多与Rosbridge进行交互的前端，包括一个用于Web浏览器交互的WebSocket服务器。Rosbridge_suite是一个包含Rosbridge的元程序包，用于Rosbridge的各种前端程序包（如WebSocket程序包）和帮助程序包。

协议和实现

Rosbridge主要包含两部分内容:协议(Potocol)和实现(Implementation)

协议

Ｒosbridge Protocol提供了非ROS程序与ROS通信的具体的格式规范，规范基于JSON格式,包括订阅topic，发布message,调用server，设置参数，压缩消息等等．例如订阅topic的格式规范如下：

{ "op": "subscribe",
    "topic": "/cmd_vel",
    "type": "geometry_msgs/Twist"
  }

此规范与所用的编程语言和传输方式无关，任何可以发送JSON格式的语音和传输方式都可以Rosbridge protocol进行交流，并且与ROS进行交互．

实现

Rosbridge_suite元程序包是实现Ｒosbridge　Protocol并提供WebSocket传输层的包的集合。

这些软件包包括：

Rosbridge_library : 核心rosbridge软件包。Rosbridge_library负责获取JSON字符串并将命令发送到ROS，反过来接收处理ROS发过来的信息，将之转换为JSON字符串，并将结果转交给非ROS程序。
rosapi : 通过服务调用来访问某些ROS操作，这些服务通常为ROS客户端库保留的服务．这些操作包括获取和设置参数，获取主题列表等等。
rosbridge_server : 虽然Rosbridge_library提供JSON到ROS转换，但它将传输层留给其他人。Rosbridge_server提供了一个WebSocket连接，所以浏览器可以与ROS“交谈”。Roslibjs是一个浏览器的JavaScript库，可以通过rosbridge_server与ROS进行交流。

源码

安装与使用

安装

Rosbridge是基于ROS的，首先要确保自己正确的安装完成了ROS之后可以启动终端执行命令：

sudo apt-get install ros- -rosbridge-server

中间的为自己的ROS版本，依照自己的版本进行安装．

使用

关于更深入的使用，可以参考本课程的视频课程，简单的入门使用可以参考链接如下：

参考链接

6.1 roscpp

通常我们要调用ROS的C++接口，首先就需要#include 。

roscpp的主要部分包括：

ros::init() : 解析传入的ROS参数，创建node第一步需要用到的函数
ros::NodeHandle : 和topic、service、param等交互的公共接口
ros::master : 包含从master查询信息的函数
ros::this_node：包含查询这个进程(node)的函数
ros::service：包含查询服务的函数
ros::param：包含查询参数服务器的函数，而不需要用到NodeHandle
ros::names：包含处理ROS图资源名称的函数

具体可见：http://docs.ros.org/api/roscpp/html/index.html

初始化节点

对于一个C++写的ROS程序，之所以它区别于普通C++程序，是因为代码中做了两层工作：

调用了ros::init()函数，从而初始化节点的名称和其他信息，一般我们ROS程序一开始都会以这种方式开始。
创建ros::NodeHandle对象，也就是节点的句柄，它可以用来创建Publisher、Subscriber以及做其他事情。

句柄(Handle)这个概念可以理解为一个“把手”，你握住了门把手，就可以很容易把整扇门拉开，而不必关心门是什么样子。NodeHandle就是对节点资源的描述，有了它你就可以操作这个节点了，比如为程序提供服务、监听某个topic上的消息、访问和修改param等等。

NodeHandle常用成员函数

NodeHandle是Node的句柄，用来对当前节点进行各种操作。在ROS中，NodeHandle是一个定义好的类，通过include，我们可以创建这个类，以及使用它的成员函数。

NodeHandle常用成员函数包括：

//创建话题的publisher 
ros::Publisher advertise(const string &topic, uint32_t queue_size, bool latch=false); 
//第一个参数为发布话题的名称
//第二个是消息队列的最大长度，如果发布的消息超过这个长度而没有被接收，那么就的消息就会出队。通常设为一个较小的数即可。
//第三个参数是是否锁存。某些话题并不是会以某个频率发布，比如/map这个topic，只有在初次订阅或者地图更新这两种情况下，/map才会发布消息。这里就用到了锁存。

//创建话题的subscriber
ros::Subscriber subscribe(const string &topic, uint32_t queue_size, void(*)(M));
//第一个参数是订阅话题的名称
//第二个参数是订阅队列的长度，如果受到的消息都没来得及处理，那么新消息入队，就消息就会出队
//第三个参数是回调函数指针，指向回调函数来处理接收到的消息

//创建服务的server，提供服务
ros::ServiceServer advertiseService(const string &service, bool(*srv_func)(Mreq &, Mres &)); 
//第一个参数是service名称
//第二个参数是服务函数的指针，指向服务函数。指向的函数应该有两个参数，分别接受请求和响应。

//创建服务的client
ros::ServiceClient serviceClient(const string &service_name, bool persistent=false); 
//第一个函数式service名称
//第二个参数用于设置服务的连接是否持续，如果为true，client将会保持与远程主机的连接，这样后续的请求会快一些。通常我们设为flase

//查询某个参数的值
bool getParam(const string &key, std::string &s); 
bool getParam (const std::string &key, double &d) const；
bool getParam (const std::string &key, int &i) const；
//从参数服务器上获取key对应的值，已重载了多个类型

//给参数赋值
void setParam (const std::string &key, const std::string &s) const；
void setParam (const std::string &key, const char *s) const;
void setParam (const std::string &key, int i) const;
//给key对应的val赋值，重载了多个类型的val

6.2 消息发布节点

定义完了消息，就可以开始写ROS代码了。通常我们会把消息收发的两端分成两个节点来写，一个节点就是一个完整的C++程序。

见topic_demo/src/talker.cpp

#include    
#include   //自定义msg产生的头文件

int main(int argc, char **argv)
{
  ros::init(argc, argv, "talker");  //用于解析ROS参数，第三个参数为本节点名
  ros::NodeHandle nh;    //实例化句柄，初始化node

  topic_demo::gps msg;  //自定义gps消息并初始化 
   ...

  ros::Publisher pub = nh.advertise<topic_demo::gps>("gps_info", 1); //创建publisher，往"gps_info"话题上发布消息
  ros::Rate loop_rate(1.0);   //定义发布的频率，1HZ 
  while (ros::ok())   //循环发布msg
  {
    ...   //处理msg
    pub.publish(msg);//以1Hz的频率发布msg
    loop_rate.sleep();//根据前面的定义的loop_rate,设置1s的暂停
  }
  return 0;
}

机器人上几乎所有的传感器，几乎都是按照固定频率发布消息这种通信方式来传输数据，只是发布频率和数据类型的区别。

6.3消息接收节点

见topic_demo/src/listener.cpp

#include 
#include 
#include 

void gpsCallback(const topic_demo::gps::ConstPtr &msg)
{  
    std_msgs::Float32 distance;  //计算离原点(0,0)的距离
    distance.data = sqrt(pow(msg->x,2)+pow(msg->y,2));
    ROS_INFO("Listener: Distance to origin = %f, state: %s",distance.data,msg->state.c_str()); //输出
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "listener");
  ros::NodeHandle n;
  ros::Subscriber sub = n.subscribe("gps_info", 1, gpsCallback);  //设置回调函数gpsCallback
  ros::spin(); //ros::spin()用于调用所有可触发的回调函数，将进入循环，不会返回，类似于在循环里反复调用spinOnce() 
  //而ros::spinOnce()只会去触发一次
  return 0;
}

在topic接收方，有一个比较重要的概念，就是回调(CallBack)，在本例中，回调就是预先给gps_info话题传来的消息准备一个回调函数，你事先定义好回调函数的操作，本例中是计算到原点的距离。只有当有消息来时，回调函数才会被触发执行。具体去触发的命令就是ros::spin()，它会反复的查看有没有消息来，如果有就会让回调函数去处理。

因此千万不要认为，只要指定了回调函数，系统就回去自动触发，你必须ros::spin()或者ros::spinOnce()才能真正使回调函数生效。

6.4 创建提供服务节点(server)

service_demo/srv/server.cpp内容如下：

#include 
#include 

bool handle_function(service_demo::Greeting::Request &req, service_demo::Greeting::Response &res){
    //显示请求信息
    ROS_INFO(“Request from %s with age %d”, req.name.c_str(), req.age);
    //处理请求，结果写入response
    res.feedback = “Hi ” + req.name + “. I’m server!”;
    //返回true，正确处理了请求
    return true;
}

int main(int argc, char** argv){
    ros::init(argc, argv, “greetings_server”);        //解析参数，命名节点
    ros::NodeHandle nh;                       //创建句柄，实例化node
    ros::ServiceServer service = nh.advertiseService(“greetings”, handle_function);  //写明服务的处理函数
    ros::spin();
    return 0;
}

在以上代码中，服务的处理操作都写在handle_function()中，它的输入参数就是Greeting的Request和Response两部分，而非整个Greeting对象。通常在处理函数中，我们对Requst数据进行需要的操作，将结果写入到Response中。在roscpp中，处理函数返回值是bool型，也就是服务是否成功执行。不要理解成输入Request，返回Response，在rospy中是这样的。

6.5创建服务请求节点(client)

service_demo/srv/client.cpp内容如下：

# include "ros/ros.h"
# include "service_demo/Greeting.h"

int main(int argc, char **argv)
{
	ros::init(argc, argv, "greetings_client");// 初始化，节点命名为"greetings_client"
	ros::NodeHandle nh;
	ros::ServiceClient client = nh.serviceClient<service_demo::Greeting>("greetings");
	// 定义service客户端，service名字为“greetings”，service类型为Service_demo
	
	// 实例化srv，设置其request消息的内容，这里request包含两个变量，name和age，见Greeting.srv
	service_demo::Greeting srv;
	srv.request.name = "HAN";
	srv.request.age = 20;

	if (client.call(srv))
	{
		// 注意我们的response部分中的内容只包含一个变量response，另，注意将其转变成字符串
		ROS_INFO("Response from server: %s", srv.response.feedback.c_str());
	}
	else
	{
		ROS_ERROR("Failed to call service Service_demo");
		return 1;
	}
	return 0;
}

以上代码比较关键的地方有两处，一个是建立一个ServiceClient，另一个是开始调用服务。建立client需要用nh.serviceClient("greetings")，指明服务的类型和服务的名称。而调用时可以直接用client.call(srv)，返回结果不是response，而是是否成功调用远程服务。

6.6 param in roscpp

param_demo
我们来看看在C++中如何进行param_demo的操作，param_demo/param.cpp文件，内容包括：

#include
int main(int argc, char **argv){
	ros::init(argc, argv, "param_demo");
	ros::NodeHandle nh;
	int parameter1, parameter2, parameter3, parameter4, parameter5;
	
	//Get Param的三种方法
	//① ros::param::get()获取参数“param1”的value，写入到parameter1上
	bool ifget1 = ros::param::get("param1", parameter1);
	//② ros::NodeHandle::getParam()获取参数，与①作用相同
	bool ifget2 = nh.getParam("param2",parameter2);
	//③ ros::NodeHandle::param()类似于①和②
	//但如果get不到指定的param，它可以给param指定一个默认值(如33333)
        nh.param("param3", parameter3, 33333);
	
	if(ifget1) //param是否取得
	    ...

        //Set Param
	//① ros::param::set()设置参数
	parameter4 = 4;
	ros::param::set("param4", parameter4);
	//② ros::NodeHandle::setParam()设置参数
	parameter5 = 5;
	nh.setParam("param5",parameter5);

	//Check Param
	//① ros::NodeHandle::hasParam()
	bool ifparam5 = nh.hasParam("param5");
	//② ros::param::has()
	bool ifparam6 = ros::param::has("param6");
	
	//Delete Param
	//① ros::NodeHandle::deleteParam()
	bool ifdeleted5 = nh.deleteParam("param5");
	//② ros::param::del()
	bool ifdeleted6 = ros::param::del("param6");
	...
}

以上是roscpp中对param进行增删改查所有操作的方法，非常直观。

param_demo中的launch文件

实际项目中我们对参数进行设置，尤其是添加参数，一般都不是在程序中，而是在launch文件中。因为launch文件可以方便的修改参数，而写成代码之后，修改参数必须重新编译。
因此我们会在launch文件中将param都定义好，比如这个demo正确的打开方式应该是roslaunch param_demo param_demo_cpp.launch

param_demo/launch/param_demo_cpp.launch内容为：

<launch>
	
	<param name="param1" value="1" />
	<param name="param2" value="2" />
	
	
	<rosparam>   
        param3: 3
        param4: 4
        param5: 5
       rosparam>
	
	
	<node pkg="param_demo" type="param_demo" name="param_demo" output="screen" />
launch>

通过和两个标签我们设置好了5个param，从而在之前的代码中进行增删改查的操作。

在name_demo.cpp中，我们分别尝试了，利用全局命名空间句柄提取全局的param和局部的param，以及在局部命名空间下的句柄提取全局的param和局部的param，详细的代码如下：

#include 
int main(int argc, char* argv[])
{
    int serial_number = -1;//serial_number初始化
    ros::init(argc, argv, "name_demo");//node初始化
    /*创建命名空间*/
    //n 是全局命名空间
    ros::NodeHandle n;
    //nh 是局部命名空间
    ros::NodeHandle nh("~");
    /*全局命名空间下的Param*/
    ROS_INFO("global namespace");
    //提取全局命名空间下的参数serial
    n.getParam("serial", serial_number);
    ROS_INFO("global_Serial was %d", serial_number);
    //提取局部命名空间下的参数serial
    n.getParam("name_demo/serial", serial_number);//在全局命名空间下，要提取局部命名空间下的参数，需要添加node name
    ROS_INFO("global_to_local_Serial was %d", serial_number);
    /*局部命名空间下的Param*/
    ROS_INFO("local namespace");
    //提取局部命名空间下的参数serial
    nh.getParam("serial", serial_number);
    ROS_INFO("local_Serial was %d", serial_number);
    //提取全局命名空间下的参数serial
    nh.getParam("/serial", serial_number);//在局部命名空间下，要提取全局命名空间下的参数，需要添加“/”
    ROS_INFO("local_to_global_Serial was %d", serial_number);
    ros::spin();
    return 0;
}

最后的结果

[ INFO] [1525095241.802257811]: global namespace
[ INFO] [1525095241.803512501]: global_Serial was 5
[ INFO] [1525095241.804515959]: global_to_local_Serial was 10
[ INFO] [1525095241.804550167]: local namespace
[ INFO] [1525095241.805126562]: local_Serial was 10
[ INFO] [1525095241.806137701]: local_to_global_Serial was 5

6.7 时钟

Time 与 Duration
ROS里经常用到的一个功能就是时钟，比如计算机器人移动距离、设定一些程序的等待时间、设定计时器等等。roscpp同样给我们提供了时钟方面的操作。
具体来说，roscpp里有两种时间的表示方法，一种是时刻（ros::Time），一种是时长（ros::Duration）。无论是Time还是Duration都具有相同的表示方法：
···
int32 sec
int32 nsec
···
Time/Duration都由秒和纳秒组成。
要使用Time和Duration，需要#include 和#include

ros::Time begin = ros::Time::now(); //获取当前时间
ros::Time at_some_time1(5,20000000);  //5.2s
ros::Time at_some_time2(5.2) //同上，重载了float类型和两个uint类型的构造函数
ros::Duration one_hour(60*60,0); //1h

double secs1 = at_some_time1.toSec();//将Time转为double型时间
double secs2 = one_hour.toSec();//将Duration转为double型时间

Time和Duration表示的概念并不相同，Time指的是某个时刻，而Duration指的是某个时段，尽管他们的数据结构都相同，但是用在不同的场景下。
ROS为我们重载了Time、Duration类型之间的加减运算，比如:

ros::Time t1 = ros::Time::now() - ros::Duration(5.5); //t1是5.5s前的时刻，Time加减Duration返回都是Time
ros::Time t2 = ros::Time::now() + ros::Duration(3.3);//t2是当前时刻往后推3.3s的时刻
ros::Duration d1 = t2 - t1;//从t1到t2的时长，两个Time相减返回Duration类型
ros::Duration d2 = d1 -ros::Duration(0,300);//两个Duration相减，还是Duration

以上是Time、Duration之间的加减运算，要注意没有Time+Time的做法。

sleep

通常在机器人任务执行中可能有需要等待的场景，这时就要用到sleep功能，roscpp中提供了两种sleep的方法：

ros::Duration(0.5).sleep(); //用Duration对象的sleep方法休眠

ros::Rate r(10); //10HZ
while(ros::ok())
{
    r.sleep();     
    //定义好sleep的频率，Rate对象会自动让整个循环以10hz休眠，即使有任务执行占用了时间
}

timer

Rate的功能是指定一个频率，让某些动作按照这个频率来循环执行。与之类似的是ROS中的定时器Timer，它是通过设定回调函数和触发时间来实现某些动作的反复执行，创建方法和topic中的subscriber很像。

void callback1(const ros::TimerEvent&)
{
  ROS_INFO("Callback 1 triggered");
}

void callback2(const ros::TimerEvent&)
{
  ROS_INFO("Callback 2 triggered");
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "talker");
  ros::NodeHandle n;

  ros::Timer timer1 = n.createTimer(ros::Duration(0.1), callback1);  //timer1每0.1s触发一次callback1函数
  ros::Timer timer2 = n.createTimer(ros::Duration(1.0), callback2);  //timer2每1.0s触发一次callback2函数

  ros::spin();  //千万别忘了spin，只有spin了才能真正去触发回调函数

  return 0;
}

6.8 日志和异常

Log

ROS为开发者和用户提供了一套日志记录和输出系统，这套系统的实现方式是基于topic，也就是每个节点都会把一些日志信息发到一个统一的topic上去，这个topic就是/rosout。
rosout本身也是一个node，它专门负责进行日志的记录。我们在启动master的时候，系统就会附带启动rosout。

在roscpp中进行日志的输出，需要先include ,这个头文件包括了五个级别的日志输出接口，分别是：

DEBUG
INFO
WARN
ERROR
FATAL
用法非常简单：

ROS_DEBUG("The velocity is %f", vel);
ROS_WARN("Warn: the use is deprecated.");
ROS_FATAL("Cannot start this node.");
...

当然也可以在一些特定场景，特定条件下输出，不过对于普通开发者来说可能用不到这么复杂的功能。具体可参考：http://wiki.ros.org/roscpp/Overview/Logging

Exception

roscpp中有两种异常类型，当有以下两种错误时，就会抛出异常：

ros::InvalidNodeNameException
当无效的基础名称传给ros::init(),通常是名称中有/,就会触发

ros::InvalidNameExcaption
当无效名称传给了roscpp

7.1 TF

ROS中机器人模型包含大量的部件，这些部件统称之为link,每一个link上面对应着一个frame, 即一个坐标系．link和frame概念是绑定在一起的。每两个frame之间都有一个broadcaster,这就是为了使得两个frame之间能够正确连通，中间都会有一个Node来发布消息来broadcaster.如果缺少Node来发布消息维护连通，那么这两个frame之间的连接就会断掉．broadcaster就是一个publisher,如果两个frame之间发生了相对运动，broadcaster就会发布相关消息．

格式规范

TransformStamped.msg的格式规范如下：

std_mags/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
                flaot64 z
                float64 w

观察标准的格式规范，首先header定义了序号，时间以及frame的名称．接着还写了child_frame，这两个frame之间要做那种变换就是由geometry_msgs/Transform来定义．Vector3三维向量表示平移，Quaternion四元数表示旋转.

7.2 TF树的数据类型

上面我们讲了，TF tree是由很多的frame之间TF拼接而成。那么TF tree是什么类型呢？如下：

tf/tfMessage.msg
tf2_msgs/TFMessage.msg

这里TF的数据类型有两个，主要的原因是版本的迭代。自ROS Hydro以来，tf第一代已被“弃用”，转而支持tf2。tf2相比tf更加简单高效。此外也添加了一些新的功能。

格式定义
tf/tfMessage.msg或tf2_msgs/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
                        flaot64 z
                        float64 w

如上，一个TransformStamped数组就是一个TF tree。

7.3 tf in c++

C++中给我们提供了很多TF的数据类型，如下表：

名称	数据类型
向量	tf::Vector3
点	tf::Point
四元数	tf::Quaternion
3*3矩阵（旋转矩阵）	tf::Matrix3x3
位姿	tf::pose
变换	tf::Transform
带时间戳的以上类型	tf::Stamped
带时间戳的变换	tf::StampedTransform

易混注意：虽然此表的最后带时间戳的变换数据类型为tf::StampedTransform,和上节我们所讲的geometry_msgs/TransformStamped.msg看起来很相似，但是其实数据类型完全不一样，tf::StampedTransform只能用在C++里，只是C++的一个类，一种数据格式，并不是一个消息。而geometry_msgs/TransformStamped.msg是一个message,它依赖于ROS，与语言无关，也即是无论何种语言，C++、Python、Java等等，都可以发送该消息。

在TF里有可能会遇到各种各样数据的转换，例如常见的四元数、旋转矩阵、欧拉角这三种数据之间的转换。tf in roscpp给了我们解决该问题的函数。详细源码在我们教学课程的代码包中。
首先在tf中与数据转化的数据都类型都包含在#include头文件中，我们将与数据转换相关API都存在tf_demo中的coordinate_transformation.cpp当中，其中列表如下：
第1部分定义空间点和空间向量

编号	函数名称	函数功能
1.1	tfScalar::tfDot(const Vector3 &v1, const Vector3 &v2)	计算两个向量的点积
1.2	tfScalar length()	计算向量的模
1.3	Vector3 &normalize()	求与已知向量同方向的单位向量
1.4	tfScalar::tfAngle(const Vector3 &v1, const Vector3 &v2)	计算两个向量的夹角
1.5	tfScale::tfDistance(const Vector3 &v1, const Vector3 &v2)	计算两个向量的距离
1.6	tfScale::tfCross(const Vector3 &v1,const Vector3 &v2)	计算两个向量的乘积

示例代码：

#include 
#include 
//退出用：ctrl+z
int main(int argc, char** argv){
//初始化
ros::init(argc, argv, "coordinate_transformation");
ros::NodeHandle node;
tf::Vector3 v1(1,1,1);
tf::Vector3 v2(1,0,1);
//第1部分，定义空间点和空间向量
std::cout<<"第1部分，定义空间点和空间向量"<

 
  第2部分定义四元数 
   
    
     
     编号 
     函数名称 
     函数功能 
     
    
    
     
     2.1 
     setRPY(const tfScalar& yaw, const stScalar &pitch, const tfScalar &roll) 
     由欧拉角计算四元数 
     
     
     2.2 
     Vector3 getAxis() 
     由四元数得到旋转轴 
     
     
     2.3 
     setRotation(const Vector3 &axis, const tfScalar& angle) 
     已知旋转轴和旋转角估计四元数 
     
    
   
  示例代码： 
  #include 
#include 
//退出用：ctrl+z
int main(int argc, char** argv){
//初始化
ros::init(argc, argv, "coordinate_transformation");
ros::NodeHandle node;
std::cout<<"第2部分，定义四元数"<
 
  第3部分定义旋转矩阵 
   
    
     
     编号 
     函数名称 
     函数功能 
     
    
    
     
     3.1 
     setRotaion(const Quaternion &q) 
     通过四元数得到旋转矩阵 
     
     
     3.2 
     getEulerYPR(tfScalar &yaw, tfScalar &pitch, tfScalar &roll ) 
     由旋转矩阵求欧拉角 
     
    
   
  示例代码： 
  #include 
#include 
//退出用：ctrl+z
int main(int argc, char** argv){
//初始化
ros::init(argc, argv, "coordinate_transformation");
ros::NodeHandle node;
//第3部分，定义旋转矩阵
std::cout<<"第3部分，定义旋转矩阵"<
 
  此外，在tf_demo的教学包中，我们还提供常见的欧拉角与四元数的互换，详见Euler2Quaternion.cpp与Quaternion2Euler.cpp 
  Euler2Quaternion.cpp 
     #include 
    #include 
    //退出用：ctrl+z
    int main(int argc, char** argv){
    //初始化
      ros::init(argc, argv, "Euler2Quaternion");
      ros::NodeHandle node;
      geometry_msgs::Quaternion q;
      double roll,pitch,yaw;
      while(ros::ok())
      {
      //输入一个相对原点的位置
      std::cout<<"输入的欧拉角：roll,pitch,yaw:";
      std::cin>>roll>>pitch>>yaw;
      //输入欧拉角，转化成四元数在终端输出
     q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
      //ROS_INFO("输出的四元数为：w=%d,x=%d,y=%d,z=%d"，q.w,q.x,q.y,q.z);
      std::cout<<"输出的四元数为：w="<<q.w<<",x="<<q.x<<",y="<<q.y<<",z="<<q.z<<std::endl;
      ros::spinOnce();
      }
      return 0;
    };
 
  Quaternion2Euler.cpp 
   #include 
    #include "nav_msgs/Odometry.h"
    #include 
    //退出用：ctrl+z
    int main(int argc, char** argv){
    //初始化
      ros::init(argc, argv, "Quaternion2Euler");
      ros::NodeHandle node;
      nav_msgs::Odometry position;
      tf::Quaternion RQ2;  
      double roll,pitch,yaw;
      while(ros::ok())
      {
      //输入一个相对原点的位置
      std::cout<<"输入的四元数：w,x,y,z:";
      std::cin>>position.pose.pose.orientation.w>>position.pose.pose.orientation.x>>position.pose.pose.orientation.y>>position.pose.pose.orientation.z;
      //输入四元数，转化成欧拉角数在终端输出
      tf::quaternionMsgToTF(position.pose.pose.orientation,RQ2);  
     // tf::Vector3 m_vector3; 方法2
     // m_vector3=RQ2.getAxis();
      tf::Matrix3x3(RQ2).getRPY(roll,pitch,yaw);  
      std::cout<<"输出的欧拉角为：roll="<<roll<<",pitch="<<pitch<<",yaw="<<yaw<<std::endl;
      //std::cout<<"输出欧拉角为：roll="<
      ros::spinOnce();
      }
      return 0;
    };    
 
  7.4 TF类 
  tf::TransformBroadcaster类 
  transformBroadcaster()
void sendTransform(const StampedTransform &transform)
void sendTransform(const std::vector &transforms)
void sendTransform(const geometry_msgs::TransformStamped &transform)
void sendTransform(const std::vector &transforms)
 
  这个类在前面讲TF树的时候提到过，这个broadcaster就是一个publisher,而sendTransform的作用是来封装publish的函数。在实际的使用中，我们需要在某个Node中构建tf::TransformBroadcaster类，然后调用sendTransform(),将transform发布到/tf的一段transform上。/tf里的transform为我们重载了多种不同的函数类型。在我们的tf_demo教学包当中提供了相关的示例代码tf.broadcaster.cpp，具体如下： 
      #include 
    #include 
    #include 
    //退出用：ctrl+z
    int main(int argc, char** argv){
    //初始化
      ros::init(argc, argv, "tf_broadcaster");
      ros::NodeHandle node;
      static tf::TransformBroadcaster br;
      tf::Transform transform;
      //geometry_msgs::Quaternion qw;
      tf::Quaternion q;
      //定义初始坐标和角度
      double roll=0,pitch=0,yaw=0,x=1.0,y=2.0,z=3.0;
      ros::Rate rate(1);
      while(ros::ok())
      {
      yaw+=0.1;//每经过一秒开始一次变换
      //输入欧拉角，转化成四元数在终端输出
      q.setRPY(roll,pitch,yaw);
          //qw=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);方法2
      transform.setOrigin(tf::Vector3(x,y,z));
      transform.setRotation(q);
      std::cout<<"发布tf变换：sendTransform函数"<<std::endl;
      br.sendTransform(tf::StampedTransform(transform,ros::Time::now(),"base_link","link1"));
      std::cout<<"输出的四元数为：w="<<q[3]<<",x="<<q[0]<<",y="<<q[1]<<",z="<<q[2]<<std::endl;
      //  std::cout<<"输出的四元数为：w="<
      rate.sleep();
      ros::spinOnce();
      }
      return 0;
    };
 
  tf::TransformListener类 
  void lookupTranform(const std::string &target_frame,const std::string &source_frame,const ros::Time &time,StampedTransform &transform)const
bool canTransform()
bool waitForTransform()const
 
  上一个类是向/tf上发的类，那么这一个就是从/tf上接收的类。首先看lookuptransform()函数，第一个参数是目标坐标系，第二个参数为源坐标系，也即是得到从源坐标系到目标坐标系之间的转换关系，第三个参数为查询时刻，第四个参数为存储转换关系的位置。值得注意，第三个参数通常用ros::Time（0）,这个表示为最新的坐标转换关系，而ros::time::now则会因为收发延迟的原因，而不能正确获取当前最新的坐标转换关系。canTransform()是用来判断两个transform之间是否连通，waitForTransform()const是用来等待某两个transform之间的连通，在我们的tf_demo教学包当中提供了相关的示例代码tf_listerner.cpp，具体如下： 
   #include 
    #include 
    #include 
    int main(int argc, char** argv){
      ros::init(argc, argv, "tf_listener");
      ros::NodeHandle node;
      tf::TransformListener listener;
      //1. 阻塞直到frame相通
      std::cout<<"1. 阻塞直到frame相通"<<std::endl;
      listener.waitForTransform("/base_link","link1",ros::Time(0),ros::Duration(4.0));
      ros::Rate rate(1);
      while (node.ok()){
    tf::StampedTransform transform;
    try{
      //2. 监听对应的tf,返回平移和旋转
     std::cout<<"2. 监听对应的tf,返回平移和旋转"<<std::endl;
      listener.lookupTransform("/base_link", "/link1",
                               ros::Time(0), transform);
                               //ros::Time(0)表示最近的一帧坐标变换，不能写成ros::Time::now()
    }
    catch (tf::TransformException &ex) {
      ROS_ERROR("%s",ex.what());
      ros::Duration(1.0).sleep();
      continue;
    }
    std::cout<<"输出的位置坐标：x="<<transform.getOrigin().x()<<",y="<<transform.getOrigin().y()<<",z="<<transform.getOrigin().z()<<std::endl;
    std::cout<<"输出的旋转四元数：w="<<transform.getRotation().getW()<<",x="<<transform.getRotation().getX()<<
    ",y="<<transform.getRotation().getY()<<",z="<<transform.getRotation().getZ()<<std::endl;
    rate.sleep();
      }
      return 0;
    };

 
  7.5 统一机器人描述格式URDF 
   $\qquad$ URDF（Unified Robot Description Format）统一机器人描述格式，URDF使用XML格式描述机器人文件。URDF语法规范，参考链接：http://wiki.ros.org/urdf/XML，URDF组件，是由不同的功能包和组件组成. 
  制作URDF模型 
  (1）添加基本模型
 (2）添加机器人link之间的相对位置关系
 (3）添加模型的尺寸，形状和颜色等
 (4）显示URDF模型 
  制作xacro模型 
   $\quad$ 什么是Xacro? 我们可以把它理解成为针对URDF的扩展性和配置性而设计的宏语言(macro language)。有了Xacro，我们就可以像编程一样来写URDF文件。XACRO格式提供了一些更高级的方式来组织编辑机器人描述. 主要提供了三种方式来使得整个描述文件变得简单。 
  （1）Constants 
  Usage：
 
  类似于C语言中的宏定义, 在头部定义后就可以${body_width}进行引用其数值，有了这个，至少我们可以把需要配置的变量进行统一管理和使用。 
  （2）Macros 
  Usage：
    


 
  Macros是xacro文件中最重要的部分. 就像宏函数一样, 完成一些最小模块的定义, 方便重用, 以及可以使用参数来标识不同的部分. 
  （3）Include 
  很多模型都是已宏的形式进行定义, 并以最小集团分成很多个文件. 而最终的机器人描述就变得非常简单了. 下面摘录一个ur5的描述文件. 从中可以看出来xacro的强大优势. 在最后的示例中我们还能够看到, urdf文件也是能够直接导入进来的. 
  Usage：


  
  

  
  

  
  

  

  
    
    
    
  


 
  include类似于C语言中的include, 先将该文件扩展到包含的位置. 但包含进来的文件很有可能只是一个参数宏的定义. 并没有被调用. 
  举例说明，打开新的终端，输入roslaunch urdf_demo display_xacro.launch,回车之后,发现所有的link和joint已经有在固定的位置上了，并且小车颜色和形状已经固定完成，详见urdf本章的demo。 
  制作gazebo模型 
  在已经制作好的xcaro模型的基础上，添加gazebo模型的组建，看起已经变得十分的具有可操作性。对于二轮差动模型通过添加libgazebo_ros_diff_drive.so插件对小车左右轮的控制。 
  

/
true
false
50
mybot_left_wheel_hinge
mybot_right_wheel_hinge
${chassisWidth+wheelWidth}
${2*wheelRadius}
20
mybot_cmd_vel
mybot_odom
odom
mybot_link


 
  通过添加libgazebo_ros_p3d.so来计算里程。 
  

map
mybot_chassis
odom
30.0


 
  最后，对gazebo模型中小车左右轮相关PID等参数进行设置 
  
  Gazebo/Orange


  0.0
  0.0
  Gazebo/Red


  
  
  
  
  
  Gazebo/Black


  
  
  
  
  
  Gazebo/Black



 
  举例说明，打开新的终端，输入roslaunch urdf_demo display_gazebo_rviz.launch,回车之后,发现所有的link和joint已经有在固定的位置上了，并且小车颜色和形状已经固定完成,gazebo界面同样显示正常，详见urdf本章的demo 
  8.1 地图 
  ROS中的地图很好理解，就是一张普通的灰度图像，通常为pgm格式。这张图像上的黑色像素表示障碍物，白色像素表示可行区域，灰色是未探索的区域 
  在SLAM建图的过程中，你可以在RViz里看到一张地图被逐渐建立起来的过程，类似于一块块拼图被拼接成一张完整的地图。这张地图对于我们定位、路径规划都是不可缺少的信息。事实上，地图在ROS中是以Topic的形式维护和呈现的，这个Topic名称就叫做/map，它的消息类型是nav_msgs/OccupancyGrid。 
  锁存
 由于/map中实际上存储的是一张图片，为了减少不必要的开销，这个Topic往往采用锁存（latched）的方式来发布。什么是锁存？其实就是：地图如果没有更新，就维持着上次发布的内容不变，此时如果有新的订阅者订阅消息，这时只会收到一个/map的消息，也就是上次发布的消息；只有地图更新了（比如SLAM又建出来新的地图），这时/map才会发布新的内容。
 锁存器的作用就是，将发布者最后一次发布的消息保存下来，然后把它自动发送给后来的订阅者。这种方式非常适合变动较慢、相对固定的数据（例如地图），然后只发布一次，相比于同样的消息不定的发布，锁存的方式既可以减少通信中对带宽的占用，也可以减少消息资源维护的开销。 
  nav_msgs/OccupancyGrid
 然后我们来看一下地图的OccupancyGrid类型是如何定义的，你可以通过rosmsg show nav_msgs/OccupancyGrid来查看消息，或者直接rosed nav_msgs OccupancyGrid.msg来查看srv文件。 
  std_msgs/Header header #消息的报头
    uint32 seq
    time stamp
    string frame_id #地图消息绑定在TF的哪个frame上，一般为map
nav_msgs/MapMetaData info #地图相关信息
    time map_load_time #加载时间
    float32 resolution #分辨率 单位：m/pixel
    uint32 width #宽 单位：pixel
    uint32 height #高 单位：pixel
    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
int8[] data #地图具体信息
 
  这个srv文件定义了/map话题的数据结构，包含了三个主要的部分:header, info和data。header是消息的报头，保存了序号、时间戳、frame等通用信息，info是地图的配置信息，它反映了地图的属性，data是真正存储这张地图数据的部分，它是一个可变长数组，int8后面加了[]，你可以理解为一个类似于vector的容器，它存储的内容有width*height个int8型的数据,也就是这张地图上每个像素。 
  8.2 Gmapping 
  Gmapping算法是目前基于激光雷达和里程计方案里面比较可靠和成熟的一个算法，它基于粒子滤波，采用RBPF的方法效果稳定，许多基于ROS的机器人都跑的是gmapping_slam。这个软件包位于ros-perception组织中的slam_gmapping仓库中。
 其中的slam_gmapping是一个metapackage，它依赖了gmapping，而算法具体实现都在gmapping软件包中，该软件包中的slam_gmapping程序就是我们在ROS中运行的SLAM节点。如果你感兴趣，可以阅读一下gmapping的源代码。 
  如果你的ROS安装的是desktop-full版本，应该默认会带gmapping。 
  gmapping的作用是根据激光雷达和里程计（Odometry）的信息，对环境地图进行构建，并且对自身状态进行估计。因此它得输入应当包括激光雷达和里程计的数据，而输出应当有自身位置和地图。 
  输入
 /tf以及/tf_static： 坐标变换，类型为第一代的tf/tfMessage或第二代的tf2_msgs/TFMessage
 其中一定得提供的有两个tf，一个是base_frame与laser_frame之间的tf，即机器人底盘和激光雷达之间的变换；一个是base_frame与odom_frame之间的tf，即底盘和里程计原点之间的坐标变换。odom_frame可以理解为里程计原点所在的坐标系。 
  /scan :激光雷达数据，类型为sensor_msgs/LaserScan 
  /scan很好理解，Gmapping SLAM所必须的激光雷达数据，而/tf是一个比较容易忽视的细节。尽管/tf这个Topic听起来很简单，但它维护了整个ROS三维世界里的转换关系，而slam_gmapping要从中读取的数据是base_frame与laser_frame之间的tf,只有这样才能够把周围障碍物变换到机器人坐标系下，更重要的是base_frame与odom_frame之间的tf，这个tf反映了里程计（电机的光电码盘、视觉里程计、IMU）的监测数据，也就是机器人里程计测得走了多少距离，它会把这段变换发布到odom_frame和laser_frame之间。 
  因此slam_gmapping会从/tf中获得机器人里程计的数据。 
  输出 
   
   /tf： 主要是输出map_frame和odom_frame之间的变换 
   /slam_gmapping/entropy： std_msgs/Float64类型，反映了机器人位姿估计的分散程度 
   /map： slam_gmapping建立的地图 
   /map_metadata： 地图的相关信息 
   里程计误差及修正 
   
  目前ROS中常用的里程计广义上包括车轮上的光电码盘、惯性导航元件（IMU）、视觉里程计，你可以只用其中的一个作为odom，也可以选择多个进行数据融合，融合结果作为odom。通常来说，实际ROS项目中的里程计会发布两个Topic： 
   
   /odom: 类型为nav_msgs/Odometry，反映里程计估测的机器人位置、方向、线速度、角速度信息。 
   /tf: 主要是输出odom_frame和base_frame之间的tf。这段tf反映了机器人的位置和方向变换，数值与/odom中的相同。 
   
  由于以上三种里程计都是对机器人的位姿进行估计，存在着累计误差，因此当运动时间较长时，odom_frame和base_frame之间变换的真实值与估计值的误差会越来越大。你可能会想，能否用激光雷达数据来修正odom_frame和base_frame的tf。事实上gmapping不是这么做的，里程计估计的是多少，odom_frame和base_frame的tf就显示多少，永远不会去修正这段tf。gmapping的做法是把里程计误差的修正发布到map_frame和odom_frame之间的tf上，也就是把误差补偿在了地图坐标系和里程计原点坐标系之间。通过这种方式来修正定位。 
  这样map_frame和base_frame，甚至和laser_frame之间就连通了，实现了机器人在地图上的定位。 
  服务
 slam_gmapping也提供了一个服务： 
   
   /dynamic_map： 其srv类型为nav_msgs/GetMap，用于获取当前的地图。 
   
  该srv定义如下：
 nav_msgs/GetMap.srv 
  # Get the map as a nav_msgs/OccupancyGrid
---
nav_msgs/OccupancyGrid map
 
  可见该服务的请求为空，即不需要传入参数，它会直接反馈当前地图。 
  参数
 slam_gmapping需要的参数很多，这里以slam_sim_demo教学包中的gmapping_demo的参数为例，注释了一些比较重要的参数，具体请查看ROS-Academy-for-Beginners/slam_sim_demo/launch/include/robot_gmapping.launch.xml 
  <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">
<param name="base_frame" value="$(arg base_frame)"/> 
<param name="odom_frame" value="$(arg odom_frame)"/> 
<param name="map_update_interval" value="1.0"/> 
<param name="maxUrange" value="20.0"/> 
<param name="maxRange" value="25.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="minimumScore" value="200"/>
<param name="srr" value="0.01"/>
<param name="srt" value="0.02"/>
<param name="str" value="0.01"/>
<param name="stt" value="0.02"/>
<param name="linearUpdate" value="0.5"/>
<param name="angularUpdate" value="0.436"/>
<param name="temporalUpdate" value="-1.0"/>
<param name="resampleThreshold" value="0.5"/>
<param name="particles" value="80"/>
<param name="xmin" value="-25.0"/>
<param name="ymin" value="-25.0"/>
<param name="xmax" value="25.0"/>
<param name="ymax" value="25.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"/>
<remap from="scan" to="$(arg scan_topic)"/>
node>
 
  8.3 Karto 
  Karto SLAM和Gmapping SLAM在工作方式上非常类似，输入的Topic同样是/tf和/scan，其中/tf里要连通odom_frame与base_frame，还有laser_frame。这里和Gmapping完全一样。
 唯一不同的地方是输出，slam_karto的输出少相比slam_gmapping了一个位姿估计的分散程度. 
  服务
 与Gmapping相同，提供/dynamic_map服务 
  参数
 这里以ROS-Academy-for-Beginners中的karto_slam为例，选取了它的参数文件slam_sim_demo/param/karto_params.yaml，关键位置做了注释： 
  # General Parameters
use_scan_matching: true
use_scan_barycenter: true
minimum_travel_distance: 0.2 
minimum_travel_heading: 0.174                  #in radians
scan_buffer_size: 70
scan_buffer_maximum_scan_distance: 20.0
link_match_minimum_response_fine: 0.8
link_scan_maximum_distance: 10.0
loop_search_maximum_distance: 4.0
do_loop_closing: true
loop_match_minimum_chain_size: 10
loop_match_maximum_variance_coarse: 0.4     # gets squared later
loop_match_minimum_response_coarse: 0.8
loop_match_minimum_response_fine: 0.8

# Correlation Parameters - Correlation Parameters
correlation_search_space_dimension: 0.3
correlation_search_space_resolution: 0.01
correlation_search_space_smear_deviation: 0.03

# Correlation Parameters - Loop Closure Parameters
loop_search_space_dimension: 8.0
loop_search_space_resolution: 0.05
loop_search_space_smear_deviation: 0.03

# Scan Matcher Parameters
distance_variance_penalty: 0.3              # gets squared later
angle_variance_penalty: 0.349                # in degrees (gets converted to radians then squared)
fine_search_angle_offset: 0.00349               # in degrees (gets converted to radians)
coarse_search_angle_offset: 0.349            # in degrees (gets converted to radians)
coarse_angle_resolution: 0.0349                # in degrees (gets converted to radians)
minimum_angle_penalty: 0.9
minimum_distance_penalty: 0.5
use_response_expansion: false
 
 
  8.4 Hector 
  Hector SLAM算法不同于前面两种算法，Hector只需要激光雷达数据，而不需要里程计数据。这种算法比较适合手持式的激光雷达，并且对激光雷达的扫描频率有一定要求。 
  Hector算法的效果不如Gmapping、Karto，因为它仅用到激光雷达信息。这样建图与定位的依据就不如多传感器结合的效果好。但Hector适合手持移动或者本身就没有里程计的机器人使用。 
  位于中心的节点叫作hector_mapping，它的输入和其他SLAM框架类似，都包括了/tf和/scan，另外Hector还订阅一个/syscommandTopic，这是一个字符串型的Topic，当接收到reset消息时，地图和机器人的位置都会初始化到最初最初的位置。 
  在输出的Topic方面，hector多了一个/poseupdate和/slam_out_pose,前者是具有协方差的机器人位姿估计，后者是没有协方差的位姿估计。 
  服务
 与Gmapping相同，提供/dynamic_map查询地图服务 
  参数
 以ROS-Academy-for-Beginners中的hector_slam为例，选取了它的launch文件slam_sim_demo/launch/hector_demo.launch为例，关键位置做了注释： 
  <node pkg="hector_mapping" type="hector_mapping" name="hector_height_mapping" output="screen">
      <param name="scan_topic" value="scan" />
    <param name="base_frame" value="base_link" />
    <param name="odom_frame" value="odom" />

    <param name="output_timing" value="false"/>
    <param name="advertise_map_service" value="true"/>
    <param name="use_tf_scan_transformation" value="true"/>
    <param name="use_tf_pose_start_estimate" value="false"/>
    <param name="pub_map_odom_transform" value="true"/>
    <param name="map_with_known_poses" value="false"/>

    <param name="map_pub_period" value="1"/>
    <param name="update_factor_free" value="0.45"/>

    <param name="map_update_distance_thresh" value="0.1"/>
    <param name="map_update_angle_thresh" value="0.05"/>

    <param name="map_resolution" value="0.05"/>
    <param name="map_size" value="1024"/>
    <param name="map_start_x" value="0"/>
    <param name="map_start_y" value="0"/>

node>
 
  9.1 Navigation Stack 
  Navigation Stack是一个ROS的metapackage，里面包含了ROS在路径规划、定位、地图、异常行为恢复等方面的package，其中运行的算法都堪称经典。Navigation Stack的主要作用就是路径规划，通常是输入各传感器的数据，输出速度。一般我们的ROS都预装了Navigation。 
  Navigation Stack的源代码位于https://github.com/ros-planning/navigation，包括了以下几个package： 
   
    
     
     包名 
     功能 
     
    
    
     
     amcl 
     定位 
     
     
     fake_localization 
     定位 
     
     
     map_server 
     提供地图 
     
     
     move_base 
     路径规划节点 
     
     
     nav_core 
     路径规划的接口类，包括base_local_planner、base_global_planner和recovery_behavior三个接口 
     
     
     base_local_planner 
     实现了Trajectory Rollout和DWA两种局部规划算法 
     
     
     dwa_local_planner 
     重新实现了DWA局部规划算法 
     
     
     parrot_planner 
     实现了较简单的全局规划算法 
     
     
     navfn 
     实现了Dijkstra和A*全局规划算法 
     
     
     global_planner 
     重新实现了Dijkstra和A*全局规划算法 
     
     
     clear_costmap_recovery 
     实现了清除代价地图的恢复行为 
     
     
     rotate_recovery 
     实现了旋转的恢复行为 
     
     
     move_slow_and_clear 
     实现了缓慢移动的恢复行为 
     
     
     costmap_2d 
     二维代价地图 
     
     
     voxel_grid 
     三维小方块（体素？） 
     
     
     robot_pose_ekf 
     机器人位姿的卡尔曼滤波 
     
    
   
  Navigation工作框架
 机器人的自主导航功能基本全靠Navigation中的pacakge，位于导航功能正中心的是move_base节点，可以理解为一个强大的路径规划器，在实际的导航任务中，你只需要启动这一个node，并且给他提供数据，就可以规划出路径和速度。
 move_base之所以能做到路径规划，是因为它包含了很多的插件，像图中的白色圆圈global_planner、local_planner、global_costmap、local_costmap、recovery_behaviors。这些插件用于负责一些更细微的任务：全局规划、局部规划、全局地图、局部地图、恢复行为。而每一个插件其实也都是一个package，放在Navigation Stack里。 
  输入 
   
   /tf:提要提供的tf包括map_frame、odom_frame、base_frame以及机器人各关节之间的完成的一棵tf树。 
   /odom:里程计信息 
   /scan或/pointcloud:传感器的输入信息，最常用的是激光雷达(sensor_msgs/LaserScan类型)，也有用点云数据(sensor_msgs/PointCloud)的。 
   /map：地图，可以由SLAM程序来提供，也可以由map_server来指定已知地图。 
   
  以上四个Topic是必须持续提供给导航系统的，下面一个是可随时发布的topic： 
   
   move_base_simple/goal:目标点位置。 
   
  有几点需要注意： 
  1.move_base并不会去发布tf，因为对于路径规划问题来说，假设地图和位置都是已知的，定位和建图是其他节点的事情。
 2.sensor_topics一般输入是激光雷达数据，但也有输入点云的情况。
 3.图中map_server是灰色，代表可选，并不表示/map这个topic是可选，必须提供地图给move_base。 
  输出 
   
   /cmd_vel:geometry_msgs/Twist类型，为每一时刻规划的速度信息。 
   
  9.2 move_base 
  move_base算得上是Navigation中的核心节点，之所以称之为核心，是因为它在导航的任务中处于支配地位，其他的一些package都是它的插件。
 move_base要运行起来，需要选择好插件，包括三种插件：base_local_planner、base_global_planner和recovery_behavior，这三种插件都得指定，否则系统会指定默认值。 
  Navigation为我们提供了不少候选的插件，可以在配置move_base时选择。
 base_local_planner插件： 
   
   base_local_planner: 实现了Trajectory Rollout和DWA两种局部规划算法 
   dwa_local_planner: 实现了DWA局部规划算法，可以看作是base_local_planner的改进版本 
   
  base_global_planner插件： 
   
   parrot_planner: 实现了较简单的全局规划算法 
   navfn: 实现了Dijkstra和A*全局规划算法 
   global_planner: 重新实现了Dijkstra和A*全局规划算法,可以看作navfn的改进版 
   
  recovery_behavior插件： 
   
   clear_costmap_recovery: 实现了清除代价地图的恢复行为 
   rotate_recovery: 实现了旋转的恢复行为 
   move_slow_and_clear: 实现了缓慢移动的恢复行为 
   
  除了以上三个需要指定的插件外，还有一个costmap插件，该插件默认已经选择好，无法更改。 
  以上所有的插件都是继承于nav_core里的接口，nav_core属于一个接口package，它只定义了三种插件的规范，也可以说定义了三种接口类，然后分别由以上的插件来继承和实现这些接口。因此如果你要研究路径规划算法，不妨研究一下nav_core定义的路径规划工作流程，然后仿照dwa_local_planner或其他插件来实现。 
  除了以上三个需要指定的插件外，还有一个costmap插件，该插件默认已经选择好，默认即为costmap_2d，不可更改，但costmap_2d提供了不同的Layer可以供我们设置 
  在这里插件的概念并不是我们抽象的描述，而是在ROS里catkin编译系统能够认出的，并且与其他节点能够耦合的C++库，插件是可以动态加载的类，也就是说插件不需要提前链接到ROS的程序上，只需在运行时加载插件就可以调用其中的功能。 
  具体关于插件的介绍，有兴趣请看http://wiki.ros.org/pluginlib. 
  插件选择(参数) 
  既然我们知道了move_base具体的一些插件，那如何来选择呢？其实非常简单。在move_base的参数设置里可以选择插件。
 move_base的参数包括以下内容： 
   
    
     
     参数 
     默认值 
     功能 
     
    
    
     
     ~base_global_planner 
     navfn/NavfnROS 
     设置全局规划器 
     
     
     ~base_local_planner 
     base_local_planner/TrajectoryPlannerROS 
     设置局部规划器 
     
     
     ~recovery_behaviors 
     [{name: conservative_reset, type: clear_costmap_recovery/ClearCostmapRecovery}, {name: rotate_recovery, type: rotate_recovery/RotateRecovery}, {name: aggressive_reset, type: clear_costmap_recovery/ClearCostmapRecovery}] 
     设置恢复行为 
     
     
     … 
     … 
     … 
     
    
   
  除了这三个选择插件的参数，还有控制频率、误差等等参数。
 具体请看http://wiki.ros.org/move_base介绍。
 在ROS-Academy-for-Beginners的代码中的navigation_sim_demo例子中，由于要配置的参数太多，通常会将配置写在一个yaml文件中，我们用param/move_base_params.yaml来保存以上参数。而关于一些具体插件，比如dwa_local_planner则也会创建一个文件param/dwa_local_planner.yaml来保存它的设置。 
  Topic与Service 
  move_base包含的Service包括： 
   
   make_plan: nav_msgs/GetPlan类型，请求为一个目标点，响应为规划的轨迹，但不执行该轨迹。 
   clear_unknown_space: std_srvs/Empty类型，允许用户清除未知区域地图。 
   clear_costmaps: std_srvs/Empty类型，允许用户清楚代价地图上的障碍物。 
   
  9.3 costmap 
  costmap是Navigation Stack里的代价地图，它其实也是move_base插件，本质上是C++的动态链接库，用过catkin_make之后生成.so文件，然后move_base在启动时会通过动态加载的方式调用其中的函数。 
  你可以将代价地图理解为，在/map之上新加的另外几层地图，不仅包含了原始地图信息，还加入了其他辅助信息。 
  代价地图有一下特点：
 １．首先，代价地图有两张，一张是local_costmap，一张是global_costmap，分别用于局部路径规划器和全局路径规划器，而这两个costmap都默认并且只能选择costmap_2d作为插件。
 ２. 无论是local_costmap还是global_costmap，都可以配置他们的Layer，可以选择多个层次。costmap的Layer包括以下几种： 
   
   Static Map Layer：静态地图层，通常都是SLAM建立完成的静态地图。 
   Obstacle Map Layer：障碍地图层，用于动态的记录传感器感知到的障碍物信息。 
   Inflation Layer：膨胀层，在以上两层地图上进行膨胀（向外扩张），以避免机器人的外壳会撞上障碍物。 
   Other Layers：你还可以通过插件的形式自己实现costmap，目前已有Social Costmap Layer、Range Sensor Layer等开源插件。 
   
  可以同时选择多个Layer并存。 
  地图插件的选择
 与move_base插件的配置类似，costmap配置也同样用yaml来保存，其本质是维护在参数服务器上。由于costmap通常分为local和global的coastmap，我们习惯把两个代价地图分开。以ROS-Academy-for-Beginners为例，配置写在了param文件夹下的global_costmap_params.yaml和local_costmap_params.yaml里。
 global_costmap_params.yaml: 
  global_costmap:
   global_frame: /map
   robot_base_frame: /base_footprint
   update_frequency: 2.0
   publish_frequency: 0.5
   static_map: true
   rolling_window: false
   transform_tolerance: 0.5
   plugins:
     - {name: static_layer,            type: "costmap_2d::StaticLayer"}
     - {name: voxel_layer,             type: "costmap_2d::VoxelLayer"}
     - {name: inflation_layer,         type: "costmap_2d::InflationLayer"}
 
  local_costmap_params.yaml: 
  local_costmap:
   global_frame: /map
   robot_base_frame: /base_footprint
   update_frequency: 5.0
   publish_frequency: 2.0
   static_map: false
   rolling_window: true
   width: 4.0
   height: 4.0
   resolution: 0.05
   origin_x: 5.0
   origin_y: 0
   transform_tolerance: 0.5
   plugins:
    - {name: voxel_layer,      type: "costmap_2d::VoxelLayer"}
    - {name: inflation_layer,     type: "costmap_2d::InflationLayer"}
 
  在plugins一项中可以设置Layer的种类，可以多层叠加。在本例中，考虑到局部地图并不需要静态地图，而只考虑传感器感知到的障碍物，因此可以删去StaticLayer。 
  9.4 map_server & amcl 
  在某些固定场景下，我们已经知道了地图（无论通过SLAM还是测量），这样机器人每次启动最好就能直接加载已知地图，而是每次开机都重建。在这种情况下，就需要有一个节点来发布/map，提供场景信息了。 
  map_server
 map_server是一个和地图相关的功能包，它可以将已知地图发布出来，供导航和其他功能使用，也可以保存SLAM建立的地图。 
  要让map_server发布/map，需要输入给它两个文件: 
   
   地图文件，通常为pgm格式； 
   地图的描述文件，通常为yaml格式 
   
  例如在ROS-Academy-for-Beginners里，我们提供了软件博物馆的地图文件，见slam_sim_demo/maps下： 
  Software_Museum.yaml 
  image: Software_Museum.pgm  #指定地图文件
resolution: 0.050000    #地图的分辨率 单位为m/pixel
origin: [-25.000000, -25.000000, 0.000000]   #地图的原点
negate: 0    #0代表 白色为空闲 黑色为占据
occupied_thresh: 0.65  #当占据的概率大于0.65认为被占据
free_thresh: 0.196     #当占据的概率小于0.196认为无障碍
 
  其中占据的概率 occ = (255-color_avg)/255.0， color_avg为RGB三个通道的平均值。 
  有了以上两个文件，你可以通过指令来加载这张地图,map_server相关命令如下: 
   
    
     
     map_server命令 
     作用 
     
    
    
     
     rosrun map_server map_server Software_Museum.yaml 
     加载自定义的地图 
     
     
     rosrun map_server map_saver -f mymap 
     保存当前地图为mymap.pgn和mymap.yaml 
     
    
   
  当我运行rosrun map_server map_server ***.yaml时，会有以下的通信接口： 
  Topic
 通常我们是在launch文件中加载map_server，发布地图。而map_server发布的消息包括： 
   
   /map_metadata: 发布地图的描述信息 
   /map: 发布锁存的地图消息 
   
  Service
 amcl的服务只有一个： 
   
   static_map: 用于请求和响应当前的静态地图。 
   
  Param
 amcl有一个参数需要设置，就是发布地图的frame。 
   
   ~frame_id: string类型，默认为map。 绑定发布的地图与tf中的哪个frame，通常就是map。 
   
  有两个概念不要搞混淆，map既是一个topic，也是一个frame，前者是topic通信方式中的一个话题，信息交互的频道，后者是tf中的一个坐标系，map_frame需要和其他的frame相连通。 
  amcl
 Adaptive Mentcarto Localization(AMCL)，蒙特卡洛自适应定位是一种很常用的定位算法，它通过比较检测到的障碍物和已知地图来进行定位。 
  AMCL上的通信架构如上图所示，与之前SLAM的框架很像，最主要的区别是/map作为了输入，而不是输出，因为AMCL算法只负责定位，而不管建图。 
  同时还有一点需要注意，AMCl定位会对里程计误差进行修正，修正的方法是把里程计误差加到map_frame和odom_frame之间，而odom_frame和base_frame之间是里程计的测量值，这个测量值并不会被修正。这一工程实现与之前gmapping、karto的做法是相同的。

编号	函数名称	函数功能
2.1	setRPY(const tfScalar& yaw, const stScalar &pitch, const tfScalar &roll)	由欧拉角计算四元数
2.2	Vector3 getAxis()	由四元数得到旋转轴
2.3	setRotation(const Vector3 &axis, const tfScalar& angle)	已知旋转轴和旋转角估计四元数

编号	函数名称	函数功能
3.1	setRotaion(const Quaternion &q)	通过四元数得到旋转矩阵
3.2	getEulerYPR(tfScalar &yaw, tfScalar &pitch, tfScalar &roll )	由旋转矩阵求欧拉角

参数	默认值	功能
~base_global_planner	navfn/NavfnROS	设置全局规划器
~base_local_planner	base_local_planner/TrajectoryPlannerROS	设置局部规划器
~recovery_behaviors	[{name: conservative_reset, type: clear_costmap_recovery/ClearCostmapRecovery}, {name: rotate_recovery, type: rotate_recovery/RotateRecovery}, {name: aggressive_reset, type: clear_costmap_recovery/ClearCostmapRecovery}]	设置恢复行为
…	…	…

map_server命令	作用
`rosrun map_server map_server Software_Museum.yaml`	加载自定义的地图
`rosrun map_server map_saver -f mymap`	保存当前地图为mymap.pgn和mymap.yaml

SLAM小白日记——ros基础入门

1.ROS

2.1Catkin编译系统

2.2package软件包

package相关命令

2.3 CMakeLists.txt

2.4 package.xml

2.5 Metapackage

2.6 其他常见文件类型

3.1 Node & Master

3.2 launch文件

3.3 Topic

3.4 Message

4.1 Service

4.2 Srv

4.3 Parameter server

2.load&&dump文件

4.4 Action

5.1 rqt

5.2 Rosbag

5.3 Rosbridge

协议

实现

安装

使用

6.1 roscpp

6.2 消息发布节点

6.3消息接收节点

6.4 创建提供服务节点(server)

6.5创建服务请求节点(client)

6.6 param in roscpp

6.7 时钟

6.8 日志和异常

7.1 TF

7.2 TF树的数据类型

7.3 tf in c++

7.4 TF类

tf::TransformListener类

7.5 统一机器人描述格式URDF

8.1 地图

8.2 Gmapping

8.3 Karto

8.4 Hector

9.1 Navigation Stack

9.2 move_base

9.3 costmap

9.4 map_server & amcl

你可能感兴趣的:(SLAM小白日记——ros基础入门)