【笔记】ROS理论与实践

花舞の街道

已于 2022-07-20 22:39:32 修改

阅读量322

点赞数 2

分类专栏：机器人学文章标签： c++ python

于 2022-07-20 22:37:05 首次发布

本文链接：https://blog.csdn.net/ASUNAchan/article/details/125901884

版权

机器人学专栏收录该内容

2 篇文章 0 订阅

订阅专栏

课程地址： https://www.bilibili.com/video/BV1Ci4y1L7ZZ?from=search&seid=12202878670955465782&spm_id_from=333.337.0.0&vd_source=209f3cae68f20d2673e7fa1a66ff3633

学习经验： 可以先学习一下CMake，感觉会有帮助，我的笔记：https://blog.csdn.net/ASUNAchan/article/details/124348115?spm=1001.2014.3001.5501

第零章.ROS架构

1. ROS文件系统

在这里插入图片描述

WorkSpace --- 自定义的工作空间

    |--- build:编译空间，用于存放CMake和catkin的缓存信息、配置信息和其他中间文件。

    |--- devel:开发空间，用于存放编译后生成的目标文件，包括头文件、动态&静态链接库、可执行文件等。

    |--- src: 源码

        |-- package：功能包(ROS基本单元)包含多个节点、库与配置文件，包名所有字母小写，只能由字母、数字与下划线组成

            |-- CMakeLists.txt 配置编译规则，比如源文件、依赖项、目标文件

            |-- package.xml 包信息，比如:包名、版本、作者、依赖项...(以前版本是 manifest.xml)

            |-- scripts 存储python文件

            |-- src 存储C++源文件

            |-- include 头文件

            |-- msg 消息通信格式文件

            |-- srv 服务通信格式文件

            |-- action 动作格式文件

            |-- launch 可一次性运行多个节点 

            |-- config 配置信息

        |-- CMakeLists.txt: 编译的基本配置

1.package.xml

该文件定义有关软件包的属性，例如软件包名称，版本号，作者，维护者以及对其他catkin软件包的依赖性。请注意，该概念类似于旧版 rosbuild 构建系统中使用的manifest.xml文件。

<?xml version="1.0"?>
<!-- 格式: 以前是 1，推荐使用格式 2 -->
<package format="2">
  <!-- 包名 -->
  <name>demo01_hello_vscode</name>
  <!-- 版本 -->
  <version>0.0.0</version>
  <!-- 描述信息 -->
  <description>The demo01_hello_vscode package</description>

  <!-- One maintainer tag required, multiple allowed, one person per tag -->
  <!-- Example:  -->
  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
  <!-- 维护人员 -->
  <maintainer email="xuzuo@todo.todo">xuzuo</maintainer>


  <!-- One license tag required, multiple allowed, one license per tag -->
  <!-- Commonly used license strings: -->
  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
  <!-- 许可证信息，ROS核心组件默认 BSD -->
  <license>TODO</license>


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


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


  <!-- The *depend tags are used to specify dependencies -->
  <!-- Dependencies can be catkin packages or system dependencies -->
  <!-- Examples: -->
  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
  <!--   <depend>roscpp</depend> -->
  <!--   Note that this is equivalent to the following: -->
  <!--   <build_depend>roscpp</build_depend> -->
  <!--   <exec_depend>roscpp</exec_depend> -->
  <!-- Use build_depend for packages you need at compile time: -->
  <!--   <build_depend>message_generation</build_depend> -->
  <!-- Use build_export_depend for packages you need in order to build against this package: -->
  <!--   <build_export_depend>message_generation</build_export_depend> -->
  <!-- Use buildtool_depend for build tool packages: -->
  <!--   <buildtool_depend>catkin</buildtool_depend> -->
  <!-- Use exec_depend for packages you need at runtime: -->
  <!--   <exec_depend>message_runtime</exec_depend> -->
  <!-- Use test_depend for packages you need only for testing: -->
  <!--   <test_depend>gtest</test_depend> -->
  <!-- Use doc_depend for packages you need only for building documentation: -->
  <!--   <doc_depend>doxygen</doc_depend> -->
  <!-- 依赖的构建工具，这是必须的 -->
  <buildtool_depend>catkin</buildtool_depend>

  <!-- 指定构建此软件包所需的软件包(编译时依赖的功能包) -->
  <build_depend>roscpp</build_depend>
  <build_depend>rospy</build_depend>
  <build_depend>std_msgs</build_depend>

  <!-- 指定根据这个包构建库所需要的包（导出依赖） -->
  <build_export_depend>roscpp</build_export_depend>
  <build_export_depend>rospy</build_export_depend>
  <build_export_depend>std_msgs</build_export_depend>

  <!-- 运行该程序包中的代码所需的程序包（执行依赖） -->  
  <exec_depend>roscpp</exec_depend>
  <exec_depend>rospy</exec_depend>
  <exec_depend>std_msgs</exec_depend>


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

  </export>
</package>

2.CMakelists.txt

文件CMakeLists.txt是CMake构建系统的输入，用于构建软件包。任何兼容CMake的软件包都包含一个或多个CMakeLists.txt文件，这些文件描述了如何构建代码以及将代码安装到何处。

cmake_minimum_required(VERSION 3.0.2) #所需 cmake 版本
project(demo01_hello_vscode) #包名称，会被 ${PROJECT_NAME} 的方式调用

## Compile as C++11, supported in ROS Kinetic and newer
# add_compile_options(-std=c++11)

## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
#设置构建所需要的软件包
find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
)

## System dependencies are found with CMake's conventions
#默认添加系统依赖
# find_package(Boost REQUIRED COMPONENTS system)


## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# 启动 python 模块支持
# catkin_python_setup()

################################################
## Declare ROS messages, services and actions ##
## 声明 ROS 消息、服务、动作... ##
################################################

## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
##   * add a build_depend tag for "message_generation"
##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
##     but can be declared for certainty nonetheless:
##     * add a exec_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
##   * add "message_generation" and every package in MSG_DEP_SET to
##     find_package(catkin REQUIRED COMPONENTS ...)
##   * add "message_runtime" and every package in MSG_DEP_SET to
##     catkin_package(CATKIN_DEPENDS ...)
##   * uncomment the add_*_files sections below as needed
##     and list every .msg/.srv/.action file to be processed
##   * uncomment the generate_messages entry below
##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)

## Generate messages in the 'msg' folder
# add_message_files(
#   FILES
#   Message1.msg
#   Message2.msg
# )

## Generate services in the 'srv' folder
# add_service_files(
#   FILES
#   Service1.srv
#   Service2.srv
# )

## Generate actions in the 'action' folder
# add_action_files(
#   FILES
#   Action1.action
#   Action2.action
# )

## Generate added messages and services with any dependencies listed here
# 生成消息、服务时的依赖包
# generate_messages(
#   DEPENDENCIES
#   std_msgs
# )

################################################
## Declare ROS dynamic reconfigure parameters ##
## 声明 ROS 动态参数配置 ##
################################################

## To declare and build dynamic reconfigure parameters within this
## package, follow these steps:
## * In the file package.xml:
##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
## * In this file (CMakeLists.txt):
##   * add "dynamic_reconfigure" to
##     find_package(catkin REQUIRED COMPONENTS ...)
##   * uncomment the "generate_dynamic_reconfigure_options" section below
##     and list every .cfg file to be processed

## Generate dynamic reconfigure parameters in the 'cfg' folder
# generate_dynamic_reconfigure_options(
#   cfg/DynReconf1.cfg
#   cfg/DynReconf2.cfg
# )

###################################
## catkin specific configuration ##
## catkin 特定配置##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if your package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
# 运行时依赖
catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES demo01_hello_vscode
#  CATKIN_DEPENDS roscpp rospy std_msgs
#  DEPENDS system_lib
)

###########
## Build ##
###########

## Specify additional locations of header files
## Your package locations should be listed before other locations
# 添加头文件路径，当前程序包的头文件路径位于其他文件路径之前
include_directories(
# include
  ${catkin_INCLUDE_DIRS}
)

## Declare a C++ library
# 声明 C++ 库
# add_library(${PROJECT_NAME}
#   src/${PROJECT_NAME}/demo01_hello_vscode.cpp
# )

## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
# 添加库的 cmake 目标依赖
# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
# 声明 C++ 可执行文件
add_executable(Hello_VSCode src/Hello_VSCode.cpp)

## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
## target back to the shorter version for ease of user use
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
#重命名c++可执行文件
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")

## Add cmake target dependencies of the executable
## same as for the library above
#添加可执行文件的 cmake 目标依赖
add_dependencies(Hello_VSCode ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

## Specify libraries to link a library or executable target against
#指定库、可执行文件的链接库
target_link_libraries(Hello_VSCode
  ${catkin_LIBRARIES}
)

#############
## Install ##
## 安装 ##
#############

# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html

## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
#设置用于安装的可执行脚本
catkin_install_python(PROGRAMS
  scripts/Hi.py
  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

## Mark executables for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
# install(TARGETS ${PROJECT_NAME}_node
#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )

## Mark libraries for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
# install(TARGETS ${PROJECT_NAME}
#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
#   RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
# )

## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
#   FILES_MATCHING PATTERN "*.h"
#   PATTERN ".svn" EXCLUDE
# )

## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
#   # myfile1
#   # myfile2
#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )

#############
## Testing ##
#############

## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_demo01_hello_vscode.cpp)
# if(TARGET ${PROJECT_NAME}-test)
#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()

## Add folders to be run by python nosetests
# catkin_add_nosetests(test)

2 ROS文件系统相关命令

ROS 的文件系统本质上都还是操作系统文件，我们可以使用Linux命令来操作这些文件，不过，在ROS中为了更好的用户体验，ROS专门提供了一些类似于Linux的命令，这些命令较之于Linux原生命令，更为简介、高效。文件操作，无外乎就是增删改查与执行等操作，接下来，我们就从这五个维度，来介绍ROS文件系统的一些常用命令。

1.增

catkin_create_pkg 自定义包名依赖包 === 创建新的ROS功能包

sudo apt install xxx === 安装 ROS功能包

2.删

sudo apt purge xxx ==== 删除某个功能包

3.查

rospack list === 列出所有功能包

rospack find 包名 === 查找某个功能包是否存在，如果存在返回安装路径

roscd 包名 === 进入某个功能包

rosls 包名 === 列出某个包下的文件

apt search xxx === 搜索某个功能包

4.改

rosed 包名文件名 === 修改功能包文件

需要安装 vim

**比如:**rosed turtlesim Color.msg

5.执行

5.1roscore

roscore === 是 ROS 的系统先决条件节点和程序的集合，必须运行 roscore 才能使 ROS 节点进行通信。

roscore 将启动:

ros master
ros 参数服务器
rosout 日志节点

用法:

roscore

或(指定端口号)

roscore -p xxxx

5.2rosrun

rosrun 包名可执行文件名 === 运行指定的ROS节点

比如:rosrun turtlesim turtlesim_node

5.3roslaunch

roslaunch 包名 launch文件名 === 执行某个包下的 launch 文件

3.ROS计算图

以 ROS 内置的小乌龟案例来演示计算图

首先，按照前面所示，运行案例；然后，启动新终端，键入: rqt_graph 或 rosrun rqt_graph rqt_graph，可以看到类似下图的网络拓扑图，该图可以显示不同节点之间的关系。

第一章.vscode 使用与基本配置

1.创建ROS工作空间

mkdir -p xxx_ws/src(必须得有 src)    //创建文件夹
cd xxx_ws                           //进入创建的该文件夹
catkin_make

2. 启动 vscode

cd xxx_ws    //进入该工作空间
code .        //打开vscode

3. vscode 中编译 ros

快捷键 ctrl + shift + B 调用编译,选择:catkin_make:build,点击后面的小齿轮

可以点击配置设置为默认，修改.vscode/tasks.json 文件

{
// 有关 tasks.json 格式的文档，请参见
    // https://go.microsoft.com/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"
        }
    ]
}

4. 创建 ROS 功能包

选定 src 右击 —> create catkin package

设置包名：

添加依赖：roscpp rospy std_msgs

5. C++ 实现

在功能包的 src 下新建 cpp 文件

#include "ros/ros.h"

int main(int argc, char *argv[])
{
    //解决乱码
    setlocale(LC_ALL,"");

    //初始化
    ros::init(argc,argv,"hello_c");    //此行中的hello_c为节点名称
    ROS_INFO("hello,哈哈");    //日志输出
    return 0;
}

PS1: 如果没有代码提示

修改 .vscode/c_cpp_properties.json

设置 “cppStandard”: “c++17”

PS2: main 函数的参数不可以被 const 修饰

PS3: 当ROS__INFO 终端输出有中文时，会出现乱码

INFO：???

解决办法：在函数开头加入下面代码的任意一句：

setlocale(LC_CTYPE, "zh_CN.utf8");
setlocale(LC_ALL, "");

PS4:CMakeLists.txt文件vscode中显示为纯文本，可点击右下角纯文本修改为Makefile

6. python 实现

在功能包(packages) 下新建 scripts 文件夹，添加 python 文件，并添加可执行权限

#! /usr/bin/env python    

#导包
import rospy

#入口
if __name__ == "__main__":
    #初始化ros节点
    rospy.init_node("hello_p")
    #输出日志
    rospy.loginfo("hello vscode!这是python！")

#! /usr/bin/env python

#现象：当不配置CMakeList.txt 执行python会抛出异常：
#
#原因：当前noetic 使用的是 python3
#解决：
#   1.直接声明解释器为 python3（不建议，可能调用的是别人的实现，不能直接修改别人的代码）
#   2.通过软链接，将 python 链接到 python3 上(建议)
#       sudo ln -s /usr/bin/python3 /usr/bin/python

#导包
import rospy

#入口
if __name__ == "__main__":
    #初始化ros节点
    rospy.init_node("hello_p")
    #输出日志
    rospy.loginfo("hello vscode!这是python！222222")

添加可执行权限:选定scripts目录，右击，在终端中打开：

给一个文件添加可执行权限：

chmod +x 自定义文件名.py

给所有python文件添加可执行权限：

chmod +x *.py

7. 配置 CMakeLists.txt

C++ 配置:

add_executable(节点名称 src/C++源文件名.cpp)

target_link_libraries(节点名称
  ${catkin_LIBRARIES}
)

python配置：

catkin_install_python(PROGRAMS 
  scripts/自定义文件名.py
  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

8. 执行

在vscode中添加一个新终端，刚好定位至当前工作空间下：

roscore

再添加一个终端：

source ./devel/setup.bash
rosrun 包名 C++(无拓展名)/python(有拓展名)节点

9.launch文件演示

1.需求

一个程序中可能需要启动多个节点，比如:ROS 内置的小乌龟案例，如果要控制乌龟运动，要启动多个窗口，分别启动 roscore、乌龟界面节点、键盘控制节点。如果每次都调用 rosrun 逐一启动，显然效率低下，如何优化?

官方给出的优化策略是使用 launch 文件，可以一次性启动多个 ROS 节点。

2.实现

(1)选定功能包右击 —> 添加 launch 文件夹

(2)选定 launch 文件夹右击 —> 添加 launch 文件(拓展名：launch)

(3)编辑 launch 文件内容

<launch>
    <!-- 添加被执行的节点 -->
    <!-- 乌龟GUI -->
    <node pkg = "hello_vscode" type = "hello_vscode_c" name ="hello" output = "screen" />
    <node pkg = "turtlesim" type = "turtlesim_node" name = "turtle_GUI" />
    <node pkg = "turtlesim" type ="turtle_teleop_key" name ="turtle_key" />

</launch>

node —> 包含的某个节点
pkg -----> 功能包
type ----> 被运行的节点文件
name --> 为节点命名
output-> 设置日志的输出目标

(4)运行 launch 文件

roslaunch 包名 launch文件名

(5)运行结果: 一次性启动了多个节点

第二章 ROS通信机制

ROS 中的基本通信机制主要有如下三种实现策略:

话题通信(发布订阅模式)
服务通信(请求响应模式)
参数服务器(参数共享模式)

2.1 话题通信

话题通信是ROS中使用频率最高的一种通信模式，话题通信是基于发布订阅模式的，也即:一个节点发布消息，另一个节点订阅该消息。话题通信的应用场景也极其广泛，比如下面一个常见场景:

机器人在执行导航功能，使用的传感器是激光雷达，机器人会采集激光雷达感知到的信息并计算，然后生成运动控制信息驱动机器人底盘运动。

在上述场景中，就不止一次使用到了话题通信。

以激光雷达信息的采集处理为例，在 ROS 中有一个节点需要时时的发布当前雷达采集到的数据，导航模块中也有节点会订阅并解析雷达数据。
再以运动消息的发布为例，导航模块会根据传感器采集的数据时时的计算出运动控制信息并发布给底盘，底盘也可以有一个节点订阅运动信息并最终转换成控制电机的脉冲信号。

以此类推，像雷达、摄像头、GPS… 等等一些传感器数据的采集，也都是使用了话题通信，换言之，话题通信适用于不断更新的数据传输相关的应用场景。

2.1.1 话题通信理论模型

话题通信实现模型是比较复杂的，该模型如下图所示,该模型中涉及到三个角色:

ROS Master (管理者)
Talker (发布者)
Listener (订阅者)

ROS Master 负责保管 Talker 和 Listener 注册的信息，并匹配话题相同的 Talker 与 Listener，帮助 Talker 与 Listener 建立连接，连接建立后，Talker 可以发布消息，且发布的消息会被 Listener 订阅。话题通信

2.1.2 话题通信基本操作A(C++)

需求:

编写发布订阅实现，要求发布方以1HZ(每秒1次)的频率发布文本消息，订阅方订阅消息并将消息内容打印输出。

分析:

在模型实现中，ROS master 不需要实现，而连接的建立也已经被封装了，需要关注的关键点有三个:

发布方
接收方
数据(此处为普通文本)

流程:

编写发布方实现；
编写订阅方实现；
编辑配置文件；
编译并执行。

（1）发布方

最基本的发布方实现：

#include "ros/ros.h"
#include "std_msgs/String.h"
/*
    发布方实现：
        1.包含头文件
            ros中文本类型数据：std_msgs/String.h
        2.初始化ROS节点
        3.创建节点句柄
        4.创建发布者对象
        5.编写发布逻辑并发布数据
*/
int main(int argc, char *argv[])
{
    //2.初始化ROS节点
    ros::init(argc,argv,"talker");  //talker为节点名称
    //3.创建节点句柄
    ros::NodeHandle nh;
    //4.创建发布者对象
    ros::Publisher pub = nh.advertise<std_msgs::String>("huati_name",10);//此处泛型为被发布的类型，huati_name为话题名称，10为队列长度
    //5.编写发布逻辑并发布数据
    //先创建被发布的消息
    std_msgs::String msg;
    //编写循环，循环中发布数据
    while(ros::ok())    //ros::ok()意思：如果节点还活着，循环就成立
    {
        msg.data = "hello";
        pub.publish(msg);

        ros::spinOnce();    //官方建议,处理回调函数
    }
    return 0;
}

运行之后，没有打印数据，可调用 rostopic echo 话题名称（此处为huati_name）显示是否正确。

完善之后的版本：

#include "ros/ros.h"
#include "std_msgs/String.h"
#include <sstream>
/*
    发布方实现：
        1.包含头文件
            ros中文本类型数据：std_msgs/String.h
        2.初始化ROS节点
        3.创建节点句柄
        4.创建发布者对象
        5.编写发布逻辑并发布数据
*/
int main(int argc, char *argv[])
{
    //解决中文乱码
    setlocale(LC_ALL,"");
    //2.初始化ROS节点
    ros::init(argc,argv,"talker");  //talker为节点名称
    //3.创建节点句柄
    ros::NodeHandle nh;
    //4.创建发布者对象
    ros::Publisher pub = nh.advertise<std_msgs::String>("huati_name",10);
    //5.编写发布逻辑并发布数据

    //要求以 1Hz 的频率发布数据，并且文本后添加编号
    //先创建被发布的消息
    std_msgs::String msg;
    //发布频率
    ros::Rate rate(1);
    //设置编号
    int count = 0;
    //编写循环，循环中发布数据
    /*
           订阅时，第一条数据丢失
        原因: 发送第一条数据时， publisher 还未在 roscore 注册完毕
        解决: 注册后，加入休眠 ros::Duration(3.0).sleep(); 延迟第一条数据的发送
    */
    ros::Duration(3.0).sleep();
    while(ros::ok())    //ros::ok()意思：如果节点还活着，循环就成立
    {
        count++;
        //拼接到hello后面
        //msg.data = "hello";
        //实现字符串拼接数字，需使用头文件<sstream>
        std::stringstream ss;
        ss << "hello --> " << count;
        msg.data = ss.str();

        pub.publish(msg);
        //日志输出
        ROS_INFO("发布的数据是：%s",ss.str().c_str());

        rate.sleep();
        ros::spinOnce();    //官方建议,处理回调函数
    }
    return 0;
}

（2）订阅方

#include "ros/ros.h"
#include "std_msgs/String.h"
/*
    订阅方实现：
        1.包含头文件
            ros中文本类型数据：std_msgs/String.h
        2.初始化ROS节点
        3.创建节点句柄
        4.创建订阅者对象
        5.处理订阅到的数据
        6.spin()函数

*/

//传入的参数为订阅到的消息，为std_msgs下的String类型，并且为常量指针的引用
void doMsg(const std_msgs::String::ConstPtr &msg)
{
    //通过msg获取并操作订阅到的数据
    ROS_INFO("订阅的数据:%s",msg->data.c_str());
}

int main(int argc, char *argv[])
{
    //解决中文乱码
    setlocale(LC_ALL,"");
    //2.初始化ROS节点
    ros::init(argc,argv,"listener");  //listener为节点名称
    //3.创建节点句柄
    ros::NodeHandle nh;
    //4.创建订阅者对象
    ros::Subscriber sub = nh.subscribe("huati_name",10,doMsg);
    //5.处理订阅到的数据

    //6.spin()函数
    ros::spin();    //回头，调用回调函数，回调函数需要被频繁执行
    return 0;
}

（3）配置 CMakeLists.txt

add_executable(demo_pub src/demo_pub.cpp)
add_executable(demo_sub src/demo_sub.cpp)

target_link_libraries(demo_pub
  ${catkin_LIBRARIES}
)
target_link_libraries(demo_sub
  ${catkin_LIBRARIES}
)

（4）执行

1.启动 roscore;

2.启动发布节点;

3.启动订阅节点。

（5）注意

订阅时，第一条数据丢失

原因: 发送第一条数据时， publisher 还未在 roscore 注册完毕

解决: 注册后，加入休眠 ros::Duration(3.0).sleep(); 延迟第一条数据的发送

（6）使用 rqt_graph 查看节点关系

2.1.3 话题通信基本操作B(Python)

需求:

编写发布订阅实现，要求发布方以10HZ(每秒10次)的频率发布文本消息，订阅方订阅消息并将消息内容打印输出。

分析:

在模型实现中，ROS master 不需要实现，而连接的建立也已经被封装了，需要关注的关键点有三个:

发布方
接收方
数据(此处为普通文本)

流程:

编写发布方实现；
编写订阅方实现；
为python文件添加可执行权限；
编辑配置文件；
编译并执行。

（1）发布方

最基本的发布方实现：

#! /usr/bin/env python

import rospy
from std_msgs.msg import String #发布的消息的类型
"""
    使用 python 实现消息发布:
        1.导包
        2.初始化 ros 节点
        3.创建发布者对象
        4.编写发布逻辑并发布数据
"""

if __name__ == "__main__":

    #2.初始化 ros 节点
    rospy.init_node("talker")   #传入的节点的名称
    #3.创建发布者对象
    pub = rospy.Publisher("huati_name", String, queue_size=10)
    #4.编写发布逻辑并发布数据
    #创建数据
    msg = String()
    #使用循环发布数据
    while not rospy.is_shutdown():
        msg.data = "hello"
        #发布数据
        pub.publish(msg)

    pass

运行之后，没有打印数据，可调用 rostopic echo 话题名称（此处为huati_name）显示是否正确。

完善之后的版本：

#! /usr/bin/env python

import rospy
from std_msgs.msg import String #发布的消息的类型
"""
    使用 python 实现消息发布:
        1.导包
        2.初始化 ros 节点
        3.创建发布者对象
        4.编写发布逻辑并发布数据
"""

if __name__ == "__main__":

    #2.初始化 ros 节点
    rospy.init_node("talker")   #传入的节点的名称
    #3.创建发布者对象
    pub = rospy.Publisher("huati_name", String, queue_size=10)
    #4.编写发布逻辑并发布数据
    #创建数据
    msg = String()
    #指定发布频率
    rate = rospy.Rate(1)
    #设置计数器
    count = 0
    #使用循环发布数据
    while not rospy.is_shutdown():
        count += 1
        msg.data = "hello" + str(count) #str()函数将数字转换为字符串
        #发布数据
        pub.publish(msg)
        rospy.loginfo("发布的数据是: %s ",msg.data)

        rate.sleep()

    pass

（2）订阅方

#! /usr/bin/env python

import rospy
from std_msgs.msg import String #发布的消息的类型
"""
    使用 python 实现消息订阅:
        1.导包
        2.初始化 ros 节点
        3.创建订阅者对象
        4.回调函数处理数据
        5.spin()
"""
def doMsg(msg):
    rospy.loginfo("订阅的数据: %s ", msg.data)


if __name__ == "__main__":

    #2.初始化 ros 节点
    rospy.init_node("listener")
    #3.创建订阅者对象
    sub = rospy.Subscriber("huati_name", String, doMsg, queue_size=10)
    #4.回调函数处理数据
    #5.spin()
    rospy.spin()
    pass

（3）配置 CMakeLists.txt

catkin_install_python(PROGRAMS
  scripts/demo_pub.py
  scripts/demo_sub.py
  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

（4）添加可执行权限

终端下进入 scripts 执行:chmod +x *.py

（5）执行

1.启动 roscore;

2.启动发布节点;

3.启动订阅节点。

2.1.4 话题通信自定义msg

在 ROS 通信协议中，数据载体是一个较为重要组成部分，ROS 中通过 std_msgs 封装了一些原生的数据类型,比如:String、Int32、Int64、Char、Bool、Empty… 但是，这些数据一般只包含一个 data 字段，结构的单一意味着功能上的局限性，当传输一些复杂的数据，比如: 激光雷达的信息… std_msgs 由于描述性较差而显得力不从心，这种场景下可以使用自定义的消息类型。

msgs只是简单的文本文件，每行具有字段类型和字段名称，可以使用的字段类型有：

int8, int16, int32, int64 (或者无符号类型: uint*)
float32, float64
string
time, duration
other msg files
variable-length array[] and fixed-length array[C]

ROS中还有一种特殊类型：Header，标头包含时间戳和ROS中常用的坐标帧信息。会经常看到msg文件的第一行具有Header标头。

**需求:**创建自定义消息，该消息包含人的信息:姓名、身高、年龄等。

流程:

按照固定格式创建 msg 文件
编辑配置文件
编译生成可以被 Python 或 C++ 调用的中间文件

（1）定义msg文件

功能包下新建 msg 目录，添加文件 Person.msg

string name
int32 age
float32 height

（2）编辑配置文件

package.xml中添加编译依赖与执行依赖：

已插入的为：

  <buildtool_depend>catkin</buildtool_depend>
  <build_depend>roscpp</build_depend>
  <build_depend>rospy</build_depend>
  <build_depend>std_msgs</build_depend>

  <build_depend>message_generation</build_depend>

  <build_export_depend>roscpp</build_export_depend>
  <build_export_depend>rospy</build_export_depend>
  <build_export_depend>std_msgs</build_export_depend>

  <exec_depend>roscpp</exec_depend>
  <exec_depend>rospy</exec_depend>
  <exec_depend>std_msgs</exec_depend>

  <exec_depend>message_runtime</exec_depend>

插入的为：

  <build_depend>message_generation</build_depend>
  <exec_depend>message_runtime</exec_depend>

CMakeLists.txt编辑 msg 相关配置：

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs

  message_generation    ##添加的部分
)

## 配置 msg 源文件
add_message_files(
  FILES
  Person.msg
)

# 生成消息时依赖于 std_msgs
generate_messages(
  DEPENDENCIES
  std_msgs
)

#执行时依赖
catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES demo02_talker_listener
  CATKIN_DEPENDS roscpp rospy std_msgs message_runtime
#  DEPENDS system_lib
)

（3）编译

编译后的中间文件查看:

C++ 需要调用的中间文件(…/工作空间/devel/include/包名/xxx.h)

中间文件1

Python 需要调用的中间文件(…/工作空间/devel/lib/python3/dist-packages/包名/msg)

在这里插入图片描述

后续调用相关 msg 时，是从这些中间文件调用的。

2.1.5 话题通信自定义msg调用A(C++)

需求:

编写发布订阅实现，要求发布方以1HZ(每秒10次)的频率发布自定义消息，订阅方订阅自定义消息并将消息内容打印输出。

分析:

在模型实现中，ROS master 不需要实现，而连接的建立也已经被封装了，需要关注的关键点有三个:

发布方
接收方
数据**(此处为自定义消息**)

流程:

编写发布方实现；
编写订阅方实现；
编辑配置文件；
编译并执行。

0.vscode 配置

为了方便代码提示以及避免误抛异常，需要先配置 vscode，将前面生成的 head 文件路径配置进 c_cpp_properties.json 的 includepath属性:

{
    "configurations": [
        {
            "browse": {
                "databaseFilename": "",
                "limitSymbolsToIncludedHeaders": true
            },
            "includePath": [
                "/opt/ros/noetic/include/**",
                "/usr/include/**",
                "/xxx/yyy工作空间/devel/include/**" //配置 head 文件的路径 
            ],
            "name": "ROS",
            "intelliSenseMode": "gcc-x64",
            "compilerPath": "/usr/bin/gcc",
            "cStandard": "c11",
            "cppStandard": "c++17"
        }
    ],
    "version": 4
}

具体操作步骤为：如下图所示，找到工作空间下/devel/include，将鼠标点在topic_pub_sub即包名的位置，右击鼠标调出终端，键入pwd，把路径复制下来，粘贴至"includePath":路径下，可以不具体到包名而用**代替。

在这里插入图片描述

1.发布方

#include "ros/ros.h"
#include "topic_pub_sub/Person.h"
/*
    发布方实现：
        1.包含头文件
            ros中文本类型数据：topic_pub_sub/Person.h
        2.初始化ROS节点
        3.创建节点句柄
        4.创建发布者对象
        5.编写发布逻辑并发布数据

*/
int main(int argc, char *argv[])
{
    //解决中文乱码
    setlocale(LC_ALL,"");
    ROS_INFO("这是消息的发布方: ");
    //2.初始化ROS节点
    ros::init(argc,argv,"talker");  //talker为节点名称
    //3.创建节点句柄
    ros::NodeHandle nh;
    //4.创建发布者对象
    ros::Publisher pub = nh.advertise<topic_pub_sub::Person>("huati_name",10);
    //5.编写发布逻辑并发布数据
    //5.1创建被发布的数据
    topic_pub_sub::Person person;
    person.name = "张三";
    person.age = 21;
    person.height = 1.75;
    //5.2设置发布频率b
    ros::Rate rate(1);
    //5.3循环发布数据
    while(ros::ok())
    {
        //修改数据
        person.age += 1;
        //核心：发布数据
        pub.publish(person);
        ROS_INFO("发布的消息: %s %d %.2f ", person.name.c_str(), person.age, person.height);

        //休眠
        rate.sleep();
        //建议
        ros::spinOnce();
    }

    return 0;
}

2.订阅方

#include "ros/ros.h"
#include "topic_pub_sub/Person.h"
/*
    订阅方实现：
        1.包含头文件
            ros中文本类型数据：std_msgs/String.h
        2.初始化ROS节点
        3.创建节点句柄
        4.创建订阅者对象
        5.处理订阅到的数据
        6.spin()函数

*/

//传入的参数为订阅到的消息，为std_msgs下的String类型，并且为常量指针的引用
void doPerson(const topic_pub_sub::Person::ConstPtr &person)
{
    //通过person(即常量指针的引用)获取并操作订阅到的数据
    ROS_INFO("订阅的数据:%s %d %.2f ",person->name.c_str(),person->age,person->height);
}

int main(int argc, char *argv[])
{
    //解决中文乱码
    setlocale(LC_ALL,"");
    ROS_INFO("这是消息的订阅方: ");
    //2.初始化ROS节点
    ros::init(argc,argv,"listener");  //listener为节点名称
    //3.创建节点句柄
    ros::NodeHandle nh;
    //4.创建订阅者对象
    ros::Subscriber sub = nh.subscribe("huati_name",10,doPerson);
    //5.处理订阅到的数据

    //6.spin()函数
    ros::spin();    //回头，调用回调函数，回调函数需要被频繁执行
    return 0;
}

3.配置 CMakeLists.txt

需要添加 add_dependencies 用以设置所依赖的消息相关的中间文件。

add_executable(demo_pub_person src/demo_pub_person.cpp)

add_dependencies(demo_pub_person ${PROJECT_NAME}_generate_messages_cpp)

target_link_libraries(demo_pub_person
  ${catkin_LIBRARIES}
)

add_executable(demo_sub_person src/demo_sub_person.cpp)

add_dependencies(demo_sub_person ${PROJECT_NAME}_generate_messages_cpp)

target_link_libraries(demo_sub_person
  ${catkin_LIBRARIES}
)

其中，add_dependencies(demo_pub_person ${PROJECT_NAME}_generate_messages_cpp)

可以保证调用时的依赖关系，其中一种情况是，编写完msg文件，但是没有编译，然后编写cpp源文件，编写完后统一编译，编译器先编译.cpp文件，但是.cpp文件依赖于自定义文件#include "topic_pub_sub/Person.h"，还没编译的话无此中间文件，为避免这种问题，所以配置add_dependencies。

4.执行

1.启动 roscore;

2.启动发布节点;

3.启动订阅节点。

2.1.6 话题通信自定义msg调用B(Python)

需求:

编写发布订阅实现，要求发布方以1HZ(每秒1次)的频率发布自定义消息，订阅方订阅自定义消息并将消息内容打印输出。

分析:

在模型实现中，ROS master 不需要实现，而连接的建立也已经被封装了，需要关注的关键点有三个:

发布方
接收方
数据(此处为自定义消息)

流程:

编写发布方实现；
编写订阅方实现；
为python文件添加可执行权限；
编辑配置文件；
编译并执行。

0.vscode配置

为了方便代码提示以及误抛异常，需要先配置 vscode，将前面生成的 python 文件路径配置进 settings.json

{
    "python.autoComplete.extraPaths": [
        "/opt/ros/noetic/lib/python3/dist-packages",
        "/xxx/yyy工作空间/devel/lib/python3/dist-packages"
    ]
}

具体操作步骤为：如下图所示，找到工作空间下/devel/lib/python3，将鼠标点在dist-packages的位置，右击鼠标调出终端，键入pwd，把路径复制下来，粘贴至"python.autoComplete.extraPaths"路径下。

1.发布方

#! /usr/bin/env python

import rospy
from topic_pub_sub.msg import Person #发布的消息的类型
"""
    使用 python 实现消息发布:
        1.导包
        2.初始化 ros 节点
        3.创建发布者对象
        4.编写发布逻辑并发布数据
"""

if __name__ == "__main__":

    #2.初始化 ros 节点
    rospy.init_node("talker")   #传入的节点的名称
    #3.创建发布者对象
    pub = rospy.Publisher("huati_name", Person, queue_size=10)
    #4.编写发布逻辑并发布数据
    #4.1创建Person数据
    person = Person()
    person.name = "奥特曼"
    person.age = 180
    person.height = 256
    #4.2指定发布频率
    rate = rospy.Rate(1)
    #4.3使用循环发布数据
    rospy.sleep(2)  #发布前先休眠2秒
    while not rospy.is_shutdown():
        #发布数据
        pub.publish(person)
        rospy.loginfo("发布的消息: %s %d %d ",person.name, person.age,person.height)

        rate.sleep()

pass

2.订阅方

#! /usr/bin/env python

import rospy
from topic_pub_sub.msg import Person #发布的消息的类型
"""
    使用 python 实现消息订阅:
        1.导包
        2.初始化 ros 节点
        3.创建订阅者对象
        4.回调函数处理数据
        5.spin()
"""
def doPerson(person):
    rospy.loginfo("订阅的数据: %s %d %d", person.name,person.age,person.height)


if __name__ == "__main__":

    #2.初始化 ros 节点
    rospy.init_node("listener")
    #3.创建订阅者对象
    sub = rospy.Subscriber("huati_name", Person, doPerson, queue_size=10)
    #4.回调函数处理数据
    #5.spin()
    rospy.spin()

pass

3.权限设置

终端下进入 scripts 执行:chmod +x *.py

4.配置 CMakeLists.txt

catkin_install_python(PROGRAMS
  scripts/demo_pub.py
  scripts/demo_sub.py
  scripts/demo_pub_person.py
  scripts/demo_sub_person.py
  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

5.执行

1.启动 roscore;

2.启动发布节点;

3.启动订阅节点。

2.2 服务通信

服务通信也是ROS中一种极其常用的通信模式，服务通信是基于请求响应模式的，是一种应答机制。也即: 一个节点A向另一个节点B发送请求，B接收处理请求并产生响应结果返回给A。比如如下场景:

机器人巡逻过程中，控制系统分析传感器数据发现可疑物体或人… 此时需要拍摄照片并留存。

在上述场景中，就使用到了服务通信。

一个节点需要向相机节点发送拍照请求，相机节点处理请求，并返回处理结果

与上述应用类似的，服务通信更适用于对时时性有要求、具有一定逻辑处理的应用场景。

概念

以请求响应的方式实现不同节点之间数据交互的通信模式。

作用

用于偶然的、对时时性有要求、有一定逻辑处理需求的数据传输场景。

2.2.1 服务通信理论模型

服务通信较之于话题通信更简单些，理论模型如下图所示，该模型中涉及到三个角色:

ROS master(管理者)
Server(服务端)
Client(客户端)

ROS Master 负责保管 Server 和 Client 注册的信息，并匹配话题相同的 Server 与 Client ，帮助 Server 与 Client 建立连接，连接建立后，Client 发送请求信息，Server 返回响应信息。

服务通信

2.2.2 服务通信自定义srv

需求:

服务通信中，客户端提交两个整数至服务端，服务端求和并响应结果到客户端，请创建服务器与客户端通信的数据载体。

流程:

srv 文件内的可用数据类型与 msg 文件一致，且定义 srv 实现流程与自定义 msg 实现流程类似:

按照固定格式创建srv文件
编辑配置文件
编译生成中间文件

（1）定义srv文件

服务通信中，数据分成两部分，请求与响应，在 srv 文件中请求和响应使用---分割，具体实现如下:

功能包下新建 srv 目录，添加 AddInts.srv 文件，内容:

# 客户端请求时发送的两个数字
int32 num1
int32 num2
---
# 服务器响应发送的数据
int32 sum

（2）编辑配置文件

package.xml中添加编译依赖与执行依赖：

<build_depend>message_generation</build_depend>
<exec_depend>message_runtime</exec_depend>

CMakeLists.txt编辑 srv 相关配置：

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs

  message_generation
)
# 需要加入 message_generation,必须有 std_msgs

## Generate services in the 'srv' folder
add_service_files(
  FILES
  AddInts.srv
)

## Generate added messages and services with any dependencies listed here
generate_messages(
  DEPENDENCIES
  std_msgs
)

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

（3）编译

编译后的中间文件查看:

C++ 需要调用的中间文件(…/工作空间/devel/include/包名/xxx.h)
在这里插入图片描述

Python 需要调用的中间文件(…/工作空间/devel/lib/python3/dist-packages/包名/srv)

2.2.3 服务通信自定义srv调用A(C++)

需求:

编写服务通信，客户端提交两个整数至服务端，服务端求和并响应结果到客户端。

分析:

在模型实现中，ROS master 不需要实现，而连接的建立也已经被封装了，需要关注的关键点有三个:

服务端
客户端
数据

流程:

编写服务端实现；
编写客户端实现；
编辑配置文件；
编译并执行。

0.vscode配置

需要像之前自定义 msg 实现一样配置c_cpp_properies.json 文件，如果以前已经配置且没有变更工作空间，可以忽略，如果需要配置，配置方式与之前相同。

1.服务端

#include "ros/ros.h"
#include "service_server_client/AddInts.h"

/*
    服务端实现：解析客户端提交的数据，并运算产生响应

        1.包含头文件
        2.初始化ros节点
        3.创建节点句柄
        4.创建一个服务对象
        5.处理请求并产生响应
        6.spin()

*/

bool doNums(service_server_client::AddInts::Request &request,
            service_server_client::AddInts::Response &response)
{
    //1.处理请求
    int num1 = request.num1;
    int num2 = request.num2;
    ROS_INFO("收到的请求数据:num1 = %d, num2 = %d ",num1,num2);

    //2.组织响应
    int sum = num1 + num2;
    response.sum = sum; 
    ROS_INFO("求和结果:sum = %d",sum);

    return true;
}

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    //2.初始化ros节点
    ros::init(argc,argv,"server");  //节点名称必须保证唯一
    //3.创建节点句柄
    ros::NodeHandle nh;
    //4.创建一个服务对象
    ros::ServiceServer server = nh.advertiseService("addInts",doNums);
    ROS_INFO("服务器端启动");
    //5.处理请求并产生响应
    //6.spin()
    ros::spin();

    return 0;
}

可以通过使用命令rosservice call 话题名来测试服务器端运行是否正确。

2.客户端

#include "ros/ros.h"
#include "service_server_client/AddInts.h"

/*
    客户端：提交两个整数，并处理响应的结果

        1.包含头文件
        2.初始化ros节点
        3.创建节点句柄
        4.创建一个客户端对象
        5.提交请求并处理响应

*/

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    //2.初始化ros节点
    ros::init(argc,argv,"client");  //节点名称必须保证唯一
    //3.创建节点句柄
    ros::NodeHandle nh;
    //4.创建一个客户端对象
    ros::ServiceClient client =  nh.serviceClient<service_server_client::AddInts>("addInts");
    //5.处理请求并产生响应
    service_server_client::AddInts ai;
    //5-1组织请求
    ai.request.num1 = 100;
    ai.request.num2 = 200;
    //5-2处理响应
    bool flag = client.call(ai);
    if(flag)
    {
        ROS_INFO("响应成功!");
        //获取结果
        ROS_INFO("响应结果: sum = %d",ai.response.sum);
    } 
    else
    {
        ROS_INFO("处理失败....");
    }

    return 0;
}

#include "ros/ros.h"
#include "service_server_client/AddInts.h"

/*
    客户端：提交两个整数，并处理响应的结果

        1.包含头文件
        2.初始化ros节点
        3.创建节点句柄
        4.创建一个客户端对象
        5.提交请求并处理响应

    实现参数的动态提交：
        1.运行时格式：rosrun 包名 节点名 num1 num2
        2.节点执行时，需获取命令中的参数并组织进 request

*/

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    //优化实现，获取命令中的参数
    if(argc != 3)
    {
        ROS_INFO("提交的参数个数不符合要求,请更改参数个数!");
        return 1;
    }

    //2.初始化ros节点
    ros::init(argc,argv,"client");  //节点名称必须保证唯一
    //3.创建节点句柄
    ros::NodeHandle nh;
    //4.创建一个客户端对象
    ros::ServiceClient client =  nh.serviceClient<service_server_client::AddInts>("addInts");
    //5.处理请求并产生响应
    service_server_client::AddInts ai;
    //5-1组织请求
    ai.request.num1 = atoi(argv[1]);    //atui():把字符串转换成整型数的一个函数
    ai.request.num2 = atoi(argv[2]);
    //5-2处理响应
    bool flag = client.call(ai);
    if(flag)
    {
        ROS_INFO("响应成功!");
        //获取结果
        ROS_INFO("响应结果: sum = %d",ai.response.sum);
    } 
    else
    {
        ROS_INFO("处理失败....");
    }

    return 0;
}

#include "ros/ros.h"
#include "service_server_client/AddInts.h"

/*
    客户端：提交两个整数，并处理响应的结果

        1.包含头文件
        2.初始化ros节点
        3.创建节点句柄
        4.创建一个客户端对象
        5.提交请求并处理响应

    实现参数的动态提交：
        1.运行时格式：rosrun 包名 节点名 num1 num2
        2.节点执行时，需获取命令中的参数并组织进 request

    问题：
        如果先启动客户端，那么会请求异常
    需求：
        如果先启动客户端，不要直接抛出异常，而是挂起，等服务器启动后，再正常请求
    解决：
        在ROS中内置了相关函数，可让客户端启动后挂起，等待服务器启动
        client.waitForExistence()
        ros::service::waitForService("服务的话题")
*/

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    //优化实现，获取命令中的参数
    if(argc != 3)
    {
        ROS_INFO("提交的参数个数不符合要求,请更改参数个数!");
        return 1;
    }

    //2.初始化ros节点
    ros::init(argc,argv,"client");  //节点名称必须保证唯一
    //3.创建节点句柄
    ros::NodeHandle nh;
    //4.创建一个客户端对象
    ros::ServiceClient client =  nh.serviceClient<service_server_client::AddInts>("addInts");
    //5.处理请求并产生响应
    service_server_client::AddInts ai;
    //5-1组织请求
    ai.request.num1 = atoi(argv[1]);    //atui():把字符串转换成整型数的一个函数
    ai.request.num2 = atoi(argv[2]);
    //5-2处理响应
    //调用判断服务器状态函数
    //函数1
    //client.waitForExistence();
    //函数2
    ros::service::waitForService("addInts");
    bool flag = client.call(ai);
    if(flag)
    {
        ROS_INFO("响应成功!");
        //获取结果
        ROS_INFO("响应结果: sum = %d",ai.response.sum);
    } 
    else
    {
        ROS_INFO("处理失败....");
    }

    return 0;
}

3.配置 CMakeLists.txt

add_executable(demo_server src/demo_server.cpp)
add_executable(demo_client src/demo_client.cpp)

add_dependencies(demo_server ${PROJECT_NAME}_gencpp)
add_dependencies(demo_client ${PROJECT_NAME}_gencpp)

target_link_libraries(demo_server
  ${catkin_LIBRARIES}
)
target_link_libraries(demo_client
  ${catkin_LIBRARIES}
)

4.执行

流程:

需要先启动服务:rosrun 包名服务
然后再调用客户端 :rosrun 包名客户端参数1 参数2

结果:

会根据提交的数据响应相加后的结果。

注意:

如果先启动客户端，那么会导致运行失败

优化:

在客户端发送请求前添加:client.waitForExistence();

或:ros::service::waitForService("AddInts");这是一个阻塞式函数，只有服务启动成功后才会继续执行

此处可以使用 launch 文件优化，但是需要注意 args 传参特点。

2.2.4 服务通信自定义srv调用B(Python)

需求:

编写服务通信，客户端提交两个整数至服务端，服务端求和并响应结果到客户端。

分析:

在模型实现中，ROS master 不需要实现，而连接的建立也已经被封装了，需要关注的关键点有三个:

服务端
客户端
数据

流程:

编写服务端实现；
编写客户端实现；
为python文件添加可执行权限；
编辑配置文件；
编译并执行。

0.vscode配置

需要像之前自定义 msg 实现一样配置settings.json 文件，如果以前已经配置且没有变更工作空间，可以忽略，如果需要配置，配置方式与之前相同。

1.服务端

#! /usr/bin/env python
import rospy
from service_server_client.srv import AddInts,AddIntsRequest,AddIntsResponse
"""
    服务端:解析客户端请求,产生响应。
        1.导包
        2.初始化ros节点
        3.创建服务端对象
        4.处理逻辑编写(回调函数)
        5.spin()
"""

#参数:封装的请求数据
#返回值：响应数据
def doNum(request):
    #1.解析提交的两个整数
    num1 = request.num1
    num2 = request.num2
    #2.求和
    sum = num1 +num2
    #3.将结果封装进响应
    response = AddIntsResponse()
    response.sum = sum

    rospy.loginfo("服务器解析的数据: num1 = %d num2 = %d sum = %d",num1, num2, sum)

    return response

if __name__ == "__main__":
    #2.初始化ros节点
    rospy.init_node("server")
    #3.创建服务端对象
    server = rospy.Service("addInts", AddInts, doNum)
    rospy.loginfo("服务器已启动!")
    #4.处理逻辑编写(回调函数)
    #5.spin()
    rospy.spin()

    pass

可以通过使用命令rosservice call 话题名来测试服务器端运行是否正确。

2.客户端

#! /usr/bin/env python
import rospy
from service_server_client.srv import AddInts,AddIntsRequest,AddIntsResponse
"""
    客户端:组织并提交请求,处理服务响应
        1.导包
        2.初始化ros节点
        3.创建客户端对象
        4.组织请求数据并发送请求
        5.处理响应
"""

if __name__ == "__main__":
    #2.初始化ros节点
    rospy.init_node("client")
    #3.创建客户端对象
    client = rospy.ServiceProxy("addInts", AddInts)
    #4.组织请求数据并发送请求
    response = client.call(12,23)
    #5.处理响应
    rospy.loginfo("响应的数据:sum = %d",response.sum)

    pass

#! /usr/bin/env python
import rospy
from service_server_client.srv import AddInts,AddIntsRequest,AddIntsResponse
import sys

"""
    客户端:组织并提交请求,处理服务响应
        1.导包
        2.初始化ros节点
        3.创建客户端对象
        4.组织请求数据并发送请求
        5.处理响应

    优化实现:
        可以在执行节点时动态传入参数
"""

if __name__ == "__main__":
    #判断参数长度
    if len(sys.argv) != 3:
        rospy.logerr("传入的参数个数不对,请重新传入参数!")
        sys.exit(1)

    #2.初始化ros节点
    rospy.init_node("client")
    #3.创建客户端对象
    client = rospy.ServiceProxy("addInts", AddInts)
    #4.组织请求数据并发送请求
    #解析传入的参数
    num1 = int(sys.argv[1])
    num2 = int(sys.argv[2])
    response = client.call(num1,num2)
    #5.处理响应
    rospy.loginfo("响应的数据:sum = %d",response.sum)

    pass

#! /usr/bin/env python
import rospy
from service_server_client.srv import AddInts,AddIntsRequest,AddIntsResponse
import sys

"""
    客户端:组织并提交请求,处理服务响应
        1.导包
        2.初始化ros节点
        3.创建客户端对象
        4.组织请求数据并发送请求
        5.处理响应

    优化实现:
        可以在执行节点时动态传入参数

    问题:
        如果先启动客户端,那么会请求异常
    需求:
        如果先启动客户端,不要直接抛出异常,而是挂起,等服务器启动后,再正常请求
    解决:
        在ROS中内置了相关函数,可让客户端启动后挂起,等待服务器启动
        函数1
        client.wait_for_service()
        函数2
        rospy.wait_for_service("话题名称")
"""

if __name__ == "__main__":
    #判断参数长度
    if len(sys.argv) != 3:
        rospy.logerr("传入的参数个数不对,请重新传入参数!")
        sys.exit(1)

    #2.初始化ros节点
    rospy.init_node("client")
    #3.创建客户端对象
    client = rospy.ServiceProxy("addInts", AddInts)
    #4.组织请求数据并发送请求
    #解析传入的参数
    num1 = int(sys.argv[1])
    num2 = int(sys.argv[2])
    #等待服务启动
    #函数1
    #client.wait_for_service()
    #函数2
    rospy.wait_for_service("addInts")

    response = client.call(num1,num2)
    #5.处理响应
    rospy.loginfo("响应的数据:sum = %d",response.sum)

    pass

3.设置权限

终端下进入 scripts 执行:chmod +x *.py

4.配置 CMakeLists.txt

catkin_install_python(PROGRAMS
  scripts/demo_server.py
  scripts/demo_client.py
  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

5.执行

流程:

需要先启动服务:rosrun 包名服务
然后再调用客户端 :rosrun 包名客户端参数1 参数2

结果:

会根据提交的数据响应相加后的结果。

2.3 参数服务器

2.3.1 参数服务器理论模型

参数服务器实现是最为简单的，该模型如下图所示,该模型中涉及到三个角色:

ROS Master (管理者)
Talker (参数设置者)
Listener (参数调用者)

ROS Master 作为一个公共容器保存参数，Talker 可以向容器中设置参数，Listener 可以获取参数。

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-WcjAnK5h-1658326498193)(https://raw.githubusercontent.com/konan6915/Pictures/main/ros_img/202204011917099.png)]

参数可使用数据类型:

32-bit integers
booleans
strings
doubles
iso8601 dates
lists
base64-encoded binary data
字典

2.3.2 参数操作A(C++)

**需求:**实现参数服务器参数的增删改查操作。

在 C++ 中实现参数服务器数据的增删改查，可以通过两套 API 实现:

ros::NodeHandle
ros::param

1.参数服务器新增(修改)参数

#include "ros/ros.h"

/*
    实现参数的新增和修改
    需求:首先设置机器人的共享参数:类型,半径(0.15)
        再修改半径(0.2)
    实现:
        ros::NodeHandle
            setParam(key,value)
        ros::param
            set()
*/

int main(int argc, char *argv[])
{
    //初始化ros节点
    ros::init(argc,argv,"set_param");
    //创建ros节点句柄
    ros::NodeHandle nh;
    //参数增-----------------------------
    //方案1 nh
    nh.setParam("type", "xiaohuang");
    nh.setParam("radius",0.15);
    //方案2 ros::param
    ros::param::set("type_param","xiaohei");
    ros::param::set("radius_param",0.15);
    //参数改-----------------------------
    //方案1 nh
    nh.setParam("radius",0.2);
    //方案2 ros::param
    ros::param::set("radius_param",0.4);

    return 0;
}

查看所设置的参数服务器参数：命令行键入rosparam list，可以得到参数服务器列表；键入rosparam get /参数名称，可以得到具体参数。

2.参数服务器获取参数

#include "ros/ros.h"
/*
    实现参数的查询
    需求:首先设置机器人的共享参数:类型,半径(0.15)
        再修改半径(0.2)
    实现:
        ros::NodeHandle
            param(键,默认值) 
                存在，返回对应结果，否则返回默认值

            getParam(键,存储结果的变量)
                存在,返回 true,且将值赋值给参数2
                若果键不存在，那么返回值为 false，且不为参数2赋值

            getParamCached(键,存储结果的变量)--提高变量获取效率
                存在,返回 true,且将值赋值给参数2
                若果键不存在，那么返回值为 false，且不为参数2赋值

            getParamNames(std::vector<std::string>)
                获取所有的键,并存储在参数 vector 中 

            hasParam(键)
                是否包含某个键，存在返回 true，否则返回 false

            searchParam(参数1，参数2)
                搜索键，参数1是被搜索的键，参数2存储搜索结果的变量
        ros::param

*/

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    //初始化ros节点
    ros::init(argc,argv,"get_param");
    //创建ros节点句柄
    ros::NodeHandle nh;
    //方案1 ros::NodeHandle
    //1.param
    double radius = nh.param("radius",0.5);
    ROS_INFO("radius = %.2f",radius);
    //2.getParam()
    double radius2 = 0;
    bool result = nh.getParam("radius",radius2);
    if(result)
    {
        ROS_INFO("获取的半径: %.2f ",radius2);
    }
    else
    {
        ROS_INFO("被查询的变量不存在!");

    }

    //3.getParamCached(键,存储结果的变量)
    //与 getParam()类似，只是底层性能上有提升
    bool result_g = nh.getParamCached("radius",radius2);
    if(result_g)
    {
        ROS_INFO("获取的半径: radius = %.2f ",radius2);
    }
    else
    {
        ROS_INFO("被查询的变量不存在!!!");

    }

    //4.getParamNames
    std::vector<std::string> names;
    nh.getParamNames(names);
    for(auto &&name : names)
    {
        ROS_INFO("遍历到的元素:%s",name.c_str());
    }

    //5.hasParam
    bool flag1 = nh.hasParam("radius");
    bool flag2 = nh.hasParam("radiusxxx");
    ROS_INFO("radius 是否存在? %d",flag1);
    ROS_INFO("radiusxxx 是否存在? %d",flag2);

    //6.searchParam
    std::string key;
    nh.searchParam("radius",key);
    ROS_INFO("搜索结果: %s",key.c_str());

    //ros::param
    double radius_param = ros::param::param("radius", 100.5);
    ROS_INFO("radius_param = %.2f",radius_param);

    std::vector<std::string> names_param;
    ros::param::getParamNames(names_param);
    for(auto &&name : names_param)
    {
        ROS_INFO("遍历到的键:%s",name.c_str());
    }

    return 0;
}

3.参数服务器删除参数

#include "ros/ros.h"

/*
    实现参数的删除
        实现:
            ros::NodeHandle
                delParam()
            ros::param
                del()
*/

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    //初始化ros节点
    ros::init(argc,argv,"del_param");
    //创建ros节点句柄
    ros::NodeHandle nh;

    //删除:NodeHandle-------------------------
    bool flag1 = nh.deleteParam("radius");
    if(flag1)
    {
        ROS_INFO("删除成功!");
    }
    else
    {
        ROS_INFO("删除失败!");
    }
    //删除:ros::param-------------------------
    bool flag2 = ros::param::del("radius_param");
    if(flag2)
    {
        ROS_INFO("radius_param删除成功!");
    }
    else
    {
        ROS_INFO("radius_param删除失败!");
    }
    return 0;
}

2.3.3 参数操作B(Python)

**需求:**实现参数服务器参数的增删改查操作。

1.参数服务器新增(修改)参数

#! /usr/bin/env python
"""
    参数服务器操作之新增与修改(二者API一样)_Python实现:

    实现参数的新增和修改
    需求:首先设置机器人的共享参数:类型,半径(0.15)
        再修改半径(0.2)
    实现:
        rospy.set_Param()
"""

import rospy

if __name__ == "__main__":

    rospy.init_node("set_param")

    # 设置各种类型参数
    rospy.set_param("type_p","xiaolan")
    rospy.set_param("radius_p",0.9)

    # 修改
    rospy.set_param("radius_p",1.1)

    pass

2.参数服务器获取参数

#! /usr/bin/env python
"""
    参数服务器操作之查询_Python实现:    
        get_param(键,默认值)
            当键存在时,返回对应的值,如果不存在返回默认值
        get_param_cached
            与get_param使用一直,只是效率高
        get_param_names
            获取参数所以键的集合
        has_param
            判断是否包含某个键
        search_param
            查找某个参数的键并返回完整的键名
"""

import rospy

if __name__ == "__main__":

    rospy.init_node("get_param")

    #1.get_param
    radius = rospy.get_param("radius_p",0.5)
    radius2 = rospy.get_param("radius_pxxx",0.5)
    rospy.loginfo("radius = %.2f", radius)
    rospy.loginfo("radius2 = %.2f", radius2)

    #2.get_param_ached
    radius3 = rospy.get_param_cached("radius_p",0.5)
    radius4 = rospy.get_param_cached("radius_pxxx",0.5)
    rospy.loginfo("radius = %.2f", radius3)
    rospy.loginfo("radius2 = %.2f", radius4)

    #3.get_param_names
    names = rospy.get_param_names()
    for name in names:
        rospy.loginfo("name = %s",name)

    #4.has_param
    flag1 = rospy.has_param("radius_p")
    if flag1:
        rospy.loginfo("radius_p存在!")
    else:
        rospy.loginfo("radius_p不存在!")

    #5.search_param
    key = rospy.search_param("type_p")
    rospy.loginfo("key = %s",key)

    pass

3.参数服务器删除参数

#! /usr/bin/env python
"""
    参数服务器操作之新增与修改(二者API一样)_Python实现:

    实现参数的删除
    实现:
        rospy.delete_param()
"""

import rospy

if __name__ == "__main__":

    rospy.init_node("del_param")
    try:
        #删除
        rospy.delete_param("radius_p")
    except Exception as e:
        rospy.loginfo("被删除的键不存在!")

    pass

2.4 常用命令

机器人系统中启动的节点少则几个，多则十几个、几十个，不同的节点名称各异，通信时使用话题、服务、消息、参数等等都各不相同，一个显而易见的问题是: 当需要自定义节点和其他某个已经存在的节点通信时，如何获取对方的话题、以及消息载体的格式呢？

在 ROS 同提供了一些实用的命令行工具，可以用于获取不同节点的各类信息，常用的命令如下:

rosnode : 操作节点
rostopic : 操作话题
rosservice : 操作服务
rosmsg : 操作msg消息
rossrv : 操作srv消息
rosparam : 操作参数

作用

和之前介绍的文件系统操作命令比较，文件操作命令是静态的，操作的是磁盘上的文件，而上述命令是动态的，在ROS程序启动后，可以动态的获取运行中的节点或参数的相关信息。

2.4.1 rosnode

rosnode 是用于获取节点信息的命令

rosnode ping    测试到节点的连接状态
rosnode list    列出活动节点
rosnode info    打印节点信息
rosnode machine 列出指定设备上节点
rosnode kill    杀死某个节点
rosnode cleanup 清除不可连接的节点

rosnode ping

测试到节点的连接状态
rosnode list

列出活动节点
rosnode info

打印节点信息
rosnode machine

列出指定设备上的节点
rosnode kill

杀死某个节点
rosnode cleanup

清除无用节点，启动乌龟节点，然后 ctrl + c 关闭，该节点并没被彻底清除，可以使用 cleanup 清除节点

2.4.2 rostopic

rostopic包含rostopic命令行工具，用于显示有关ROS 主题的调试信息，包括发布者，订阅者，发布频率和ROS消息。它还包含一个实验性Python库，用于动态获取有关主题的信息并与之交互。

rostopic bw     显示主题使用的带宽
rostopic delay  显示带有 header 的主题延迟
rostopic echo   打印消息到屏幕
rostopic find   根据类型查找主题
rostopic hz     显示主题的发布频率
rostopic info   显示主题相关信息
rostopic list   显示所有活动状态下的主题
rostopic pub    将数据发布到主题
rostopic type   打印主题类型
Copy

rostopic list(-v)

直接调用即可，控制台将打印当前运行状态下的主题名称

rostopic list -v : 获取话题详情(比如列出：发布者和订阅者个数…)

rostopic pub

可以直接调用命令向订阅者发布消息

为roboware 自动生成的发布/订阅模型案例中的订阅者发布一条字符串

rostopic pub /主题名称 消息类型 消息内容
rostopic pub /chatter std_msgs gagaxixi
Copy

为小乌龟案例的订阅者发布一条运动信息

rostopic pub /turtle1/cmd_vel geometry_msgs/Twist
 "linear:
  x: 1.0
  y: 0.0
  z: 0.0
angular:
  x: 0.0
  y: 0.0
  z: 2.0"
//只发布一次运动信息

rostopic pub -r 10 /turtle1/cmd_vel geometry_msgs/Twist
 "linear:
  x: 1.0
  y: 0.0
  z: 0.0
angular:
  x: 0.0
  y: 0.0
  z: 2.0"
// 以 10HZ 的频率循环发送运动信息

rostpic echo

获取指定话题当前发布的消息
rostopic info

获取当前话题的小关信息

消息类型

发布者信息

订阅者信息
rostopic type

列出话题的消息类型
rostopic find 消息类型

根据消息类型查找话题
rostopic delay

列出消息头信息
rostopic hz

列出消息发布频率
rostopic bw

列出消息发布带宽

2.4.3 rosmsg

rosmsg是用于显示有关 ROS消息类型的信息的命令行工具。

rosmsg 演示

rosmsg show    显示消息描述
rosmsg info    显示消息信息
rosmsg list    列出所有消息
rosmsg md5    显示 md5 加密后的消息
rosmsg package    显示某个功能包下的所有消息
rosmsg packages    列出包含消息的功能包

rosmsg list

会列出当前 ROS 中的所有 msg
rosmsg packages

列出包含消息的所有包

rosmsg package

列出某个包下的所有msg

//rosmsg package 包名 
rosmsg package turtlesim
Copy

rosmsg show

显示消息描述

//rosmsg show 消息名称
rosmsg show turtlesim/Pose
结果:
float32 x
float32 y
float32 theta
float32 linear_velocity
float32 angular_velocity

rosmsg info

作用与 rosmsg show 一样
rosmsg md5 (资料:[http://wiki.ros.org/ROS/Technical%20Overview#Message_serialization_and_msg_MD5_sums](http://wiki.ros.org/ROS/Technical Overview#Message_serialization_and_msg_MD5_sums))

一种校验算法，保证数据传输的一致性

2.4.4 rosservice

rosservice包含用于列出和查询ROSServices的rosservice命令行工具。

调用部分服务时，如果对相关工作空间没有配置 path，需要进入工作空间调用 source ./devel/setup.bash

rosservice args 打印服务参数
rosservice call    使用提供的参数调用服务
rosservice find    按照服务类型查找服务
rosservice info    打印有关服务的信息
rosservice list    列出所有活动的服务
rosservice type    打印服务类型
rosservice uri    打印服务的 ROSRPC uri
Copy

rosservice list

列出所有活动的 service

~ rosservice list
/clear
/kill
/listener/get_loggers
/listener/set_logger_level
/reset
/rosout/get_loggers
/rosout/set_logger_level
/rostopic_4985_1578723066421/get_loggers
/rostopic_4985_1578723066421/set_logger_level
/rostopic_5582_1578724343069/get_loggers
/rostopic_5582_1578724343069/set_logger_level
/spawn
/turtle1/set_pen
/turtle1/teleport_absolute
/turtle1/teleport_relative
/turtlesim/get_loggers
/turtlesim/set_logger_level

rosservice args

打印服务参数

rosservice args /spawn
x y theta name
Copy

rosservice call

调用服务

为小乌龟的案例生成一只新的乌龟

rosservice call /spawn "x: 1.0
y: 2.0
theta: 0.0
name: 'xxx'"
name: "xxx"

//生成一只叫 xxx 的乌龟
Copy

rosservice find

根据消息类型获取话题
rosservice info

获取服务话题详情
rosservice type

获取消息类型
rosservice uri

获取服务器 uri

2.4.5 rossrv

rossrv是用于显示有关ROS服务类型的信息的命令行工具，与 rosmsg 使用语法高度雷同。

rossrv show    显示服务消息详情
rossrv info    显示服务消息相关信息
rossrv list    列出所有服务信息
rossrv md5    显示 md5 加密后的服务消息
rossrv package    显示某个包下所有服务消息
rossrv packages    显示包含服务消息的所有包

rossrv list

会列出当前 ROS 中的所有 srv 消息
rossrv packages

列出包含服务消息的所有包

rossrv package

列出某个包下的所有msg

//rossrv package 包名 
rossrv package turtlesim

rossrv show

显示消息描述

//rossrv show 消息名称
rossrv show turtlesim/Spawn
结果:
float32 x
float32 y
float32 theta
string name
---
string name

rossrv info

作用与 rossrv show 一致
rossrv md5

对 service 数据使用 md5 校验(加密)

2.4.6 rosparam

rosparam包含rosparam命令行工具，用于使用YAML编码文件在参数服务器上获取和设置ROS参数。

rosparam set    设置参数
rosparam get    获取参数
rosparam load   从外部文件加载参数
rosparam dump   将参数写出到外部文件
rosparam delete 删除参数
rosparam list   列出所有参数

rosparam list

列出所有参数

rosparam list

//默认结果
/rosdistro
/roslaunch/uris/host_helloros_virtual_machine__42911
/rosversion
/run_id

rosparam set

设置参数

rosparam set name huluwa

//再次调用 rosparam list 结果
/name
/rosdistro
/roslaunch/uris/host_helloros_virtual_machine__42911
/rosversion
/run_id

rosparam get

获取参数
```
rosparam get name

//结果
huluwa
```

rosparam delete

删除参数

rosparam delete name

//结果
//去除了name

rosparam load(先准备 yaml 文件)

从外部文件加载参数
```
rosparam load xxx.yaml
```
rosparam dump

将参数写出到外部文件
```
rosparam dump yyy.yaml
```

2.5 通信机制实操

本节主要是通过ROS内置的turtlesim案例，结合已经介绍ROS命令获取节点、话题、话题消息、服务、服务消息与参数的信息，最终再以编码的方式实现乌龟运动的控制、乌龟位姿的订阅、乌龟生成与乌龟窗体背景颜色的修改。

启动乌龟功能包：

roscore

rosrun turtlesim turtlesim_node

rosrun turtlesim

2.5.1 实操01_话题发布

**需求描述:**编码实现乌龟运动控制，让小乌龟做圆周运动。

实现分析：

1.进行节点替换，替换ros内置的键盘控制节点，自己写一个节点控制乌龟运动。

2.需要了解所替换节点的通信机制，使用的是话题通信发布订阅模型，需要了解使用的话题以及消息，因为只有通过话题才能将节点匹配上。可使用ros命令与计算图得到。

3.编写消息发布节点程序。

实现流程:

通过计算图结合ros命令获取话题与消息信息。
编码实现运动控制节点。
启动 roscore、turtlesim_node 以及自定义的控制节点，查看运行结果。

1.话题与消息获取

准备: 先启动键盘控制乌龟运动案例。

1.1话题获取

获取话题:/turtle1/cmd_vel

通过计算图查看话题，启动计算图:rqt_graph

乌龟计算图

或者通过 rostopic 列出话题:

rostopic list

/rosout
/rosout_agg
/turtle1/cmd_vel
/turtle1/color_sensor
/turtle1/pose

1.2消息获取

**获取消息类型:**geometry_msgs/Twist

rostopic info /turtle1/cmd_vel

可以得到：

Type: geometry_msgs/Twist

Publishers: 
 * /teleop_turtle (http://liuhaoran-System-Name:37569/)

Subscribers: 
 * /turtlesim (http://liuhaoran-System-Name:37211/)

或者：

rostopic type /turtle1/cmd_vel

得到：

geometry_msgs/Twist

获取消息格式:

rosmsg info geometry_msgs/Twist

响应结果:

geometry_msgs/Vector3 linear
  float64 x
  float64 y
  float64 z
geometry_msgs/Vector3 angular
  float64 x
  float64 y
  float64 z

linear(线速度) 下的xyz分别对应在x、y和z方向上的速度(单位是 m/s)；

angular(角速度)下的xyz分别对应x轴上的翻滚、y轴上俯仰和z轴上偏航的速度(单位是rad/s)。

2.实现发布节点

可以使用命令来控制乌龟运动：

rostopic pub -r 10 /turtle1/cmd_vel geometry_msgs/Twist

创建功能包需要依赖的功能包: roscpp rospy std_msgs geometry_msgs

实现方案A: C++

#include "ros/ros.h"
#include "geometry_msgs/Twist.h"

/*
    需求:发布速度消息
        话题: /turtle1/cmd_vel
        消息: geometry_msgs/Twist

    实现流程:
        1.包含头文件
        2.初始化 ROS 节点
        3.创建节点句柄
        4.创建发布者对象
        5.循环发布运动控制消息
        6.spinOnce();
*/

int main(int argc, char *argv[])
{
    //2.初始化 ROS 节点
    ros::init(argc,argv,"pub_turtle");
    //3.创建节点句柄
    ros::NodeHandle nh;
    //4.创建发布者对象
    ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 10);
    //5.循环发布运动控制消息
    ros::Rate rate(10);  //设置发布频率
    geometry_msgs::Twist twist;
    twist.linear.x = 1.0;
    twist.linear.y = 0;
    twist.linear.z = 0;

    twist.angular.x = 0;
    twist.angular.y = 0;
    twist.angular.z = 1.2;

    //循环发布
    while (ros::ok())
    {
        pub.publish(twist);
        //休眠
        rate.sleep();
        //回头
        //6.spinOnce();
        ros::spinOnce();
    }

    return 0;
}

实现方案B: Python

#! /usr/bin/env python
import rospy
from geometry_msgs.msg import Twist #发布的消息的类型
"""
    需求:发布速度消息
        话题: /turtle1/cmd_vel
        消息: geometry_msgs/Twist

    实现流程:
        1.导包
        2.初始化 ROS 节点
        3.创建发布者对象
        4.循环发布运动控制消息
"""

if __name__ == "__main__":

    #2.初始化 ros 节点
    rospy.init_node("pub_turtle")   #传入的节点的名称
    #3.创建发布者对象
    pub = rospy.Publisher("/turtle1/cmd_vel", Twist, queue_size=10)
    #4.编写发布逻辑并发布数据
    #指定发布频率
    rate = rospy.Rate(1)
    #创建速度消息
    twist = Twist()
    twist.linear.x = 1
    twist.linear.y = 0
    twist.linear.z = 0

    twist.angular.x = 0
    twist.angular.y = 0
    twist.angular.z = 1.2
    #使用循环发布数据
    rospy.sleep(2)  #发布前先休眠2秒
    while not rospy.is_shutdown():
        #发布数据
        pub.publish(twist)
        rate.sleep()

    pass

2.5.2 实操02_话题订阅

需求描述: 已知turtlesim中的乌龟显示节点，会发布当前乌龟的位姿(窗体中乌龟的坐标以及朝向)，要求控制乌龟运动，并时时打印当前乌龟的位姿。

实现分析:

首先，需要启动乌龟显示以及运动控制节点并控制乌龟运动。
要通过ROS命令，来获取乌龟位姿发布的话题以及消息。
编写订阅节点，订阅并打印乌龟的位姿。

实现流程:

通过ros命令获取话题与消息信息。
编码实现位姿获取节点。
启动 roscore、turtlesim_node 、控制节点以及位姿订阅节点，控制乌龟运动并输出乌龟的位姿。

0.launch文件

在功能包下创建launch文件夹，创建start_turtle.launch文件，文件内容（注释快捷键Ctrl+Shit +A）：

<!-- 启动乌龟GUI与键盘控制节点 -->
<launch>
    <!-- 乌龟GUI -->
    <node pkg = "turtlesim" type = "turtlesim_node" name = "turtle1" output = "screen"/>
    <!-- 键盘控制 -->
    <node pkg = "turtlesim" type = "turtle_teleop_key" name = "key" output = "screen"/>
</launch>

启动乌龟程序：

source ./devel/setup.bash
roslaunch test_turtle start_turtle.launch

这样便实现使用一个文件启动多个节点的功能。

1.话题与消息获取

获取话题:/turtle1/pose:

rostopic list

/rosout
/rosout_agg
/turtle1/cmd_vel
/turtle1/color_sensor
/turtle1/pose

**获取消息类型:**turtlesim/Pose

rostopic type  /turtle1/pose

turtlesim/Pose

获取消息格式:

rosmsg info turtlesim/Pose

响应结果:

float32 x
float32 y
float32 theta
float32 linear_velocity
float32 angular_velocity

2.实现订阅节点

创建功能包需要依赖的功能包: roscpp rospy std_msgs turtlesim

实现方案A: C++

#include "ros/ros.h"
#include "turtlesim/Pose.h"

/*
    需求:订阅乌龟的位姿信息，并输出到控制台
        话题: /turtle1/pose
        消息: turtlesim/Pose

    实现流程:
        1.包含头文件
        2.初始化 ROS 节点
        3.创建节点句柄
        4.创建订阅者对象
        5.处理订阅到的数据（回调函数）
        6.spin()
*/

//处理订阅到的数据
void doPose(const turtlesim::Pose::ConstPtr &pose)
{
    ROS_INFO("乌龟的位姿信息:坐标(%.2f , %.2f), 朝向(%.2f),线速度:%.2f, 角速度: %.2f",
            pose->x,pose->y,pose->theta,pose->linear_velocity,pose->angular_velocity);
}

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    //2.初始化 ROS 节点
    ros::init(argc,argv,"sub_turtle");
    //3.创建节点句柄
    ros::NodeHandle nh;
    //4.创建订阅者对象
    ros::Subscriber sub = nh.subscribe("/turtle1/pose",100,doPose);
    //5.处理订阅到的数据（回调函数）

    //6.spin()
    ros::spin();

    return 0;
}

实现方案B: Python

#! /usr/bin/env python
import rospy
from turtlesim.msg import Pose #发布的消息的类型
"""
    需求:订阅乌龟的位姿信息,并输出到控制台
        话题: /turtle1/pose
        消息: turtlesim/Pose

    实现流程:
        1.包含头文件
        2.初始化 ROS 节点
        3.创建订阅者对象
        4.处理订阅到的数据(回调函数)
        5.spin()
"""

def doPose(pose):
    rospy.loginfo("乌龟的位姿信息:坐标(%.2f , %.2f), 朝向(%.2f),线速度:%.2f, 角速度: %.2f",
                    pose.x,pose.y,pose.theta,pose.linear_velocity,pose.angular_velocity)

if __name__ == "__main__":

    #2.初始化 ros 节点
    rospy.init_node("sub_turtle")   #传入的节点的名称
    #3.创建订阅者对象
    sub = rospy.Subscriber("/turtle1/pose", Pose,doPose,queue_size=10)

    rospy.spin()
    pass

2.5.3 实操03_服务调用

**需求描述:**编码实现向 turtlesim 发送请求，在乌龟显示节点的窗体指定位置生成一乌龟，这是一个服务请求操作。

实现分析:

首先，需要启动乌龟显示节点。
要通过ROS命令，来获取乌龟生成服务的服务名称以及服务消息类型。
编写服务请求节点，生成新的乌龟。

实现流程:

通过ros命令获取服务与服务消息信息。
编码实现服务请求节点。
启动 roscore、turtlesim_node 、乌龟生成节点，生成新的乌龟。

1.服务名称与服务消息获取

获取话题:/spawn

rosservice list

/clear
/key/get_loggers
/key/set_logger_level
/kill
/reset
/rosout/get_loggers
/rosout/set_logger_level
/spawn
/turtle1/get_loggers
/turtle1/set_logger_level
/turtle1/set_pen
/turtle1/teleport_absolute
/turtle1/teleport_relative

**获取消息类型:**turtlesim/Spawn

rosservice type /spawn

或者：

rosservice info /spawn

Node: /turtle1
URI: rosrpc://liuhaoran-System-Name:46277
Type: turtlesim/Spawn
Args: x y theta name

获取消息格式:

rossrv info turtlesim/Spawn

响应结果:

float32 x
float32 y
float32 theta
string name
---
string name

2.服务客户端实现

可以使用命令来添加乌龟：

rosservice call /spawn

创建功能包需要依赖的功能包: roscpp rospy std_msgs turtlesim

实现方案A: C++

#include "ros/ros.h"
#include "turtlesim/Spawn.h"
/*
    需求:向服务端发布请求，生成一只新乌龟
        话题：/spawn
        消息：turtlesim/Spawn

    实现流程:
        1.包含头文件
        2.初始化 ROS 节点
        3.创建节点句柄
        4.创建客户端对象
        5.组织数据并发送
        6.处理响应
*/
int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    //2.初始化 ROS 节点
    ros::init(argc,argv,"client_turtle");
    //3.创建节点句柄
    ros::NodeHandle nh;
    //4.创建客户端对象
    ros::ServiceClient client =  nh.serviceClient<turtlesim::Spawn>("/spawn");
    //5.组织数据并发送
    turtlesim::Spawn sp;
    //5-1组织请求
    sp.request.x = 5.5;   //atui():把字符串转换成整型数的一个函数
    sp.request.y = 3;
    sp.request.theta = 1.5;
    sp.request.name = "turtle2";
    //5-2发送请求
    //调用判断服务器状态函数
    //函数1
    //client.waitForExistence();
    //函数2
    ros::service::waitForService("/spawn");

    bool flag = client.call(sp);
    //6.处理响应
    if(flag)
    {
        ROS_INFO("响应成功!");
        //获取结果
        ROS_INFO("新乌龟叫: %s",sp.response.name.c_str());
    } 
    else
    {
        ROS_INFO("处理失败....");
    }

    return 0;
}

实现方案B: Python

#! /usr/bin/env python
import rospy
#from turtlesim.srv import *
from turtlesim.srv import Spawn,SpawnRequest,SpawnResponse
"""
    客户端:组织并提交请求,处理服务响应
        1.导包
        2.初始化ros节点
        3.创建客户端对象
        4.组织请求数据并发送请求
        5.处理响应
    解决:
        在ROS中内置了相关函数,可让客户端启动后挂起,等待服务器启动
        函数1
        client.wait_for_service()
        函数2
        rospy.wait_for_service("话题名称")
"""
if __name__ == "__main__":

    #2.初始化ros节点
    rospy.init_node("client_turtle")
    #3.创建客户端对象
    client = rospy.ServiceProxy("/spawn", Spawn)
    #4.组织请求数据并发送请求
    #解析传入的参数
    request = SpawnRequest()
    request.x = 6
    request.y = 4
    request.theta = 2
    request.name = "turtle3"
    #等待服务启动
    #函数1
    #client.wait_for_service()
    #函数2
    rospy.wait_for_service("/spawn")
    try:
        response = client.call(request)
        #5.处理响应
        rospy.loginfo("新乌龟叫: %s",response.name)
    except Exception as e:
        rospy.logerr("请求处理异常!")

    pass

2.5.4 实操04_参数设置

需求描述: 修改turtlesim乌龟显示节点窗体的背景色，已知背景色是通过参数服务器的方式以 rgb 方式设置的。

实现分析:

首先，需要启动乌龟显示节点。
要通过ROS命令，来获取参数服务器中设置背景色的参数。
编写参数设置节点，修改参数服务器中的参数值。

实现流程:

通过ros命令获取参数。
编码实现服参数设置节点。
启动 roscore、turtlesim_node 与参数设置节点，查看运行结果。

1.参数名获取

获取参数列表:

rosparam list

响应结果:

/turtlesim/background_b
/turtlesim/background_g
/turtlesim/background_r

获取rgb三元色的值：

rosparam get /turtlesim/background_r
rosparam get /turtlesim/background_g
rosparam get /turtlesim/background_b

69
86
255

2.参数修改

通过命令修改：

rosparam set /turtlesim/background_r 255
rosparam set /turtlesim/background_g 50
rosparam set /turtlesim/background_b 2

实现方案A: C++

#include "ros/ros.h"

/*  
    需求：修改参数服务器中turtlesim背景色的相关参数
    步骤：
        1.初始化ros节点
        2.创建节点句柄（此处不一定，和后续API有关）
        3.修改参数
*/

int main(int argc, char *argv[])
{
    //1.初始化ros节点
    ros::init(argc,argv,"param_turtle");

    //3修改参数
    //如果调用ros::param,则不需要创建节点句柄
    ros::param::set("/turtlesim/background_r",0);
    ros::param::set("/turtlesim/background_g",255);  
    ros::param::set("/turtlesim/background_b",40); 

    return 0;
}

#include "ros/ros.h"

/*  
    需求：修改参数服务器中turtlesim背景色的相关参数
    步骤：
        1.初始化ros节点
        2.创建节点句柄（此处不一定，和后续API有关）
        3.修改参数
*/

int main(int argc, char *argv[])
{
    //1.初始化ros节点
    ros::init(argc,argv,"param_turtle");

    //2.创建节点句柄
    ros::NodeHandle nh("turtlesim");
    nh.setParam("background_r",30);
    nh.setParam("background_g",200);
    nh.setParam("background_b",100);

    //3修改参数
    //如果调用ros::param,则不需要创建节点句柄
    /*
    ros::param::set("/turtlesim/background_r",0);
    ros::param::set("/turtlesim/background_g",255);  
    ros::param::set("/turtlesim/background_b",40); 
    */

    return 0;
}

实现方案B: Python

#! /usr/bin/env python
"""
    实现:
        rospy.set_Param()
"""
import rospy

if __name__ == "__main__":

    rospy.init_node("param_turtle")

    # 修改背景色
    rospy.set_param("/turtlesim/background_r",0)
    rospy.set_param("/turtlesim/background_g",0)
    rospy.set_param("/turtlesim/background_b",0)

    pass

2.6 通信机制比较

三种通信机制中，参数服务器是一种数据共享机制，可以在不同的节点之间共享数据，话题通信与服务通信是在不同的节点之间传递数据的，三者是ROS中最基础也是应用最为广泛的通信机制。

这其中，话题通信和服务通信有一定的相似性也有本质上的差异，在此将二者做一下简单比较:

二者的实现流程是比较相似的，都是涉及到四个要素:

要素1: 消息的发布方/客户端(Publisher/Client)
要素2: 消息的订阅方/服务端(Subscriber/Server)
要素3: 话题名称(Topic/Service)
要素4: 数据载体(msg/srv)

可以概括为: 两个节点通过话题关联到一起，并使用某种类型的数据载体实现数据传输。

二者的实现也是有本质差异的，具体比较如下:

	Topic(话题)	Service(服务)
通信模式	发布/订阅	请求/响应
同步性	异步	同步
底层协议	ROSTCP/ROSUDP	ROSTCP/ROSUDP
缓冲区	有	无
时时性	弱	强
节点关系	多对多	一对多(一个 Server)
通信数据	msg	srv
使用场景	连续高频的数据发布与接收:雷达、里程计	偶尔调用或执行某一项特定功能：拍照、语音识别

不同通信机制有一定的互补性，都有各自适应的应用场景。尤其是话题与服务通信，需要结合具体的应用场景与二者的差异，选择合适的通信机制。

第三章 ROS通信机制进阶

3.1 常用API

首先，建议参考官方API文档或参考源码:

ROS节点的初始化相关API;
NodeHandle 的基本使用相关API;
话题的发布方，订阅方对象相关API;
服务的服务端，客户端对象相关API;
时间相关API;
日志输出相关API。

1.C++相关

（1）初始化

运行时在末尾加上_length:=2

rosrun plumbing_apis demo_apis_pub _length:=2

使用rosparam list查看：

/rosdistro
/roslaunch/uris/host_liuhaoran_system_name__45505
/rosversion
/run_id
/talker/length

具体的值，使用rosparam get /talker/length，可得length的值为2。

/** @brief ROS初始化函数。
 *
 * 该函数可以解析并使用节点启动时传入的参数(通过参数设置节点名称、命名空间...) 
 *
 * 该函数有多个重载版本，如果使用NodeHandle建议调用该版本。 
 *
 * \param argc 参数个数
 * \param argv 参数列表
 * \param name 节点名称，需要保证其唯一性，不允许包含命名空间
 * \param options 节点启动选项，被封装进了ros::init_options
 *
 */
void init(int &argc, char **argv, const std::string& name, uint32_t options = 0);

//2.初始化ROS节点
/*
    作用：ros初始化函数

    参数：
        1) argc     ---> 封装实参的个数（n+1）
        2) argv     ---> 封装参数的数组
        3) name     ---> 为节点命名(需保证唯一性)
        4) options  ---> 节点启动选项

        返回值：void
    使用：
        1) argc 与 argv 的使用
            如果按照ros中的特定格式传入实参，那么ros可以加以使用，比如用来设置全局参数，给节点重命名
        2) options 的使用
            节点名称需保证唯一性，导致的问题：同一个节点不能重复启动
            结果：当有重名 的节点启动时，之前的节点会被关闭
            需求：特定场景下，需要一个节点多次启动才能正常运行,如何做？
            解决：设置启动项：ros::init_options::AnonymousName 如下所示

*/
ros::init(argc,argv,"talker",ros::init_options::AnonymousName);  //talker为节点名称

（2）话题与服务相关对象

在 roscpp 中，话题和服务的相关对象一般由 NodeHandle 创建。

NodeHandle有一个重要作用是可以用于设置命名空间，这是后期的重点，但是本章暂不介绍。

//4.创建发布者对象
/*
    作用：创建发布者对象

    模板：被发布消息的类型
    参数：
        1) 话题名称
        2) 队列长度
        3) latch(可选)  如果设置为true，会保存发布方最后一条消息，
                            并且新的订阅连接到发布方时，发布方会将这条消息发送给订阅者
    使用：
        latch 设置为true的作用？
        以静态地图发布为例，方案1可使用固定频率发送地图数据但效率较低；
        方案2可将地图发布对象的 latch 设置为true，并且发布方只发送一次数据，每当订阅者连接时，
        将地图数据发送给订阅者(只发送一次)，这样提高了数据的发送效率

*/
   ros::Publisher pub = nh.advertise<std_msgs::String>("huati_name",10,true);

（3）回旋函数

在ROS程序中，频繁的使用了 ros::spin() 和 ros::spinOnce() 两个回旋函数，可以用于处理回调函数。

1.spinOnce()

/**
 * \brief 处理一轮回调
 *
 * 一般应用场景:
 *     在循环体内，处理所有可用的回调函数
 * 
 */
ROSCPP_DECL void spinOnce();

2.spin()

/** 
 * \brief 进入循环处理回调 
 */
ROSCPP_DECL void spin();

3.二者比较

**相同点:**二者都用于处理回调函数；

**不同点:**ros::spin() 是进入了循环执行回调函数，而 ros::spinOnce() 只会执行一次回调函数(没有循环)，在 ros::spin() 后的语句不会执行到，而 ros::spinOnce() 后的语句可以执行。

（4）时间

ROS中时间相关的API是极其常用，比如:获取当前时刻、持续时间的设置、执行频率、休眠、定时器…都与时间相关。

#include "ros/ros.h"
/*
    需求1：演示时间相关的操作(获取当前时刻 + 设置指定时刻)
    实现：
        1.准备工作(头文件，节点初始化，句柄创建)
        2.获取当前时刻
        3.设置指定时刻

    需求2：程序执行中持续 5 秒
    实现：
        1.创建持续时间对象
        2.休眠

    需求3：已知程序开始运行的时刻，和程序运行的时间，求运行的结束时刻
    实现：
        1.获取开始执行的时刻
        2.模拟运行时间(n秒)
        3.计算结束时刻 = 开始 + 持续时间

    需求4：每隔一秒钟，在控制台输出一段文本
    实现：
        策略1： ros::Rate(1)
        策略2： 定时器
    注意点：
        创建：nh.createTimer()
        参数1：时间间隔
        参数2：回调函数(参数为时间事件TimerEvent)
        参数3：是否只执行一次
        参数4：是否自动启动(当设置为false时，需手动调用 Timer.start())

        定时器启动后：ros::spin()

*/
void cb(const ros::TimerEvent& event)
{
    ROS_INFO("--------------");
    ROS_INFO("函数被调用的时刻:%.2f",event.current_real.toSec());
}

int main(int argc, char *argv[])
{
    //1.准备工作
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"set_time");
    ros::NodeHandle nh; //此步骤必需,否则导致时间API调用失败(在NodeHandle中会初始化时间操作)

    //2.获取当前时刻
    //now()会将当前时刻封装并返回
    //当前时刻：now()被调用执行的那一刻
    //参考系：1970.01.01 00:00:00   得到的是距此时间的时间
    ros::Time right_now = ros::Time::now();
    ROS_INFO("当前时刻: %.2f ",right_now.toSec());
    ROS_INFO("当前时刻: %d ",right_now.sec);

    //3.设置指定时刻
    ros::Time t1(300,134105);
    ros::Time t2(20.345);
    ROS_INFO("t1 = %.6f ",t1.toSec());
    ROS_INFO("t2 = %.6f ",t2.toSec());

    //-----------------------------------------------------------------------------
    ROS_INFO("-----------------------持续时间-------------------------");
    ros::Time start = ros::Time::now();
    ROS_INFO("开始休眠 %.2f ",start.toSec());
    ros::Duration du(4.5);  //持续时间4.5秒
    du.sleep();
    ros::Time end = ros::Time::now();
    ROS_INFO("结束休眠 %.2f ",end.toSec());

    //-----------------------------------------------------------------------------
    ROS_INFO("-----------------------时间运算-------------------------");
    //1.获取开始执行的时刻
    ros::Time begin = ros::Time::now();
    ROS_INFO("开始时刻 %.2f ",begin.toSec());
    //2.模拟运行时间(n秒)
    ros::Duration du1(5);
    //3.计算结束时刻 = 开始 + 持续时间
    ros::Time stop = begin + du1;
    ROS_INFO("结束时刻 %.2f ",stop.toSec());

    //-----------------------------------------------------------------------------
    ROS_INFO("-----------------------时刻运算-------------------------");
    //时刻与时刻运算
    //ros::Time sum = begin +stop;  //不可相加
    ros::Duration du2 = stop - begin;
    ROS_INFO("时刻相减 %.2f ",du2.toSec());

    //-----------------------------------------------------------------------------
    ROS_INFO("------------------------定时器-------------------------");
    /* 
        ros::Timer createTimer(Duration period,     //时间间隔----1s
                const TimerCallback& callback,      //回调函数----封装业务
                bool oneshot = false,               //定时器是否是一次性
                bool autostart = true)              //是否自动启动

     */
    ros::Timer timer =  nh.createTimer(ros::Duration(1),cb);

    ros::spin();

    return 0;
}

（5）其他函数

在发布实现时，一般会循环发布消息，循环的判断条件一般由节点状态来控制，C++中可以通过 ros::ok() 来判断节点状态是否正常，而 python 中则通过 rospy.is_shutdown() 来实现判断，导致节点退出的原因主要有如下几种:

节点接收到了关闭信息，比如常用的 ctrl + c 快捷键就是关闭节点的信号；
同名节点启动，导致现有节点退出；
程序中的其他部分调用了节点关闭相关的API(C++中是ros::shutdown()，python中是rospy.signal_shutdown())

另外，日志相关的函数也是极其常用的，在ROS中日志被划分成如下级别:

DEBUG(调试):只在调试时使用，此类消息不会输出到控制台；
INFO(信息):标准消息，一般用于说明系统内正在执行的操作；
WARN(警告):提醒一些异常情况，但程序仍然可以执行；
ERROR(错误):提示错误信息，此类错误会影响程序运行；
FATAL(严重错误):此类错误将阻止节点继续运行。

日志函数:

#include "ros/ros.h"

/* 
    ROS 中日志信息
        演示不同级别日志的基本使用
 */

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"hello_log");
    ros::NodeHandle nh;

    //日志输出
    ROS_DEBUG("rosdebug-->调试信息");
    ROS_INFO("rosinfo-->一般信息");
    ROS_WARN("roswarn-->警告信息");
    ROS_ERROR("roserror-->错误信息");
    ROS_FATAL("rosfatal-->严重信息");

    return 0;
}

2.Python相关

（1）初始化

#2.初始化 ros 节点
    """ 
        作用:ROS初始化

        参数:
            name            ------> 设置节点名称
            argv=None       ------> 封装节点调用时传递的参数
            anonymous=False ------> 可为节点名称生成随机后缀,解决重名问题

        使用:
            1.argv使用
                可按照ros中指定的语法格式传参,ros可以解析并加以使用
            2.anonymous的使用
    """
    rospy.init_node("talker",anonymous=True)   #传入的节点的名称

（2）话题与服务相关对象

#3.创建发布者对象
    """  
        内容: latch
            bool 值, 默认为 False
        作用: 
            如果设置为 True, 可将发布的最后一条数据保存,且后续当新的订阅对象连接时会将该数据发送给订阅者
        使用:
            latch = True
    """
    pub = rospy.Publisher("huati_name", String, queue_size=10,latch=True)

（3）时间

#! /usr/bin/env python
import rospy
"""
    需求1:演示时间相关的操作(获取当前时刻 + 设置指定时刻)
    实现:
        1.准备工作(头文件,节点初始化,句柄创建)
        2.获取当前时刻
        3.设置指定时刻

    需求2:程序执行中持续 5 秒
    实现:
        1.创建持续时间对象
        2.休眠

    需求3:已知程序开始运行的时刻,和程序运行的时间,求运行的结束时刻
    实现:
        1.获取开始执行的时刻
        2.模拟运行时间(n秒)
        3.计算结束时刻 = 开始 + 持续时间

    需求4:每隔一秒钟,在控制台输出一段文本
    实现:
        策略1: ros::Rate(1)
        策略2: 定时器
    注意点:
        创建:nh.createTimer()
        参数1:时间间隔
        参数2:回调函数(参数为时间事件TimerEvent)
        参数3:是否只执行一次
        参数4:是否自动启动(当设置为false时,需手动调用 Timer.start())

        定时器启动后:ros::spin()
"""

def doMsg(event):
    rospy.loginfo("+++++++++++++++++")
    rospy.loginfo("调用回调函数的时刻:%.2f ",event.current_real.to_sec())


if __name__ == "__main__":

    #需求1:演示时间相关的操作(获取当前时刻 + 设置指定时刻)
    rospy.init_node("set_time_p")
    #获取时刻
    right_now = rospy.Time.now()
    rospy.loginfo("当前时刻: %.2f ",right_now.to_sec())
    rospy.loginfo("当前时刻: %d ",right_now.secs)
    #设置指定
    time1 = rospy.Time(100) #将时间封装成Time对象
    time2 = rospy.Time(100,418957)
    rospy.loginfo("指定时刻1: %.2f ",time1.to_sec())
    rospy.loginfo("指定时刻2: %.2f ",time2.to_sec())
    #从某个时间值获取时间对象
    time3 = rospy.Time.from_sec(210.12)
    rospy.loginfo("指定时刻3: %.2f ",time3.to_sec())

    rospy.loginfo("--------------------------------------")
    #需求2:程序执行中持续 5 秒
    #1.创建持续时间对象
    start = rospy.Time.now()
    rospy.loginfo("休眠前: %.2f ",start.to_sec())
    du = rospy.Duration(2)
    #2.将持续时间休眠
    rospy.sleep(du)
    end = rospy.Time.now()
    rospy.loginfo("休眠后: %.2f ",end.to_sec())

    rospy.loginfo("--------------------------------------")
    #需求3:已知程序开始运行的时刻,和程序运行的时间,求运行的结束时刻
    #1.获取一个时刻
    begin = rospy.Time.now()
    #2.设置一个持续时间
    du2 = rospy.Duration(3)
    #3.计算结束时刻 = 开始 + 持续时间
    stop = begin + du2
    rospy.loginfo("结束时间: %.2f ",stop.to_sec())

    rospy.loginfo("--------------------------------------")
    #需求4: 创建定时器,实现类似于 rospy.Rate 的功能(隔某个时间执行某种操作)
    """ 
        @param period: desired period between callbacks
        @type  period: rospy.Duration
        @param callback: callback to be called
        @type  callback: function taking rospy.TimerEvent
        @param oneshot: if True, fire only once, otherwise fire continuously until shutdown is called [default: False]
        @type  oneshot: bool
        @param reset: if True, timer is reset when rostime moved backward. [default: False]
        @type  reset: bool
    """
    rospy.Timer(rospy.Duration(2), doMsg)   #创建一个定时器对象
    rospy.spin()

    pass

（4）其他函数

1.节点状态判断

def is_shutdown():
    """
    @return: True 如果节点已经被关闭
    @rtype: bool
    """

2.节点关闭函数

def signal_shutdown(reason):
    """
    关闭节点
    @param reason: 节点关闭的原因，是一个字符串
    @type  reason: str
    """
def on_shutdown(h):
    """
    节点被关闭时调用的函数
    @param h: 关闭时调用的回调函数，此函数无参
    @type  h: fn()
    """

while not rospy.is_shutdown():
        count += 1

        #发布数据
        if count <= 10:
            msg.data = "hello" + str(count) #str()函数将数字转换为字符串
            pub.publish(msg)
            rospy.loginfo("发布的数据是: %s ",msg.data)
        else:
            #关闭节点
            rospy.on_shutdown(cb)
            rospy.signal_shutdown("关闭节点!")

        rate.sleep()

3.日志函数

rospy.logdebug("hello,debug")  #不会输出
rospy.loginfo("hello,info")  #默认白色字体
rospy.logwarn("hello,warn")  #默认黄色字体
rospy.logerr("hello,error")  #默认红色字体
rospy.logfatal("hello,fatal") #默认红色字体

3.2 ROS中的头文件与源文件

本节主要介绍ROS的C++实现中，如何使用头文件与源文件的方式封装代码，具体内容如下:

设置头文件，可执行文件作为源文件；
分别设置头文件，源文件与可执行文件。

在ROS中关于头文件的使用，核心内容在于CMakeLists.txt文件的配置，不同的封装方式，配置上也有差异。

3.2.1 自定义头文件调用

需求: 设计头文件，可执行文件本身作为源文件。

流程:

编写头文件；
编写可执行文件(同时也是源文件)；
编辑配置文件并执行。

1.头文件

在功能包下的 include/功能包名目录下新建头文件: hello.h，示例内容如下:

#ifndef _HELLO_H
#define _HELLO_H
/* 
    声明 namespace
            --class
                --run函数
*/
namespace hello_ns{
class MyHello{

public:
    void run();

};

}
#endif

注意:

在 VScode 中，为了后续包含头文件时不抛出异常，请配置 .vscode 下 c_cpp_properties.json 的 includepath属性

"/home/用户/工作空间/src/功能包/include/**"

2.可执行文件

在 src 目录下新建文件:hello.cpp，示例内容如下:

#include "ros/ros.h"
#include "plumbing_head/hello.h"

namespace hello_ns{

void MyHello::run(){
    ROS_INFO("run()执行....");

}
}

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"hello_head");
    //函数调用
    hello_ns::MyHello myHello;
    myHello.run();


    return 0;
}

3.配置文件

配置CMakeLists.txt文件，头文件相关配置如下:

include_directories(
include
  ${catkin_INCLUDE_DIRS}
)

可执行配置文件配置方式与之前一致。

3.2.2 自定义源文件调用

**需求:**设计头文件与源文件，在可执行文件中包含头文件。

流程:

编写头文件；
编写源文件；
编写可执行文件；
编辑配置文件并执行。

1.头文件

#ifndef _HELLO_H
#define _HELLO_H
/* 
    声明 namespace
            --class
                --run函数
*/
namespace hello_ns{
class MyHello{

public:
    void run();

};

}
#endif

2.源文件

在 src 目录下新建文件:hello.cpp，示例内容如下:

#include "ros/ros.h"
#include "plumbing_head_src/hello.h"
namespace hello_ns{

void MyHello::run(){
    ROS_INFO("源文件run()执行....");

}
}

3.可执行文件

在 src 目录下新建文件: use_hello.cpp，示例内容如下:

#include "ros/ros.h"
#include "plumbing_head_src/hello.h"

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"hello_head_src");

    hello_ns::MyHello myHello;
    myHello.run();

    return 0;
}

4.配置文件

头文件与源文件相关配置:

include_directories(
include
  ${catkin_INCLUDE_DIRS}
)

## 声明C++库
add_library(head_src  #库名
  include/${PROJECT_NAME}/hello.h #头文件
  src/hello.cpp #源文件
)

add_dependencies(head_src ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

target_link_libraries(head_src
  ${catkin_LIBRARIES}
)

可执行文件配置:

add_executable(use_hello src/use_hello.cpp)
add_dependencies(use_hello ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

#此处需要添加之前设置的 head_src 库
target_link_libraries(use_hello
  head_src
  ${catkin_LIBRARIES}
)

3.3 Python模块导入

与C++类似的，在Python中导入其他模块时，也需要相关处理。

**需求:**首先新建一个Python文件A，再创建Python文件UseA，在UseA中导入A并调用A的实现。

实现:

新建两个Python文件，使用 import 实现导入关系；
添加可执行权限、编辑配置文件并执行UseA。

1.新建两个Python文件并使用import导入

文件A实现(包含一个变量):

#! /usr/bin/env python
num = 1000

文件B核心实现:

import os
import sys

path = os.path.abspath(".")
# 核心
sys.path.insert(0,path + "/src/plumbing_pub_sub/scripts")

import tools

....
....
    rospy.loginfo("num = %d",tools.num)

#! /usr/bin/env python

import os
import sys

import rospy
from std_msgs.msg import String #发布的消息的类型

#设置临时环境变量
#路径固定影响代码的可移植性
#sys.path.insert(0,"/home/liuhaoran/demo_ws/src/plumbing_path_python/scripts")
#优化: 动态获取路径
path = os.path.abspath(".")
sys.path.insert(0,path + "/plumbing_path_python/scripts")
import tools

if __name__ == "__main__":

    #2.初始化 ros 节点
    rospy.init_node("talker")   #传入的节点的名称
    #可能会抛出异常: ModuleNotFoundError: No module named 'tools' 
    """ 
        原因: rosrun 执行时,参考路径是工作空间的路径,在工作空间下无法查找依赖的模块
        解决: 可以声明python的环境变量,当依赖某个模块时,先去指定的环境变量中查找依赖
    """
    #path = os.path.abspath(".")
    #rospy.loginfo("执行时参考的路径:%s",path)
    #结果:执行时参考的路径:/home/liuhaoran/demo_ws
    rospy.loginfo("num = %d",tools.num)

    pass

第四章 ROS运行管理

ROS是多进程(节点)的分布式框架，一个完整的ROS系统实现：

可能包含多台主机；
每台主机上又有多个工作空间(workspace)；
每个的工作空间中又包含多个功能包(package)；
每个功能包又包含多个节点(Node)，不同的节点都有自己的节点名称；
每个节点可能还会设置一个或多个话题(topic)…

ROS组织形式

在多级层深的ROS系统中，其实现与维护可能会出现一些问题，比如，如何关联不同的功能包，繁多的ROS节点应该如何启动？功能包、节点、话题、参数重名时应该如何处理？不同主机上的节点如何通信？

本章主要内容介绍在ROS中上述问题的解决策略(见本章目录)，预期达成学习目标也与上述问题对应：

掌握元功能包使用语法；
掌握launch文件的使用语法；
理解什么是ROS工作空间覆盖，以及存在什么安全隐患；
掌握节点名称重名时的处理方式；
掌握话题名称重名时的处理方式；
掌握参数名称重名时的处理方式；
能够实现ROS分布式通信。

4.1 ROS元功能包

**场景:**完成ROS中一个系统性的功能，可能涉及到多个功能包，比如实现了机器人导航模块，该模块下有地图、定位、路径规划…等不同的子级功能包。那么调用者安装该模块时，需要逐一的安装每一个功能包吗？

显而易见的，逐一安装功能包的效率低下，在ROS中，提供了一种方式可以将不同的功能包打包成一个功能包，当安装某个功能模块时，直接调用打包后的功能包即可，该包又称之为元功能包(metapackage)。

（1）概念

MetaPackage是Linux的一个文件管理系统的概念。是ROS中的一个虚包，里面没有实质性的内容，但是它依赖了其他的软件包，通过这种方法可以把其他包组合起来，我们可以认为它是一本书的目录索引，告诉我们这个包集合中有哪些子包，并且该去哪里下载。

例如：

sudo apt install ros-noetic-desktop-full 命令安装ros时就使用了元功能包，该元功能包依赖于ROS中的其他一些功能包，安装该包时会一并安装依赖。

还有一些常见的MetaPackage：navigation moveit! turtlebot3 …

（2）作用

方便用户的安装，我们只需要这一个包就可以把其他相关的软件包组织到一起安装了。

（3）实现

首先: 新建一个功能包

然后: 修改 package.xml ,内容如下:

 <exec_depend>被集成的功能包</exec_depend>
 .....
 <export>
   <metapackage />
 </export>

最后: 修改 CMakeLists.txt,内容如下:

cmake_minimum_required(VERSION 3.0.2)
project(demo)
find_package(catkin REQUIRED)
catkin_metapackage()

PS:CMakeLists.txt 中不可以有换行。

4.2 ROS节点运行管理launch文件

关于 launch 文件的使用我们已经不陌生了，在第一章内容中，就曾经介绍到:

一个程序中可能需要启动多个节点，比如:ROS 内置的小乌龟案例，如果要控制乌龟运动，要启动多个窗口，分别启动 roscore、乌龟界面节点、键盘控制节点。如果每次都调用 rosrun 逐一启动，显然效率低下，如何优化?

采用的优化策略便是使用roslaunch 命令集合 launch 文件启动管理节点，并且在后续教程中，也多次使用到了 launch 文件。

概念

launch 文件是一个 XML 格式的文件，可以启动本地和远程的多个节点，还可以在参数服务器中设置参数。

作用

简化节点的配置与启动，提高ROS程序的启动效率。

使用

以 turtlesim 为例演示

1.新建launch文件

在功能包下添加 launch目录, 目录下新建 xxxx.launch 文件，编辑 launch 文件

<!-- 启动乌龟GUI与键盘控制节点 -->
<launch>
    <!-- 乌龟GUI -->
    <node pkg = "turtlesim" type = "turtlesim_node" name = "turtle1" output = "screen"/>
    <!-- 键盘控制 -->
    <node pkg = "turtlesim" type = "turtle_teleop_key" name = "key" output = "screen"/>
</launch>

2.调用 launch 文件

roslaunch 包名 xxx.launch

注意: roslaunch 命令执行launch文件时，首先会判断是否启动了 roscore,如果启动了，则不再启动，否则，会自动调用 roscore

PS: 本节主要介绍launch文件的使用语法，launch 文件中的标签，以及不同标签的一些常用属性。

4.2.1 launch文件标签之launch

<launch>标签是所有 launch 文件的根标签，充当其他标签的容器

1.属性

deprecated = "弃用声明"

告知用户当前 launch 文件已经弃用

<!-- 启动乌龟GUI与键盘控制节点 -->
<launch deprecated = "此文件已过时,不建议使用!">
    <!-- 乌龟GUI -->
    <node pkg = "turtlesim" type = "turtlesim_node" name = "turtle1" output = "screen"/>
    <!-- 键盘控制 -->
    <node pkg = "turtlesim" type = "turtle_teleop_key" name = "key" output = "screen"/>
</launch>

2.子级标签

所有其它标签都是launch的子级。

4.2.2 launch文件标签之node

<node>标签用于指定 ROS 节点，是最常见的标签，需要注意的是: roslaunch 命令不能保证按照 node 的声明顺序来启动节点(节点的启动是多进程的)

1.属性

pkg=“包名”

节点所属的包
type=“nodeType”

节点类型(与之相同名称的可执行文件)
name=“nodeName”

节点名称(在 ROS 网络拓扑中节点的名称)
args=“xxx xxx xxx” (可选)

将参数传递给节点
machine=“机器名”

在指定机器上启动节点
respawn=“true | false” (可选)

如果节点退出，是否自动重启
respawn_delay=" N" (可选)

如果 respawn 为 true, 那么延迟 N 秒后启动节点
required=“true | false” (可选)

该节点是否必须，如果为 true,那么如果该节点退出，将杀死整个 roslaunch
ns=“xxx” (可选)

在指定命名空间 xxx 中启动节点
clear_params=“true | false” (可选)

在启动前，删除节点的私有空间的所有参数
output=“log | screen” (可选)

日志发送目标，可以设置为 log 日志文件，或 screen 屏幕,默认是 log

2.子级标签

env 环境变量设置
remap 重映射节点名称
rosparam 参数设置
param 参数设置

4.2.3 launch文件标签之include

include标签用于将另一个 xml 格式的 launch 文件导入到当前文件

1.属性

file=“$(find 包名)/xxx/xxx.launch”

要包含的文件路径
ns=“xxx” (可选)

在指定命名空间导入文件

<!-- 需要复用 start_turtle.launch 文件 -->

<launch>
    <!-- 包含 -->
    <include file = "$(find test_turtle)/launch/start_turtle.launch" />
    <!-- 其他需要的节点 -->

</launch>

2.子级标签

env 环境变量设置
arg 将参数传递给被包含的文件

4.2.4 launch文件标签之remap

用于话题重命名

1.属性

from=“xxx”

原始话题名称
to=“yyy”

目标名称

<!-- 启动乌龟GUI与键盘控制节点 -->
<launch>
    <!-- 乌龟GUI -->
    <node pkg = "turtlesim" type = "turtlesim_node" name = "turtle1" output = "screen">
        <remap from = "/turtle1/cmd_vel" to = "/cmd_vel" />
    </node>
    <!-- 键盘控制 -->
    <node pkg = "turtlesim" type = "turtle_teleop_key" name = "key" output = "screen"/>
</launch>

2.子级标签

4.2.5 launch文件标签之param

<param>标签主要用于在参数服务器上设置参数，参数源可以在标签中通过 value 指定，也可以通过外部文件加载，在<node>标签中时，相当于私有命名空间。

1.属性

name=“命名空间/参数名”

参数名称，可以包含命名空间
value=“xxx” (可选)

定义参数值，如果此处省略，必须指定外部文件作为参数源
type=“str | int | double | bool | yaml” (可选)

指定参数类型，如果未指定，roslaunch 会尝试确定参数类型，规则如下:
- 如果包含 ‘.’ 的数字解析未浮点型，否则为整型
- “true” 和 “false” 是 bool 值(不区分大小写)
- 其他是字符串

<launch>
    <!-- param 使用 向参数服务器设置参数-->
    <!-- 格式1: launch 下, node 外 -->
    <param name = "param_A" type = "int" value = "100" />

    <!-- 添加被执行的节点 -->
    <!-- 乌龟GUI -->
    <node pkg = "turtlesim" type = "turtlesim_node" name = "turtle_GUI" >
        <!-- 格式2: node 下 -->
        <param name = "param_B" type = "double" value = "1.25" />
    </node>

    <node pkg = "turtlesim" type ="turtle_teleop_key" name ="turtle_key" />

</launch>

2.子级标签

4.2.6 launch文件标签之rosparam

<rosparam>标签可以从 YAML 文件导入参数，或将参数导出到 YAML 文件，也可以用来删除参数，<rosparam>标签在<node>标签中时被视为私有。

1.属性

command=“load | dump | delete” (可选，默认 load)

加载、导出或删除参数
file=“$(find xxxxx)/xxx/yyy…”

加载或导出到的 yaml 文件
param=“参数名称”
ns=“命名空间” (可选)

<launch>
    <!-- rosparam 使用: 操作参数服务器参数 -->
    <!-- 格式1: launch 下,node 外 -->
    <!-- 加载参数 -->
    <rosparam command = "load" file = "$(find launch_turtle)/launch/params.yaml" />
    <!-- 导出参数 -->
    <rosparam command = "dump" file = "$(find launch_turtle)/launch/params_out.yaml" />
    <!-- 删除参数 -->
    <rosparam command = "delete" param = "bg_G" />
    <!-- 添加被执行的节点 -->
    <!-- 乌龟GUI -->
    <node pkg = "turtlesim" type = "turtlesim_node" name = "turtle_GUI" >
        <!-- 格式2 node下 -->
        <rosparam command = "load" file = "$(find launch_turtle)/launch/params.yaml" />
    </node>

    <node pkg = "turtlesim" type ="turtle_teleop_key" name ="turtle_key" />

</launch>

2.子级标签

4.2.7 launch文件标签之group

<group>标签可以对节点分组，具有 ns 属性，可以让节点归属某个命名空间

1.属性

ns=“名称空间” (可选)
clear_params=“true | false” (可选)

启动前，是否删除组名称空间的所有参数(慎用…此功能危险)

<launch>
    <!-- 启动两对乌龟GUI与键盘控制节点 -->
    <group ns = "first">
        <node pkg = "turtlesim" type = "turtlesim_node" name = "turtle_GUI" />
        <node pkg = "turtlesim" type ="turtle_teleop_key" name ="turtle_key" />
    </group>

    <group ns = "second">
        <node pkg = "turtlesim" type = "turtlesim_node" name = "turtle_GUI" />
        <node pkg = "turtlesim" type ="turtle_teleop_key" name ="turtle_key" />
    </group>

</launch>

2.子级标签

除了launch 标签外的其他标签

4.2.8 launch文件标签之arg

<arg>标签是用于动态传参，类似于函数的参数，可以增强launch文件的灵活性

1.属性

name=“参数名称”
default=“默认值” (可选)
value=“数值” (可选)

不可以与 default 并存
doc=“描述”

参数说明

2.子级标签

3.示例

launch文件传参语法实现,hello.lcaunch

<launch>
    <arg name="xxx" />
    <param name="param" value="$(arg xxx)" />
</launch>

命令行调用launch传参
```
roslaunch hello.launch xxx:=值
```

<launch>
    <!-- 演示arg的使用 需要设置多个参数，这些参数使用同一个值(小车的长度)-->
    <!-- <param name = "A" value = "0.5"/>
    <param name = "B" value = "0.5"/>
    <param name = "C" value = "0.5"/> -->

    <arg name = "car_length" default = "0.55" />
    <param name = "A" value = "$(argv car_length)"/>
    <param name = "B" value = "$(argv car_length)"/>
    <param name = "C" value = "$(argv car_length)"/>

</launch>

4.3 ROS工作空间覆盖

所谓工作空间覆盖，是指不同工作空间中，存在重名的功能包的情形。

ROS 开发中，会自定义工作空间且自定义工作空间可以同时存在多个，可能会出现一种情况: 虽然特定工作空间内的功能包不能重名，但是自定义工作空间的功能包与内置的功能包可以重名或者不同的自定义的工作空间中也可以出现重名的功能包，那么调用该名称功能包时，会调用哪一个呢？比如：自定义工作空间A存在功能包 turtlesim，自定义工作空间B也存在功能包 turtlesim，当然系统内置空间也存在turtlesim，如果调用turtlesim包，会调用哪个工作空间中的呢？

（1）实现

0.新建工作空间A与工作空间B，两个工作空间中都创建功能包: turtlesim。

1.在 ~/.bashrc 文件（主目录下按ctrl+H）下追加当前工作空间的 bash 格式如下:

source /home/用户/路径/工作空间A/devel/setup.bash
source /home/用户/路径/工作空间B/devel/setup.bash

2.新开命令行:source .bashrc加载环境变量

3.查看ROS环境环境变量echo $ROS_PACKAGE_PATH

结果:自定义工作空间B:自定义空间A:系统内置空间

4.调用命令:roscd turtlesim会进入自定义工作空间B

（2）原因

ROS 会解析 .bashrc 文件，并生成 ROS_PACKAGE_PATH ROS包路径，该变量中按照 .bashrc 中配置设置工作空间优先级，在设置时需要遵循一定的原则:ROS_PACKAGE_PATH 中的值，和 .bashrc 的配置顺序相反—>后配置的优先级更高，如果更改自定义空间A与自定义空间B的source顺序，那么调用时，将进入工作空间A。

（3）结论

功能包重名时，会按照 ROS_PACKAGE_PATH 查找，配置在前的会优先执行。

（4）隐患

存在安全隐患，比如当前工作空间B优先级更高，意味着当程序调用 turtlesim 时，不会调用工作空间A也不会调用系统内置的 turtlesim，如果工作空间A在实现时有其他功能包依赖于自身的 turtlesim，而按照ROS工作空间覆盖的涉及原则，那么实际执行时将会调用工作空间B的turtlesim，从而导致执行异常，出现安全隐患。

BUG 说明:

当在 .bashrc 文件中 source 多个工作空间后，可能出现的情况，在 ROS PACKAGE PATH 中只包含两个工作空间，可以删除自定义工作空间的 build 与 devel 目录，重新 catkin_make，然后重新载入 .bashrc 文件，问题解决。

4.4 ROS节点名称重名

场景:ROS 中创建的节点是有名称的，C++初始化节点时通过API:ros::init(argc,argv,"xxxx");来定义节点名称，在Python中初始化节点则通过 rospy.init_node("yyyy") 来定义节点名称。在ROS的网络拓扑中，是不可以出现重名的节点的，因为假设可以重名存在，那么调用时会产生混淆，这也就意味着，不可以启动重名节点或者同一个节点启动多次，的确，在ROS中如果启动重名节点的话，之前已经存在的节点会被直接关闭，但是如果有这种需求的话，怎么优化呢？

在ROS中给出的解决策略是使用命名空间或名称重映射。

命名空间就是为名称添加前缀，名称重映射是为名称起别名。这两种策略都可以解决节点重名问题，两种策略的实现途径有多种:

rosrun 命令
launch 文件
编码实现

以上三种途径都可以通过命名空间或名称重映射的方式，来避免节点重名，本节将对三者的使用逐一演示，三者要实现的需求类似。

4.4.1 rosrun设置命名空间与重映射

1.rosrun设置命名空间

1.1设置命名空间演示

语法: rosrun 包名节点名 __ns:=新名称

rosrun turtlesim turtlesim_node __ns:=ergou

rosrun turtlesim turtlesim_node __ns:=xiaohong

两个节点都可以正常运行。

1.2运行结果

rosnode list查看节点信息,显示结果:

/xxx/turtlesim
/yyy/turtlesim

2.rosrun名称重映射

2.1为节点起别名

语法: rosrun 包名节点名 __name:=新名称

rosrun turtlesim  turtlesim_node __name:=t1 |  rosrun turtlesim   turtlesim_node /turtlesim:=t1(不适用于python)

rosrun turtlesim  turtlesim_node __name:=t2 |  rosrun turtlesim   turtlesim_node /turtlesim:=t2(不适用于python)

两个节点都可以运行

2.2运行结果

rosnode list查看节点信息,显示结果:

/t1
/t2

3.rosrun命名空间与名称重映射叠加

3.1设置命名空间同时名称重映射

语法: rosrun 包名节点名 __ns:=新名称 __name:=新名称

rosrun turtlesim turtlesim_node __ns:=/xxx __name:=tn

3.2运行结果

rosnode list查看节点信息,显示结果:

/xxx/tn

使用环境变量也可以设置命名空间,启动节点前在终端键入如下命令:

export ROS_NAMESPACE=xxxx

4.4.2 launch文件设置命名空间与重映射

介绍 launch 文件的使用语法时，在 node 标签中有两个属性: name 和 ns，二者分别是用于实现名称重映射与命名空间设置的。使用launch文件设置命名空间与名称重映射也比较简单。

1.launch文件

<!-- 需启动多个乌龟GUI节点 -->
<launch>
    <!-- 默认 -->
    <node pkg = "turtlesim" type = "turtlesim_node" name = "turtlesim" />
    <!-- 名称重映射 -->
    <node pkg = "turtlesim" type = "turtlesim_node" name = "t1" />
    <!-- 命名空间 -->
    <node pkg = "turtlesim" type = "turtlesim_node" name = "turtlesim" ns = "ergou" />
    <!-- 命名空间 + 名称重映射 -->
    <node pkg = "turtlesim" type = "turtlesim_node" name = "t2" ns = "xiaohua" />
</launch>

在 node 标签中，name 属性是必须的，ns 可选。

2.运行

rosnode list查看节点信息,显示结果:

/ergou/turtlesim
/t1
/turtlesim
/xiaohua/t2

4.4.3 编码设置命名空间与重映射

如果自定义节点实现，那么可以更灵活的设置命名空间与重映射实现。

1.C++ 实现:重映射

1.1名称别名设置

核心代码:ros::init(argc,argv,"zhangsan",ros::init_options::AnonymousName);

1.2执行

会在名称后面添加时间戳。

2.C++ 实现:命名空间

2.1命名空间设置

核心代码

  std::map<std::string, std::string> map;
  map["__ns"] = "xxxx";
  ros::init(map,"wangqiang");

2.2执行

节点名称设置了命名空间。

3.Python 实现:重映射

3.1名称别名设置

核心代码:rospy.init_node("lisi",anonymous=True)

3.2执行

会在节点名称后缀时间戳。

4.5 ROS话题名称设置

在ROS中节点名称可能出现重名的情况，同理话题名称也可能重名。

在 ROS 中节点终端，不同的节点之间通信都依赖于话题，话题名称也可能出现重复的情况，这种情况下，系统虽然不会抛出异常，但是可能导致订阅的消息非预期的，从而导致节点运行异常。这种情况下需要将两个节点的话题名称由相同修改为不同。

又或者，两个节点是可以通信的，两个节点之间使用了相同的消息类型，但是由于，话题名称不同，导致通信失败。这种情况下需要将两个节点的话题名称由不同修改为相同。

在实际应用中，按照逻辑，有些时候可能需要将相同的话题名称设置为不同，也有可能将不同的话题名设置为相同。在ROS中给出的解决策略与节点名称重命类似，也是使用名称重映射或为名称添加前缀。根据前缀不同，有全局、相对、和私有三种类型之分。

全局(参数名称直接参考ROS系统，与节点命名空间平级)
相对(参数名称参考的是节点的命名空间，与节点名称平级)
私有(参数名称参考节点名称，是节点名称的子级)

名称重映射是为名称起别名，为名称添加前缀，该实现比节点重名更复杂些，不单是使用命名空间作为前缀、还可以使用节点名称最为前缀。两种策略的实现途径有多种:

rosrun 命令
launch 文件
编码实现

本节将对三者的使用逐一演示，三者要实现的需求类似。

案例

在ROS中提供了一个比较好用的键盘控制功能包: ros-noetic-teleop-twist-keyboard，该功能包，可以控制机器人的运动，作用类似于乌龟的键盘控制节点，可以使用 sudo apt install ros-noetic-teleop-twist-keyboard 来安装该功能包，然后执行: rosrun teleop_twist_keyboard teleop_twist_keyboard.py，在启动乌龟显示节点，不过此时前者不能控制乌龟运动，因为，二者使用的话题名称不同，前者使用的是 cmd_vel话题，后者使用的是 /turtle1/cmd_vel话题。需要将话题名称修改为一致，才能使用，如何实现？

4.5.1 rosrun设置话题重映射

rosrun名称重映射语法: rorun 包名节点名话题名:=新话题名称

实现teleop_twist_keyboard与乌龟显示节点通信方案由两种：

1.方案1

将 teleop_twist_keyboard 节点的话题设置为/turtle1/cmd_vel

启动键盘控制节点:rosrun teleop_twist_keyboard teleop_twist_keyboard.py /cmd_vel:=/turtle1/cmd_vel

启动乌龟显示节点: rosrun turtlesim turtlesim_node

二者可以实现正常通信

2.方案2

将乌龟显示节点的话题设置为 /cmd_vel

启动键盘控制节点:rosrun teleop_twist_keyboard teleop_twist_keyboard.py

启动乌龟显示节点: rosrun turtlesim turtlesim_node /turtle1/cmd_vel:=/cmd_vel

二者可以实现正常通信

4.5.2 launch文件设置话题重映射

launch 文件设置话题重映射语法:

<node pkg="xxx" type="xxx" name="xxx">
    <remap from="原话题" to="新话题" />
</node>

实现teleop_twist_keyboard与乌龟显示节点通信方案由两种：

1.方案1

将 teleop_twist_keyboard 节点的话题设置为/turtle1/cmd_vel

2.方案2

将乌龟显示节点的话题设置为 /cmd_vel

<!-- 启动乌龟GUI与键盘控制节点 -->
<launch>
    <!-- 乌龟GUI -->
    <node pkg = "turtlesim" type = "turtlesim_node" name = "turtle1" output = "screen">
        <remap from = "/turtle1/cmd_vel" to = "/cmd_vel" />
    </node>
    <!-- 键盘控制 -->
    <node pkg = "turtlesim" type = "turtle_teleop_key" name = "key" output = "screen"/>
</launch>

4.5.3 编码设置话题名称

话题的名称与节点的命名空间、节点的名称是有一定关系的，话题名称大致可以分为三种类型:

全局(话题参考ROS系统，与节点命名空间平级)
相对(话题参考的是节点的命名空间，与节点名称平级)
私有(话题参考节点名称，是节点名称的子级)

1.C++ 实现

演示准备:

1.初始化节点设置一个节点名称

ros::init(argc,argv,"hello")

2.设置不同类型的话题

3.启动节点时，传递一个 __ns:= xxx

4.节点启动后，使用 rostopic 查看话题信息

1.1全局名称

**格式:**以/开头的名称，和节点名称无关

比如:/xxx/yyy/zzz

示例1:ros::Publisher pub = nh.advertise<std_msgs::String>("/chatter",1000);

结果1:/chatter

示例2:ros::Publisher pub = nh.advertise<std_msgs::String>("/chatter/money",1000);

结果2:/chatter/money

1.2相对名称

**格式:**非/开头的名称,参考命名空间(与节点名称平级)来确定话题名称

示例1:ros::Publisher pub = nh.advertise<std_msgs::String>("chatter",1000);

结果1:xxx/chatter

示例2:ros::Publisher pub = nh.advertise<std_msgs::String>("chatter/money",1000);

结果2:xxx/chatter/money

1.3私有名称

**格式:**以~开头的名称

示例1:

ros::NodeHandle nh("~");
ros::Publisher pub = nh.advertise<std_msgs::String>("chatter",1000);

结果1:/xxx/hello/chatter

示例2:

ros::NodeHandle nh("~");
ros::Publisher pub = nh.advertise<std_msgs::String>("chatter/money",1000);

结果2:/xxx/hello/chatter/money

PS:当使用~,而话题名称有时/开头时，那么话题名称是绝对的

示例3:

ros::NodeHandle nh("~");
ros::Publisher pub = nh.advertise<std_msgs::String>("/chatter/money",1000);

结果3:/chatter/money

具体代码:

#include "ros/ros.h"
#include "std_msgs/String.h"

/* 
    需求:演示不同类型的话题名称设置
        设置话题名称与命名空间
*/

int main(int argc, char *argv[])
{
    ros::init(argc,argv,"hello");
    //ros::NodeHandle nh;
    //核心:设置不同类型的话题
    //1.全局--话题名需要以 / 开头(也可设置自己的命名空间)，这种情况下和节点(命名空间)无关
    //ros::Publisher pub = nh.advertise<std_msgs::String>("/huati_name",10);
    //ros::Publisher pub = nh.advertise<std_msgs::String>("/yyy/huati_name",10);

    //2.相对--非 / 开头
    //ros::Publisher pub = nh.advertise<std_msgs::String>("huati_name",10);
    //ros::Publisher pub = nh.advertise<std_msgs::String>("yyy/huati_name",10);

    //3.私有--需要创建特定NodeHandel
    ros::NodeHandle nh("~");
    ros::Publisher pub = nh.advertise<std_msgs::String>("huati_name",10);

    while(ros::ok())
    {

    }

    return 0;
}

2.Python 实现

演示准备:

1.初始化节点设置一个节点名称

rospy.init_node("hello")

2.设置不同类型的话题

3.启动节点时，传递一个 __ns:= xxx

4.节点启动后，使用 rostopic 查看话题信息

2.1全局名称

**格式:**以/开头的名称，和节点名称无关

示例1:pub = rospy.Publisher("/chatter",String,queue_size=1000)

结果1:/chatter

示例2:pub = rospy.Publisher("/chatter/money",String,queue_size=1000)

结果2:/chatter/money

2.2相对名称

**格式:**非/开头的名称,参考命名空间(与节点名称平级)来确定话题名称

示例1:pub = rospy.Publisher("chatter",String,queue_size=1000)

结果1:xxx/chatter

示例2:pub = rospy.Publisher("chatter/money",String,queue_size=1000)

结果2:xxx/chatter/money

2.3私有名称

格式: 以~开头的名称

示例1:pub = rospy.Publisher("~chatter",String,queue_size=1000)

结果1:/xxx/hello/chatter

示例2:pub = rospy.Publisher("~chatter/money",String,queue_size=1000)

结果2:/xxx/hello/chatter/money

4.6 ROS参数名称设置

在ROS中节点名称话题名称可能出现重名的情况，同理参数名称也可能重名。

当参数名称重名时，那么就会产生覆盖，如何避免这种情况？

关于参数重名的处理，没有重映射实现，为了尽量的避免参数重名，都是使用为参数名添加前缀的方式，实现类似于话题名称，有全局、相对、和私有三种类型之分。

全局(参数名称直接参考ROS系统，与节点命名空间平级)
相对(参数名称参考的是节点的命名空间，与节点名称平级)
私有(参数名称参考节点名称，是节点名称的子级)

设置参数的方式也有三种:

rosrun 命令
launch 文件
编码实现

三种设置方式前面都已经有所涉及，但是之前没有涉及命名问题，本节将对三者命名的设置逐一演示。

4.6.1 rosrun设置参数

rosrun 在启动节点时，也可以设置参数:

语法: rosrun 包名节点名称 _参数名:=参数值

1.设置参数

启动乌龟显示节点，并设置参数 radius = 100

rosrun turtlesim turtlesim_node _radius:=100

2.运行

rosparam list查看节点信息,显示结果:

/turtlesim/A
/turtlesim/background_b
/turtlesim/background_g
/turtlesim/background_r
Copy

结果显示，参数A前缀节点名称，也就是说rosrun执行设置参数参数名使用的是私有模式

4.6.2 launch文件设置参数

通过 launch 文件设置参数的方式前面已经介绍过了，可以在 node 标签外，或 node 标签中通过 param 或 rosparam 来设置参数。在 node 标签外设置的参数是全局性质的，参考的是 / ，在 node 标签中设置的参数是私有性质的，参考的是 /命名空间/节点名称。

<launch>
    <!-- param 使用 向参数服务器设置参数-->
    <!-- 格式1: launch 下, node 外 (全局) -->
    <param name = "radius" type = "double" value = "0.2" />

    <!-- 添加被执行的节点 -->
    <!-- 乌龟GUI -->
    <node pkg = "turtlesim" type = "turtlesim_node" name = "turtle_GUI" ns = "xxx">
        <!-- 格式2: node 下 -->
        <param name = "radius" type = "double" value = "1.25" />
    </node>

    <node pkg = "turtlesim" type ="turtle_teleop_key" name ="turtle_key" />

</launch>

4.6.3 编码设置参数

编码的方式可以更方便的设置:全局、相对与私有参数。

1.C++实现

在 C++ 中，可以使用 ros::param 或者 ros::NodeHandle 来设置参数。

1.1 ros::param设置参数

设置参数调用API是ros::param::set，该函数中，参数1传入参数名称，参数2是传入参数值，参数1中参数名称设置时，如果以 / 开头，那么就是全局参数，如果以 ~ 开头，那么就是私有参数，既不以 / 也不以 ~ 开头，那么就是相对参数。代码示例:

ros::param::set("/set_A",100); //全局,和命名空间以及节点名称无关
ros::param::set("set_B",100); //相对,参考命名空间
ros::param::set("~set_C",100); //私有,参考命名空间与节点名称

运行时，假设设置的 namespace 为 xxx，节点名称为 yyy，使用 rosparam list 查看:

/set_A
/xxx/set_B
/xxx/yyy/set_C

1.2 ros::NodeHandle设置参数

设置参数时，首先需要创建 NodeHandle 对象，然后调用该对象的 setParam 函数，该函数参数1为参数名，参数2为要设置的参数值，如果参数名以 / 开头，那么就是全局参数，如果参数名不以 / 开头，那么，该参数是相对参数还是私有参数与NodeHandle 对象有关，如果NodeHandle 对象创建时如果是调用的默认的无参构造，那么该参数是相对参数，如果NodeHandle 对象创建时是使用:

ros::NodeHandle nh(“~”)，那么该参数就是私有参数。代码示例:

ros::NodeHandle nh;
nh.setParam("/nh_A",100); //全局,和命名空间以及节点名称无关

nh.setParam("nh_B",100); //相对,参考命名空间

ros::NodeHandle nh_private("~");
nh_private.setParam("nh_C",100);//私有,参考命名空间与节点名称

运行时，假设设置的 namespace 为 xxx，节点名称为 yyy，使用 rosparam list 查看:

/nh_A
/xxx/nh_B
/xxx/yyy/nh_C

2.python实现

python 中关于参数设置的语法实现比 C++ 简洁一些，调用的API时 rospy.set_param，该函数中，参数1传入参数名称，参数2是传入参数值，参数1中参数名称设置时，如果以 / 开头，那么就是全局参数，如果以 ~ 开头，那么就是私有参数，既不以 / 也不以 ~ 开头，那么就是相对参数。代码示例:

rospy.set_param("/py_A",100)  #全局,和命名空间以及节点名称无关
rospy.set_param("py_B",100)  #相对,参考命名空间
rospy.set_param("~py_C",100)  #私有,参考命名空间与节点名称

运行时，假设设置的 namespace 为 xxx，节点名称为 yyy，使用 rosparam list 查看:

/py_A
/xxx/py_B
/xxx/yyy/py_C

4.7 ROS分布式通信

ROS是一个分布式计算环境。一个运行中的ROS系统可以包含分布在多台计算机上多个节点。根据系统的配置方式，任何节点可能随时需要与任何其他节点进行通信。

因此，ROS对网络配置有某些要求：

所有端口上的所有机器之间必须有完整的双向连接。
每台计算机必须通过所有其他计算机都可以解析的名称来公告自己。

实现

1.准备

先要保证不同计算机处于同一网络中，最好分别设置固定IP，如果为虚拟机，需要将网络适配器改为桥接模式；

2.配置文件修改

分别修改不同计算机的 /etc/hosts 文件，在该文件中加入对方的IP地址和计算机名:

主机端:

从机的IP    从机计算机名
Copy

从机端:

主机的IP    主机计算机名
Copy

设置完毕，可以通过 ping 命令测试网络通信是否正常。

IP地址查看名: ifconfig

计算机名称查看: hostname

3.配置主机IP

配置主机的 IP 地址

~/.bashrc 追加

export ROS_MASTER_URI=http://主机IP:11311
export ROS_HOSTNAME=主机IP
Copy

4.配置从机IP

配置从机的 IP 地址，从机可以有多台，每台都做如下设置:

~/.bashrc 追加

export ROS_MASTER_URI=http://主机IP:11311
export ROS_HOSTNAME=从机IP
Copy

测试

1.主机启动 roscore(必须)

2.主机启动订阅节点，从机启动发布节点，测试通信是否正常

3.反向测试，主机启动发布节点，从机启动订阅节点，测试通信是否正常。

第五章 ROS常用组件

在ROS中内置一些比较实用的工具，通过这些工具可以方便快捷的实现某个功能或调试程序，从而提高开发效率，本章主要介绍ROS中内置的如下组件:

TF坐标变换，实现不同类型的坐标系之间的转换；
rosbag 用于录制ROS节点的执行过程并可以重放该过程；
rqt 工具箱，集成了多款图形化的调试工具。

本章预期达成的学习目标:

了解 TF 坐标变换的概念以及应用场景；
能够独立完成TF案例:小乌龟跟随；
可以使用 rosbag 命令或编码的形式实现录制与回放；
能够熟练使用rqt中的图形化工具。

案例演示: 小乌龟跟随实现，该案例是ros中内置案例，终端下键入启动命令

roslaunch turtle_tf2 turtle_tf2_demo_cpp.launch`或`roslaunch turtle_tf2 turtle_tf2_demo.launch

键盘可以控制一只乌龟运动，另一只跟随运动。

5.1 TF坐标变换

机器人系统上，有多个传感器，如激光雷达、摄像头等，有的传感器是可以感知机器人周边的物体方位(或者称之为:坐标，横向、纵向、高度的距离信息)的，以协助机器人定位障碍物，可以直接将物体相对该传感器的方位信息，等价于物体相对于机器人系统或机器人其它组件的方位信息吗？显示是不行的，这中间需要一个转换过程。更具体描述如下:

场景1:雷达与小车

现有一移动式机器人底盘，在底盘上安装了一雷达，雷达相对于底盘的偏移量已知，现雷达检测到一障碍物信息，获取到坐标分别为(x,y,z)，该坐标是以雷达为参考系的，如何将这个坐标转换成以小车为参考系的坐标呢？

场景2:现有一带机械臂的机器人(比如:PR2)需要夹取目标物，当前机器人头部摄像头可以探测到目标物的坐标(x,y,z)，不过该坐标是以摄像头为参考系的，而实际操作目标物的是机械臂的夹具，当前我们需要将该坐标转换成相对于机械臂夹具的坐标，这个过程如何实现？

当然，根据我们高中学习的知识，在明确了不同坐标系之间的的相对关系，就可以实现任何坐标点在不同坐标系之间的转换，但是该计算实现是较为常用的，且算法也有点复杂，因此在 ROS 中直接封装了相关的模块: 坐标变换(TF)。

概念

**tf:**TransForm Frame,坐标变换

**坐标系:**ROS 中是通过坐标系统开标定物体的，确切的将是通过右手坐标系来标定的。

作用

在 ROS 中用于实现不同坐标系之间的点或向量的转换。

案例

**小乌龟跟随案例：**如本章引言部分演示。

说明

在ROS中坐标变换最初对应的是tf，不过在 hydro 版本开始, tf 被弃用，迁移到 tf2,后者更为简洁高效，tf2对应的常用功能包有:

tf2_geometry_msgs:可以将ROS消息转换成tf2消息。

tf2: 封装了坐标变换的常用消息。

tf2_ros:为tf2提供了roscpp和rospy绑定，封装了坐标变换常用的API。

5.1.1 坐标msg消息

订阅发布模型中数据载体 msg 是一个重要实现，首先需要了解一下，在坐标转换实现中常用的 msg:geometry_msgs/TransformStamped和geometry_msgs/PointStamped

前者用于传输坐标系相关位置信息，后者用于传输某个坐标系内坐标点的信息。在坐标变换中，频繁的需要使用到坐标系的相对关系以及坐标点信息。

1.geometry_msgs/TransformStamped

命令行键入:rosmsg info geometry_msgs/TransformStamped

std_msgs/Header header                     #头信息
  uint32 seq                                #|-- 序列号
  time stamp                                #|-- 时间戳
  string frame_id                           #|-- 坐标 ID
string child_frame_id                    #子坐标系的 id
geometry_msgs/Transform transform        #坐标信息
  geometry_msgs/Vector3 translation        #偏移量
    float64 x                                #|-- X 方向的偏移量
    float64 y                                #|-- Y 方向的偏移量
    float64 z                                #|-- Z 方向上的偏移量
  geometry_msgs/Quaternion rotation        #四元数
    float64 x                                
    float64 y                                
    float64 z                                
    float64 w

四元数用于表示坐标的相对姿态

2.geometry_msgs/PointStamped

命令行键入:rosmsg info geometry_msgs/PointStamped

std_msgs/Header header                    #头
  uint32 seq                                #|-- 序号
  time stamp                                #|-- 时间戳
  string frame_id                            #|-- 所属坐标系的 id
geometry_msgs/Point point                #点坐标
  float64 x                                    #|-- x y z 坐标
  float64 y
  float64 z

5.1.2 静态坐标变换

所谓静态坐标变换，是指两个坐标系之间的相对位置是固定的。

需求描述:

现有一机器人模型，核心构成包含主体与雷达，各对应一坐标系，坐标系的原点分别位于主体与雷达的物理中心，已知雷达原点相对于主体原点位移关系如下: x 0.2 y0.0 z0.5。当前雷达检测到一障碍物，在雷达坐标系中障碍物的坐标为 (2.0 3.0 5.0),请问，该障碍物相对于主体的坐标是多少？

实现分析:

坐标系相对关系，可以通过发布方发布
订阅方，订阅到发布的坐标系相对关系，再传入坐标点信息(可以写死)，然后借助于 tf 实现坐标变换，并将结果输出

**实现流程:**C++ 与 Python 实现流程一致

新建功能包，添加依赖
编写发布方实现
编写订阅方实现
执行并查看结果

方案A:C++实现

1.创建功能包

创建项目功能包依赖于 tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs

2.发布方

#include "ros/ros.h"
#include "tf2_ros/static_transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"
/* 
    需求:发布两个坐标系之间的相对关系

    流程:
        1.包含头文件
        2.设置编码 节点初始化 节点句柄
        3.创建发布对象
        4.组织被发布的消息
        5.发布数据
        6.spin()
*/

int main(int argc, char *argv[])
{
    //2.设置编码 节点初始化 节点句柄
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"static_pub");
    ros::NodeHandle nh;

    //3.创建发布对象(此处需包含头文件tf2_ros/static_transform_broadcaster.h)
    //静态坐标转换广播器
    tf2_ros::StaticTransformBroadcaster pub;

    //4.组织被发布的消息(此处需包含头文件geometry_msgs/TransformStamped.h)
    //坐标系信息
    geometry_msgs::TransformStamped tfs;
    //----设置头信息
    tfs.header.stamp = ros::Time::now();
    tfs.header.frame_id = "base_link";//相对坐标系被参考的那个(基坐标系)
    //----设置子级坐标系
    tfs.child_frame_id = "laser_link";
    //----设置子级相对于父级的偏移量
    tfs.transform.translation.x = 0.2;
    tfs.transform.translation.y = 0.0;
    tfs.transform.translation.z = 0.5;
    //四元数设置需要参考欧拉角转换(需包含头文件tf2/LinearMath/Quaternion.h)
    tf2::Quaternion qtn;    //创建四元数对象
    //向该对象设置欧拉角,这个对象可以将欧拉角转换成四元数
    qtn.setRPY(0,0,0);  //欧拉角为弧度
    tfs.transform.rotation.x = qtn.getX();
    tfs.transform.rotation.y = qtn.getY();
    tfs.transform.rotation.z = qtn.getZ();
    tfs.transform.rotation.w = qtn.getW();

    //5.发布数据
    pub.sendTransform(tfs);

    ros::spin();

    return 0;
}

3.发布方测试

(1)运行节点

(2)命令行输入rostopic list，得

/rosout
/rosout_agg
/tf_static

输入rostopic echo /tf_static，得

transforms: 
  - 
    header: 
      seq: 0
      stamp: 
        secs: 1648901683
        nsecs: 423133596
      frame_id: "base_link"
    child_frame_id: "laser_link"
    transform: 
      translation: 
        x: 0.2
        y: 0.0
        z: 0.5
      rotation: 
        x: 0.0
        y: 0.0
        z: 0.0
        w: 1.0
---

(3)另一种方法：命令行键入rviz

4.订阅方

#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "geometry_msgs/PointStamped.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
/*  
    订阅方:订阅发布的坐标系相对关系,传入一个坐标点,调用 tf 实现转换

    流程:
        1.包含头文件
        2.设置编码 节点初始化 节点句柄
        3.创建订阅对象  ---> 订阅坐标系相对关系
        4.组织一个坐标点数据
        5.转换算法,调用 tf 内置实现
        6.输出
*/
int main(int argc, char *argv[])
{
    //2.设置编码 节点初始化 节点句柄
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"static_sub");
    ros::NodeHandle nh;

    //3.创建订阅对象 需导入 tf2_ros/transform_listener.与tf2_ros/buffer.h
    //两者一般结合使用,listener订阅数据，数据通过buffer保存
    //3-1创建一个buffer缓存
    tf2_ros::Buffer buffer;
    //3-2再创建的监听对象(监听对象可以将订阅的数据存入buffer)
    tf2_ros::TransformListener listener(buffer);

    //4.组织一个坐标点数据 需导入geometry_msgs/PointStamped.h
    geometry_msgs::PointStamped ps;
    ps.header.frame_id = "laser_link";
    ps.header.stamp = ros::Time::now();
    ps.point.x = 2.0;
    ps.point.y = 3.0;
    ps.point.z = 5.0;
    //转换之前添加休眠
    //ros::Duration(2).sleep();
    //5.转换算法,调用 tf 内置实现
    ros::Rate rate(10);
    while (ros::ok())
    {
        //核心代码 ---> 将ps转换成相对于base_link的坐标点
        geometry_msgs::PointStamped ps_out;
        /*  
            调用了 buffer 的转换函数 transform
            参数1: 被转换的坐标点
            参数2: 目标坐标系
            返回值: 输出的坐标点

            注意: 
                1.调用时需包含头文件tf2_geometry_msgs/tf2_geometry_msgs.h
                2.运行时抛出异常,"base_link" passed to lookupTransform argument target_frame does not exist.
                    "base_link" 不存在
                原因: 订阅数据需要时间，在调用 transform 转换时坐标系的相对关系没有订阅到,因此出现异常
                解决: 
                    方案一: 在调用转换函数前休眠 ros::Duration(2).sleep();
                    方案二: 进行异常处理
        */
       try
       {
            ps_out = buffer.transform(ps,"base_link");  //需包含tf2_geometry_msgs/tf2_geometry_msgs.h
            //6.最后输出
            ROS_INFO("转换后的坐标值:(%.2f, %.2f, %.2f)",ps_out.point.x,ps_out.point.y,ps_out.point.z);
       }
       catch(const std::exception& e)
       {
           ROS_INFO("异常消息: %s",e.what());
       }


        rate.sleep();
        ros::spinOnce();
    }


    return 0;
}

方案B:Python实现

1.创建功能包

创建项目功能包依赖于 tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs

2.发布方

#! /usr/bin/env python
import rospy
import tf2_ros
import tf
from geometry_msgs.msg import TransformStamped
"""  
    发布方: 发布两个坐标系的相对关系(车辆底盘--base_link  雷达--laser_link)
    流程:
        1.导包
        2.初始化节点
        3.创建发布对象
        4.组织被发布的数据
        5.发布数据
        6.spin()
"""

if __name__ == "__main__":

    #2.初始化节点
    rospy.init_node("static_pub")
    #3.创建发布对象(import tf2_ros)
    pub = tf2_ros.StaticTransformBroadcaster()
    #4.组织被发布的数据(from geometry_msgs.msg import TransformStamped)
    ts = TransformStamped()
    #4-1 header
    ts.header.stamp = rospy.Time.now()
    ts.header.frame_id = "base_link"
    ts.child_frame_id = "laser_link"
    #4-2 transform
    ts.transform.translation.x = 0.2
    ts.transform.translation.y = 0.0
    ts.transform.translation.z = 0.5
    #从欧拉角转换成四元数(import tf)
    qtn = tf.transformations.quaternion_from_euler(0, 0, 0)
    #设置四元数
    ts.transform.rotation.x = qtn[0]
    ts.transform.rotation.y = qtn[1]
    ts.transform.rotation.z = qtn[2]
    ts.transform.rotation.w = qtn[3]
    #5.发布数据
    pub.sendTransform(ts)
    #6.spin()
    rospy.spin()

    pass

3.订阅方

#! /usr/bin/env python
import rospy
import tf2_ros
from tf2_geometry_msgs import PointStamped
"""  
    发布方: 发布两个坐标系的相对关系(车辆底盘--base_link  雷达--laser_link)
    流程:
        1.导包
        2.初始化节点
        3.创建订阅对象
        4.组织一个坐标点数据
        5.转换算法,调用 tf 内置实现
        6.输出
"""

if __name__ == "__main__":

    #2.初始化节点
    rospy.init_node("static_sub")
    #3.创建订阅对象
    #3-1创建缓存对象
    buffer = tf2_ros.Buffer()
    #3-2创建订阅对象(将缓存传入)
    sub = tf2_ros.TransformListener(buffer)

    #4.组织一个坐标点数据
    ps = tf2_geometry_msgs.PointStamped()
    ps.header.stamp = rospy.Time.now()
    ps.header.frame_id = "laser_link"

    ps.point.x = 2.0
    ps.point.y = 3.0
    ps.point.z = 5.0

    #5.转换算法,调用 tf 内置实现
    rate = rospy.Rate(10)
    while not rospy.is_shutdown():
        ps_out = tf2_geometry_msgs.PointStamped()
        try:
            """ 
                转换实现:
                    参数1: 被转换的坐标点
                    参数2: 目标坐标系
                    返回值: 转换后的坐标点
            """
            ps_out = buffer.transform(ps, "base_link")
            rospy.loginfo("转换后的坐标值_P:(%.2f, %.2f, %.2f)",ps_out.point.x,ps_out.point.y,ps_out.point.z)
        except Exception as e:
            rospy.loginfo("错误提示:%s",e)


        rate.sleep()

    pass

补充:

当坐标系之间的相对位置固定时，那么所需参数也是固定的: 父系坐标名称、子级坐标系名称、x偏移量、y偏移量、z偏移量、x 翻滚角度、y俯仰角度、z偏航角度，实现逻辑相同，参数不同，那么 ROS 系统就已经封装好了专门的节点，使用方式如下:

rosrun tf2_ros static_transform_publisher x偏移量 y偏移量 z偏移量 z偏航角度 y俯仰角度 x翻滚角度 父级坐标系 子级坐标系

示例:rosrun tf2_ros static_transform_publisher 0.2 0 0.5 0 0 0 /baselink /laser

也建议使用该种方式直接实现静态坐标系相对信息发布。

5.1.3 动态坐标变换

所谓动态坐标变换，是指两个坐标系之间的相对位置是变化的。

需求描述:

启动 turtlesim_node,该节点中窗体有一个世界坐标系(左下角为坐标系原点)，乌龟是另一个坐标系，键盘控制乌龟运动，将两个坐标系的相对位置动态发布。

实现分析:

乌龟本身不但可以看作坐标系，也是世界坐标系中的一个坐标点
订阅 turtle1/pose,可以获取乌龟在世界坐标系的 x坐标、y坐标、偏移量以及线速度和角速度
将 pose 信息转换成坐标系相对信息并发布

**实现流程:**C++ 与 Python 实现流程一致

新建功能包，添加依赖
创建坐标相对关系发布方(同时需要订阅乌龟位姿信息)
创建坐标相对关系订阅方
执行

方案A:C++实现

1.创建功能包

创建项目功能包依赖于 tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs、turtlesim

2.发布方

#include "ros/ros.h"
#include "turtlesim/Pose.h"
#include "tf2_ros/transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"
/*  
    发布方: 需要订阅乌龟的位姿信息，转换为相对于窗体的坐标关系并发布
    准 备:  
        话题:rostopic list --> turtle1/pose
        消息:rosmsg info turtlesim/Pose -->
            float32 x
            float32 y
            float32 theta
            float32 linear_velocity
            float32 angular_velocity
        流程: 
            1.包含头文件
            2.设置编码,初始化,节点句柄
            3.创建订阅对象,订阅 /turtlesim/pose
            4.回调函数处理订阅的消息:将位姿信息转换为坐标相对关系并发布 --> 关注点
            5.spin()

*/
void doPose(const turtlesim::Pose::ConstPtr& pose)
{
    //获取位姿信息,转换成坐标系相对关系
    // 1)创建发布对象   tf2_ros/transform_broadcaster.h
    static tf2_ros::TransformBroadcaster pub;   //static 静态的每次调用回调函数都使用同一个对象
    // 2）组织被发布的数据
    geometry_msgs::TransformStamped ts;
    ts.header.frame_id = "world";
    ts.header.stamp = ros::Time::now();
    ts.child_frame_id = "turtle1";
    //坐标系偏移量设置
    ts.transform.translation.x = pose->x;
    ts.transform.translation.y = pose->y;
    ts.transform.translation.z = 0;
    //坐标系四元数
    //位姿信息中没有四元数,但有偏航角度,又已知乌龟图二维,无翻滚与俯仰
    //得:乌龟的欧拉角 0 0 theta
    tf2::Quaternion qtn;
    qtn.setRPY(0,0,pose->theta);
    ts.transform.rotation.x = qtn.getX();
    ts.transform.rotation.y = qtn.getY();
    ts.transform.rotation.z = qtn.getZ();
    ts.transform.rotation.w = qtn.getW();
    // 3）发布数据
    pub.sendTransform(ts);

}
int main(int argc, char *argv[])
{
    //2.设置编码,初始化,节点句柄
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"dynamic_pub");
    ros::NodeHandle nh;

    //3.创建订阅对象,订阅 /turtlesim/pose
    ros::Subscriber sub = nh.subscribe("turtle1/pose",100,doPose);

    //4.回调函数处理订阅的消息:将位姿信息转换为坐标相对关系并发布 --> 关注点
    ros::spin();
    return 0;
}

3.订阅方

#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "geometry_msgs/PointStamped.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
/*  
    订阅方:订阅发布的坐标系相对关系,传入一个坐标点,调用 tf 实现转换

    流程:
        1.包含头文件
        2.设置编码 节点初始化 节点句柄
        3.创建订阅对象  ---> 订阅坐标系相对关系
        4.组织一个坐标点数据
        5.转换算法,调用 tf 内置实现
        6.输出
*/
int main(int argc, char *argv[])
{
    //2.设置编码 节点初始化 节点句柄
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"dynamic_sub");
    ros::NodeHandle nh;

    //3.创建订阅对象 需导入 tf2_ros/transform_listener.与tf2_ros/buffer.h
    //两者一般结合使用,listener订阅数据，数据通过buffer保存
    //3-1创建一个buffer缓存
    tf2_ros::Buffer buffer;
    //3-2再创建的监听对象(监听对象可以将订阅的数据存入buffer)
    tf2_ros::TransformListener listener(buffer);

    //4.组织一个坐标点数据 需导入geometry_msgs/PointStamped.h
    geometry_msgs::PointStamped ps;
    ps.header.frame_id = "turtle1";
    ps.header.stamp = ros::Time(0.0);
    ps.point.x = 0;
    ps.point.y = 0;
    ps.point.z = 0;
    //转换之前添加休眠
    //ros::Duration(2).sleep();
    //5.转换算法,调用 tf 内置实现
    ros::Rate rate(1);
    while (ros::ok())
    {
        //核心代码 ---> 将ps转换成相对于base_link的坐标点
        geometry_msgs::PointStamped ps_out;
        /*  
            调用了 buffer 的转换函数 transform
            参数1: 被转换的坐标点
            参数2: 目标坐标系
            返回值: 输出的坐标点

            注意: 
                1.调用时需包含头文件tf2_geometry_msgs/tf2_geometry_msgs.h
                2.运行时抛出异常,"base_link" passed to lookupTransform argument target_frame does not exist.
                    "base_link" 不存在
                原因: 订阅数据需要时间，在调用 transform 转换时坐标系的相对关系没有订阅到,因此出现异常
                解决: 
                    方案一: 在调用转换函数前休眠 ros::Duration(2).sleep();
                    方案二: 进行异常处理
        */
       try
       {
            ps_out = buffer.transform(ps,"world");  //需包含tf2_geometry_msgs/tf2_geometry_msgs.h
            //6.最后输出
            ROS_INFO("转换后的坐标值:(%.2f, %.2f, %.2f)",ps_out.point.x,ps_out.point.y,ps_out.point.z);
       }
       catch(const std::exception& e)
       {
           ROS_INFO("异常消息: %s",e.what());
       }

        rate.sleep();
        ros::spinOnce();
    }


    return 0;
}

方案B:Python实现

1.创建功能包

创建项目功能包依赖于 tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs、turtlesim

2.发布方

#! /usr/bin/env python
import rospy
import tf2_ros
import tf
from geometry_msgs.msg import TransformStamped
from turtlesim.msg import Pose
"""  
    发布方: 订阅乌龟的坐标信息,转换成坐标系的相对关系然后发布
    准 备:  
        话题:rostopic list --> turtle1/pose
        消息:rosmsg info turtlesim/Pose -->
            float32 x
            float32 y
            float32 theta
            float32 linear_velocity
            float32 angular_velocity
    流程:
        1.导包
        2.初始化节点
        3.创建订阅对象
        4.回调函数处理订阅到的消息
        5.spin()
"""
def doPose(pose):
    #获取位姿信息,转换成坐标系相对关系
    # 1)创建发布对象   tf2_ros/transform_broadcaster.h
    pub = tf2_ros.TransformBroadcaster()

    # 2）组织被发布的数据
    ts = TransformStamped()
    ts.header.frame_id = "world"
    ts.header.stamp = rospy.Time.now()
    ts.child_frame_id = "turtle1"

    #坐标系偏移量设置
    ts.transform.translation.x = pose.x
    ts.transform.translation.y = pose.y
    ts.transform.translation.z = 0

    #从欧拉角转换成四元数(import tf)
    qtn = tf.transformations.quaternion_from_euler(0, 0, pose.theta)
    #设置四元数
    ts.transform.rotation.x = qtn[0]
    ts.transform.rotation.y = qtn[1]
    ts.transform.rotation.z = qtn[2]
    ts.transform.rotation.w = qtn[3]

    pub.sendTransform(ts)

if __name__ == "__main__":

    #2.初始化节点
    rospy.init_node("dynamic_pub")
    #3.创建订阅对象
    sub = rospy.Subscriber("turtle1/pose",Pose,doPose,queue_size=100)

    #5.spin()
    rospy.spin()

    pass

3.订阅方

#! /usr/bin/env python
import rospy
import tf2_ros
from tf2_geometry_msgs import PointStamped
"""  
    发布方: 发布两个坐标系的相对关系
    流程:
        1.导包
        2.初始化节点
        3.创建订阅对象
        4.组织一个坐标点数据
        5.转换算法,调用 tf 内置实现
        6.输出
"""

if __name__ == "__main__":

    #2.初始化节点
    rospy.init_node("d_sub")
    #3.创建订阅对象
    #3-1创建缓存对象
    buffer = tf2_ros.Buffer()
    #3-2创建订阅对象(将缓存传入)
    sub = tf2_ros.TransformListener(buffer)

    #4.组织一个坐标点数据
    ps = tf2_geometry_msgs.PointStamped()
    ps.header.stamp = rospy.Time()
    ps.header.frame_id = "turtle1"

    ps.point.x = 0
    ps.point.y = 0
    ps.point.z = 0

    #5.转换算法,调用 tf 内置实现
    rate = rospy.Rate(10)
    while not rospy.is_shutdown():
        ps_out = tf2_geometry_msgs.PointStamped()
        try:
            """ 
                转换实现:
                    参数1: 被转换的坐标点
                    参数2: 目标坐标系
                    返回值: 转换后的坐标点
            """
            ps_out = buffer.transform(ps, "world")
            rospy.loginfo("转换后的坐标值_P:(%.2f, %.2f, %.2f)",ps_out.point.x,ps_out.point.y,ps_out.point.z)
        except Exception as e:
            rospy.loginfo("错误提示:%s",e)


        rate.sleep()

    pass

5.1.4 多坐标变换

需求描述:

现有坐标系统，父级坐标系统 world,下有两子级系统 son1，son2，son1 相对于 world，以及 son2 相对于 world 的关系是已知的，求 son1原点在 son2中的坐标，又已知在 son1中一点的坐标，要求求出该点在 son2 中的坐标

实现分析:

首先，需要发布 son1 相对于 world，以及 son2 相对于 world 的坐标消息
然后，需要订阅坐标发布消息，并取出订阅的消息，借助于 tf2 实现 son1 和 son2 的转换
最后，还要实现坐标点的转换

实现流程: C++ 与 Python 实现流程一致

新建功能包，添加依赖
创建坐标相对关系发布方(需要发布两个坐标相对关系)
创建坐标相对关系订阅方
执行

方案A:C++实现

1.创建功能包

创建项目功能包依赖于 tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs

2.发布方

为了方便，使用静态坐标变换发布

<launch>
    <!-- 发布son1 相对于 world 以及 son2 相对于 world 的坐标关系 -->
    <node pkg = "tf2_ros" type = "static_transform_publisher" name = "son1" args = "5 0 0 0 0 0 /world /son1" output = "screen" />
    <node pkg = "tf2_ros" type = "static_transform_publisher" name = "son2" args = "3 0 0 0 0 0 /world /son2" output = "screen" />

</launch>

3.订阅方

#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "geometry_msgs/PointStamped.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#include "geometry_msgs/TransformStamped.h"
/*  
    订阅方实现:
        1.计算son1与son2的相对关系
        2.计算son1中的某个坐标点在son2中的坐标值

    流程:
        1.包含头文件
        2.编码,初始化,节点句柄
        3.创建订阅对象
        4.编写解析逻辑
        5.spinOnce()

*/
int main(int argc, char *argv[])
{
    //2.编码,初始化,节点句柄
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"tfs_sub");
    ros::NodeHandle nh;
    //3.创建订阅对象    
    tf2_ros::Buffer buffer;     //tf2_ros/buffer.h
    tf2_ros::TransformListener sub(buffer);     //tf2_ros/transform_listener.h
    //4.编写解析逻辑

    //创建坐标点
    geometry_msgs::PointStamped psAtson1;

    psAtson1.header.stamp = ros::Time::now();
    psAtson1.header.frame_id = "son1";
    psAtson1.point.x = 1;
    psAtson1.point.y = 2;
    psAtson1.point.z = 3;

    ros::Rate rate(10);
    while (ros::ok())
    {
        try
        {
            //1.计算son1和son2的相对关系
            /*  
                A 相对于 B
                参数1:目标坐标系   B
                参数2:源坐标系     A
                参数3:ros::Time(0) 取间隔最短的两个坐标关系帧计算相对关系
                返回值:geometry_msgs::TransformStamped 源相对与目标的相对关系
            */
            geometry_msgs::TransformStamped son1Toson2 = buffer.lookupTransform("son2","son1",ros::Time(0));
            ROS_INFO("son1 相对于 son2 的信息: 父级:%s,子级:%s,偏移量:(%.f,%.f,%.f)",
                    son1Toson2.header.frame_id.c_str(),     //son2
                    son1Toson2.child_frame_id.c_str(),
                    son1Toson2.transform.translation.x,
                    son1Toson2.transform.translation.y,
                    son1Toson2.transform.translation.z);     //son1
            //2.son1中的坐标点在son2中的坐标值
            geometry_msgs::PointStamped psAtson2 = buffer.transform(psAtson1,"son2");
            ROS_INFO("坐标点在son2中的值: (%.f,%.f,%.f)",psAtson2.point.x,psAtson2.point.y,psAtson2.point.z);
        }
        catch(const std::exception& e)
        {
            ROS_INFO("错误提示: %s",e.what());
        }

        rate.sleep();
        ros::spinOnce();
    }

    //5.spinOnce()
    return 0;
}

方案B:Python实现

1.创建功能包

创建项目功能包依赖于 tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs

2.发布方

为了方便，使用静态坐标变换发布

<launch>
    <!-- 发布son1 相对于 world 以及 son2 相对于 world 的坐标关系 -->
    <node pkg = "tf2_ros" type = "static_transform_publisher" name = "son1" args = "5 0 0 0 0 0 /world /son1" output = "screen" />
    <node pkg = "tf2_ros" type = "static_transform_publisher" name = "son2" args = "3 0 0 0 0 0 /world /son2" output = "screen" />

</launch>

3.订阅方

#! /usr/bin/env python
import rospy
import tf2_ros
from tf2_geometry_msgs import tf2_geometry_msgs
from geometry_msgs.msg import TransformStamped
"""  
    订阅方实现:
        1.计算son1与son2的相对关系
        2.计算son1中的某个坐标点在son2中的坐标值
    流程:
        1.导包
        2.初始化节点
        3.创建订阅对象
        4.编写解析逻辑
"""


if __name__ == "__main__":

    #2.初始化节点
    rospy.init_node("tfs_sub_p")
    #3.创建订阅对象
    buffer = tf2_ros.Buffer()
    sub = tf2_ros.TransformListener(buffer)
    #4.编写解析逻辑
    psAtson1 = tf2_geometry_msgs.PointStamped()
    psAtson1.header.stamp = rospy.Time.now()
    psAtson1.header.frame_id = "son1"
    psAtson1.point.x = 1
    psAtson1.point.y = 2
    psAtson1.point.z = 3

    rate = rospy.Rate(1)
    while not rospy.is_shutdown():
        try:
            son1Toson2 = buffer.lookup_transform("son2", "son1", rospy.Time(0))
            rospy.loginfo("son1 相对于 son2 的信息: 父级:%s,子级:%s,偏移量:(%.f,%.f,%.f)",
                    son1Toson2.header.frame_id,    
                    son1Toson2.child_frame_id,
                    son1Toson2.transform.translation.x,
                    son1Toson2.transform.translation.y,
                    son1Toson2.transform.translation.z)  

            psAtson2 = buffer.transform(psAtson1, "son2")
            rospy.loginfo("坐标点在son2中的值: (%.2f,%.2f,%.2f)",psAtson2.point.x,psAtson2.point.y,psAtson2.point.z)
        except Exception as e:
            rospy.logwarn("错误提示:%s",e)

        rate.sleep()

5.1.5 坐标系关系查看

在机器人系统中，涉及的坐标系有多个，为了方便查看，ros 提供了专门的工具，可以用于生成显示坐标系关系的 pdf 文件，该文件包含树形结构的坐标系图谱。

6.1准备

首先调用rospack find tf2_tools查看是否包含该功能包，如果没有，请使用如下命令安装:

sudo apt install ros-noetic-tf2-tools

6.2使用

6.2.1生成 pdf 文件

启动坐标系广播程序之后，运行如下命令:

rosrun tf2_tools view_frames.py

会产生类似于下面的日志信息:

[INFO] [1592920556.827549]: Listening to tf data during 5 seconds...
[INFO] [1592920561.841536]: Generating graph in frames.pdf file...

查看当前目录会生成一个 frames.pdf 文件

6.2.2查看文件

可以直接进入目录打开文件，或者调用命令查看文件:evince frames.pdf

内如如图所示:

5.1.6 TF坐标变换实操

需求描述:

程序启动之初: 产生两只乌龟，中间的乌龟(A) 和左下乌龟(B), B 会自动运行至A的位置，并且键盘控制时，只是控制 A 的运动，但是 B 可以跟随 A 运行。

实现分析:

乌龟跟随实现的核心，是乌龟A和B都要发布相对世界坐标系的坐标信息，然后，订阅到该信息需要转换获取A相对于B坐标系的信息，最后，再生成速度信息，并控制B运动。

启动乌龟显示节点
在乌龟显示窗体中生成一只新的乌龟(需要使用服务)
编写两只乌龟发布坐标信息的节点
编写订阅节点订阅坐标信息并生成新的相对关系生成速度信息

实现流程: C++ 与 Python 实现流程一致

新建功能包，添加依赖
编写服务客户端，用于生成一只新的乌龟
编写发布方，发布两只乌龟的坐标信息
编写订阅方，订阅两只乌龟信息，生成速度信息并发布
运行

方案A:C++实现

1.创建功能包

创建项目功能包依赖于 tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs、turtlesim

2.服务客户端(生成乌龟)

#include "ros/ros.h"
#include "turtlesim/Spawn.h"
/*
    需求:向服务端发布请求，生成一只新乌龟
        话题：/spawn
        消息：turtlesim/Spawn

    实现流程:
        1.包含头文件
        2.初始化 ROS 节点
        3.创建节点句柄
        4.创建客户端对象
        5.组织数据并发送
        6.处理响应
*/
int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    //2.初始化 ROS 节点
    ros::init(argc,argv,"client_turtle");
    //3.创建节点句柄
    ros::NodeHandle nh;
    //4.创建客户端对象
    ros::ServiceClient client =  nh.serviceClient<turtlesim::Spawn>("/spawn");
    //5.组织数据并发送
    turtlesim::Spawn sp;
    //5-1组织请求
    sp.request.x = 2.0;   //atui():把字符串转换成整型数的一个函数
    sp.request.y = 4.0;
    sp.request.theta = 1.57;
    sp.request.name = "turtle2";
    //5-2发送请求
    //调用判断服务器状态函数
    //函数1
    //client.waitForExistence();
    //函数2
    ros::service::waitForService("/spawn");

    bool flag = client.call(sp);
    //6.处理响应
    if(flag)
    {
        ROS_INFO("响应成功!");
        //获取结果
        ROS_INFO("新乌龟叫: %s",sp.response.name.c_str());
    } 
    else
    {
        ROS_INFO("处理失败....");
    }

    return 0;
}

3.发布方(发布两只乌龟的坐标信息)

可以订阅乌龟的位姿信息，然后再转换成坐标信息，两只乌龟的实现逻辑相同，只是订阅的话题名称，生成的坐标信息等稍有差异，可以将差异部分通过参数传入:

该节点需要启动两次
每次启动时都需要传入乌龟节点名称(第一次是 turtle1 第二次是 turtle2)

#include "ros/ros.h"
#include "turtlesim/Pose.h"
#include "tf2_ros/transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"
/*  
    发布方: 需要订阅乌龟的位姿信息，转换为相对于窗体的坐标关系并发布
    准 备:  
        话题:rostopic list --> turtle1/pose
        消息:rosmsg info turtlesim/Pose 
        流程: 
            1.包含头文件
            2.设置编码,初始化,节点句柄
            3.创建订阅对象,订阅 /turtlesim/pose
            4.回调函数处理订阅的消息:将位姿信息转换为坐标相对关系并发布 --> 关注点
            5.spin()

*/
std::string turtle_name;
void doPose(const turtlesim::Pose::ConstPtr& pose)
{
    //获取位姿信息,转换成坐标系相对关系
    // 1)创建发布对象   tf2_ros/transform_broadcaster.h
    static tf2_ros::TransformBroadcaster pub;   //static 静态的每次调用回调函数都使用同一个对象
    // 2）组织被发布的数据
    geometry_msgs::TransformStamped ts;
    ts.header.frame_id = "world";
    ts.header.stamp = ros::Time::now();
    //关键点2:动态传入child_frame_id
    ts.child_frame_id = turtle_name;
    //坐标系偏移量设置
    ts.transform.translation.x = pose->x;
    ts.transform.translation.y = pose->y;
    ts.transform.translation.z = 0;
    //坐标系四元数
    //位姿信息中没有四元数,但有偏航角度,又已知乌龟图二维,无翻滚与俯仰
    //得:乌龟的欧拉角 0 0 theta
    tf2::Quaternion qtn;
    qtn.setRPY(0,0,pose->theta);
    ts.transform.rotation.x = qtn.getX();
    ts.transform.rotation.y = qtn.getY();
    ts.transform.rotation.z = qtn.getZ();
    ts.transform.rotation.w = qtn.getW();
    // 3）发布数据
    pub.sendTransform(ts);

}
int main(int argc, char *argv[])
{


    //2.设置编码,初始化,节点句柄
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"pub_turtle");
    ros::NodeHandle nh;
    /*  
        解析launch文件通过 args 传入的参数
    */
    if (argc != 2)
    {
        ROS_ERROR("请传入一个参数!");
        return 1;
    }
    else
    {
        turtle_name = argv[1];
    }
    //3.创建订阅对象,订阅 /turtlesim/pose
    //关键点1: 订阅的话题名称,turtle1或turtle2需要动态传入
    ros::Subscriber sub = nh.subscribe(turtle_name + "/pose",100,doPose);

    //4.回调函数处理订阅的消息:将位姿信息转换为坐标相对关系并发布 --> 关注点
    ros::spin();
    return 0;
}

4.订阅方(解析坐标信息并生成速度信息)

#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "geometry_msgs/PointStamped.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#include "geometry_msgs/TransformStamped.h"
#include "geometry_msgs/Twist.h"
/*  
    需求1:换算出 turtle1 相对于 turtle2 的关系
    需求2:计算角速度和线速度并发布
    流程:
        1.包含头文件
        2.编码,初始化,节点句柄
        3.创建订阅对象
        4.编写解析逻辑
        5.spinOnce()

*/
int main(int argc, char *argv[])
{
    //2.编码,初始化,节点句柄
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"tfs_sub");
    ros::NodeHandle nh;
    //3.创建订阅对象    
    tf2_ros::Buffer buffer;     //tf2_ros/buffer.h
    tf2_ros::TransformListener sub(buffer);     //tf2_ros/transform_listener.h

    //A.创建发布对象
    ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel",100);
    //4.编写解析逻辑
    ros::Rate rate(10);
    while (ros::ok())
    {
        try
        {
            geometry_msgs::TransformStamped tur1Totur2 = buffer.lookupTransform("turtle2","turtle1",ros::Time(0));
            /*
            ROS_INFO("turtle1 相对于 turtle2 的信息: 父级:%s,子级:%s,偏移量:(%.f,%.f,%.f)",
                    son1Toson2.header.frame_id.c_str(),     //turtle2
                    son1Toson2.child_frame_id.c_str(),      //turtle1
                    son1Toson2.transform.translation.x,
                    son1Toson2.transform.translation.y,
                    son1Toson2.transform.translation.z);   */

            //B.根据相对关系计算并组织速度消息
            geometry_msgs::Twist twist;
            twist.linear.x = 0.5 * sqrt(pow(tur1Totur2.transform.translation.x,2) + pow(tur1Totur2.transform.translation.y,2));
            twist.angular.z = 4 * atan2(tur1Totur2.transform.translation.y,tur1Totur2.transform.translation.x);
            //C.发布
            pub.publish(twist);
        }
        catch(const std::exception& e)
        {
            ROS_INFO("错误提示: %s",e.what());
        }

        rate.sleep();
        ros::spinOnce();
    }

    //5.spinOnce()
    return 0;
}

5.运行

使用 launch 文件组织需要运行的节点，内容示例如下:

<launch>
    <!-- 1.启动乌龟GUI节点 -->
    <node pkg = "turtlesim" type = "turtlesim_node" name = "turtle1" output = "screen" />
    <node pkg = "turtlesim" type = "turtle_teleop_key" name = "key" output = "screen" />
    <!-- 2.生成新的乌龟的节点 -->
    <node pkg = "tf04_test" type = "test01_new_turtle" name = "turtle2" output = "screen" />

    <!-- 3.需要启动两个乌龟相对于世界坐标系关系的发布 -->
    <!--  
        基本实现思路:
            1.节点只编写一个
            2.这个节点需要启动两次
            3.节点启动时动态传参: 第一次turtle1 第二次turtle2
    -->
    <node pkg = "tf04_test" type = "test02_pub_turtle" name = "pub1" args = "turtle1" output = "screen" />
    <node pkg = "tf04_test" type = "test02_pub_turtle" name = "pub2" args = "turtle2" output = "screen" />

    <!-- 4.订阅turtle1 turtle2相对于世界坐标系的坐标消息,并转换成 turtle1 相对于 turtle2 的坐标关系,再生成速度消息 -->、
    <node pkg = "tf04_test" type = "test03_control_turtle2" name = "control" output = "screen" />
</launch>

方案B:Python实现

1.创建功能包

创建项目功能包依赖于 tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs、turtlesim

2.服务客户端(生成乌龟)

#! /usr/bin/env python
import rospy
#from turtlesim.srv import *
from turtlesim.srv import Spawn,SpawnRequest,SpawnResponse
"""
    客户端:组织并提交请求,处理服务响应
        1.导包
        2.初始化ros节点
        3.创建客户端对象
        4.组织请求数据并发送请求
        5.处理响应
    解决:
        在ROS中内置了相关函数,可让客户端启动后挂起,等待服务器启动
        函数1
        client.wait_for_service()
        函数2
        rospy.wait_for_service("话题名称")
"""
if __name__ == "__main__":

    #2.初始化ros节点
    rospy.init_node("client_turtle")
    #3.创建客户端对象
    client = rospy.ServiceProxy("/spawn", Spawn)
    #4.组织请求数据并发送请求
    #解析传入的参数
    request = SpawnRequest()
    request.x = 6
    request.y = 4
    request.theta = 2
    request.name = "turtle2"
    #等待服务启动
    #函数1
    #client.wait_for_service()
    #函数2
    rospy.wait_for_service("/spawn")
    try:
        response = client.call(request)
        #5.处理响应
        rospy.loginfo("新乌龟叫: %s",response.name)
    except Exception as e:
        rospy.logerr("请求处理异常!")

    pass

3.发布方(发布两只乌龟的坐标信息)

#! /usr/bin/env python
import rospy
import tf2_ros
import tf
from geometry_msgs.msg import TransformStamped
from turtlesim.msg import Pose
import sys
"""  
    发布方: 订阅乌龟的坐标信息,转换成坐标系的相对关系然后发布
    准 备:  
        话题:rostopic list --> turtle1/pose
        消息:rosmsg info turtlesim/Pose -->
    流程:
        1.导包
        2.初始化节点
        3.创建订阅对象
        4.回调函数处理订阅到的消息
        5.spin()
"""
#接收乌龟名称的变量
turtele_name = ""

def doPose(pose):
    #获取位姿信息,转换成坐标系相对关系
    # 1)创建发布对象   tf2_ros/transform_broadcaster.h
    pub = tf2_ros.TransformBroadcaster()

    # 2）组织被发布的数据
    ts = TransformStamped()
    ts.header.frame_id = "world"
    ts.header.stamp = rospy.Time.now()
    #修改2 
    ts.child_frame_id = turtele_name

    #坐标系偏移量设置
    ts.transform.translation.x = pose.x
    ts.transform.translation.y = pose.y
    ts.transform.translation.z = 0

    #从欧拉角转换成四元数(import tf)
    qtn = tf.transformations.quaternion_from_euler(0, 0, pose.theta)
    #设置四元数
    ts.transform.rotation.x = qtn[0]
    ts.transform.rotation.y = qtn[1]
    ts.transform.rotation.z = qtn[2]
    ts.transform.rotation.w = qtn[3]

    pub.sendTransform(ts)

if __name__ == "__main__":

    #2.初始化节点
    rospy.init_node("dynamic_pub")
    #解析传入的参数(现在传入的参数的个数? 文件全路径 + 传入的参数 + 节点名称 + 日志文件路径)
    if len(sys.argv) != 4:
        rospy.loginfo("参数个数不对!")
        sys.exit(1)
    else:
        turtele_name = sys.argv[1]

    #3.创建订阅对象
    sub = rospy.Subscriber(turtele_name + "/pose",Pose,doPose,queue_size=100)

    #5.spin()
    rospy.spin()

    pass

4.订阅方(解析坐标信息并生成速度信息)

#! /usr/bin/env python
import rospy
import tf2_ros
from tf2_geometry_msgs import tf2_geometry_msgs
from geometry_msgs.msg import TransformStamped,Twist
import math
"""  
    订阅方实现:
        1.计算son1与son2的相对关系
        2.计算son1中的某个坐标点在son2中的坐标值
    流程:
        1.导包
        2.初始化节点
        3.创建订阅对象
        4.编写解析逻辑
"""


if __name__ == "__main__":

    #2.初始化节点
    rospy.init_node("tfs_sub_p")
    #3.创建订阅对象
    buffer = tf2_ros.Buffer()
    sub = tf2_ros.TransformListener(buffer)

    #创建速度消息发布对象
    pub = rospy.Publisher("/turtle2/cmd_vel", Twist, queue_size=100)
    #4.编写解析逻辑

    rate = rospy.Rate(10)
    while not rospy.is_shutdown():
        try:
            tur1Totur2 = buffer.lookup_transform("turtle2", "turtle1", rospy.Time(0))
            """
            rospy.loginfo("turtle1 相对于 turtle2 的信息: 父级:%s,子级:%s,偏移量:(%.f,%.f,%.f)",
                    tur1Totur2.header.frame_id,    
                    tur1Totur2.child_frame_id,
                    tur1Totur2.transform.translation.x,
                    tur1Totur2.transform.translation.y,
                    tur1Totur2.transform.translation.z)  
            """
            #组织 Twist 消息
            twist = Twist()
            twist.linear.x = 0.5 * math.sqrt(math.pow(tur1Totur2.transform.translation.x, 2) + math.pow(tur1Totur2.transform.translation.y, 2))
            twist.angular.z = 4 * math.atan2(tur1Totur2.transform.translation.y, tur1Totur2.transform.translation.x)
            #发布消息
            pub.publish(twist)

        except Exception as e:
            rospy.loginfo("错误提示:%s",e)

        rate.sleep()

5.运行

使用 launch 文件组织需要运行的节点，内容示例如下:

<launch>
    <!-- 1.启动乌龟GUI节点 -->
    <node pkg = "turtlesim" type = "turtlesim_node" name = "turtle1" output = "screen" />
    <node pkg = "turtlesim" type = "turtle_teleop_key" name = "key" output = "screen" />
    <!-- 2.生成新的乌龟的节点 -->
    <node pkg = "tf04_test" type = "test01_new_turtle.py" name = "turtle2" output = "screen" />

    <!-- 3.需要启动两个乌龟相对于世界坐标系关系的发布 -->
    <!--  
        基本实现思路:
            1.节点只编写一个
            2.这个节点需要启动两次
            3.节点启动时动态传参: 第一次turtle1 第二次turtle2
    -->
    <node pkg = "tf04_test" type = "test02_pub_turtle.py" name = "pub1" args = "turtle1" output = "screen" />
    <node pkg = "tf04_test" type = "test02_pub_turtle.py" name = "pub2" args = "turtle2" output = "screen" />

    <!-- 4.订阅turtle1 turtle2相对于世界坐标系的坐标消息,并转换成 turtle1 相对于 turtle2 的坐标关系,再生成速度消息 -->
    <node pkg = "tf04_test" type = "test03_control_turtle2.py" name = "control" output = "screen" />
</launch>

5.2 rosbag

机器人传感器获取到的信息，有时我们可能需要时时处理，有时可能只是采集数据，事后分析，比如:

机器人导航实现中，可能需要绘制导航所需的全局地图，地图绘制实现，有两种方式，方式1：可以控制机器人运动，将机器人传感器感知到的数据时时处理，生成地图信息。方式2：同样是控制机器人运动，将机器人传感器感知到的数据留存，事后，再重新读取数据，生成地图信息。两种方式比较，显然方式2使用上更为灵活方便。

在ROS中关于数据的留存以及读取实现，提供了专门的工具: rosbag。

概念

是用于录制和回放 ROS 主题的一个工具集。

作用

实现了数据的复用，方便调试、测试。

本质

rosbag本质也是ros的节点，当录制时，rosbag是一个订阅节点，可以订阅话题消息并将订阅到的数据写入磁盘文件；当重放时，rosbag是一个发布节点，可以读取磁盘文件，发布文件中的话题消息。

5.2.1 rosbag使用_命令行

需求:

ROS 内置的乌龟案例并操作，操作过程中使用 rosbag 录制，录制结束后，实现重放

实现:

1.准备

创建目录保存录制的文件

mkdir ./xxx
cd xxx

2.开始录制

rosbag record -a -O 目标文件
rosbag record -a -o bags/hello.bag

操作小乌龟一段时间，结束录制使用 ctrl + c，在创建的目录中会生成bag文件。

3.查看文件

rosbag info 文件名

4.回放文件

rosbag play 文件名

重启乌龟节点，会发现，乌龟按照录制时的轨迹运动。

5.2.2 rosbag使用_编码

命令实现不够灵活，可以使用编码的方式，增强录制与回放的灵活性,本节将通过简单的读写实现演示rosbag的编码实现。

方案A:C++实现

1.写 bag

#include "ros/ros.h"
#include "rosbag/bag.h"
#include "std_msgs/String.h"
/*  
    需求:使用 rosbag 向磁盘文件写数据(话题 + 消息)
    流程:
        1.包含头文件
        2.初始化
        3.创建 rosbag 对象
        4.打开文件流
        5.写数据
        6.关闭文件流
*/

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"bag_write");
    ros::NodeHandle nh;
    //3.创建bag对象
    rosbag::Bag bag;
    //4.打开文件流
    bag.open("test.bag",rosbag::BagMode::Write);
    //5.写数据
    std_msgs::String msg;
    msg.data = "hello world";
    /*  
        参数1: 话题
        参数2: 时间戳
        参数3: 消息
    */
    bag.write("/chatter",ros::Time::now(),msg);
    bag.write("/chatter",ros::Time::now(),msg);
    bag.write("/chatter",ros::Time::now(),msg);
    bag.write("/chatter",ros::Time::now(),msg);

    //6.关闭文件流
    bag.close();

    return 0;
}

2.读bag

#include "ros/ros.h"
#include "rosbag/bag.h"
#include "rosbag/view.h"
#include "std_msgs/String.h"
#include "std_msgs/Int32.h"
/*  
    需求:使用 rosbag 读取 磁盘上的 bag 文件
    流程:
        1.包含头文件
        2.初始化
        3.创建 rosbag 对象
        4.打开文件流
        5.读数据
        6.关闭文件流
*/
int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");

    ros::init(argc,argv,"bag_read");
    ros::NodeHandle nh;

    //3.创建 bag 对象
    rosbag::Bag bag;
    //4.打开 bag 文件
    bag.open("/home/rosdemo/demo/test.bag",rosbag::BagMode::Read);
    //5.读数据,取出话题,时间戳,消息
    //先获取消息的集合,再迭代取出消息的字段
    for (rosbag::MessageInstance const m : rosbag::View(bag))
    {
        std::string topic = m.getTopic();
        ros::Time time = m.getTime();
        std_msgs::StringPtr p =  m.instantiate<std_msgs::String>();
        ROS_INFO("解析的内容:话题:%s,时间戳:%.2f,消息:%s",topic.c_str(),time.toSec(),p->data.c_str());
    }

    //6.关闭文件流
    bag.close();
    return 0;
}

方案B:Python实现

1.写 bag

#! /usr/bin/env python
import rospy
import rosbag
from std_msgs.msg import String
"""  
     需求:使用 rosbag 向磁盘文件写数据(话题 + 消息)
    流程:
        1.导包
        2.初始化
        3.创建 rosbag 对象
        4.打开文件流
        5.写数据
        6.关闭文件流
"""

if __name__ == "__main__":
    #2.初始化节点 
    rospy.init_node("write_bag_p")

    #3.创建 rosbag 对象
    bag = rosbag.Bag("hello_p.bag",'w')

    # 写数据
    msg = String()
    msg.data = "hahahaha"

    bag.write("/chatter",msg)
    bag.write("/chatter",msg)
    bag.write("/chatter",msg)
    # 关闭流
    bag.close()

5.3 rqt工具箱

之前，在 ROS 中使用了一些实用的工具,比如: ros_bag 用于录制与回放、tf2_tools 可以生成 TF 树 … 这些工具大大提高了开发的便利性，但是也存在一些问题: 这些工具的启动和使用过程中涉及到一些命令操作，应用起来不够方便，在ROS中，提供了rqt工具箱，在调用工具时以图形化操作代替了命令操作，应用更便利，提高了操作效率，优化了用户体验。

概念

ROS基于 QT 框架，针对机器人开发提供了一系列可视化的工具，这些工具的集合就是rqt

作用

可以方便的实现 ROS 可视化调试，并且在同一窗口中打开多个部件，提高开发效率，优化用户体验。

组成

rqt 工具箱组成有三大部分

rqt——核心实现，开发人员无需关注
rqt_common_plugins——rqt 中常用的工具套件
rqt_robot_plugins——运行中和机器人交互的插件(比如: rviz)

5.3.1 rqt安装启动与基本使用

1.安装

一般只要你安装的是desktop-full版本就会自带工具箱

如果需要安装可以以如下方式安装

$ sudo apt-get install ros-noetic-rqt
$ sudo apt-get install ros-noetic-rqt-common-plugins

2.启动

rqt的启动方式有两种:

方式1:rqt
方式2:rosrun rqt_gui rqt_gui

3.基本使用

启动 rqt 之后，可以通过 plugins 添加所需的插件

5.3.2 rqt常用插件:rqt_graph

**简介:**可视化显示计算图

**启动:**可以在 rqt 的 plugins 中添加，或者使用rqt_graph启动

5.3.3 rqt常用插件:rqt_console

**简介:**rqt_console 是 ROS 中用于显示和过滤日志的图形化插件

**准备:**编写 Node 节点输出各个级别的日志信息

/*  
    ROS 节点:输出各种级别的日志信息

*/
#include "ros/ros.h"

int main(int argc, char *argv[])
{
    ros::init(argc,argv,"log_demo");
    ros::NodeHandle nh;

    ros::Rate r(0.3);
    while (ros::ok())
    {
        ROS_DEBUG("Debug message d");
        ROS_INFO("Info message oooooooooooooo");
        ROS_WARN("Warn message wwwww");
        ROS_ERROR("Erroe message EEEEEEEEEEEEEEEEEEEE");
        ROS_FATAL("Fatal message FFFFFFFFFFFFFFFFFFFFFFFFFFFFF");
        r.sleep();
    }
    return 0;
}

5.3.4 rqt常用插件:rqt_plot

**简介:**图形绘制插件，可以以 2D 绘图的方式绘制发布在 topic 上的数据

**准备:**启动 turtlesim 乌龟节点与键盘控制节点，通过 rqt_plot 获取乌龟位姿

**启动:**可以在 rqt 的 plugins 中添加，或者使用rqt_plot启动

5.3.5 rqt常用插件:rqt_bag

**简介:**录制和重放 bag 文件的图形化插件

**准备:**启动 turtlesim 乌龟节点与键盘控制节点

**启动:**可以在 rqt 的 plugins 中添加，或者使用rqt_bag启动

第六章机器人系统仿真

对于ROS新手而言，可能会有疑问:学习机器人操作系统，实体机器人是必须的吗？答案是否定的，机器人一般价格不菲，为了降低机器人学习、调试成本，在ROS中提供了系统的机器人仿真实现，通过仿真，可以实现大部分需求，本章主要就是围绕“仿真”展开的，比如，本章会介绍:

如何创建并显示机器人模型；
如何搭建仿真环境；
如何实现机器人模型与仿真环境的交互。

本章预期的学习目标如下:

能够独立使用URDF创建机器人模型，并在Rviz和Gazebo中分别显示；
能够使用Gazebo搭建仿真环境；
能够使用机器人模型中的传感器(雷达、摄像头、编码器…)获取仿真环境数据。

6.1 概述

机器人操作系统学习、开发与测试过程中，会遇到诸多问题，比如:

场景1:机器人一般价格不菲，学习ROS要购买一台机器人吗？

场景2:机器人与之交互的外界环境具有多样性，如何实现复杂的环境设计？

场景3:测试时，直接将未经验证的程序部署到实体机器人运行，安全吗？

…

在诸如此类的场景中，ROS中的仿真就显得尤为重要了。

1.概念

机器人系统仿真： 是通过计算机对实体机器人系统进行模拟的技术，在 ROS 中，仿真实现涉及的内容主要有三:对机器人建模(URDF)、创建仿真环境(Gazebo)以及感知环境(Rviz)等系统性实现。

2.作用

2.1仿真优势:

仿真在机器人系统研发过程中占有举足轻重的地位，在研发与测试中较之于实体机器人实现，仿真有如下几点的显著优势:

1.**低成本:**当前机器人成本居高不下，动辄几十万，仿真可以大大降低成本，减小风险

2.**高效:**搭建的环境更为多样且灵活，可以提高测试效率以及测试覆盖率

3.**高安全性:**仿真环境下，无需考虑耗损问题

2.2仿真缺陷:

机器人在仿真环境与实际环境下的表现差异较大，换言之，仿真并不能完全做到模拟真实的物理世界，存在一些"失真"的情况，原因:

1.仿真器所使用的物理引擎目前还不能够完全精确模拟真实世界的物理情况

2.仿真器构建的是关节驱动器（电机&齿轮箱）、传感器与信号通信的绝对理想情况，目前不支持模拟实际硬件缺陷或者一些临界状态等情形

3.相关组件

3.1URDF

URDF是 Unified Robot Description Format 的首字母缩写，直译为统一(标准化)机器人描述格式，可以以一种 XML 的方式描述机器人的部分结构，比如底盘、摄像头、激光雷达、机械臂以及不同关节的自由度…,该文件可以被 C++ 内置的解释器转换成可视化的机器人模型，是 ROS 中实现机器人仿真的重要组件。

3.2rviz

RViz 是 ROS Visualization Tool 的首字母缩写，直译为ROS的三维可视化工具。它的主要目的是以三维方式显示ROS消息，可以将数据进行可视化表达。例如:可以显示机器人模型，可以无需编程就能表达激光测距仪（LRF）传感器中的传感器到障碍物的距离，RealSense、Kinect或Xtion等三维距离传感器的点云数据（PCD， Point Cloud Data），从相机获取的图像值等。

3.3gazebo

Gazebo是一款3D动态模拟器，用于显示机器人模型并创建仿真环境,能够在复杂的室内和室外环境中准确有效地模拟机器人。与游戏引擎提供高保真度的视觉模拟类似，Gazebo提供高保真度的物理模拟，其提供一整套传感器模型，以及对用户和程序非常友好的交互方式。

6.2 URDF集成Rviz基本流程

前面介绍过，URDF 不能单独使用，需要结合 Rviz 或 Gazebo，URDF 只是一个文件，需要在 Rviz 或 Gazebo 中渲染成图形化的机器人模型，当前，首先演示URDF与Rviz的集成使用，因为URDF与Rviz的集成较之于URDF与Gazebo的集成更为简单，后期，基于Rviz的集成实现，我们再进一步介绍URDF语法。

需求描述:

在 Rviz 中显示一个盒状机器人

结果演示:

实现流程：

准备:新建功能包，导入依赖
核心:编写 urdf 文件
核心:在 launch 文件集成 URDF 与 Rviz
在 Rviz 中显示机器人模型

1.创建功能包，导入依赖

创建一个新的功能包，名称自定义，导入依赖包:urdf与xacro

在当前功能包下，再新建几个目录:

urdf: 存储 urdf 文件的目录

meshes:机器人模型渲染文件(暂不使用)

config: 配置文件

launch: 存储 launch 启动文件

2.编写 URDF 文件

新建一个子级文件夹:urdf(可选)，文件夹中添加一个.urdf文件,复制如下内容:

<robot name="mycar">
    <link name="base_link">
        <visual>
            <geometry>
                <box size="0.5 0.2 0.1" />
            </geometry>
        </visual>
    </link>
</robot>

3.在 launch 文件中集成 URDF 与 Rviz

在launch目录下，新建一个 launch 文件，该 launch 文件需要启动 Rviz，并导入 urdf 文件，Rviz 启动后可以自动载入解析urdf文件，并显示机器人模型，核心问题:如何导入 urdf 文件? 在 ROS 中，可以将 urdf 文件的路径设置到参数服务器，使用的参数名是:robot_description,示例代码如下:

<launch>
    <!-- 1.在参数服务器载入 urdf 文件 -->
    <!-- 设置参数 -->
    <param name="robot_description" textfile="$(find urdf01_rviz)/urdf/urdf/demo01_helloworld.urdf" />

    <!-- 2.启动 rviz -->
    <node pkg="rviz" type="rviz" name="rviz" />

</launch>

4.在 Rviz 中显示机器人模型

rviz 启动后，会发现并没有盒装的机器人模型，这是因为默认情况下没有添加机器人显示组件，需要手动添加，添加方式如下:设置完毕后，可以正常显示了。

5.优化 rviz 启动

重复启动launch文件时，Rviz 之前的组件配置信息不会自动保存，需要重复执行步骤4的操作，为了方便使用，可以使用如下方式优化:

首先，将当前配置保存进config目录然后，launch文件中 Rviz 的启动配置添加参数:args,值设置为-d 配置文件路径

<launch>
    <!-- 1.在参数服务器载入 urdf 文件 -->
    <!-- 设置参数 -->
    <param name="robot_description" textfile="$(find urdf01_rviz)/urdf/urdf/demo01_helloworld.urdf" />

    <!-- 2.启动 rviz -->
    <node pkg="rviz" type="rviz" name="rviz" args = "-d $(find urdf01_rviz)/config/show_mycar.rviz"/>

</launch>

再启动时，就可以包含之前的组件配置了，使用更方便快捷。

6.3 URDF语法详解

URDF 文件是一个标准的 XML 文件，在 ROS 中预定义了一系列的标签用于描述机器人模型，机器人模型可能较为复杂，但是 ROS 的 URDF 中机器人的组成却是较为简单，可以主要简化为两部分:连杆(link标签) 与关节(joint标签)，接下来我们就通过案例了解一下 URDF 中的不同标签:

robot 根标签，类似于 launch文件中的launch标签
link 连杆标签
joint 关节标签
gazebo 集成gazebo需要使用的标签

关于gazebo标签，后期在使用 gazebo 仿真时，才需要使用到，用于配置仿真环境所需参数，比如: 机器人材料属性、gazebo插件等，但是该标签不是机器人模型必须的，只有在仿真时才需设置。

6.3.1 URDF语法详解01_robot

urdf 中为了保证 xml 语法的完整性，使用了robot标签作为根标签，所有的 link 和 joint 以及其他标签都必须包含在 robot 标签内,在该标签内可以通过 name 属性设置机器人模型的名称

1.属性

name: 指定机器人模型的名称

2.子标签

其他标签都是子级标签

6.3.2 URDF语法详解02_link

urdf 中的 link 标签用于描述机器人某个部件(也即刚体部分)的外观和物理属性，比如: 机器人底座、轮子、激光雷达、摄像头…每一个部件都对应一个 link, 在 link 标签内，可以设计该部件的形状、尺寸、颜色、惯性矩阵、碰撞参数等一系列属性

1.属性

name —> 为连杆命名

2.子标签

visual —> 描述外观(对应的数据是可视的)
- geometry 设置连杆的形状
  - 标签1: box(盒状)
    - 属性:size=长(x) 宽(y) 高(z)
  - 标签2: cylinder(圆柱)
    - 属性:radius=半径 length=高度
  - 标签3: sphere(球体)
    - 属性:radius=半径
  - 标签4: mesh(为连杆添加皮肤)
    - 属性: filename=资源路径(格式:package:/// /文件 )
- origin 设置偏移量与倾斜弧度
  - 属性1: xyz=x偏移 y偏移 z偏移
  - 属性2: rpy=x翻滚 y俯仰 z偏航 (单位是弧度)
- metrial 设置材料属性(颜色)
  - 属性: name
  - 标签: color
    - 属性: rgba=红绿蓝权重值与透明度 (每个权重值以及透明度取值[0,1])
collision —> 连杆的碰撞属性
Inertial —> 连杆的惯性矩阵

在此，只演示visual使用。

3.案例

**需求:**分别生成长方体、圆柱与球体的机器人部件

    <link name="base_link">
        <visual>
            <!-- 形状 -->
            <geometry>
                <!-- 长方体的长宽高 -->
                <!-- <box size="0.5 0.3 0.1" /> -->
                <!-- 圆柱，半径和长度 -->
                <!-- <cylinder radius="0.5" length="0.1" /> -->
                <!-- 球体，半径-->
                <!-- <sphere radius="0.3" /> -->

            </geometry>
            <!-- xyz坐标 rpy翻滚俯仰与偏航角度(3.14=180度 1.57=90度) -->
            <origin xyz="0 0 0" rpy="0 0 0" />
            <!-- 颜色: r=red g=green b=blue a=alpha -->
            <material name="black">
                <color rgba="0.7 0.5 0 0.5" />
            </material>
        </visual>
    </link>

6.3.3 URDF语法详解03_joint

urdf 中的 joint 标签用于描述机器人关节的运动学和动力学属性，还可以指定关节运动的安全极限，机器人的两个部件(分别称之为 parent link 与 child link)以"关节"的形式相连接，不同的关节有不同的运动形式: 旋转、滑动、固定、旋转速度、旋转角度限制…,比如:安装在底座上的轮子可以360度旋转，而摄像头则可能是完全固定在底座上。

joint标签对应的数据在模型中是不可见的

1.属性

name —> 为关节命名
type —> 关节运动形式
- continuous: 旋转关节，可以绕单轴无限旋转
- revolute: 旋转关节，类似于 continues,但是有旋转角度限制
- prismatic: 滑动关节，沿某一轴线移动的关节，有位置极限
- planer: 平面关节，允许在平面正交方向上平移或旋转
- floating: 浮动关节，允许进行平移、旋转运动
- fixed: 固定关节，不允许运动的特殊关节

2.子标签

parent(必需的)

parent link的名字是一个强制的属性：
- link:父级连杆的名字，是这个link在机器人结构树中的名字。
child(必需的)

child link的名字是一个强制的属性：
- link:子级连杆的名字，是这个link在机器人结构树中的名字。
origin
- 属性: xyz=各轴线上的偏移量 rpy=各轴线上的偏移弧度。
axis
- 属性: xyz用于设置围绕哪个关节轴运动。

3.案例

**需求:**创建机器人模型，底盘为长方体，在长方体的前面添加一摄像头，摄像头可以沿着 Z 轴 360 度旋转。

<!-- 
    需求: 创建机器人模型，底盘为长方体，
         在长方体的前面添加一摄像头，
         摄像头可以沿着 Z 轴 360 度旋转
 -->
<robot name = "mycar">
    <!-- 底盘 -->
    <link name = "base_link">
        <visual>
            <geometry>
                <box size = "0.3 0.2 0.1" />
            </geometry>
            <origin xyz = "0 0 0" rpy = "0 0 0" />
            <material name = "blue">
                <color rgba = "0.8 0.5 0 0.5" />
            </material>
        </visual>
    </link>

    <!-- 摄像头 -->
    <link name = "camera">
        <visual>
            <geometry>
                <box size = "0.02 0.05 0.05" />
            </geometry>
            <!-- 先使用默认(后期需要修改) -->
            <origin xyz = "0 0 0.025" rpy="0 0 0" />
            <material name = "camera_cloor">
                <color rgba = "0 0 1 0.5" />
            </material>
        </visual>
    </link>

    <!-- 关节 -->
    <joint name = "camera2baselink" type = "continuous">
        <parent link = "base_link"/>
        <child link = "camera" />
        <!-- 需要计算两个 link 的物理中心之间的偏移量 -->
        <origin xyz = "0.12 0 0.05" rpy = "0 0 0" />
        <!-- 关节旋转参考的坐标轴 -->
        <axis xyz="0 0 1" />
    </joint>

</robot>

launch文件示例如下:

<launch>
    <!-- 1.在参数服务器载入 urdf 文件 -->
    <!-- 设置参数 -->
    <param name="robot_description" textfile="$(find urdf01_rviz)/urdf/urdf/demo03_joint.urdf" />

    <!-- 2.启动 rviz -->
    <node pkg="rviz" type="rviz" name="rviz" args = "-d $(find urdf01_rviz)/config/show_mycar.rviz"/>\

    <!--  
        只有上述两条语句:
            1.摄像头显示位置与颜色异常
            2.提示:No transform from [camera] to [base_link] 缺少 camera 到 base_link 的坐标变换
              原因:rviz 中显示 URDF 时,必须发布不同部件之间的坐标系关系
              解决:ROS中提供了机器人模型显示的坐标与发布节点(两个)
    -->
    <!-- 关节状态发布节点 -->
    <node pkg = "joint_state_publisher" type = "joint_state_publisher" name = "joint_state_publisher" />
    <!-- 机器人状态发布节点 -->
    <node pkg = "robot_state_publisher" type = "robot_state_publisher" name = "robot_state_publisher" />
    <!-- 添加控制关节运动的节点(可选 用于测试) -->
    <node pkg = "joint_state_publisher_gui" type = "joint_state_publisher_gui" name = "joint_state_publisher_gui" />

</launch>

4.base_footprint优化urdf

前面实现的机器人模型是半沉到地下的，因为默认情况下: 底盘的中心点位于地图原点上，所以会导致这种情况产生，可以使用的优化策略，将初始 link 设置为一个尺寸极小的 link(比如半径为 0.001m 的球体，或边长为 0.001m 的立方体)，然后再在初始 link 上添加底盘等刚体，这样实现，虽然仍然存在初始link半沉的现象，但是基本可以忽略了。这个初始 link 一般称之为 base_footprint

<!-- 
    需求: 创建机器人模型，底盘为长方体，
         在长方体的前面添加一摄像头，
         摄像头可以沿着 Z 轴 360 度旋转
 -->
<robot name = "mycar">
    <!-- 添加一个尺寸极小的link,再去关联初始link与base_link,
        关节的高度刚好和base_link下沉的高度一致(半个底盘高度) 
    -->
    <link name = "base_footprint">
        <visual>
            <geometry>
                <box size = "0.001 0.001 0.001" />
            </geometry>
        </visual>
    </link>
    <!-- 底盘 -->
    <link name = "base_link">
        <visual>
            <geometry>
                <box size = "0.3 0.2 0.1" />
            </geometry>
            <origin xyz = "0 0 0" rpy = "0 0 0" />
            <material name = "blue">
                <color rgba = "0.8 0.5 0 0.5" />
            </material>
        </visual>
    </link>

    <!-- 摄像头 -->
    <link name = "camera">
        <visual>
            <geometry>
                <box size = "0.02 0.05 0.05" />
            </geometry>
            <!-- 先使用默认(后期需要修改) -->
            <origin xyz = "0 0 0.025" rpy="0 0 0" />
            <material name = "camera_cloor">
                <color rgba = "0 0 1 0.5" />
            </material>
        </visual>
    </link>
    <!-- 关联 base_footprint 与 base_link -->
    <joint name = "link2footprint" type = "fixed">
        <parent link = "base_footprint"/>
        <child link = "base_link" />
        <!-- 需要计算两个 link 的物理中心之间的偏移量 -->
        <origin xyz = "0 0 0.05" rpy = "0 0 0" />
    </joint>  

    <!-- 关节 -->
    <joint name = "camera2baselink" type = "continuous">
        <parent link = "base_link"/>
        <child link = "camera" />
        <!-- 需要计算两个 link 的物理中心之间的偏移量 -->
        <origin xyz = "0.12 0 0.05" rpy = "0 0 0" />
        <!-- 关节旋转参考的坐标轴 -->
        <axis xyz="0 0 1" />
    </joint>

</robot>

launch 文件内容不变。

6.3.4 URDF练习

需求描述:

创建一个四轮圆柱状机器人模型，机器人参数如下,底盘为圆柱状，半径 10cm，高 8cm，四轮由两个驱动轮和两个万向支撑轮组成，两个驱动轮半径为 3.25cm,轮胎宽度1.5cm，两个万向轮为球状，半径 0.75cm，底盘离地间距为 1.5cm(与万向轮直径一致)

结果演示:实现流程:

创建机器人模型可以分步骤实现

新建 urdf 文件，并与 launch 文件集成
搭建底盘
在底盘上添加两个驱动轮
在底盘上添加两个万向轮

<launch>
    <!-- 1.在参数服务器载入 urdf 文件 -->
    <!-- 设置参数 -->
    <param name="robot_description" textfile="$(find urdf01_rviz)/urdf/urdf/demo05_test.urdf" />

    <!-- 2.启动 rviz -->
    <node pkg="rviz" type="rviz" name="rviz" args = "-d $(find urdf01_rviz)/config/show_mycar.rviz"/>

    <!-- 3.关节状态发布节点 -->
    <node pkg = "joint_state_publisher" type = "joint_state_publisher" name = "joint_state_publisher" />
    <!-- 4.机器人状态发布节点 -->
    <node pkg = "robot_state_publisher" type = "robot_state_publisher" name = "robot_state_publisher" />
    <!-- 5.添加控制关节运动的节点(可选 用于测试) -->
    <node pkg = "joint_state_publisher_gui" type = "joint_state_publisher_gui" name = "joint_state_publisher_gui" />

</launch>

<robot name = "mycar">
    <!-- 1.添加 base_footprint -->
    <link name = "base_footprint">
        <visual>
            <geometry>
                <sphere radius = "0.001" />
            </geometry>
        </visual>
    </link>
    <!-- 2.添加底盘 -->
    <!--  
        形状:圆柱
        半径:0.1m
        高度：0.08m
        离地间距:0.015m
    -->
    <!-- 2-1 link -->
    <link name = "base_link">
        <visual>
            <geometry>
                <cylinder radius = "0.1" length = "0.08" />
            </geometry>
            <origin xyz = "0 0 0" rpy = "0 0 0" />
            <material name = "baselink_color">
                <color rgba = "1.0 0.5 0.3 0.5" />
            </material>
        </visual>
    </link>
    <!-- 2-2 joint -->
    <joint name = "link2footprint" type = "fixed">
        <parent link = "base_footprint"/>
        <child link = "base_link" />
        <!-- 需要计算两个 link 的物理中心之间的偏移量 -->
        <origin xyz = "0 0 0.055" rpy = "0 0 0" />
    </joint>
    <!-- 3.添加驱动轮 -->
    <!--  
        形状:圆柱
        半径:0.0325m
        长度:0.015m
    -->
    <!-- 3-1 link -->
    <link name = "left_wheel">
        <visual>
            <geometry>
                <cylinder radius = "0.0325" length = "0.01" />
            </geometry>
            <origin xyz = "0 0 0" rpy = "1.5708 0 0" />
            <material name = "wheel_color">
                <color rgba = "0 0 0 0.3" />
            </material>
        </visual>
    </link>
    <link name = "right_wheel">
        <visual>
            <geometry>
                <cylinder radius = "0.0325" length = "0.01" />
            </geometry>
            <origin xyz = "0 0 0" rpy = "1.5708 0 0" />
            <material name = "wheel_color">
                <color rgba = "0 0 0 0.3" />
            </material>
        </visual>
    </link>
    <!-- 3-2 joint -->
    <joint name = "left2link" type = "continuous">
        <parent link = "base_link"/>
        <child link = "left_wheel" />
        <!-- 需要计算两个 link 的物理中心之间的偏移量 -->
        <origin xyz = "0 0.1 -0.0225" rpy = "0 0 0" />
        <axis xyz = "0 1 0" />
    </joint>   

    <joint name = "right2link" type = "continuous">
        <parent link = "base_link"/>
        <child link = "right_wheel" />
        <!-- 需要计算两个 link 的物理中心之间的偏移量 -->
        <origin xyz = "0 -0.1 -0.0225" rpy = "0 0 0" />
        <axis xyz = "0 1 0" />
    </joint>  

    <!-- 4.添加万向轮 -->
    <!--  
        形状:球
        半径:0.0075
    -->
    <!-- 4-1 link -->
    <link name = "front_wheel">
        <visual>
            <geometry>
                <sphere radius = "0.0075" />
            </geometry>
            <origin xyz = "0 0 0" rpy = "0 0 0" />
            <material name = "baselink_color">
                <color rgba = "0 0 0 0.3" />
            </material>
        </visual>
    </link>

    <link name = "back_wheel">
        <visual>
            <geometry>
                <sphere radius = "0.0075" />
            </geometry>
            <origin xyz = "0 0 0" rpy = "0 0 0" />
            <material name = "baselink_color">
                <color rgba = "0 0 0 0.3" />
            </material>
        </visual>
    </link>
    <!-- 4-2 joint -->
    <joint name = "front2link" type = "continuous">
        <parent link = "base_link"/>
        <child link = "front_wheel" />
        <!-- 需要计算两个 link 的物理中心之间的偏移量 -->
        <origin xyz = "0.08 0 -0.0475" rpy = "0 0 0" />
        <axis xyz = "0 1 0" />
    </joint> 
    <joint name = "back2link" type = "continuous">
        <parent link = "base_link"/>
        <child link = "back_wheel" />
        <!-- 需要计算两个 link 的物理中心之间的偏移量 -->
        <origin xyz = "-0.08 0 -0.0475" rpy = "0 0 0" />
        <axis xyz = "0 1 0" >
    </joint> 

</robot>

6.3.5 URDF工具

在 ROS 中，提供了一些工具来方便 URDF 文件的编写，比如:

check_urdf命令可以检查复杂的 urdf 文件是否存在语法问题
urdf_to_graphiz命令可以查看 urdf 模型结构，显示不同 link 的层级关系

当然，要使用工具之前，首先需要安装，安装命令:sudo apt install liburdfdom-tools

1.check_urdf 语法检查

进入urdf文件所属目录，调用:check_urdf urdf文件，如果不抛出异常，说明文件合法,否则非法

2.urdf_to_graphiz 结构查看

6.4 URDF优化_xacro

前面 URDF 文件构建机器人模型的过程中，存在若干问题。

问题1:在设计关节的位置时，需要按照一定的公式计算，公式是固定的，但是在 URDF 中依赖于人工计算，存在不便，容易计算失误，且当某些参数发生改变时，还需要重新计算。

问题2:URDF 中的部分内容是高度重复的，驱动轮与支撑轮的设计实现，不同轮子只是部分参数不同，形状、颜色、翻转量都是一致的，在实际应用中，构建复杂的机器人模型时，更是易于出现高度重复的设计，按照一般的编程涉及到重复代码应该考虑封装。

…

如果在编程语言中，可以通过变量结合函数直接解决上述问题，在 ROS 中，已经给出了类似编程的优化方案，称之为:Xacro

概念

Xacro 是 XML Macros 的缩写，Xacro 是一种 XML 宏语言，是可编程的 XML。

原理

Xacro 可以声明变量，可以通过数学运算求解，使用流程控制控制执行顺序，还可以通过类似函数的实现，封装固定的逻辑，将逻辑中需要的可变的数据以参数的方式暴露出去，从而提高代码复用率以及程序的安全性。

作用

较之于纯粹的 URDF 实现，可以编写更安全、精简、易读性更强的机器人模型文件，且可以提高编写效率。

6.4.1 Xacro_快速体验

**目的:**简单了解 xacro 的基本语法。

需求描述:

使用xacro优化上一节案例中驱动轮实现，需要使用变量封装底盘的半径、高度，使用数学公式动态计算底盘的关节点坐标，使用 Xacro 宏封装轮子重复的代码并调用宏创建两个轮子(注意: 在此，演示 Xacro 的基本使用，不必要生成合法的 URDF )。

准备:

创建功能包，导入 urdf 与 xacro。

1.Xacro文件编写

编写 Xacro 文件，以变量的方式封装属性(常量半径、高度、车轮半径…)，以函数的方式封装重复实现(车轮的添加)。

<robot name="mycar" xmlns:xacro="http://wiki.ros.org/xacro">
    <!-- 属性封装 -->
    <xacro:property name="wheel_radius" value="0.0325" />
    <xacro:property name="wheel_length" value="0.0015" />
    <xacro:property name="PI" value="3.1415927" />
    <xacro:property name="base_link_length" value="0.08" />
    <xacro:property name="lidi_space" value="0.015" />

    <!-- 宏 -->
    <xacro:macro name="wheel_func" params="wheel_name flag" >
        <link name="${wheel_name}_wheel">
            <visual>
                <geometry>
                    <cylinder radius="${wheel_radius}" length="${wheel_length}" />
                </geometry>

                <origin xyz="0 0 0" rpy="${PI / 2} 0 0" />

                <material name="wheel_color">
                    <color rgba="0 0 0 0.3" />
                </material>
            </visual>
        </link>

        <!-- 3-2.joint -->
        <joint name="${wheel_name}2link" type="continuous">
            <parent link="base_link"  />
            <child link="${wheel_name}_wheel" />
            <!-- 
                x 无偏移
                y 车体半径
                z z= 车体高度 / 2 + 离地间距 - 车轮半径

            -->
            <origin xyz="0 ${0.1 * flag} ${(base_link_length / 2 + lidi_space - wheel_radius) * -1}" rpy="0 0 0" />
            <axis xyz="0 1 0" />
        </joint>

    </xacro:macro>
    <xacro:wheel_func wheel_name="left" flag="1" />
    <xacro:wheel_func wheel_name="right" flag="-1" />
</robot>

2.Xacro文件转换成 urdf 文件

命令行进入 xacro文件所属目录，执行:rosrun xacro xacro xxx.xacro > xxx.urdf, 会将 xacro 文件解析为 urdf 文件，内容如下:

<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- |    This document was autogenerated by xacro from test.xacro                     | -->
<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
<!-- =================================================================================== -->
<robot name="mycar">
  <link name="left_wheel">
    <visual>
      <geometry>
        <cylinder length="0.0015" radius="0.0325"/>
      </geometry>
      <origin rpy="1.57079635 0 0" xyz="0 0 0"/>
      <material name="wheel_color">
        <color rgba="0 0 0 0.3"/>
      </material>
    </visual>
  </link>
  <!-- 3-2.joint -->
  <joint name="left2link" type="continuous">
    <parent link="base_link"/>
    <child link="left_wheel"/>
    <!-- 
                x 无偏移
                y 车体半径
                z z= 车体高度 / 2 + 离地间距 - 车轮半径

            -->
    <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.0015" radius="0.0325"/>
      </geometry>
      <origin rpy="1.57079635 0 0" xyz="0 0 0"/>
      <material name="wheel_color">
        <color rgba="0 0 0 0.3"/>
      </material>
    </visual>
  </link>
  <!-- 3-2.joint -->
  <joint name="right2link" type="continuous">
    <parent link="base_link"/>
    <child link="right_wheel"/>
    <!-- 
                x 无偏移
                y 车体半径
                z z= 车体高度 / 2 + 离地间距 - 车轮半径

            -->
    <origin rpy="0 0 0" xyz="0 -0.1 -0.0225"/>
    <axis xyz="0 1 0"/>
  </joint>
</robot>

注意: 该案例编写生成的是非法的 URDF 文件，目的在于演示 Xacro 的极简使用以及优点。

6.4.2 Xacro_语法详解

xacro 提供了可编程接口，类似于计算机语言，包括变量声明调用、函数声明与调用等语法实现。在使用 xacro 生成 urdf 时，根标签robot中必须包含命名空间声明:xmlns:xacro="http://wiki.ros.org/xacro"

1.属性与算数运算

用于封装 URDF 中的一些字段，比如: PAI 值，小车的尺寸，轮子半径 …

属性定义

<xacro:property name="xxxx" value="yyyy" />

属性调用

${属性名称}

算数运算

${数学表达式}

<robot name = "mycar" xmlns:xacro = "http://wiki.ros.org/xacro" >
    <!-- 1.属性定义 -->
    <xacro:property name = "PI" value = "3.1415927" />
    <xacro:property name = "radius" value = "0.03" />
    <!-- 2.属性调用 -->
    <myUsePropertyxxx name = "${PI}" />
    <myUsePropertyxxx name = "${radius}" />
    <!-- 3.算术运算 -->
    <myUsePropertyyyy result = "${PI/2}" />

</robot>

2.宏

类似于函数实现，提高代码复用率，优化代码结构，提高安全性

宏定义

<xacro:macro name="宏名称" params="参数列表(多参数之间使用空格分隔)">

    .....

    参数调用格式: ${参数名}

</xacro:macro>

宏调用

<xacro:宏名称 参数1=xxx 参数2=xxx/>

<robot name = "mycar" xmlns:xacro = "http://wiki.ros.org/xacro" >
    <!-- 1.宏定义 -->
    <xacro:macro name = "getSum" params = "num1 num2" >
        <result value = "${num1 + num2}" />
    </xacro:macro>
    <!-- 2.宏调用 -->
    <xacro:getSum num1 = "1" num2 = "2" />

</robot>

3.文件包含

机器人由多部件组成，不同部件可能封装为单独的 xacro 文件，最后再将不同的文件集成，组合为完整机器人，可以使用文件包含实现

文件包含

<robot name = "mycar" xmlns:xacro = "http://wiki.ros.org/xacro" >
    <!-- 演示文件包含 -->
    <xacro:include filename = "demo02_field.urdf.xacro" />
    <xacro:include filename = "demo03_macro.urdf.xacro" />
</robot>

6.4.3 Xacro_完整使用流程示例

需求描述:

使用 Xacro 优化 URDF 版的小车底盘模型实现

1.编写 Xacro 文件

<robot name = "mycar" xmlns:xacro = "http://wiki.ros.org/xacro" >
    <xacro:property name = "footprint_radius" value = "0.001" />
    <!-- 1.添加 base_footprint -->
    <link name = "base_footprint">
        <visual>
            <geometry>
                <sphere radius = "${footprint_radius}" />
            </geometry>
        </visual>
    </link> 

    <!-- 2.添加底盘 -->
    <!--  
        形状:圆柱
        半径:0.1m
        高度：0.08m
        离地间距:0.015m
    -->
    <xacro:property name = "base_radius" value = "0.1" />
    <xacro:property name = "base_length" value = "0.08" />
    <xacro:property name = "height2wall" value = "0.015" />
    <xacro:property name = "base_joint_z" value = "${base_length/2 + height2wall}" />
    <!-- 2-1 link -->
    <link name = "base_link">
        <visual>
            <geometry>
                <cylinder radius = "${base_radius}" length = "${base_length}" />
            </geometry>
            <origin xyz = "0 0 0" rpy = "0 0 0" />
            <material name = "baselink_color">
                <color rgba = "1.0 0.5 0.3 0.5" />
            </material>
        </visual>
    </link>
    <!-- 2-2 joint -->
    <joint name = "link2footprint" type = "fixed">
        <parent link = "base_footprint"/>
        <child link = "base_link" />
        <!-- 需要计算两个 link 的物理中心之间的偏移量 -->
        <origin xyz = "0 0 ${base_joint_z}" rpy = "0 0 0" />
    </joint>
</robot>

2.集成launch文件

**方式1:**先将 xacro 文件转换出 urdf 文件，然后集成

先将 xacro 文件解析成 urdf 文件:rosrun xacro xacro xxx.xacro > xxx.urdf然后再按照之前的集成方式直接整合 launch 文件,内容示例:

<launch>
    <!-- 1.在参数服务器载入 urdf 文件 -->
    <!-- 设置参数 -->
    <param name="robot_description" textfile="$(find urdf01_rviz)/urdf/xacro/demo05_car_base.urdf" />

    <!-- 2.启动 rviz -->
    <node pkg="rviz" type="rviz" name="rviz" args = "-d $(find urdf01_rviz)/config/show_mycar.rviz"/>

    <!-- 3.关节状态发布节点 -->
    <node pkg = "joint_state_publisher" type = "joint_state_publisher" name = "joint_state_publisher" />
    <!-- 4.机器人状态发布节点 -->
    <node pkg = "robot_state_publisher" type = "robot_state_publisher" name = "robot_state_publisher" />
    <!-- 5.添加控制关节运动的节点(可选 用于测试) -->
    <node pkg = "joint_state_publisher_gui" type = "joint_state_publisher_gui" name = "joint_state_publisher_gui" />

</launch>

**方式2:**在 launch 文件中直接加载 xacro(建议使用)

launch 内容示例:

<launch>
    <!-- 1.在参数服务器载入 urdf 文件 -->
    <!-- 设置参数 -->
    <param name="robot_description" command="$(find xacro)/xacro $(find urdf01_rviz)/urdf/xacro/demo05_car_base.urdf.xacro" />

    <!-- 2.启动 rviz -->
    <node pkg="rviz" type="rviz" name="rviz" args = "-d $(find urdf01_rviz)/config/show_mycar.rviz"/>

    <!-- 3.关节状态发布节点 -->
    <node pkg = "joint_state_publisher" type = "joint_state_publisher" name = "joint_state_publisher" />
    <!-- 4.机器人状态发布节点 -->
    <node pkg = "robot_state_publisher" type = "robot_state_publisher" name = "robot_state_publisher" />
    <!-- 5.添加控制关节运动的节点(可选 用于测试) -->
    <node pkg = "joint_state_publisher_gui" type = "joint_state_publisher_gui" name = "joint_state_publisher_gui" />

</launch>

6.4.4 Xacro_实操

需求描述:

在前面小车底盘基础之上，添加摄像头和雷达传感器。

实现分析:

机器人模型由多部件组成，可以将不同组件设置进单独文件，最终通过文件包含实现组件的拼装。

实现流程:

首先编写摄像头和雷达的 xacro 文件
然后再编写一个组合文件，组合底盘、摄像头与雷达
最后，通过 launch 文件启动 Rviz 并显示模型

1.摄像头和雷达 Xacro 文件实现

摄像头 xacro 文件:

<robot name = "mycar" xmlns:xacro = "http://wiki.ros.org/xacro" >
    <!-- 摄像头部件 -->
    <!-- 1.参数 -->
    <!--  
        参数:
            连杆属性: 长 宽 高
            关节: x y z
    -->
    <xacro:property name = "camera_Lx" value = "0.02" />  <!-- x -->
    <xacro:property name = "camera_Wy" value = "0.05" />  <!-- y -->
    <xacro:property name = "camera_Hz" value = "0.05" />  <!-- z -->
    <xacro:property name = "joint_camera_x" value = "0.08" />
    <xacro:property name = "joint_camera_y" value = "0" />
    <xacro:property name = "joint_camera_z" value = "${base_length/2 + camera_Hz/2}" />
    <!-- 2.设计连杆和关节 -->
    <link name = "camera">
        <visual>
            <geometry>
                <box size = "${camera_Lx} ${camera_Wy} ${camera_Hz}" />
            </geometry>

            <material name = "black">
                <color rgba = "0 0 0 0.8" />
            </material>
        </visual>

    </link>

    <joint name = "camera2base" type = "fixed">
        <parent link = "base_link" />
        <child link = "camera" />
        <origin xyz = "${joint_camera_x} ${joint_camera_y} ${joint_camera_z}" />
    </joint>

</robot>

雷达 xacro 文件:

<robot name = "mycar" xmlns:xacro = "http://wiki.ros.org/xacro" >
    <!-- 雷达部件 -->
    <!-- 参数 -->
    <!--  
        1.支架:
            支架尺寸: 半径 高度
            关节偏移量: x y z
        2.雷达
            雷达尺寸:
            关节偏移量
    -->
    <xacro:property name = "support_radius" value = "0.01" />
    <xacro:property name = "support_length" value = "0.15" />
    <xacro:property name = "laser_radius" value = "0.03" />
    <xacro:property name = "laser_length" value = "0.05" />

    <xacro:property name = "joint_support_x" value = "0" />
    <xacro:property name = "joint_support_y" value = "0" />
    <xacro:property name = "joint_support_z" value = "${base_length/2 + support_length/2}" />

    <xacro:property name = "joint_laser_x" value = "0" />
    <xacro:property name = "joint_laser_y" value = "0" />
    <xacro:property name = "joint_laser_z" value = "${support_length/2 + laser_length/2}" />
    <!-- 1.支架 -->
    <link name = "support">
        <visual>
            <geometry>
                <cylinder radius = "${support_radius}" length = "${support_length}" />
            </geometry>
            <material name = "yellow">
                <color rgba = "0.8 0.5 0 0.6" />
            </material>
        </visual>
    </link>
    <joint name = "support2base" type = "fixed">
        <parent link = "base_link" />
        <child link = "support" />
        <origin xyz = "${joint_support_x} ${joint_support_y} ${joint_support_z}" rpy = "0 0 0" />
    </joint>
    <!-- 2.雷达 -->
    <link name = "laser">
        <visual>
            <geometry>
                <cylinder radius = "${laser_radius}" length = "${laser_length}" />
            </geometry>
            <material name = "black">
                <color rgba = "0 0 0 0.6" />
            </material>
        </visual>
    </link>
    <joint name = "laser2support" type = "fixed">
        <parent link = "support" />
        <child link = "laser" />
        <origin xyz = "${joint_laser_x} ${joint_laser_y} ${joint_laser_z}" rpy = "0 0 0" />
    </joint>
</robot>

2.组合底盘摄像头与雷达的 xacro 文件

<robot name = "mycar" xmlns:xacro = "http://wiki.ros.org/xacro" >
    <!-- 包含底盘, 摄像头与雷达的 xacro 文件 -->
    <xacro:include filename = "demo05_car_base.urdf.xacro" />
    <xacro:include filename = "demo06_car_camera.urdf.xacro" />
    <xacro:include filename = "demo07_car_laser.urdf.xacro" />
</robot>

3.launch 文件

<launch>
    <!-- 1.在参数服务器载入 urdf 文件 -->
    <!-- 设置参数 -->
    <param name="robot_description" command="$(find xacro)/xacro $(find urdf01_rviz)/urdf/xacro/car.urdf.xacro" />

    <!-- 2.启动 rviz -->
    <node pkg="rviz" type="rviz" name="rviz" args = "-d $(find urdf01_rviz)/config/show_mycar.rviz"/>

    <!-- 3.关节状态发布节点 -->
    <node pkg = "joint_state_publisher" type = "joint_state_publisher" name = "joint_state_publisher" />
    <!-- 4.机器人状态发布节点 -->
    <node pkg = "robot_state_publisher" type = "robot_state_publisher" name = "robot_state_publisher" />
    <!-- 5.添加控制关节运动的节点(可选 用于测试) -->
    <node pkg = "joint_state_publisher_gui" type = "joint_state_publisher_gui" name = "joint_state_publisher_gui" />

</launch>

6.5 Rviz中控制机器人模型运动

通过 URDF 结合 rviz 可以创建并显示机器人模型，不过，当前实现的只是静态模型，如何控制模型的运动呢？在此，可以调用 Arbotix 实现此功能。

简介

**Arbotix:**Arbotix 是一款控制电机、舵机的控制板，并提供相应的 ros 功能包，这个功能包的功能不仅可以驱动真实的 Arbotix 控制板，它还提供一个差速控制器，通过接受速度控制指令更新机器人的 joint 状态，从而帮助我们实现机器人在 rviz 中的运动。

这个差速控制器在 arbotix_python 程序包中，完整的 arbotix 程序包还包括多种控制器，分别对应 dynamixel 电机、多关节机械臂以及不同形状的夹持器。

Arbotix使用流程

接下来，通过一个案例演示 arbotix 的使用。

需求描述:

控制机器人模型在 rviz 中做圆周运动

结果演示:

实现流程:

安装 Arbotix
创建新功能包，准备机器人 urdf、xacro 文件
添加 Arbotix 配置文件
编写 launch 文件配置 Arbotix
启动 launch 文件并控制机器人模型运动

1.安装 Arbotix

**方式1:**命令行调用

sudo apt-get install ros-<<VersionName()>>-arbotix

sudo apt-get install ros-noetic-arbotix

将 <<VsersionName()>> 替换成当前 ROS 版本名称，如果提示功能包无法定位，请采用方式2。

**方式2:**源码安装

先从 github 下载源码，然后调用 catkin_make 编译

git clone https://github.com/vanadiumlabs/arbotix_ros.git

2.创建新功能包，准备机器人 urdf、xacro

urdf 和 xacro 调用上一讲实现即可

3.添加 arbotix 所需的配置文件

添加 arbotix 所需配置文件

# 该文件是控制器配置,一个机器人模型可能有多个控制器，比如: 底盘、机械臂、夹持器(机械手)....
# 因此，根 name 是 controller
controllers: {
   # 单控制器设置
   base_controller: {
          #类型: 差速控制器
       type: diff_controller,
       #参考坐标
       base_frame_id: base_footprint, 
       #两个轮子之间的间距
       base_width: 0.2,
       #控制频率
       ticks_meter: 2000, 
       #PID控制参数，使机器人车轮快速达到预期速度
       Kp: 12, 
       Kd: 12, 
       Ki: 0, 
       Ko: 50, 
       #加速限制
       accel_limit: 1.0 
    }
}

另请参考: http://wiki.ros.org/arbotix_python/diff_controller

4.launch 文件中配置 arbotix 节点

launch 示例代码

<node name="arbotix" pkg="arbotix_python" type="arbotix_driver" output="screen">
     <rosparam file="$(find my_urdf05_rviz)/config/hello.yaml" command="load" />
     <param name="sim" value="true" />
</node>

代码解释:

调用了 arbotix_python 功能包下的 arbotix_driver 节点

arbotix 驱动机器人运行时，需要获取机器人信息，可以通过 file 加载配置文件

在仿真环境下，需要配置 sim 为 true

5.启动 launch 文件并控制机器人模型运动

**启动launch:**roslaunch xxxx …launch

配置 rviz:

控制小车运动:

此时调用 rostopic list 会发现一个熟悉的话题: /cmd_vel也就说我们可以发布 cmd_vel 话题消息控制小陈运动了，该实现策略有多种，可以另行编写节点，或者更简单些可以直接通过如下命令发布消息:

rostopic pub -r 10 /cmd_vel geometry_msgs/Twist '{linear: {x: 0.2, y: 0, z: 0}, angular: {x: 0, y: 0, z: 0.5}}'

现在，小车就可以运动起来了。

6.6 URDF集成Gazebo

URDF 需要集成进 Rviz 或 Gazebo 才能显示可视化的机器人模型，前面已经介绍了URDF 与 Rviz 的集成，本节主要介绍:

URDF 与 Gazebo 的基本集成流程；
如果要在 Gazebo 中显示机器人模型，URDF 需要做的一些额外配置；
关于Gazebo仿真环境的搭建。

6.6.1 URDF与Gazebo基本集成流程

URDF 与 Gazebo 集成流程与 Rviz 实现类似，主要步骤如下:

创建功能包，导入依赖项
编写 URDF 或 Xacro 文件
启动 Gazebo 并显示机器人模型

1.创建功能包

创建新功能包，导入依赖包: urdf、xacro、gazebo_ros、gazebo_ros_control、gazebo_plugins

2.编写URDF文件

 rosrun gazebo_ros spawn_model -urdf -model car -param robot_description

<robot name = "mycar" >
    <link name = "base_link">
        <visual>
            <geometry>
                <box size = "0.5 0.3 0.1" />
            </geometry>
            <origin xyz = "0 0 0" rpy = "0 0 0 " />
            <material name = "yellow">
                <color rgba = "0.5 0.3 0 0.5" />
            </material>
        </visual>

        <!-- 1.设置碰撞参数 -->
        <!-- 如果是标准几何体,直接复制 visual 的 geometry 和 origin 即可 -->
        <collision>
            <geometry>
                <box size = "0.5 0.3 0.1" />
            </geometry>
            <origin xyz = "0 0 0" rpy = "0 0 0 " />
        </collision>
        <!-- 2.设置惯性矩阵 -->
        <inertial>
            <origin xyz = "0 0 0" />
            <mass value = "2" />
            <inertia ixx = "1" ixy = "0" ixz = "0" iyy = "1" iyz="0"  izz = "1" />
        </inertial>

    </link>
    <!-- gazebo 颜色设置标签 -->
    <gazebo reference = "base_link" >
        <material>Gazebo/Red</material>
    </gazebo>
</robot>

注意，当 URDF 需要与 Gazebo 集成时，和 Rviz 有明显区别:

1.必须使用 collision 标签，因为既然是仿真环境，那么必然涉及到碰撞检测，collision 提供碰撞检测的依据。

2.必须使用 inertial 标签，此标签标注了当前机器人某个刚体部分的惯性矩阵，用于一些力学相关的仿真计算。

3.颜色设置，也需要重新使用 gazebo 标签标注，因为之前的颜色设置为了方便调试包含透明度，仿真环境下没有此选项。

3.启动Gazebo并显示模型

launch 文件实现:

<launch>
    <!-- 1.在参数服务器中载入 urdf -->
    <param name = "robot_description" textfile = "$(find urdf02_gazebo)/urdf/demo01_helloworld.urdf" />
    <!-- 2.启动Gazebo仿真环境 -->
    <include file = "$(find gazebo_ros)/launch/empty_world.launch" />
    <!-- 3.在Gazebo中添加机器人模型 -->
    <node pkg = "gazebo_ros" type = "spawn_model" name = "spawn_model" args = "-urdf -model car -param robot_description" />
</launch>

代码解释:

<include file="$(find gazebo_ros)/launch/empty_world.launch" />
<!-- 启动 Gazebo 的仿真环境，当前环境为空环境 -->

命令rosrun gazebo_ros spawn_model -urdf -model car -param robot_description整合进launch文件如下所示

<node pkg="gazebo_ros" type="spawn_model" name="model" args="-urdf -model mycar -param robot_description"  />

<!-- 
    在 Gazebo 中加载一个机器人模型，该功能由 gazebo_ros 下的 spawn_model 提供:
    -urdf 加载的是 urdf 文件
    -model mycar 模型名称是 mycar
    -param robot_description 从参数 robot_description 中载入模型
    -x 模型载入的 x 坐标
    -y 模型载入的 y 坐标
    -z 模型载入的 z 坐标
-->

6.6.2 URDF集成Gazebo相关设置

较之于 rviz，gazebo在集成 URDF 时，需要做些许修改，比如:必须添加 collision 碰撞属性相关参数、必须添加 inertial 惯性矩阵相关参数，另外，如果直接移植 Rviz 中机器人的颜色设置是没有显示的，颜色设置也必须做相应的变更。

1.collision

如果机器人link是标准的几何体形状，和link的 visual 属性设置一致即可。

2.inertial

惯性矩阵的设置需要结合link的质量与外形参数动态生成，标准的球体、圆柱与立方体的惯性矩阵公式如下(已经封装为 xacro 实现):

球体惯性矩阵

<!-- Macro for inertia matrix -->
    <xacro:macro name="sphere_inertial_matrix" params="m r">
        <inertial>
            <mass value="${m}" />
            <inertia ixx="${2*m*r*r/5}" ixy="0" ixz="0"
                iyy="${2*m*r*r/5}" iyz="0" 
                izz="${2*m*r*r/5}" />
        </inertial>
    </xacro:macro>

圆柱惯性矩阵

<xacro:macro name="cylinder_inertial_matrix" params="m r h">
        <inertial>
            <mass value="${m}" />
            <inertia ixx="${m*(3*r*r+h*h)/12}" ixy = "0" ixz = "0"
                iyy="${m*(3*r*r+h*h)/12}" iyz = "0"
                izz="${m*r*r/2}" /> 
        </inertial>
    </xacro:macro>

立方体惯性矩阵

 <xacro:macro name="Box_inertial_matrix" params="m l w h">
       <inertial>
               <mass value="${m}" />
               <inertia ixx="${m*(h*h + l*l)/12}" ixy = "0" ixz = "0"
                   iyy="${m*(w*w + l*l)/12}" iyz= "0"
                   izz="${m*(w*w + h*h)/12}" />
       </inertial>
   </xacro:macro>

需要注意的是，原则上，除了 base_footprint 外，机器人的每个刚体部分都需要设置惯性矩阵，且惯性矩阵必须经计算得出，如果随意定义刚体部分的惯性矩阵，那么可能会导致机器人在 Gazebo 中出现抖动，移动等现象。

3.颜色设置

在 gazebo 中显示 link 的颜色，必须要使用指定的标签:

<gazebo reference="link节点名称">
     <material>Gazebo/Blue</material>
</gazebo>

**PS：**material 标签中，设置的值区分大小写，颜色可以设置为 Red Blue Green Black …

6.6.3 URDF集成Gazebo实操

需求描述:

将之前的机器人模型(xacro版)显示在 gazebo 中

结果演示:实现流程:

需要编写封装惯性矩阵算法的 xacro 文件
为机器人模型中的每一个 link 添加 collision 和 inertial 标签，并且重置颜色属性
在 launch 文件中启动 gazebo 并添加机器人模型

1.编写封装惯性矩阵算法的 xacro 文件

<robot name="base" xmlns:xacro="http://wiki.ros.org/xacro">
    <!-- Macro for inertia matrix -->
    <xacro:macro name="sphere_inertial_matrix" params="m r">
        <inertial>
            <mass value="${m}" />
            <inertia ixx="${2*m*r*r/5}" ixy="0" ixz="0"
                iyy="${2*m*r*r/5}" iyz="0" 
                izz="${2*m*r*r/5}" />
        </inertial>
    </xacro:macro>

    <xacro:macro name="cylinder_inertial_matrix" params="m r h">
        <inertial>
            <mass value="${m}" />
            <inertia ixx="${m*(3*r*r+h*h)/12}" ixy = "0" ixz = "0"
                iyy="${m*(3*r*r+h*h)/12}" iyz = "0"
                izz="${m*r*r/2}" /> 
        </inertial>
    </xacro:macro>

    <xacro:macro name="Box_inertial_matrix" params="m l w h">
       <inertial>
               <mass value="${m}" />
               <inertia ixx="${m*(h*h + l*l)/12}" ixy = "0" ixz = "0"
                   iyy="${m*(w*w + l*l)/12}" iyz= "0"
                   izz="${m*(w*w + h*h)/12}" />
       </inertial>
   </xacro:macro>
</robot>

2.复制相关 xacro 文件，并设置 collision inertial 以及 color 等参数

A.底盘 Xacro 文件

<robot name = "mycar" xmlns:xacro = "http://wiki.ros.org/xacro" >
    <xacro:property name = "footprint_radius" value = "0.001" />
    <!-- 1.添加 base_footprint -->
    <link name = "base_footprint">
        <visual>
            <geometry>
                <sphere radius = "${footprint_radius}" />
            </geometry>
        </visual>
    </link> 

    <!-- 2.添加底盘 -->
    <!--  
        形状:圆柱
        半径:0.1m
        高度：0.08m
        离地间距:0.015m
    -->
    <xacro:property name = "base_radius" value = "0.1" />
    <xacro:property name = "base_length" value = "0.08" />
    <xacro:property name = "base_mass" value = "2" />
    <xacro:property name = "height2wall" value = "0.015" />
    <xacro:property name = "base_joint_z" value = "${base_length/2 + height2wall}" />
    <!-- 2-1 link -->
    <link name = "base_link">
        <visual>
            <geometry>
                <cylinder radius = "${base_radius}" length = "${base_length}" />
            </geometry>
            <origin xyz = "0 0 0" rpy = "0 0 0" />
            <material name = "baselink_color">
                <color rgba = "1.0 0.5 0.3 0.5" />
            </material>
        </visual>

        <!-- 1.设置碰撞参数 -->
        <!-- 如果是标准几何体,直接复制 visual 的 geometry 和 origin 即可 -->
        <collision>
            <geometry>
                <cylinder radius = "${base_radius}" length = "${base_length}" />
            </geometry>
            <origin xyz = "0 0 0" rpy = "0 0 0" />
        </collision>
        <!-- 2.调用惯性矩阵 -->
        <xacro:cylinder_inertial_matrix m = "${base_mass}" r = "${base_radius}" h = "${base_length}" />
    </link>

    <!-- gazebo 颜色设置标签 -->
    <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" />
        <!-- 需要计算两个 link 的物理中心之间的偏移量 -->
        <origin xyz = "0 0 ${base_joint_z}" rpy = "0 0 0" />
    </joint>

    <!-- 3.添加驱动轮 -->
    <!--  
        形状:圆柱
        半径:0.0325m
        长度:0.015m
    -->
    <!-- 属性 -->
    <xacro:property name = "wheel_radius" value = "0.0325"/>
    <xacro:property name = "wheel_length" value = "0.015"/>
    <xacro:property name = "wheel_mass" value = "0.05" />
    <xacro:property name = "PI" value = "3.141592653"/>
    <xacro:property name = "wheel_joint_z" value = "${base_length/2 + height2wall - wheel_radius}"/>

    <!-- 宏 -->
    <xacro:macro name = "wheel_func" params = "wheel_name flag">
    <!--  
        wheel_name: left or right
        flag: 1 or -1
    -->
    <!-- 3-1 link -->
    <link name = "${wheel_name}_wheel">
        <visual>
            <geometry>
                <cylinder radius = "${wheel_radius}" length = "${wheel_length}" />
            </geometry>
            <origin xyz = "0 0 0" rpy = "${PI/2} 0 0" />
            <material name = "wheel_color">
                <color rgba = "0 0 0 0.3" />
            </material>
        </visual>
        <collision>
            <geometry>
                <cylinder radius = "${wheel_radius}" length = "${wheel_length}" />
            </geometry>
            <origin xyz = "0 0 0" rpy = "${PI/2} 0 0" />
        </collision>
        <!-- 2.调用惯性矩阵 -->
        <xacro:cylinder_inertial_matrix m = "${wheel_mass}" r = "${wheel_radius}" h = "${wheel_length}" />

    </link>
    <gazebo reference = "${wheel_name}_wheel" >
        <material>Gazebo/Red</material>
    </gazebo>

    <!-- 3-2 joint -->
    <joint name = "${wheel_name}2link" type = "continuous">
        <parent link = "base_link"/>
        <child link = "${wheel_name}_wheel" />
        <!-- 需要计算两个 link 的物理中心之间的偏移量 -->
        <origin xyz = "0 ${0.1 * flag} ${-wheel_joint_z}" rpy = "0 0 0" />
        <axis xyz = "0 1 0" />
    </joint>   
    </xacro:macro>

    <xacro:wheel_func wheel_name = "left" flag = "1"/>
    <xacro:wheel_func wheel_name = "right" flag = "-1"/>

    <!-- 4.添加万向轮 -->
    <!--  
        形状:球
        半径:0.0075
    -->
    <xacro:property name = "sm_wheel_radius" value = "0.0075"/>
    <xacro:property name = "sm_wheel_mass" value = "0.01" />
    <xacro:property name = "sm_joint_z" value = "${base_length/2 + height2wall - sm_wheel_radius}"/>

    <!-- 4-1 link -->
    <xacro:macro name = "sm_wheel_func" params = "sm_wheel_name flag">
        <link name = "${sm_wheel_name}_wheel">
            <visual>
                <geometry>
                    <sphere radius = "${sm_wheel_radius}" />
                </geometry>
                <origin xyz = "0 0 0" rpy = "0 0 0" />
                <material name = "baselink_color">
                    <color rgba = "0 0 0 0.3" />
                </material>
            </visual>
            <collision>
                <geometry>
                    <sphere radius = "${sm_wheel_radius}" />
                </geometry>
                <origin xyz = "0 0 0" rpy = "0 0 0" />
            </collision>
            <!-- 2.调用惯性矩阵 -->
            <xacro:sphere_inertial_matrix m = "${sm_wheel_mass}" r = "${sm_wheel_radius}" />

        </link>
        <gazebo reference = "${sm_wheel_name}_wheel" >
            <material>Gazebo/Red</material>
        </gazebo>

        <!-- 4-2 joint -->
        <joint name = "${sm_wheel_name}2link" type = "continuous">
            <parent link = "base_link"/>
            <child link = "${sm_wheel_name}_wheel" />
            <!-- 需要计算两个 link 的物理中心之间的偏移量 -->
            <origin xyz = "${0.08 * flag} 0 ${-sm_joint_z}" rpy = "0 0 0" />
            <axis xyz = "0 1 0" />
        </joint>    
    </xacro:macro>

    <xacro:sm_wheel_func sm_wheel_name = "front" flag = "1"/>
    <xacro:sm_wheel_func sm_wheel_name = "back" flag = "-1"/>
</robot>

B.摄像头 Xacro 文件

<robot name = "mycar" xmlns:xacro = "http://wiki.ros.org/xacro" >
    <!-- 摄像头部件 -->
    <!-- 1.参数 -->
    <!--  
        参数:
            连杆属性: 长 宽 高
            关节: x y z
    -->
    <xacro:property name = "camera_mass" value = "0.05" />
    <xacro:property name = "camera_Lx" value = "0.02" />  <!-- x -->
    <xacro:property name = "camera_Wy" value = "0.05" />  <!-- y -->
    <xacro:property name = "camera_Hz" value = "0.05" />  <!-- z -->
    <xacro:property name = "joint_camera_x" value = "0.08" />
    <xacro:property name = "joint_camera_y" value = "0" />
    <xacro:property name = "joint_camera_z" value = "${base_length/2 + camera_Hz/2}" />
    <!-- 2.设计连杆和关节 -->
    <link name = "camera">
        <visual>
            <geometry>
                <box size = "${camera_Lx} ${camera_Wy} ${camera_Hz}" />
            </geometry>

            <material name = "black">
                <color rgba = "0 0 0 0.8" />
            </material>
        </visual>
        <collision>
            <geometry>
                <box size = "${camera_Lx} ${camera_Wy} ${camera_Hz}" />
            </geometry>
            <origin xyz = "0 0 0" rpy = "0 0 0" />
        </collision>
        <!-- 2.调用惯性矩阵 -->
        <xacro:Box_inertial_matrix m = "${camera_mass}" l = "${camera_Lx}" w = "${camera_Wy}" h = "${camera_Hz}"/>

    </link>
    <gazebo reference = "camera" >
        <material>Gazebo/Blue</material>
    </gazebo>

    <joint name = "camera2base" type = "fixed">
        <parent link = "base_link" />
        <child link = "camera" />
        <origin xyz = "${joint_camera_x} ${joint_camera_y} ${joint_camera_z}" />
    </joint>

</robot>

C.雷达 Xacro 文件

<robot name = "mycar" xmlns:xacro = "http://wiki.ros.org/xacro" >
    <!-- 雷达部件 -->
    <!-- 参数 -->
    <!--  
        1.支架:
            支架尺寸: 半径 高度
            关节偏移量: x y z
        2.雷达
            雷达尺寸:
            关节偏移量
    -->
    <xacro:property name = "support_radius" value = "0.01" />
    <xacro:property name = "support_length" value = "0.15" />
    <xacro:property name = "laser_radius" value = "0.03" />
    <xacro:property name = "laser_length" value = "0.05" />
    <xacro:property name = "laser_mass" value = "0.1" />
    <xacro:property name = "support_mass" value = "0.3" />

    <xacro:property name = "joint_support_x" value = "0" />
    <xacro:property name = "joint_support_y" value = "0" />
    <xacro:property name = "joint_support_z" value = "${base_length/2 + support_length/2}" />

    <xacro:property name = "joint_laser_x" value = "0" />
    <xacro:property name = "joint_laser_y" value = "0" />
    <xacro:property name = "joint_laser_z" value = "${support_length/2 + laser_length/2}" />
    <!-- 1.支架 -->
    <link name = "support">
        <visual>
            <geometry>
                <cylinder radius = "${support_radius}" length = "${support_length}" />
            </geometry>
            <material name = "yellow">
                <color rgba = "0.8 0.5 0 0.6" />
            </material>
        </visual>
        <collision>
            <geometry>
                <cylinder radius = "${support_radius}" length = "${support_length}" />
            </geometry>
        </collision>
        <!-- 2.调用惯性矩阵 -->
        <xacro:cylinder_inertial_matrix m = "${support_mass}" r = "${support_radius}" h = "${support_length}" />
    </link>
    <gazebo reference = "support" >
        <material>Gazebo/Green</material>
    </gazebo>
    <joint name = "support2base" type = "fixed">
        <parent link = "base_link" />
        <child link = "support" />
        <origin xyz = "${joint_support_x} ${joint_support_y} ${joint_support_z}" rpy = "0 0 0" />
    </joint>
    <!-- 2.雷达 -->
    <link name = "laser">
        <visual>
            <geometry>
                <cylinder radius = "${laser_radius}" length = "${laser_length}" />
            </geometry>
            <material name = "black">
                <color rgba = "0 0 0 0.6" />
            </material>
        </visual>
        <collision>
            <geometry>
                <cylinder radius = "${laser_radius}" length = "${laser_length}" />
            </geometry>
        </collision>
        <!-- 2.调用惯性矩阵 -->
        <xacro:cylinder_inertial_matrix m = "${laser_mass}" r = "${laser_radius}" h = "${laser_length}" />

    </link>
    <gazebo reference = "laser" >
        <material>Gazebo/Black</material>
    </gazebo>
    <joint name = "laser2support" type = "fixed">
        <parent link = "support" />
        <child link = "laser" />
        <origin xyz = "${joint_laser_x} ${joint_laser_y} ${joint_laser_z}" rpy = "0 0 0" />
    </joint>
</robot>

D.组合底盘、摄像头与雷达的 Xacro 文件

<robot name = "mycar" xmlns:xacro = "http://wiki.ros.org/xacro" >
    <!-- 包含惯性矩阵文件 -->
    <xacro:include filename = "head.xacro" />
    <!-- 包含底盘, 摄像头与雷达的 xacro 文件 -->
    <xacro:include filename = "demo05_car_base.urdf.xacro" />
    <xacro:include filename = "demo06_car_camera.urdf.xacro" />
    <xacro:include filename = "demo07_car_laser.urdf.xacro" />
</robot>

3.在 gazebo 中执行

launch 文件:

<launch>
    <!-- 1.在参数服务器中载入 urdf -->
    <param name = "robot_description" command = "$(find xacro)/xacro $(find urdf02_gazebo)/urdf/car.urdf.xacro" />
    <!-- 2.启动Gazebo仿真环境 -->
    <include file = "$(find gazebo_ros)/launch/empty_world.launch" />
    <!-- 3.在Gazebo中添加机器人模型 -->
    <node pkg = "gazebo_ros" type = "spawn_model" name = "spawn_model" args = "-urdf -model car -param robot_description" />
</launch>

6.6.4 Gazebo仿真环境搭建

到目前为止，我们已经可以将机器人模型显示在 Gazebo 之中了，但是当前默认情况下，在 Gazebo 中机器人模型是在 empty world 中，并没有类似于房间、家具、道路、树木… 之类的仿真物，如何在 Gazebo 中创建仿真环境呢？

Gazebo 中创建仿真实现方式有两种:

方式1: 直接添加内置组件创建仿真环境
方式2: 手动绘制仿真环境(更为灵活)

也还可以直接下载使用官方或第三方提高的仿真环境插件。

1.添加内置组件创建仿真环境

1.1启动 Gazebo 并添加组件

1.2保存仿真环境

添加完毕后，选择 file —> Save World as 选择保存路径(功能包下: worlds 目录)，文件名自定义，后缀名设置为 .world

1.3 启动

<launch>
    <!-- 1.在参数服务器中载入 urdf -->
    <param name = "robot_description" command = "$(find xacro)/xacro $(find urdf02_gazebo)/urdf/car.urdf.xacro" />
    <!-- 2.启动Gazebo仿真环境 -->
    <include file = "$(find gazebo_ros)/launch/empty_world.launch" >
        <arg name = "world_name" value = "$(find urdf02_gazebo)/worlds/box_house.world" />
    </include>
    <!-- 3.在Gazebo中添加机器人模型 -->
    <node pkg = "gazebo_ros" type = "spawn_model" name = "spawn_model" args = "-urdf -model car -param robot_description" />
</launch>

核心代码: 启动 empty_world 后，再根据arg加载自定义的仿真环境

<include file = "$(find gazebo_ros)/launch/empty_world.launch" >
        <arg name = "world_name" value = "$(find urdf02_gazebo)/worlds/box_house.world" />
</include>

2.自定义仿真环境

2.1 启动 gazebo 打开构建面板，绘制仿真环境

2.2 保存构建的环境

点击: 左上角 file —> Save (保存路径功能包下的: models)

然后 file —> Exit Building Editor

2.3 保存为 world 文件

可以像方式1一样再添加一些插件，然后保存为 world 文件(保存路径功能包下的: worlds)

2.4 启动

同方式1

3.使用官方提供的插件

当前 Gazebo 提供的仿真道具有限，还可以下载官方支持，可以提供更为丰富的仿真实现，具体实现如下:

3.1 下载官方模型库

git clone https://github.com/osrf/gazebo_models

之前是:hg clone https://bitbucket.org/osrf/gazebo_models但是已经不可用

注意: 此过程可能比较耗时

3.2 将模型库复制进 gazebo

将得到的gazebo_models文件夹内容复制到 /usr/share/gazebo-*/models

3.3 应用

重启 Gazebo，选择左侧菜单栏的 insert 可以选择并插入相关道具了

6.7 URDF、Gazebo与Rviz综合应用

关于URDF(Xacro)、Rviz 和 Gazebo 三者的关系，前面已有阐述: URDF 用于创建机器人模型、Rviz 可以显示机器人感知到的环境信息，Gazebo 用于仿真，可以模拟外界环境，以及机器人的一些传感器，如何在 Gazebo 中运行这些传感器，并显示这些传感器的数据(机器人的视角)呢？本节主要介绍的重点就是将三者结合:通过 Gazebo 模拟机器人的传感器，然后在 Rviz 中显示这些传感器感知到的数据。主要内容包括:

运动控制以及里程计信息显示
雷达信息仿真以及显示
摄像头信息仿真以及显示
kinect 信息仿真以及显示

6.7.1 机器人运动控制以及里程计信息显示

gazebo 中已经可以正常显示机器人模型了，那么如何像在 rviz 中一样控制机器人运动呢？在此，需要涉及到ros中的组件: ros_control。

1.ros_control 简介

**场景:**同一套 ROS 程序，如何部署在不同的机器人系统上，比如：开发阶段为了提高效率是在仿真平台上测试的，部署时又有不同的实体机器人平台，不同平台的实现是有差异的，如何保证 ROS 程序的可移植性？ROS 内置的解决方式是 ros_control。

**ros_control:**是一组软件包，它包含了控制器接口，控制器管理器，传输和硬件接口。ros_control 是一套机器人控制的中间件，是一套规范，不同的机器人平台只要按照这套规范实现，那么就可以保证与ROS 程序兼容，通过这套规范，实现了一种可插拔的架构设计，大大提高了程序设计的效率与灵活性。

gazebo 已经实现了 ros_control 的相关接口，如果需要在 gazebo 中控制机器人运动，直接调用相关接口即可

2.运动控制实现流程(Gazebo)

承上，运动控制基本流程:

已经创建完毕的机器人模型，编写一个单独的 xacro 文件，为机器人模型添加传动装置以及控制器
将此文件集成进xacro文件
启动 Gazebo 并发布 /cmd_vel 消息控制机器人运动

2.1 为 joint 添加传动装置以及控制器

两轮差速配置

<robot name="my_car_move" xmlns:xacro="http://wiki.ros.org/xacro">

    <!-- 传动实现:用于连接控制器与关节 -->
    <xacro:macro name="joint_trans" params="joint_name">
        <!-- Transmission is important to link the joints and the controller -->
        <transmission name="${joint_name}_trans">
            <type>transmission_interface/SimpleTransmission</type>
            <joint name="${joint_name}">
                <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
            </joint>
            <actuator name="${joint_name}_motor">
                <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
                <mechanicalReduction>1</mechanicalReduction>
            </actuator>
        </transmission>
    </xacro:macro>

    <!-- 每一个驱动轮都需要配置传动装置 -->
    <xacro:joint_trans joint_name="left2link" />
    <xacro:joint_trans joint_name="right2link" />

    <!-- 控制器 -->
    <gazebo>
        <plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">
            <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>${base_radius * 2}</wheelSeparation> <!-- 车轮间距 -->
            <wheelDiameter>${wheel_radius * 2}</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>

</robot>

2.2 xacro文件集成

最后还需要将上述 xacro 文件集成进总的机器人模型文件，代码示例如下:

<robot name = "mycar" xmlns:xacro = "http://wiki.ros.org/xacro" >
    <!-- 包含惯性矩阵文件 -->
    <xacro:include filename = "head.xacro" />
    <!-- 包含底盘, 摄像头与雷达的 xacro 文件 -->
    <xacro:include filename = "demo05_car_base.urdf.xacro" />
    <xacro:include filename = "demo06_car_camera.urdf.xacro" />
    <xacro:include filename = "demo07_car_laser.urdf.xacro" />

    <!-- 运动控制 -->
    <xacro:include filename = "gazebo/move.xacro" />
</robot>

2.3 启动 gazebo并控制机器人运动

launch文件:

<launch>
    <!-- 1.在参数服务器中载入 urdf -->
    <param name = "robot_description" command = "$(find xacro)/xacro $(find urdf02_gazebo)/urdf/car.urdf.xacro" />
    <!-- 2.启动Gazebo仿真环境 -->
    <include file = "$(find gazebo_ros)/launch/empty_world.launch" >
        <arg name = "world_name" value = "$(find urdf02_gazebo)/worlds/box_house.world" />
    </include>
    <!-- 3.在Gazebo中添加机器人模型 -->
    <node pkg = "gazebo_ros" type = "spawn_model" name = "spawn_model" args = "-urdf -model car -param robot_description" />
</launch>

启动 launch 文件，使用 topic list 查看话题列表，会发现多了 /cmd_vel 然后发布 cmd_vel 消息控制即可

可使用键盘控制(或者可以编写单独的节点控制)

rosrun teleop_twist_keyboard teleop_twist_keyboard.py

6.7.2 雷达信息仿真以及显示

通过 Gazebo 模拟激光雷达传感器，并在 Rviz 中显示激光数据。

实现流程:

雷达仿真基本流程:

已经创建完毕的机器人模型，编写一个单独的 xacro 文件，为机器人模型添加雷达配置；
将此文件集成进xacro文件；
启动 Gazebo，使用 Rviz 显示雷达信息。

1.Gazebo 仿真雷达

1.1 新建 Xacro 文件，配置雷达传感器信息

<robot name="my_sensors" xmlns:xacro="http://wiki.ros.org/xacro">

  <!-- 雷达 -->
  <gazebo reference="laser">
    <sensor type="ray" name="rplidar">
      <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 name="gazebo_rplidar" filename="libgazebo_ros_laser.so">
        <topicName>/scan</topicName>
        <frameName>laser</frameName>
      </plugin>
    </sensor>
  </gazebo>

</robot>
Copy

1.2 xacro 文件集成

将步骤1的 Xacro 文件集成进总的机器人模型文件，代码示例如下:

<!-- 组合小车底盘与传感器 -->
<robot name="my_car_camera" xmlns:xacro="http://wiki.ros.org/xacro">
    <xacro:include filename="my_head.urdf.xacro" />
    <xacro:include filename="my_base.urdf.xacro" />
    <xacro:include filename="my_camera.urdf.xacro" />
    <xacro:include filename="my_laser.urdf.xacro" />
    <xacro:include filename="move.urdf.xacro" />
    <!-- 雷达仿真的 xacro 文件 -->
    <xacro:include filename="my_sensors_laser.urdf.xacro" />
</robot>
Copy

1.3启动仿真环境

编写launch文件，启动gazebo，此处略…

2.Rviz 显示雷达数据

先启动 rviz,添加雷达信息显示插件

6.7.3 摄像头信息仿真以及显示

通过 Gazebo 模拟摄像头传感器，并在 Rviz 中显示摄像头数据。

实现流程:

摄像头仿真基本流程:

已经创建完毕的机器人模型，编写一个单独的 xacro 文件，为机器人模型添加摄像头配置；
将此文件集成进xacro文件；
启动 Gazebo，使用 Rviz 显示摄像头信息。

1.Gazebo 仿真摄像头

1.1 新建 Xacro 文件，配置摄像头传感器信息

<robot name="my_sensors" xmlns:xacro="http://wiki.ros.org/xacro">
  <!-- 被引用的link -->
  <gazebo reference="camera">
    <!-- 类型设置为 camara -->
    <sensor type="camera" name="camera_node">
      <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 name="gazebo_camera" filename="libgazebo_ros_camera.so">
        <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>
</robot>
Copy

1.2 xacro 文件集成

将步骤1的 Xacro 文件集成进总的机器人模型文件，代码示例如下:

<!-- 组合小车底盘与传感器 -->
<robot name="my_car_camera" xmlns:xacro="http://wiki.ros.org/xacro">
    <xacro:include filename="my_head.urdf.xacro" />
    <xacro:include filename="my_base.urdf.xacro" />
    <xacro:include filename="my_camera.urdf.xacro" />
    <xacro:include filename="my_laser.urdf.xacro" />
    <xacro:include filename="move.urdf.xacro" />
    <!-- 摄像头仿真的 xacro 文件 -->
    <xacro:include filename="my_sensors_camara.urdf.xacro" />
</robot>
Copy

1.3启动仿真环境

编写launch文件，启动gazebo，此处略…

6.7.4 kinect信息仿真以及显示

通过 Gazebo 模拟kinect摄像头，并在 Rviz 中显示kinect摄像头数据。

实现流程:

kinect摄像头仿真基本流程:

已经创建完毕的机器人模型，编写一个单独的 xacro 文件，为机器人模型添加kinect摄像头配置；
将此文件集成进xacro文件；
启动 Gazebo，使用 Rviz 显示kinect摄像头信息。

1.Gazebo仿真Kinect

1.1 新建 Xacro 文件，配置 kinetic传感器信息

<robot name="my_sensors" xmlns:xacro="http://wiki.ros.org/xacro">
    <gazebo reference="kinect link名称">  
      <sensor type="depth" name="camera">
        <always_on>true</always_on>
        <update_rate>20.0</update_rate>
        <camera>
          <horizontal_fov>${60.0*PI/180.0}</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 name="kinect_camera_controller" filename="libgazebo_ros_openni_kinect.so">
          <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>kinect link名称</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>
Copy

1.2 xacro 文件集成

将步骤1的 Xacro 文件集成进总的机器人模型文件，代码示例如下:

<!-- 组合小车底盘与传感器 -->
<robot name="my_car_camera" xmlns:xacro="http://wiki.ros.org/xacro">
    <xacro:include filename="my_head.urdf.xacro" />
    <xacro:include filename="my_base.urdf.xacro" />
    <xacro:include filename="my_camera.urdf.xacro" />
    <xacro:include filename="my_laser.urdf.xacro" />
    <xacro:include filename="move.urdf.xacro" />
    <!-- kinect仿真的 xacro 文件 -->
    <xacro:include filename="my_sensors_kinect.urdf.xacro" />
</robot>
Copy

1.3启动仿真环境

编写launch文件，启动gazebo，此处略…

2 Rviz 显示 Kinect 数据

启动 rviz,添加摄像头组件查看数据

3.补充:kinect 点云数据显示

在kinect中也可以以点云的方式显示感知周围环境，在 rviz 中操作如下:

**问题:**在rviz中显示时错位。

**原因:**在kinect中图像数据与点云数据使用了两套坐标系统，且两套坐标系统位姿并不一致。

解决:

1.在插件中为kinect设置坐标系，修改配置文件的<frameName>标签内容：

<frameName>support_depth</frameName>
Copy

2.发布新设置的坐标系到kinect连杆的坐标变换关系，在启动rviz的launch中，添加:

<node pkg="tf2_ros" type="static_transform_publisher" name="static_transform_publisher" args="0 0 0 -1.57 0 -1.57 /support /support_depth" />
Copy

3.启动rviz，重新显示。

第七章机器人导航(仿真)

导航是机器人系统中最重要的模块之一，比如现在较为流行的服务型室内机器人，就是依赖于机器人导航来实现室内自主移动的，本章主要就是介绍仿真环境下的导航实现，主要内容有:

导航相关概念
导航实现:机器人建图(SLAM)、地图服务、定位、路径规划…以可视化操作为主。
导航消息:了解地图、里程计、雷达、摄像头等相关消息格式。

预期达成的学习目标:

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

7.1 概述

1.概念

在ROS中机器人导航(Navigation)由多个功能包组合实现，ROS 中又称之为导航功能包集，关于导航模块，官方介绍如下:

一个二维导航堆栈，它接收来自里程计、传感器流和目标姿态的信息，并输出发送到移动底盘的安全速度命令。

更通俗的讲: 导航其实就是机器人自主的从 A 点移动到 B 点的过程。

2.作用

秉着"不重复发明轮子"的原则，ROS 中导航相关的功能包集为机器人导航提供了一套通用的实现，开发者不再需要关注于导航算法、硬件交互… 等偏复杂、偏底层的实现，这些实现都由更专业的研发人员管理、迭代和维护，开发者可以更专注于上层功能，而对于导航功能的调用，只需要根据自身机器人相关参数合理设置各模块的配置文件即可，当然，如果有必要，也可以基于现有的功能包二次开发实现一些定制化需求，这样可以大大提高研发效率，缩短产品落地时间。总而言之，对于一般开发者而言，ROS 的导航功能包集优势如下:

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

7.1.1导航模块简介

机器人是如何实现导航的呢？或换言之，机器人是如何从 A 点移动到 B 点呢？ROS 官方为了提供了一张导航功能包集的图示,该图中囊括了 ROS 导航的一些关键技术:

假定我们已经以特定方式配置机器人，导航功能包集将使其可以运动。上图概述了这种配置方式。白色的部分是必须且已实现的组件，灰色的部分是可选且已实现的组件，蓝色的部分是必须为每一个机器人平台创建的组件。

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

全局地图（全局概览图：定位+路径规划）
自身定位（确定在地图中的位置）
路径规划（全局路径规划+局部路径规划）
运动控制（控制速度和方向）
环境感知（感知周围环境）

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

1.全局地图

在现实生活中，当我们需要实现导航时，可能会首先参考一张全局性质的地图，然后根据地图来确定自身的位置、目的地位置，并且也会根据地图显示来规划一条大致的路线… 对于机器人导航而言，也是如此，在机器人导航中地图是一个重要的组成元素，当然如果要使用地图，首先需要绘制地图。关于地图建模技术不断涌现，这其中有一门称之为 SLAM 的理论脱颖而出:

SLAM(simultaneous localization and mapping),也称为CML (Concurrent Mapping and Localization), 即时定位与地图构建，或并发建图与定位。SLAM问题可以描述为: 机器人在未知环境中从一个未知位置开始移动,在移动过程中根据位置估计和地图进行自身定位，同时在自身定位的基础上建造增量式地图，以绘制出外部环境的完全地图。
在 ROS 中，较为常用的 SLAM 实现也比较多，比如: gmapping、hector_slam、cartographer、rgbdslam、ORB_SLAM …
当然如果要完成 SLAM ，机器人必须要具备感知外界环境的能力，尤其是要具备获取周围环境深度信息的能力。感知的实现需要依赖于传感器，比如: 激光雷达、摄像头、RGB-D摄像头…
SLAM 可以用于地图生成，而生成的地图还需要被保存以待后续使用，在 ROS 中保存地图的功能包是 map_server

另外注意: SLAM 虽然是机器人导航的重要技术之一，但是二者并不等价，确切的讲，SLAM 只是实现地图构建和即时定位。

2.自身定位

导航伊始和导航过程中，机器人都需要确定当前自身的位置，如果在室外，那么 GPS 是一个不错的选择，而如果室内、隧道、地下或一些特殊的屏蔽 GPS 信号的区域，由于 GPS 信号弱化甚至完全不可用，那么就必须另辟蹊径了，比如前面的 SLAM 就可以实现自身定位，除此之外，ROS 中还提供了一个用于定位的功能包: amcl

amcl(adaptiveMonteCarloLocalization)自适应的蒙特卡洛定位,是用于2D移动机器人的概率定位系统。它实现了自适应（或KLD采样）蒙特卡洛定位方法，该方法使用粒子过滤器根据已知地图跟踪机器人的姿态。

3.路径规划

导航就是机器人从A点运动至B点的过程，在这一过程中，机器人需要根据目标位置计算全局运动路线，并且在运动过程中，还需要时时根据出现的一些动态障碍物调整运动路线，直至到达目标点，该过程就称之为路径规划。在 ROS 中提供了 move_base 包来实现路径规则,该功能包主要由两大规划器组成:

全局路径规划(gloable_planner)

根据给定的目标点和全局地图实现总体的路径规划，使用 Dijkstra 或 A* 算法进行全局路径规划，计算最优路线，作为全局路线

本地时时规划(local_planner)

在实际导航过程中，机器人可能无法按照给定的全局最优路线运行，比如:机器人在运行中，可能会随时出现一定的障碍物… 本地规划的作用就是使用一定算法(Dynamic Window Approaches) 来实现障碍物的规避，并选取当前最优路径以尽量符合全局最优路径

全局路径规划与本地路径规划是相对的，全局路径规划侧重于全局、宏观实现，而本地路径规划侧重与当前、微观实现。

4.运动控制

导航功能包集假定它可以通过话题"cmd_vel"发布geometry_msgs/Twist类型的消息，这个消息基于机器人的基座坐标系，它传递的是运动命令。这意味着必须有一个节点订阅"cmd_vel"话题，将该话题上的速度命令转换为电机命令并发送。

5.环境感知

感知周围环境信息，比如: 摄像头、激光雷达、编码器…，摄像头、激光雷达可以用于感知外界环境的深度信息，编码器可以感知电机的转速信息，进而可以获取速度信息并生成里程计信息。

在导航功能包集中，环境感知也是一重要模块实现，它为其他模块提供了支持。其他模块诸如: SLAM、amcl、move_base 都需要依赖于环境感知。

7.1.2 导航之坐标系

1.简介

定位是导航中的重要实现之一，所谓定位，就是参考某个坐标系(比如:以机器人的出发点为原点创建坐标系)在该坐标系中标注机器人。定位原理看似简单，但是这个这个坐标系不是客观存在的，我们也无法以上帝视角确定机器人的位姿，定位实现需要依赖于机器人自身，机器人需要逆向推导参考系原点并计算坐标系相对关系，该过程实现常用方式有两种:

通过里程计定位:时时收集机器人的速度信息计算并发布机器人坐标系与父级参考系的相对关系。
通过传感器定位:通过传感器收集外界环境信息通过匹配计算并发布机器人坐标系与父级参考系的相对关系。

两种方式在导航中都会经常使用。

2.特点

两种定位方式都有各自的优缺点。

里程计定位:

优点:里程计定位信息是连续的，没有离散的跳跃。
缺点:里程计存在累计误差，不利于长距离或长期定位。

传感器定位:

优点:比里程计定位更精准；
缺点:传感器定位会出现跳变的情况，且传感器定位在标志物较少的环境下，其定位精度会大打折扣。

两种定位方式优缺点互补，应用时一般二者结合使用。

3.坐标系变换

上述两种定位实现中，机器人坐标系一般使用机器人模型中的根坐标系(base_link 或 base_footprint)，里程计定位时，父级坐标系一般称之为 odom，如果通过传感器定位，父级参考系一般称之为 map。当二者结合使用时，map 和 odom 都是机器人模型根坐标系的父级，这是不符合坐标变换中"单继承"的原则的，所以，一般会将转换关系设置为: map -> doom -> base_link 或 base_footprint。

7.1.3导航条件说明

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

1.硬件

虽然导航功能包集被设计成尽可能的通用，在使用时仍然有三个主要的硬件限制：

它是为差速驱动的轮式机器人设计的。它假设底盘受到理想的运动命令的控制并可实现预期的结果，命令的格式为：x速度分量，y速度分量，角速度(theta)分量。
它需要在底盘上安装一个单线激光雷达。这个激光雷达用于构建地图和定位。
导航功能包集是为正方形的机器人开发的，所以方形或圆形的机器人将是性能最好的。它也可以工作在任意形状和大小的机器人上，但是较大的机器人将很难通过狭窄的空间。

2.软件

导航功能实现之前，需要搭建一些软件环境:

毋庸置疑的，必须先要安装 ROS
当前导航基于仿真环境，先保证上一章的机器人系统仿真可以正常执行

在仿真环境下，机器人可以正常接收 /cmd_vel 消息，并发布里程计消息，传感器消息发布也正常，也即导航模块中的运动控制和环境感知实现完毕

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

7.2 导航实现

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

SLAM建图(选用较为常见的gmapping)
地图服务(可以保存和重现地图)
机器人定位
路径规划
上述流程介绍完毕，还会对功能进一步集成实现探索式的SLAM建图。

准备工作

请先安装相关的ROS功能包:

安装 gmapping 包(用于构建地图):sudo apt install ros-<ROS版本>-gmapping
安装地图服务包(用于保存与读取地图):sudo apt install ros-<ROS版本>-map-server
安装 navigation 包(用于定位以及路径规划):sudo apt install ros-<ROS版本>-navigation

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

7.2.1 导航实现01_SLAM建图

SLAM算法有多种，当前我们选用gmapping，后续会再介绍其他几种常用的SLAM实现。

1.gmapping简介

gmapping 是ROS开源社区中较为常用且比较成熟的SLAM算法之一，gmapping可以根据移动机器人里程计数据和激光雷达数据来绘制二维的栅格地图，对应的，gmapping对硬件也有一定的要求:

该移动机器人可以发布里程计消息
机器人需要发布雷达消息(该消息可以通过水平固定安装的雷达发布，或者也可以将深度相机消息转换成雷达消息)

关于里程计与雷达数据，仿真环境中可以正常获取的，不再赘述，栅格地图如案例所示。

gmapping 安装前面也有介绍，命令如下:

sudo apt install ros-<ROS版本>-gmapping

2.gmapping节点说明

gmapping 功能包中的核心节点是:slam_gmapping。为了方便调用，需要先了解该节点订阅的话题、发布的话题、服务以及相关参数。

2.1订阅的Topic

tf (tf/tfMessage)

用于雷达、底盘与里程计之间的坐标变换消息。

scan(sensor_msgs/LaserScan)

SLAM所需的雷达信息。

2.2发布的Topic

map_metadata(nav_msgs/MapMetaData)

地图元数据，包括地图的宽度、高度、分辨率等，该消息会固定更新。

map(nav_msgs/OccupancyGrid)

地图栅格数据，一般会在rviz中以图形化的方式显示。

~entropy(std_msgs/Float64)

机器人姿态分布熵估计(值越大，不确定性越大)。

2.3服务

dynamic_map(nav_msgs/GetMap)

用于获取地图数据。

2.4参数

~base_frame(string, default:“base_link”)

机器人基坐标系。

~map_frame(string, default:“map”)

地图坐标系。

~odom_frame(string, default:“odom”)

里程计坐标系。

~map_update_interval(float, default: 5.0)

地图更新频率，根据指定的值设计更新间隔。

~maxUrange(float, default: 80.0)

激光探测的最大可用范围(超出此阈值，被截断)。

~maxRange(float)

激光探测的最大范围。

… 参数较多，上述是几个较为常用的参数，其他参数介绍可参考官网。

2.5所需的坐标变换

雷达坐标系→基坐标系

一般由 robot_state_publisher 或 static_transform_publisher 发布。

基坐标系→里程计坐标系

一般由里程计节点发布。

2.6发布的坐标变换

地图坐标系→里程计坐标系

地图到里程计坐标系之间的变换。

3.gmapping使用

3.1编写gmapping节点相关launch文件

launch文件编写可以参考 github 的演示 launch文件：https://github.com/ros-perception/slam_gmapping/blob/melodic-devel/gmapping/launch/slam_gmapping_pr2.launch

复制并修改如下:

<launch>
    <!-- 仿真环境下,将该参数设置为true -->
    <param name="use_sim_time" value="true"/>
    <!-- gmapping节点 -->
    <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">
        <!-- 设置雷达话题 -->
        <remap from="scan" to="scan" />
        <!-- 关键参数:坐标系 -->
        <param name = "base_frame" value = "base_footprint" />
        <param name = "map_frame" value = "map" />
        <param name = "odom_frame" 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>

    <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" name="rviz" type="rviz" />
</launch>

关键代码解释：

<remap from="scan" to="scan"/><!-- 雷达话题 -->
<param name="base_frame" value="base_footprint"/><!--底盘坐标系-->
<param name="odom_frame" value="odom"/> <!--里程计坐标系-->

3.2执行

1.先启动 Gazebo 仿真环境(此过程略)

roslaunch urdf02_gazebo demo03_env.launch

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

roslaunch nav_demo nav_slam.launch

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

rosrun teleop_twist_keyboard teleop_twist_keyboard.py

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

添加的组件有四个RobotModel显示机器人实体，TF显示坐标系关系，LaserScan显示雷达信息，注意需要设置话题/scan，Map创建地图，注意需要设置话题map。最后，就可以通过键盘控制gazebo中的机器人运动，同时，在rviz中可以显示gmapping发布的栅格地图数据了，下一步，还需要将地图单独保存。

7.2.2 导航实现02_地图服务

上一节我们已经实现通过gmapping的构建地图并在rviz中显示了地图，不过，上一节中地图数据是保存在内存中的，当节点关闭时，数据也会被一并释放，我们需要将栅格地图序列化到的磁盘以持久化存储，后期还要通过反序列化读取磁盘的地图数据再执行后续操作。在ROS中，地图数据的序列化与反序列化可以通过 map_server 功能包实现。

1.map_server简介

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

map_server安装前面也有介绍，命令如下:

sudo apt install ros-<ROS版本>-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 pkg="map_server" type="map_saver" name="map_save" args="-f $(arg filename)" />
</launch>

其中 mymap 是指地图的保存路径以及保存的文件名称。

SLAM建图完毕后，执行该launch文件即可。

测试:

首先，参考上一节，依次启动仿真环境，键盘控制节点与SLAM节点；

然后，通过键盘控制机器人运动并绘图；

最后，通过上述地图保存方式保存地图。

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

2.3保存结果解释

xxx.pgm 本质是一张图片，直接使用图片查看程序即可打开。

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

#1.声明地图图片资源的路径
image: /home/liuhaoran/ros/demo_ws02/src/nav_demo/map/nav.pgm
#2.地图刻度尺 单位: 米/像素
resolution: 0.050000
#3.地图的位姿(相对于rviz中原点的位姿)
#值1:x方向上的偏移量
#值2:y方向上的偏移量
#值3:地图上的偏航角度(单位i是弧度)
origin: [-50.000000, -50.000000, 0.000000]

#地图中的障碍物判断
#最终地图结果: 白色是可通行区域,黑色是障碍物,蓝灰是未知区域
#判断规则:
#1.地图中的每个像素取值[0,255] 白色255 黑色0 像素值设为x
#2.根据像素值计算一个比例: p = (255-x)/255 白色为0,黑色为1,灰色介于0到1之间
#3.判断是否是障碍物: p>occupied_thresh是障碍物 p<free_thresh则无物,可以通行
#4.取反
negate: 0
#5.占用阈值
occupied_thresh: 0.65
#6.空闲阈值
free_thresh: 0.196

解释:

image:被描述的图片资源路径，可以是绝对路径也可以是相对路径。
resolution: 图片分片率(单位: m/像素)。
origin: 地图中左下像素的二维姿势，为（x，y，偏航），偏航为逆时针旋转（偏航= 0表示无旋转）。
occupied_thresh: 占用概率大于此阈值的像素被视为完全占用。
free_thresh: 占用率小于此阈值的像素被视为完全空闲。
negate: 是否应该颠倒白色/黑色自由/占用的语义。

map_server 中障碍物计算规则:

地图中的每一个像素取值在 [0,255] 之间，白色为 255，黑色为 0，该值设为 x；
map_server 会将像素值作为判断是否是障碍物的依据，首先计算比例: p = (255 - x) / 255.0，白色为0，黑色为1(negate为true，则p = x / 255.0)；
根据步骤2计算的比例判断是否是障碍物，如果 p > occupied_thresh 那么视为障碍物，如果 p < free_thresh 那么视为无物。

备注:

图片也可以根据需求编辑。

3.map_server使用之地图服务(map_server)

3.1map_server节点说明

发布的话题

map_metadata（nav_msgs / MapMetaData）

发布地图元数据。

map（nav_msgs / OccupancyGrid）

地图数据。

服务

static_map（nav_msgs / GetMap）

通过此服务获取地图。

参数

〜frame_id（字符串，默认值：“map”）

地图坐标系。

3.2地图读取

通过 map_server 的 map_server 节点可以读取栅格地图数据，编写 launch 文件如下:

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

其中参数是地图描述文件的资源路径，执行该launch文件，该节点会发布话题:map(nav_msgs/OccupancyGrid)

3.3地图显示

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

7.2.3 导航实现03_定位

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

1.amcl简介

AMCL(adaptive Monte Carlo Localization) 是用于2D移动机器人的概率定位系统，它实现了自适应（或KLD采样）蒙特卡洛定位方法，可以根据已有地图使用粒子滤波器推算机器人位置。

amcl已经被集成到了navigation包，navigation安装前面也有介绍，命令如下:

sudo apt install ros-<ROS版本>-navigation

2.amcl节点说明

amcl 功能包中的核心节点是:amcl。为了方便调用，需要先了解该节点订阅的话题、发布的话题、服务以及相关参数。

2.1订阅的Topic

scan(sensor_msgs/LaserScan)

激光雷达数据。

tf(tf/tfMessage)

坐标变换消息。

initialpose(geometry_msgs/PoseWithCovarianceStamped)

用来初始化粒子滤波器的均值和协方差。

map(nav_msgs/OccupancyGrid)

获取地图数据。

2.2发布的Topic

amcl_pose(geometry_msgs/PoseWithCovarianceStamped)

机器人在地图中的位姿估计。

particlecloud(geometry_msgs/PoseArray)

位姿估计集合，rviz中可以被 PoseArray 订阅然后图形化显示机器人的位姿估计集合。

tf(tf/tfMessage)

发布从 odom 到 map 的转换。

2.3服务

global_localization(std_srvs/Empty)

初始化全局定位的服务。

request_nomotion_update(std_srvs/Empty)

手动执行更新和发布更新的粒子的服务。

set_map(nav_msgs/SetMap)

手动设置新地图和姿态的服务。

2.4调用的服务

static_map(nav_msgs/GetMap)

调用此服务获取地图数据。

2.5参数

~odom_model_type(string, default:“diff”)

里程计模型选择: “diff”,“omni”,“diff-corrected”,“omni-corrected” (diff 差速、omni 全向轮)

~odom_frame_id(string, default:“odom”)

里程计坐标系。

~base_frame_id(string, default:“base_link”)

机器人极坐标系。

~global_frame_id(string, default:“map”)

地图坐标系。

… 参数较多，上述是几个较为常用的参数，其他参数介绍可参考官网。

2.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

该目录下会列出两个文件: 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="transform_tolerance" value="0.2" />
        <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_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"/>

        <!-- 设置坐标系:odom map 机器人基坐标系 -->
        <param name="odom_frame_id" value="odom"/>
        <param name="base_frame_id" value="base_footprint"/>

        <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" name="rviz" type="rviz" />
    <!-- 加载地图服务 -->
    <include file = "$(find nav_demo)/launch/nav03_map_server.launch" />
    <!-- amcl -->
    <include file = "$(find nav_demo)/launch/nav04_amcl.launch" />
</launch>

当然，launch文件中地图服务节点和amcl节点中的包名、文件名需要根据自己的设置修改。

3.3执行

1.先启动 Gazebo 仿真环境(此过程略)；

2.启动键盘控制节点：

rosrun teleop_twist_keyboard teleop_twist_keyboard.py

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

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

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

7.2.4 导航实现04_路径规划

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

1.move_base简介

move_base 功能包提供了基于动作(action)的路径规划实现，move_base 可以根据给定的目标点，控制机器人底盘运动至目标位置，并且在运动过程中会连续反馈机器人自身的姿态与目标点的状态信息。如前所述(7.1)move_base主要由全局路径规划与本地路径规划组成。

move_base已经被集成到了navigation包，navigation安装前面也有介绍，命令如下:

sudo apt install ros-<ROS版本>-navigation

2.move_base节点说明

move_base功能包中的核心节点是:move_base。为了方便调用，需要先了解该节点action、订阅的话题、发布的话题、服务以及相关参数。

2.1动作

动作订阅

move_base/goal(move_base_msgs/MoveBaseActionGoal)

move_base 的运动规划目标。

move_base/cancel(actionlib_msgs/GoalID)

取消目标。

动作发布

move_base/feedback(move_base_msgs/MoveBaseActionFeedback)

连续反馈的信息，包含机器人底盘坐标。

move_base/status(actionlib_msgs/GoalStatusArray)

发送到move_base的目标状态信息。

move_base/result(move_base_msgs/MoveBaseActionResult)

操作结果(此处为空)。

2.2订阅的Topic

move_base_simple/goal(geometry_msgs/PoseStamped)

运动规划目标(与action相比，没有连续反馈，无法追踪机器人执行状态)。

2.3发布的Topic

cmd_vel(geometry_msgs/Twist)

输出到机器人底盘的运动控制消息。

2.4服务

~make_plan(nav_msgs/GetPlan)

请求该服务，可以获取给定目标的规划路径，但是并不执行该路径规划。

~clear_unknown_space(std_srvs/Empty)

允许用户直接清除机器人周围的未知空间。

~clear_costmaps(std_srvs/Empty)

允许清除代价地图中的障碍物，可能会导致机器人与障碍物碰撞，请慎用。

2.5参数

请参考官网。

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>

    <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen" clear_params="true">
        <rosparam file="$(find 功能包)/param/costmap_common_params.yaml" command="load" ns="global_costmap" />
        <rosparam file="$(find 功能包)/param/costmap_common_params.yaml" command="load" ns="local_costmap" />
        <rosparam file="$(find 功能包)/param/local_costmap_params.yaml" command="load" />
        <rosparam file="$(find 功能包)/param/global_costmap_params.yaml" command="load" />
        <rosparam file="$(find 功能包)/param/base_local_planner_params.yaml" command="load" />
    </node>

</launch>

launch文件解释:

启动了 move_base 功能包下的 move_base 节点，respawn 为 false，意味着该节点关闭后，不会被重启；clear_params 为 true，意味着每次启动该节点都要清空私有参数然后重新载入；通过 rosparam 会载入若干 yaml 文件用于配置参数，这些yaml文件的配置以及作用详见下一小节内容。

4.2配置文件

关于配置文件的编写，可以参考一些成熟的机器人的路径规划实现，比如: turtlebot3，github链接：https://github.com/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。

配置文件修改以及解释:

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用于导航和清除障碍物。
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 #机器人坐标系
  # 以此实现坐标变换

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

  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 #等待坐标变换发布信息的超时时间

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

  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文件集成

如果要实现导航，需要集成地图服务、amcl 、move_base 与 Rviz 等，集成示例如下:

<launch>
    <!-- 设置地图的配置文件 -->
    <arg name="map" default="nav.yaml" />
    <!-- 运行地图服务器，并且加载设置的地图-->
    <node name="map_server" pkg="map_server" type="map_server" args="$(find mycar_nav)/map/$(arg map)"/>
    <!-- 启动AMCL节点 -->
    <include file="$(find mycar_nav)/launch/amcl.launch" />

    <!-- 运行move_base节点 -->
    <include file="$(find mycar_nav)/launch/path.launch" />
    <!-- 运行rviz -->
    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find mycar_nav)/rviz/nav.rviz" />

</launch>

4.4测试

1.先启动 Gazebo 仿真环境(此过程略)；

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

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

全局代价地图与本地代价地图组件配置如下:

全局路径规划与本地路径规划组件配置如下:

4.通过Rviz工具栏的 2D Nav Goal设置目的地实现导航。

也可以在导航过程中，添加新的障碍物，机器人也可以自动躲避障碍物。

7.2.5 导航与SLAM建图

场景:在 7.2.1 导航实现01_SLAM建图中，我们是通过键盘控制机器人移动实现建图的，而后续又介绍了机器人的自主移动实现，那么可不可以将二者结合，实现机器人自主移动的SLAM建图呢？

上述需求是可行的。虽然可能会有疑问，导航时需要地图信息，之前导航实现时，是通过 map_server 包的 map_server 节点来发布地图信息的，如果不先通过SLAM建图，那么如何发布地图信息呢？SLAM建图过程中本身就会时时发布地图信息，所以无需再使用map_server，SLAM已经发布了话题为 /map 的地图消息了，且导航需要定位模块，SLAM本身也是可以实现定位的。

该过程实现比较简单，步骤如下:

编写launch文件，集成SLAM与move_base相关节点；
执行launch文件并测试。

1.编写launch文件

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

<!-- 集成slam与导航，实现机器人自主移动的地图构建 -->
<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仿真环境；

2.然后执行launch文件；

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

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

7.3 导航相关消息

7.3.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

7.3.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

7.3.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

7.3.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

7.3.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

7.3.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.3.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
Copy

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

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

调用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     #是否有无效点