ROS学习

使用vscode创建并应用一个ros项目：

	1.使用命令行创建工作空间：
		mkdir -p 工作空间名称/src(必须得有 src)
		cd 工作空间名称
		catkin_make（编译）
	2.进入工作空间，启动vscode：
		cd 工作空间名称
		code .
	3.vscode中编译ros：
		ctrl + shift + B 调用编译，选择:catkin_make:build，并点击右侧齿轮自动创建tasks.json文件，修改.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键入包名（Package name）和依赖包名（Dependencies:roscpp rospy std_msgs）
		// 平替在终端中输入catkin_create_pkg 自定义ROS包名 依赖名(roscpp rospy std_msgs）的过程，会体现在package.xml中<build_depend>和<exec_depend>以及CMakeLists.txt L10 find_package中，如果再创建好后要增删依赖包，也只需要在上述文件的对应位置进行修改即可·
	5.C++实现：
		在小src下新建cpp文件
		如果没有代码提示需要修改 .vscode/c_cpp_properties.json中的"cppStandard"为"c++17"
		当ROS__INFO 终端输出有中文时，会出现乱码，此时要在在函数开头加入下面代码的任意一句：setlocale(LC_CTYPE, "zh_CN.utf8")/setlocale(LC_ALL, "")
	6.Python实现：
		在小src同级下新建scripts文件夹添加py文件和可执行权限chmod +x xxx.py（之后命令行语句都可以在vscode下新建终端，一种是点击终端加号，一种是选中文件夹右键点击集成终端）
	7.编辑小src文件夹下的CMakeLists.txt文件：（取消注释）
		C++ 配置:
			L136:add_executable(节点名称 src/C++源文件名.cpp)
L145:add_dependencies(节点名称 ${PROJECT_NAME}节点依赖)：节点依赖对于话题通信为_generate_messages_cpp，对于服务通信为_gencpp，	如果没有自定义文件格式则无需配置
			L149:target_link_libraries(节点名称 ${catkin_LIBRARIES})
如果有多个cpp文件，就要对每个cpp文件都进行上述步骤
		Python 配置:
			L162:catkin_install_python(PROGRAMS scripts/自定义文件名.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
			//其实可以把python的文化名.py就认为是它的节点名
	8.编写launch启动：
		在小src同级下创建文件夹launch，在其中创建launch文件xxx.launch
		一旦键入<launch>和</launch>就意味着进行了roscore
		创建一个节点就相当于进行一个rosrun，格式为<node创建节点 pkg="包名" type="节点名(c++)/文件名.py(python)" name="标识名" output="screen" />
	9.编译执行：（编译执行终端需架设在工作空间下）
		每次重新修改好ros文件，除了ctrl+s之外还需要进行重新编译ctrl+shift+B和重新执行
		重新执行的方法即在vscode终端下输入命令：source ./devel/setup.bash => roslaunch 包名 launch文件名
//非launch启动时重新执行的命令为source ./devel/setup.bash
		//roslaunch平替多开终端并进行roscore，rosrun 包名 C++节点名/文件名.py的过程

ros文件的基本架构：

其中工作空间catkin workspace和大src由mkdir创建，build/devel由catkin_make创建，包名package由create catkin package创建，其下会自动创建inc，src文件夹用于存放c++文件，CMakeLists.txt用于编译c++和python文件，package.xml定义有关软件包的属性（依赖的包和构建工具），文件夹scripts由自己创建用于存放python文件，文件夹launch由自己创建用于存放launch文件用于多启动，文件夹msg由自己创建用于存放msg文件用于规定服务通信下的自定义通讯格式，文件夹srv由自己创建用于存放srv文件用于规定话题通信下的自定义通讯格式。

话题通讯：

基于发布订阅模式，一个节点发布消息，另一个节点订阅该消息。常用于不断更新的、少逻辑处理的数据传输场景，如激光雷达的数据采集。
话题通讯的理论模型如下：

话题通讯涉及到三个角色:ROS Master (管理者)，Talker (服务端)，Listener (客户端)
ROS Master 负责保管 Talker 和 Listener 注册的信息，并匹配话题相同的 Talker 与 Listener，帮助 Talker 与 Listener 建立连接，连接建立后，Talker 可以发布消息，且发布的消息会被 Listener 订阅。注意事项如下：
1.Talker 与 Listener 的启动无先后顺序要求

1. Talker 与 Listener 都可以有多个
2. Talker 与 Listener 连接建立后，不再需要 ROS Master。即便关闭ROS Master，Talker 与 Listener 照常通信。

ROS官方提供的话题通讯基本格式如下：

当使用自定义通讯格式进行话题通讯时，需要进行以下操作：
1.定义msg文件：在小src同级下创建msg文件夹并创建xxx.msg文件，在其中以类似结构体的格式输入需要定义的自定义通讯格式构成，例如：
string name
uint16 age
float64 height
2.编辑package.xml：
这里简单介绍一下package文件的构成，package的核心代码如下：
①依赖的构建工具

catkin
②指定构建此软件包所需的软件包 （编译依赖）

roscpp

rospy

std_msgs
③指定根据这个包构建库所需要的包（编译外部依赖）

roscpp

rospy

std_msgs
④运行该程序包中的代码所需的程序包（执行依赖）

roscpp

rospy

std_msgs
我们要在编译依赖中添加message_generation
在执行依赖中添加message_runtime
3.编辑CMakeLists.txt：
L10：取消注释并修改编译依赖为：

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  message_generation
)

L51：取消注释并配置msg源文件：

add_message_files(
  FILES
  xxx.msg
)

L71：取消注释使自定义消息格式依赖于 std_msgs：

generate_messages(
  DEPENDENCIES
  std_msgs
)

L108：取消注释并修改执行依赖为：

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

4.编译：此时C++调用的中间文件位于(…/工作空间/devel/include/包名/xxx.h)
Python调用的中间文件位于(…/工作空间/devel/lib/python3/dist-packages/包名/msg/xxx.py)，之后要想使用这一自定义通讯格式就需要包含这个头文件。

代码实现C++话题通讯：
1.如果使用自定义格式，需要添加c_cpp_properties.json 的 includepath属性如下：
“/xxx/yyy工作空间/devel/include/*“ //配置 head 文件的路径
2.包含通讯所需格式的头文件：
对于ROS官方提供的基本格式需要包含#include “std_msgs/xxx.h”
对于自定义的格式需要包含#include “包名/xxx.h”（即自定义格式经编译得到的头文件）
3.发布方实现：
①初始化ros节点并创建节点句柄：
ros::init(argc, argv, “Publisher”);
ros::NodeHandle nh;
②创建发布者对象：（发布者和订阅者的话题topic需要一致）
官方格式：ros::Publisher pub = nh.advertise(“topic”, 10);
自定义格式：ros::Publisher pub = nh.advertise<包名::xxx>(“topic”, 10);
③编辑要发送的数据：
创建对象：std_msgs::xxx/包名::xxx msg;
4.订阅方实现：
①初始化ros节点并创建节点句柄：
ros::init(argc, argv, “Publisher”);
ros::NodeHandle nh;
②创建订阅者对象：
官方：ros::Subscriber sub = nh.subscribe(“topic”,10,doMsg);
自定义：ros::Subscriber sub = nh.subscribe<包名::xxx>(“topic”,10,doMsg);
③循环调用回调函数：ros::spin();
④搭建回调函数doMsg：void doMsg(const 包名::xxx::ConstPtr &msg){}

服务通信：

基于请求响应模式，是一种应答机制，一个节点A向另一个节点B发送请求，B接收处理请求并产生响应结果返回给A，常用于偶然的、对时时性有要求、有一定逻辑处理需求的数据传输场景，例如摄像头数据的采集与访问。

服务通信的理论模型如下:
服务通信涉及到三个角色:ROS Master (管理者)，Talker (发布者)，Listener (订阅者)
ROS Master 负责保管 Talker 和 Listener 注册的信息，并匹配服务相同的 Talker 与 Listener，帮助 Talker 与 Listener 建立连接，连接建立后，Client 发送请求信息，而Server 则返回响应信息。注意事项如下:
1.客户端请求被处理时，需要保证服务器已经启动（这点与话题通信不同）
2.服务端和客户端都可以存在多个
同理于话题通信，服务通信也存在自定义通信格式srv，当使用自定义通讯格式进行服务通讯时，需要进行以下操作：
1.定义srv文件：在小src同级下创建srv文件夹并创建xxx.srv文件，在其中以类似结构体的格式输入需要定义的自定义通讯格式构成。在 srv 文件中，数据分成请求与响应两部分，之间使用—-分割，例如:

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

2.编辑package.xml：此处需要修改的与话题通信一致，即：
在编译依赖中添加message_generation
在执行依赖中添加message_runtime
3.编辑CMakeLists.txt：
L10：取消注释并修改编译依赖为：

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  message_generation
)

L58：取消注释并配置srv源文件：（对应于话题通信的L51配置msg源文件）

add_service_files(
FILES
xxx.srv
)

L71：取消注释使自定义消息格式依赖于 std_msgs：

generate_messages(
  DEPENDENCIES
  std_msgs
)

L108：取消注释并修改执行依赖为：

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

4.编译：此时C++调用的中间文件位于(…/工作空间/devel/include/包名/xxx.h)
除了xxx.h外，由于服务通信包含请求方与响应方两部分，故还有xxxRequest.h和xxxResponse.h两个文件。
Python调用的中间文件位于(…/工作空间/devel/lib/python3/dist-packages/包名/msg/xxx.py)，之后要想使用这一自定义通讯格式就需要包含这个头文件。

代码实现C++服务通讯：

1.如果使用自定义格式，需要添加c_cpp_properties.json 的 includepath属性如下：
“/xxx/yyy工作空间/devel/include/**” //配置 head 文件的路径
2.包含通讯所需格式的头文件：
自定义格式需要包含#include “包名/xxx.h”（即自定义格式经编译得到的头文件）
3.服务端实现：
①初始化ros节点并创建节点句柄：
ros::init(argc, argv, “Server”);
ros::NodeHandle nh;
②创建服务端对象：（服务端和客户端的服务需要一致，服务端不需要泛型）
ros::ServiceServer server = nh.advertiseService(“xxx”, doReq);
③循环调用回调函数：ros::spin();
④搭建回调函数doReq：bool doReq(包名::xxx::Request &request,包名::xxx::Response &response){}用于处理请求并组织响应（Req和Res都有对应对象）
4.客户端实现：
①初始化ros节点并创建节点句柄：
ros::init(argc, argv, “Client”);
ros::NodeHandle nh;
②创建客户端对象：
ros::ServiceClient client = nh.serviceClient<包名::xxx>(“xxx”);
③组织请求并处理响应：
ServiceCommunication::xxx ai;
ai.request/response….
bool flag = client.call(ai); ④挂起客户端等待服务端启动：
client.waitForExistence();或者ros::service::waitForService(“xxx”);

参数服务器：

用于设置、存储和获取静态的非二进制的简单数据，不是为高性能而设计的。
参数服务器的理论模型如下：

参数服务器涉及到三个角色:ROS Master (管理者)，Talker (设置者)，Listener (调用者)
ROS Master 作为一个公共容器保存参数，Talker 可以向容器中设置参数，Listener 可以获取参数。
在 C++ 中实现参数服务器数据的增删改查，可以通过两套 API 实现:
①ros::NodeHandle
②ros::param
具体的实现方法可以参考代码相关部分
值得指出的是，参数服务器本身是随着roscore被一同创建出来的，因而它不需要任何显示的创建方式，且其本身会存储一部分系统创建的静态数据。
创建参数的另一种方法是在运行节点命令后加_参数键:=值，例如：
rosrun 包名 节点名 _length:=2即创建了一个值为2的参数length

常用的ROS命令行命令：

1.文件操作命令：
文件操作命令是静态的，操作的是磁盘上的文件
roscore：开启ros内核，运行ros程序前必须先开启ros内核
rosrun：运行ros节点，格式为：rosrun 包名 C++节点名/文件名.py [传入参数列表]
roslaunch：运行launch文件，格式为：roslaunch 包名 launch文件名
2.动态运行命令：
在ROS程序启动后，动态运行命令可以动态的获取运行中的节点或参数的相关信息，在测试和运行中对程序调试与纠错有重要作用。
rosnode：操作ros节点，是用于获取节点信息的命令。
rosnode ping 节点名 测试到节点的连接状态
rosnode list 列出活动节点
rosnode info 节点名 打印节点信息
rosnode machine 设备名 列出指定设备上节点
rosnode kill 节点名 杀死某个节点
rosnode cleanup 清除不可连接的节点
rostopic：操作话题，用于显示调试信息，包括发布者，订阅者，发布频率和ROS消息。
rostopic echo 话题名 获取指定话题当前发布的消息（扮演订阅方角色）
rostopic hz 话题名 显示话题的发布频率
rostopic info 话题名 显示话题信息(消息类型,发布者信息,订阅者信息)
rostopic list 显示所有活动状态下的话题

rostopic pub 将数据发布到话题（扮演发布方角色）
单次发布：rostopic pub /话题名称 消息类型 消息内容
连续发布：rostopic pub -r 发布频率 /话题名称 消息类型 消息内容
rostopic type 话题名 列出话题的消息类型
rosservice：操作ros服务
rosservice call服务名 使用提供的参数调用服务（扮演客户端的角色）
rosservice call AddInts tabtab
rosservice info服务名 打印有关服务的信息
rosservice list 列出所有活动的服务
rosservice type服务名 打印服务使用的数据类型
rosmsg：操作msg消息，用于话题通信
rosmsg info消息名 显示消息信息（包含哪些类型）
rosmsg list 列出所有可用的msg消息类型
rossrv：操作srv消息，用于服务通信，方法与rosmsg完全一致
rosparam：操作ros 参数，用于参数服务器
rosparam set 参数键 值 设置参数
rosparam get 参数键 获取参数
rosparam delete 参数键 删除参数
rosparam list 列出所有参数
rosparam load xxx.yaml 从外部文件加载参数
rosparam dump xxx.yaml 将参数写出到外部文件

用小海龟进行复习模拟：

启动ros内核：roscore
启动海龟图形化界面节点：rosrun turtlesim turtlesim_node
启动海龟键盘控制节点：rosrun turtlesim turtle_teleop_key
1.话题发布模拟：
通过计算图查看话题，启动计算图：rqt_graph（也可以使用rostopic list）

可以看到键盘控制节点和图形化界面节点通过话题/turtle1/cmd_vel通信，并使Publisher键盘控制节点的消息以图形化的形式反馈在Subscriber图形化界面节点的乌龟移动上。
此外，我们通过对话题/turtle1/cmd_vel进行信息查询rostopic info /turtle1/cmd_vel
可以得到通信双方的节点名称和通信的信息类型为geometry_msgs/Twist

利用rosmsg info geometry_msgs/Twist我们便可以得到该信息的格式如下

其中linear为线速度，angular为角速度，易得对于只能向头朝向移动的2D小海龟，只有linear-x和angular-z的值非零，可以使用rostopic echo /turtle1/cmd_vel验证。
如果我们要使海龟做圆周运动，其方法就是向话题发布我们想要的命令，以此替代键盘达到的控制效果，使用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: 1.0” 并修改其值是一个方法。
如果用代码实现，则可以创建CirclePublisher.cpp并如下写制：

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

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

ros::Publisher pub = nh.advertise<geometry_msgs::Twist>
("/turtle1/cmd_vel", 10);
ros::Rate rate(10);
geometry_msgs::Twist twist;
twist.linear.x = 1.0f;
twist.linear.y = 0.0f;
twist.linear.z = 0.0f;
twist.angular.x = 0.0f;
twist.angular.y = 0.0f;
twist.angular.z = 1.0f;

while (ros::ok())
{
pub.publish(twist);
rate.sleep();
ros::spinOnce();
}
return 0;
}

2.话题订阅模拟：
海龟图形化界面节点turtlesim会实时发布当前海龟的位姿和速度
对应发布到的话题为/turtle1/pose，使用rostopic info /turtle1/pose可得：

可见通信的信息类型为turtlesim/Pose且其只有发布方而没有订阅方
利用rosmsg info turtlesim/Pose得到该信息的格式如下：

值得指出的是坐标的原点是屏幕的左下角，角度变化只在[-π,π]之间变化
如果我们要获取当前海龟的位姿和速度信息并打印在屏幕上，其方法就是订阅话题的消息。
直接使用ros命令rostopic echo /turtle1/pose订阅是一个方法
如果用代码实现，则可以创建PoseSubscriber.cpp并如下写制：

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

void doPose(const turtlesim::Pose::ConstPtr &pose)
{
ROS_INFO("The turtle's x=%.2f,y=%.2f,theta=%.2f,linear=%.2f,
angular=%.2f",pose->x,pose->y,pose->theta,pose->linear_velocity, 	pose->angular_velocity);
}

int main(int argc, char *argv[])
{
ros::init(argc, argv, "PoseSubscriber");
ros::NodeHandle nh;
ros::Subscriber sub = nh.subscribe<turtlesim::Pose>("/turtle1/pose", 100, 	doPose);
ros::spin();
return 0;
}

3.服务调用模拟：
向turtlesim发送请求，在图形化界面指定位置生成一只新的海龟，这是一个服务请求操作。
同样，首要的问题也是找到对应服务的名称，可以使用rosservice list找到/spawn服务
使用rosservice info /spawn可得：

可见得到服务的信息分别为提供服务的节点、节点的资源路径、当前服务使用的消息类型和请求服务需要提供的字段（位姿和名字）
利用rossrv info turtlesim/Spawn得到该信息的格式如下：

可以看到在请求上述四个参数后服务会响应新海龟的名称
同样的，我们可以使用ros命令rosservice call /spawn tabtab进行简单实现。
如果用代码实现，则可以创建SpawnService.cpp并如下写制：

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

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

ros::ServiceClient client = nh.serviceClient<turtlesim::Spawn>("/spawn");
turtlesim::Spawn spawn;
spawn.request.x = 1.0f;
spawn.request.y = 4.0f;
spawn.request.theta = 1.57f;
spawn.request.name = "turtle2";

ros::service::waitForService("/spawn");
bool flag = client.call(spawn);
if (flag)
ROS_INFO("Success!The new turtle's name is %s", 
spawn.response.name.c_str());
else
ROS_INFO("Failed!");
return 0;
}

4.参数服务器模拟：
可以通过修改修改turtlesim的参数服务器中的背景rgb参数来改变海龟图形化界面的背景色，这是一个参数服务器操作。
通过rosparam list可以找到控制背景色的三个参数：/turtlesim/background_r/g/b
通过rosparam get /turtlesim/background_r/g/b可以得到其值分别为69/86/255
可以使用ros命令rosparam set /turtlesim/background_r/g/b 0-255进行修改。
值得注意的是，需要将图形化界面重新启动才能产生效果。
如果用代码实现，则可以创建SpawnService.cpp并如下写制：

#include "ros/ros.h"

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

ros::param::set("/turtlesim/background_b", 0);
ros::param::set("/turtlesim/background_g", 0);
ros::param::set("/turtlesim/background_r", 0);
return 0;
}

此外，我们还可以在启动节点时直接设置参数，或者通过launch文件传参的方法实现。

<node pkg="turtlesim" type="turtlesim_node" name="set_bg" output="screen"><rosparam command="load" file="$(find demo03_test_parameter)/cfg/color.yaml" /></node>

综合上述内容，我们讲解了ROS中最基本的也是最核心的通信机制实现: 话题通信、服务通信、参数服务器。三种通信机制中，参数服务器是一种数据共享机制，可以在不同的节点之间共享数据，而话题通信与服务通信则可以在不同的节点之间传递数据。
对比话题通信与服务通信两者可以看到明显的异同点：
二者的实现流程是比较相似的，都是涉及到四个要素:
要素1: 消息的发布方/客户端(Publisher/Client)
要素2: 消息的订阅方/服务端(Subscriber/Server)
要素3: 话题名称(Topic/Service)
要素4: 数据载体(msg/srv)
可以概括为: 两个节点通过话题关联到一起，并使用某种类型的数据载体实现数据传输。
同时，二者的实现也是有本质差异的，具体比较如下:

不同通信机制有一定的互补性，都有各自适应的应用场景。尤其是话题与服务通信，需要结合具体的应用场景与二者的差异，选择合适的通信机制。
接下来我们讲一讲ROS常用的API，其中部分已在之前的代码里有体现：
1.初始化API：
ROS节点的初始化使用ros::init实现：
void ros::init(int &argc,char **argv,const std::string& name,uint32 options=0)
使用示例：ros::init(argc, argv, “CirclePublisher”);
参数列表：argc 传入参数个数
argv 传入参数列表
name 节点名称，需要保证其唯一性，不允许包含命名空间
options 节点启动选项，ROS中当有同名的节点同时处于运行状态时，先启动 的节点将会被关闭，为了使同名节点也能同时处于运行状态，可以设置options 为ros::init_options::AnonymousName实现，此时同名节点后会产生不同值 的随机数，以此避免重名问题。
返回值：void无返回值
2.话题与服务API：
ROS话题和服务的相关对象使用ros::NodeHandle创建：
故首先就要创建节点句柄ros::NodeHandle nh
①发布端对象：
1>创建发布端对象：调用节点句柄的advertise函数实现

template <class M>
Publisher advertise(const std::string& topic, uint32_t queue_size, bool latch = false)
使用示例：
ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 10);

泛型：M 话题中发布消息的类型格式
参数列表：topic 发布消息使用的话题
queue_size 等待发送给订阅者的最大消息数量
latch如果为 true,该话题发布的最后一条消息将被保存，并且后期当有订阅者 连接时会将该消息发送给订阅者，适用于发布的一些静态消息，例如导航地图
返回值：调用成功时，会返回一个发布端对象ros::Publisher
2>发布消息：调用发布端对象的publish函数实现

template <typename M>void publish(const M& message) const

使用示例：pub.publish(twist);
泛型：M 发布的消息message的类型格式
参数列表：message 向话题发布的信息，需要与话题中发布消息的类型格式一致
返回值：void无返回值
②订阅端对象：
1>创建订阅端对象：调用节点句柄的subscribe函数实现

template<class M>
Subscriber subscribe(const std::string& topic, uint32_t queue_size, void(*fp)
(const boost::shared_ptr<M const>&))

使用示例：

ros::Subscriber sub = nh.subscribe<turtlesim::Pose>("/turtle1/pose", 100, doMsg);

void doMsg(const 包名::xxx::ConstPtr &msg){}
泛型：M 话题中订阅消息的类型格式
参数列表：topic 订阅的话题
queue_size 消息队列长度，超出长度时，头部的消息将被弃用
fp 当订阅到一条消息时，需要执行的回调函数
返回值：调用成功时，会返回一个订阅端对象ros::Subscriber
③服务端对象：
1>创建服务端对象：调用节点句柄的advertiseService函数实现

template<class MReq, class MRes>
ServiceServer advertiseService(const std::string& service, bool(*srv_func)
(MReq&, MRes&))

使用示例：

ros::ServiceServer server = nh.advertiseService("AddInts", doReq);
bool doReq(ServiceCommunication::AddInts::Request &request,
ServiceCommunication::AddInts::Response &response){}

泛型：MReq 接收客户端消息并做处理的回调函数请求
MRes 接收客户端消息并做处理的回调函数响应
参数列表：service 服务的名称
srv_func 接收到请求时，需要处理请求的回调函数
返回值：调用成功时，会返回一个服务端对象ros::ServiceServer
④客户端对象：
1>创建客户端对象：调用节点句柄的serviceClient函数实现

template<class Service>
ServiceClient serviceClient(const std::string& service_name, bool persistent = false, const M_string& header_values = M_string())

使用示例：

ros::ServiceClient client = nh.serviceClient<ServiceCommunication::
AddInts>("AddInts");

泛型：Service 服务中消息的类型格式
参数列表：service_name 服务的名称
返回值：调用成功时，会返回一个客户端对象ros::ServiceClient
2>调用服务：调用客户端对象的call函数实现
templatebool call(Service& service)
使用示例：turtlesim::Spawn spawn;
bool flag = client.call(spawn);
泛型：Service 调用服务的消息service的类型格式
参数列表：service调用服务的消息，需要与服务中消息的类型格式一致
返回值：调用成功时返回True，否则返回False
3>等待服务函数：使用ros::service或ros::ServiceClient
使用示例：ros::service::waitForService(“/spawn”);
client.waitForExistence();
3.回旋函数API：
ROS中使用了 ros::spin() 和 ros::spinOnce() 两个回旋函数用于处理回调函数。
回调函数是一个在特定事件或条件发生时被调用的函数。它的主要目的是允许软件组件之间进行异步通信，即一个组件可以在某个特定事件发生时通知另一个组件，
回调函数通常作为参数传递给另一个函数，以便在需要时被调用。这个被传递的函数通常被称为回调函数，而调用回调函数的函数则被称为调用方。回调函数的一个关键特点是它们通常在编写代码时被定义，但在稍后的某个时间点被调用。这种延迟调用的特性使得回调函数在异步编程和事件驱动编程中非常有用。
ros::spin() 和 ros::spinOnce()二者都用于处理回调函数，但ros::spin()会循环执行回调函数，而 ros::spinOnce() 只会执行一次回调函数，在 ros::spin() 后的语句不会执行到，而 ros::spinOnce() 后的语句可以执行。
4.时间API：
ROS中时间相关的API极其常用，比如获取当前时刻、持续时间的设置、执行频率、休眠、定时器等都与时间相关。
①初始化节点ros::init并创建句柄ros::NodeHandle：
调用时间API前必须初始化节点并创建句柄，否则时间无法初始化导致后续API调用失败
ros::init(argc,argv,”RosTime”);
ros::NodeHandle nh;
②得到时刻对象：ros::Time是时刻对象，其中封装了很多内容而不单单是一个时刻值，使用ros::Time::now()获取当前时刻的对象：ros::Time right_now = ros::Time::now();
或是使用初始化函数设置一个时刻对象，传入参数为要设置的时刻值，例如：
ros::Time someTime(100.3);
值得注意的是，当前时刻的表示为时间戳，即离1970.1.1 00:00:00的秒数
③获取时间间隔对象：ros::Duration是时间间隔对象，其中也封装了很多内容。
可以使用初始化函数设置一个时间间隔对象，传入参数为要设置的时间间隔值，例如：
ros::Duration du(10);
④获取当前的时刻值：使用ros::Time对象的toSec()方法，例如：
someTime.toSec() //double类型，可以打印得到其值为100.3
⑤休眠相当于时间间隔对象值的时间：使用ros::Duration对象的sleep()方法，例如：
du.sleep(); //相当于使程序休眠10s再继续执行接下来的内容
⑥时间间隔对象和时刻对象可以进行相加减，其中两个时刻对象只能相减，其返回值仍为一时刻对象；而两个时间间隔对象可以任意相加减，其返回值仍为一时间间隔对象；值得注意的是，时间间隔对象和时刻对象之间也可以任意相加减，而其返回值为一时刻对象。
⑦设置运行频率：使用ros::Rate对象的sleep方法实现
ros::Rate rate(10); //初始化ros::Rate对象，指定频率为10Hz
while (true)
{
ROS_INFO(“——————-“);
rate.sleep(); //休眠，休眠时间为0.1s，变相即设置运行频率为10H
}
⑧定时器：ROS 中内置了专门的定时器ros::Timer，可以实现与 ros::Rate类似的效果
Timer createTimer(Duration period, const TimerCallback& callback, bool oneshot = false,bool autostart = true) const;
使用示例：ros::Timer timer = nh.createTimer(ros::Duration(0.5),doSomeThing);
void doSomeThing(const ros::TimerEvent &event){
ROS_INFO(“event:%s”,std::to_string(event.current_real.toSec()).c_str());
}

参数列表：period 时间间隔对象，表示定时器的时间间隔
callback 回调，当定时器的时间间隔完成后调用，输入为时间事件TimerEvent
oneshot 如果设置为 true,只执行一次回调函数，设置为 false,就循环执行。
autostart 如果为true，返回已经启动的定时器,设置为 false，需要手动启动，
即在代码对应的启动位置调用ros::Timer的start()方法，例如timer.start()
由于回调函数的存在， ros::spin()显然也是要添加的
5.关闭节点API：使用ros::shutdown()实现
除了代码实现，当节点接收到了关闭信息(ctrl + c等)或同名节点启动也会导致节点关闭
6.日志输出API：除了ROS_INFO之外，还有以下几种日志输出API
ROS_DEBUG(“hello,DEBUG”); //调试信息，不会输出到控制台
ROS_INFO(“hello,INFO”); //标准消息，说明系统内正在执行的操作，白色字体
ROS_WARN(“Hello,WARN”); //警告信息，程序存在异常但仍然可以执行，黄色字体
ROS_ERROR(“hello,ERROR”); //错误信息，此类错误会影响程序运行，红色字体
ROS_FATAL(“hello,FATAL”); //严重错误，此类错误将阻止节点继续运行，红色字体

自定义头文件、源文件和可执行文件的调用：

当功能包由头文件和源文件组成，其中源文件和可执行文件为同一文件时，为了使源文件定义头文件的声明，需要包含头文件，格式为#include “../include/包名/头文件.h”。为了包含头文件时不抛出异常，可以配置.vscode 下 c_cpp_properties.json 的 includepath属性加上”/home/用户/工作空间/src/功能包/include/**”，可以通过pwd得到。
此时.h和.cpp的编写不做赘述，主要看一下CMakeLists.txt的配置：
为了完成头文件的配置，需要把L119的注释去掉：

include_directories(
include
  ${catkin_INCLUDE_DIRS}
)

源文件的配置仍然不变，即add_executable,add_dependencies,target_link_libraries
当功能包由头文件、源文件和可执行文件共同组成时（即源文件只进行对头文件声明的定义，可执行文件才是程序入口），在CMakeLists.txt的配置上又有所不同：
为了完成头文件的配置，需要把L119的注释去掉：

include_directories(
include
  ${catkin_INCLUDE_DIRS}
)

为了完成头文件和源文件的联立配置，需要声明一套C++库：

add_library(库名
  include/包名/头文件.h
  src/源文件.cpp
)

接着将这个新声明的自定义库添加依赖和链接库：

add_dependencies(库名 ${${PROJECT_NAME}_EXPORTED_TARGETS} 
${catkin_EXPORTED_TARGETS})
target_link_libraries(库名
  ${catkin_LIBRARIES}
)

最后是可执行文件，仍为add_executable,add_dependencies,target_link_libraries
不同点在于配置链接库时需要添加之前新声明的自定义库：

target_link_libraries(use_head
  库名
  ${catkin_LIBRARIES}
)

ROS元功能包：

完成ROS中一个系统性的功能可能涉及到多个功能包，比如机器人导航模块下有地图、定位、路径规划等不同的子级功能包。为此，ROS中提供了一种方式可以将不同的功能包打包成一个具有相同工作目标的功能包集合，这个集合就是元功能包(metapackage)。
元功能包是ROS中的一个虚包，里面没有实质性的内容，但是它依赖了其他的软件包，通过这种方法可以把其他包组合起来，我们可以认为它是一本书的目录索引，告诉我们这个包集合中有哪些子包。
创建ROS元功能包的方法如下：
首先新建一个包，作为虚包，只需要包名而不需要依赖(roscpp rospy std_msgs)
然后:修改package.xml ,内容如下:

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

最后:修改 CMakeLists.txt,内容如下:
cmake_minimum_required(VERSION 3.0.2)
project(xxx)
find_package(catkin REQUIRED)
catkin_metapackage()
注意，所有的其他东西包括注释全都不要，只留这四行

Launch文件的使用：

一个程序中可能需要启动多个节点，比如ROS内置的小乌龟案例，如果要控制乌龟运动，要启动roscore、乌龟界面节点、键盘控制节点。如果每次都调用 rosrun 逐一启动，显然效率低下，采用的优化策略便是使用roslaunch 命令集合 launch 文件启动管理节点。并且在后续教程中，也多次使用到了 launch 文件。
launch 文件是一个 XML 格式的文件，可以启动本地和远程的多个节点，还可以在参数服务器中设置参数，它可以简化节点的配置与启动，从而提高ROS程序的启动效率。
0.XML文件的基本格式：

一个XML文档由一个或多个元素构成，一个元素又可以包含属性和内容两部分。元素是XML文档的基本结构单元，由开始标签和结束标签组成。元素可以包含子元素，也可以包含内容。开始标签用<xxx>表示，结束标签为开始标签加/组成</xxx>。例如<element>数据</element>，其中数据就是element元素的内容或子元素。
元素也可以包含属性，属性用于提供关于元素的附加信息。属性出现在元素的开始标签中，位于元素名称之后，以等号（=）分隔属性名称和属性值。例如：<element 属性名="属性值">数据</element>。如果一个元素在定义时不包含任何文本内容或其他子元素，则称其为自闭合元素，此时可以将语句简化为<element 数据 /> 。
1.以 turtlesim 为例演示Launch文件的使用方法：
1>新建launch文件
在功能包下添加 launch目录, 目录下新建 xxxx.launch 文件，编辑 launch 文件
<launch>
<node pkg="turtlesim" type="turtlesim_node" name="myTurtle" 
output="screen" />
<node pkg="turtlesim" type="turtle_teleop_key"
name="myTurtleContro" output="screen" />
</launch>
2>调用 launch 文件
roslaunch 包名 xxx.launch
注意:roslaunch 命令执行launch文件时会自动启动roscore
2.launch标签：
<launch>标签是所有launch文件的根标签，充当其他标签的容器
属性：deprecated = "弃用声明"，告知用户当前 launch 文件已经弃用
子标签：所有其它标签都是launch的子标签
示例：<launch deprecated=”此文件已被弃用”>
<node...>
</launch>
3.node标签：
<node>标签用于指定ROS节点，但roslaunch命令不能保证按照node的声明顺序来启动节点，因为节点的启动是多进程的。
属性：pkg="包名"，确定节点所属的包名
type="节点名"，确定节点名(CMakeLists中定义的名称)
name="节点称号”，确定节点称号(ros::init中定义的网络拓扑称号)
output="log | screen" (可选)，日志发送目标，可以设置为日志文件或屏幕
args="xxx xxx xxx" (可选)，将参数传递给节点，中间用空格分隔
machine="机器名"，在指定机器上启动节点
respawn="true | false" (可选)，如果节点意外退出，是否自动重启
respawn_delay=" N" (可选)，如果respawn=true, 那么延迟 N 秒后启动节点
required="true | false" (可选)，该节点是否必须，如果为true,那么如果该节点	  退出，将杀死整个roslaunch
ns="xxx" (可选)，在指定命名空间xxx中启动节点，此时如果使用rosnode list	  查看的话，该节点的路径将会变为/xxx/节点名
子标签：remap 重映射节点名称
rosparam 参数设置
param 参数设置
示例：<launch>
<node pkg="turtlesim" type="turtlesim_node" name="myTurtle" 
output="screen" />
  </launch>
4.include标签：
如果不同的launch文件之间需要启动相同的节点，可以使用<include>标签将另一个xml格式的 launch 文件导入到当前文件，以此优化代码文件，实现代码复用，同时在当前文件下还可以在导入文件的基础上增加节点。
属性：file="$(find 包名)/xxx(通常为launch)/xxx.launch"，要包含的文件路径
ns="xxx" (可选)，在指定命名空间导入文件
子标签：arg	将参数传递给被包含的文件
示例：<launch>
<include file="$(find launch01_basic)launch/StartTurtle.launch" />
  </launch>
5.remap标签：
用于话题重命名，以此改变节点订阅的话题
属性：from="xxx"，原始话题名称
to="yyy"，目标话题名称
子标签：无
示例：<node pkg="turtlesim" type="turtlesim_node" name="my_turtle" output="screen">
<remap from="/turtle1/cmd_vel" to="/cmd_vel" />
</node>
6.param标签：
<param>标签主要用于在参数服务器上设置参数，参数源可以在标签中通过 value 指定，也可以通过外部文件加载。<param>标签可以放在<launch>下在可以放在<node>下，<launch>下相当于公有参数，<node>下相当于节点的私有参数。
可以使用rosparam list查看设置的变量及其所在的位置。
属性：name="命名空间/参数名"，参数名称，可以包含命名空间
value="xxx" ，定义参数值
type="str | int | double | bool"，指定参数类型
子标签：无
示例：<node pkg="turtlesim" type="turtlesim_node" name="my_turtle" 		  output="screen">
<param name="ParamB" type="double" value="3.14" />
</node>
7.rosparam标签：
<rosparam>标签可以从YAML文件导入参数，或将参数导出到 YAML文件，也可以用来删除参数，同样，<rosparam>也可以放在<launch>或者<node>下。
YAML是一种数据序列化格式，用来确定数据组、名称及其对应的值。它使用缩进表示层级，
由键值对组成数据，键和值之间以冒号分隔，且冒号后面必须有一个空格。
属性：command="load | dump | delete"，加载、导出或删除参数
file="$(find 包名)/xxx/yyy...."，加载或导出到的 yaml 文件，加载导出用
需要注意的是，导出文件需要和调用文件分开，否则无法保证参数均被存储
param="参数名称"，删除参数用
ns="命名空间" (可选)
子标签：无
示例：
<rosparam command="load" file="$(find RosLaunch)/launch/params.yaml" />
<rosparam command="dump" file="$(find RosLaunch)/launch/param.yaml"/>
<rosparam command="delete" param=”bg_B”/>
8.group标签：
<group>标签可以对节点分组，具有 ns 属性，可以让节点归属某个命名空间，从而实现节点的聚类。此外，将同名节点归于不同的命名空间可以避免重名问题。
通过rosnode list可以发现同名节点之前附加上了命名空间路径从而实现区分。
属性：ns="名称空间" (可选)
子标签：除了launch 标签外的其他标签
示例：<group ns="first">
<node pkg="turtlesim" type="turtlesim_node" name="my_turtle" 	 	  output="screen" />
</group>
9.arg标签：
<arg>标签是用于动态传参，类似于函数的参数，可以增强launch文件的灵活性
属性：name="参数名称"，在launch文件中可以使用$(arg name)表示对应的参数值
default="默认值"，不传参时的参数默认值
子标签：无
传参方法：在roslaunch语句后添加键值对roslaunch 包名 xxx.launch yyy:=值
示例：<arg name="test" default=”0.5” />
      <param name="param" value="$(arg test)" />
  roslaunch RosLaunch arg.launch car_length:=0.6

重名问题：

1.功能包重名：
虽然特定工作空间内的功能包不能重名，但是自定义工作空间的功能包与内置的功能包可以重名或者不同的自定义的工作空间中也可以出现重名的功能包，那么调用该名称功能包时，会调用哪一个呢？比如：自定义工作空间A存在功能包 turtlesim，自定义工作空间B也存在功能包 turtlesim，当然系统内置空间也存在turtlesim，如果调用turtlesim包，会调用哪个工作空间中的呢？
ROS会将当前启用的所有工作空间存放在环境变量ROS_PACKAGE_PATH中，通过查看echo $ROS_PACKAGE_PATH可以得到一个工作空间顺序，当不同工作空间的功能包重名时，会按照 ROS_PACKAGE_PATH 查找，并优先执行顺序在前的工作空间下的功能包，这被称为ROS的工作空间覆盖。
工作空间覆盖问题显然存在安全隐患，比如当前工作空间B优先级更高，意味着当程序调用 turtlesim 时，不会调用工作空间A也不会调用系统内置的 turtlesim，如果工作空间A在实现时有其他功能包依赖于自身的 turtlesim，而按照ROS工作空间覆盖的涉及原则，那么实际执行时将会调用工作空间B的turtlesim，从而导致执行异常。
ROS对于功能包重名暂无解决方法，故应选择主动避免。
2.节点重名：
在ROS中如果启动重名节点的话，之前已经存在的节点会被直接关闭，但是如果确实有这种需求，ROS提供了使用命名空间或名称重映射（起别名）的解决方法。要实现这两种解决办法还各有三种实现途径，分别是使用rosrun命令，修改launch文件或编码实现。
①使用rosrun命令：
1>使用命名空间：rosrun 包名 同名节点 ns:=工作空间
2>名称重映射:rosrun 包名 同名节点 name:=别名
3>上述两种方法可以叠加：rosrun 包名 同名节点 ns:=工作空间 name:=别名
4>观察修正结果：使用rosnode list，使用命名空间会在节点前加前缀，使用名称重映射则会直接将节点改名。
②修改launch文件：
介绍launch文件的使用语法时曾经讲过，在node标签中有name和ns两个属性，二者分别用于实现节点名称重映射与设置命名空间。

1>使用命名空间：<node pkg="包名" type="同名节点" ns="工作空间"/>
2>名称重映射：<node pkg="包名" type="同名节点" name="别名" />
3>叠加：<node pkg="包名" type="同名节点" name="别名" ns="工作空间"/>

③编码实现：
1>使用命名空间：在代码初始化ROS节点时补充设置为节点添加时间戳后缀
ros::init(argc,argv,”xxx”,ros::init_options::AnonymousName);
3.话题重名：
ROS不同的节点之间通信都依赖于话题，话题名称也可能出现重复的情况。这种情况下，系统虽然不会抛出异常，但是可能导致订阅的消息非预期的，从而导致节点运行异常。这种情况下需要将两个节点的话题名称由相同修改为不同。
又或者，两个节点的话题名称虽然相同，但位于不同的工作空间下导致通信失败。这种情况下需要将两个节点的话题名称由不同修改为相同。
其解决方法仍然是话题名称重映射或为名称添加前缀。特殊的，对于话题而言，为名称添加前缀有全局、相对和私有三种方法。使用rostopic list查看话题，其中全局话题表示为/话题名，相对话题表示为/命名空间/话题名，私有话题表示为/命名空间/节点名/话题名
①使用rosrun命令：rosrun 包名 节点名 原话题名:=新话题名

②修改launch文件：使用node节点的remap属性

<node pkg=”包名” type=”节点名” name="xxx">
    <remap from="原话题名" to="新话题名" />
</node>

③编码实现：
1>全局话题：话题名以/开头，此时话题全名和节点名称无关
例如ros::Publisher pub = nh.advertise(“/chatter”,1000);
此时话题全名为/chatter
2>相对话题：话题名以非/开头，此时话题全名位于节点所在工作空间下与节点平齐
例如ros::Publisher pub = nh.advertise(“chatter”,1000);
此时话题全名为/节点所处的工作空间名/chatter
2>私有话题：节点句柄添加”~”元素时以非/开头的话题名
例如：ros::NodeHandle nh(“~”);
ros::Publisher pub = nh.advertise(“chatter”,1000);
此时话题全名为/节点所处的工作空间名/节点名/chatter
4.参数名称重名：
同理在ROS中参数名称也可能重名。关于参数重名的处理没有重映射实现，为了尽量的避免参数重名，都是使用为参数名添加前缀的方式，实现类似于话题名称，有全局、相对和私有三种类型之分。
①使用rosrun命令： rosrun 包名 节点名 _参数名:=参数值
值得注意的是，rosrun执行设置参数参数名使用的是私有类型。
②修改launch文件：在node标签外或内通过param标签来设置参数。在node标签外设置的参数是全局性质的，参数全名是/参数名，在node标签中设置的参数是私有性质的，参数全名是/命名空间/节点名称/参数名。
例如：

<param name="p1" value="100" />
<node pkg="turtlesim" type="turtlesim_node" name="t1">
  <param name="p2" value="100" />
</node>

此时参数全名分别是 /p1和 /xxx/p2
③编码实现：
1>全局变量：变量名以/开头，此时变量全名和节点名称无关
例如：ros::param::set(“/set_A”,100);
此时话题全名为/set_A
2>相对变量：变量名以非/开头，此时变量全名位于节点所在工作空间下与节点平齐
例如：ros::param::set(“set_B”,100);
此时话题全名为/节点所处的工作空间名/set_B
3>私有变量：变量名以~开头，此时变量全名位于节点下
例如：ros::param::set(“~set_C”,100);
此时话题全名为/节点所处的工作空间名/节点名/set_C

分布式通信：

一个运行中的ROS系统可以包含分布在多台计算机上多个节点。如果要实现不同计算机间不同节点的通信，我们需要进行以下几步：
1.保证不同计算机处于同一网络中，最好分别设置固定IP。
2.配置文件修改：
分别修改不同计算机的 /etc/hosts 文件，在该文件中加入对方的IP地址和计算机名:
主机端:从机的IP 从机计算机名；从机端:主机的IP 主机计算机名
设置完毕，可以通过 ping 命令测试网络通信是否正常。
注：IP地址查看: ifconfig / 计算机名称查看: hostname
3.配置主机IP：向~/.bashrc中追加：

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

4.配置从机IP：向~/.bashrc中追加：

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

5.测试方法：主机启动roscore，而后双方分别启动节点测试通信是否正常即可

ROS坐标变换TF：

机器人系统上，有多个传感器，如激光雷达、摄像头等，有的传感器是可以感知机器人周边的物体方位的，以协助机器人定位障碍物。可以直接将物体相对该传感器的方位信息，等价于物体相对于机器人系统或机器人其它组件的方位信息吗？显示是不行的，这中间需要一个转换过程，为此ROS提供了坐标变换模块TF来实现这个功能。
1.先介绍一下四元数的概念，四元数是由实部和虚部组成的扩展复数，一个四元数可以表示为 q = w + xi + yj + zk，其中 w 是实部，(x, y, z) 是虚部对应的旋转轴上的分量。通过四元数可以方便地表示旋转操作，一个四元数 q 可以表示为 q = cos(θ/2) + u sin(θ/2)，其中 θ 是旋转角度，u 是单位旋转轴向量。例如，对于绕以 (1, 0, 0) 为轴，角度为 π/2 的旋转操作，可以构建四元数 q = cos(π/4) + i sin(π/4)。四元数本身可以克服欧拉角带来的万向死锁问题。
2.ROS的坐标变化基于订阅发布模型，其对应的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方向
  float64 y										//Y方向
  float64 z										//Z方向

3.静态坐标变换（两个坐标系之间的相对位置固定）：
发布与订阅关系：坐标系的相对关系通过发布方发布，订阅方订阅发布的坐标系相对关系，再传入坐标点信息，然后借助于tf实现坐标变换，并将结果输出。
代码实现：
①创建项目功能包：
包括tf2 tf2_ros tf2_geometry_msgs roscpp rospy std_msgs geometry_msgs
②发布方实现：

#include "ros/ros.h"
#include "tf2_ros/static_transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"

int main(int argc, char *argv[])
{
// 初始化节点和节点句柄
ros::init(argc, argv, "StaticTfPublisher");
ros::NodeHandle nh;
// 创建发布者对象
tf2_ros::StaticTransformBroadcaster pub;
// 组织被发布的消息：父子坐标系及其相对关系
geometry_msgs::TransformStamped tfs;
tfs.header.stamp = ros::Time::now();
tfs.header.frame_id = "base_link";
tfs.child_frame_id = "laser";
tfs.transform.translation.x = 0.2;
tfs.transform.translation.y = 0.0;
tfs.transform.translation.z = 0.5;
// 将欧拉角变为四元数
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();
// 发布数据
pub.sendTransform(tfs);
// spin()回调
ros::spin();
return 0;
}
③订阅方实现：
#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"
int main(int argc, char *argv[])
{
// 初始化节点和节点句柄
ros::init(argc, argv, "StaticTfSubscriber");
ros::NodeHandle nh;
// 创建buffer和坐标变换监听对象，用于存储订阅得到的数据
tf2_ros::Buffer buffer;
tf2_ros::TransformListener listener(buffer);
// 组织一个PointStamped对象表示在laser坐标系下的坐标
geometry_msgs::PointStamped ps;
ps.header.frame_id = "laser";
ps.header.stamp = ros::Time::now();
ps.point.x = 2.0;
ps.point.y = 3.0;
ps.point.z = 5.0;
ros::Rate rate(10);
while (ros::ok())
{
//try-catch避免缓存接收延迟而导致坐标转换失败
try
{
// 组织一个PointStamped对象表示在base_link坐标系下的坐标
geometry_msgs::PointStamped ps_out;
ps_out = buffer.transform(ps, "base_link");
// 得到结果并输出
ROS_INFO("TF according to %s is:(%2f,%2f,%2f)",
ps_out.header.frame_id.c_str(),
ps_out.point.x,
ps_out.point.y,
ps_out.point.z);
}
catch (const std::exception &e)
{
ROS_INFO("Error:%s", e.what());
}

rate.sleep();
ros::spinOnce();
}
return 0;
}

④总结：
可以看到此时发布方发布坐标间的相对位置关系，订阅方借助自身得到的以某一坐标系为基准的坐标值去订阅话题以实现TF坐标转换到另一坐标系为基准的坐标值。
⑤补充：
1>ROS本身封装了专门的节点用于实现静态坐标系相对关系信息的发布，借助命令：
rosrun tf2_ros static_transform_publisher x偏移量 y偏移量 z偏移量 偏航角度 俯仰角度 翻滚角度 父级坐标系 子级坐标系
可以实现发布方代码的全流程，即发布父子坐标系的名字以及之间的相对关系
2>可以借助于rviz来实现以图形化的形式显示坐标系关系，具体操作如下：
输入命令rviz后设置Fixed Frame为base_link;
点击左下的 add 按钮，在弹出的窗口中选择 TF 组件，即可显示坐标关系。
3>使用rosrun tf2_tools view_frames.py可以生成坐标系树状关系结构的pdf
4.动态坐标变换（两个坐标系之间的相对位置不再固定）：
以turtlesim为例，该节点中窗体有一个世界坐标系(左下角为坐标系原点)，而乌龟本身为另一个坐标系，由于乌龟是可以运动的，所以世界坐标系与乌龟坐标系之间的相对位置不固定。假设有一个点相对于乌龟坐标系的值已知，那么把该点转换为世界坐标系的过程就是动态坐标变换。订阅 turtle1/pose,可以获取乌龟在世界坐标系的 x坐标、y坐标、偏移量以及线速度和角速度。
代码实现：
①创建项目功能包：
包括tf2 tf2_ros tf2_geometry_msgs roscpp rospy std_msgs geometry_msgs
②发布方实现：
void doPose(const turtlesim::Pose::ConstPtr &pose)
{
static tf2_ros::TransformBroadcaster pub;
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;
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();
pub.sendTransform(ts);
}

int main(int argc, char *argv[])
{
// 初始化节点和节点句柄
ros::init(argc, argv, “DynamicTfPublisher”);
ros::NodeHandle nh;
// 订阅发布乌龟位姿的节点，以此得到动态坐标系（乌龟坐标系）的实时位置
// 然后在回调函数里利用TransformStamped发布两坐标系间的相对关系
ros::Subscriber sub = nh.subscribe(“/turtle1/pose”, 100, doPose);
ros::spin();
return 0;
}
③订阅方实现：
无论是静态坐标系还是动态坐标系，某一坐标系下的坐标点相对坐标原点的关系总是一样的，因而动态坐标系的订阅方实现与静态坐标系几乎没有不同，只需要把父子坐标系的名字改成需要考虑的即可。唯一值得注意的是此时组织的PointStamped对象的时间戳stamp不应该是ros::Time::now()而应该是ros::Time(0.0)，原因在于ROS会检验动态坐标系发布的时间戳和某一点相对于某一坐标的时间戳的差值，如果两者有较大的差别则ROS会认为此时前者的值在后者看来已经过时了，也就是此时的坐标变化不准确而失去意义，因而ROS会抛出异常并不再处理。可以看到如果我们把发布方和订阅方的时间戳都设置为ros::Time::now()，发布和订阅之间所产生的时间差将导致ROS抛出异常。而将其设置为ros::Time(0.0)后ROS会认为该信息对时间不敏感，也就能输出正常的结果了。
④总结：
可以看到此时发布方先订阅了乌龟位姿节点以获取动态的乌龟坐标系值，而后再发布动态坐标间的相对位置关系，订阅方借助自身得到的以某一坐标系为基准的坐标值去订阅话题以实现TF坐标转换到另一坐标系为基准的坐标值。同样我们可以使用rviz可视化的看到坐标系间的关系，可以看到此时乌龟坐标系是会随着键盘节点命令而运动的。
5.多坐标变换：
现有坐标系统，父级坐标系统 world,下有两子级系统 son1，son2，son1 相对于 world，以及 son2 相对于 world 的关系是已知的，求 son1原点在 son2中的坐标，又已知在 son1中一点的坐标，要求求出该点在 son2 中的坐标。
代码实现：
①创建项目功能包：
包括tf2 tf2_ros tf2_geometry_msgs roscpp rospy std_msgs geometry_msgs
②发布方实现：直接使用launch文件调用静态坐标系中提到的专用节点来发布坐标关系

<launch>
    <node pkg="tf2_ros" type="static_transform_publisher" name="son1" 	args="0.2 0.8 0.3 0 0 0 /world /son1" output="screen" />
<node pkg="tf2_ros" type="static_transform_publisher" name="son2" 		args="0.5 0 0 0 0 0 /world /son2" output="screen" />
</launch>

③订阅方实现：

#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"

int main(int argc, char *argv[])
{
// 初始化节点和节点句柄
ros::init(argc, argv, "MultiTfSubscriber");
ros::NodeHandle nh;
// 创建buffer和坐标变换监听对象，用于存储订阅得到的数据
tf2_ros::Buffer buffer;
tf2_ros::TransformListener listener(buffer);
// 组织一个PointStamped对象表示在son1坐标系下的坐标
geometry_msgs::PointStamped ps_son1;
ps_son1.header.frame_id = "son1";
ps_son1.header.stamp = ros::Time::now();
ps_son1.point.x = 1.0;
ps_son1.point.y = 2.0;
ps_son1.point.z = 3.0;
ros::Rate rate(10);
while (ros::ok())
{
try
{
// 坐标系的相对关系：son1相对于son2（son2为父坐标系，son1为子坐	标系），ros::Time(0)表示取时间戳相差最近的两个值
geometry_msgs::TransformStamped son1Toson2 = 		buffer.lookupTransform("son2", "son1", ros::Time(0));
ROS_INFO("Father:%s,Son:%s,Offset:(%2f,%2f,%2f)",
son1Toson2.header.frame_id.c_str(),
son1Toson2.child_frame_id.c_str(),
son1Toson2.transform.translation.x,
son1Toson2.transform.translation.y,
son1Toson2.transform.translation.z);
// 坐标点的关系：组织PointStamped对象表示在son2坐标系下的坐标
geometry_msgs::PointStamped ps_son2 
= buffer.transform(ps_son1, "son2");
ROS_INFO("The location transformed according to %s is:
(%2f,%2f,%2f)",
ps_son2.header.frame_id.c_str(),
ps_son2.point.x,
ps_son2.point.y,
ps_son2.point.z);
}
catch (const std::exception &e)
{
ROS_INFO("Error:%s", e.what());
}

rate.sleep();
ros::spinOnce();
}
return 0;
}

④总结：
首先发布方需要发布son1和son2相对于world的坐标消息，然后需要订阅坐标发布消息，借助于tf2实现son1和son2的转换，最后还要实现坐标点的转换。

rosbag：

机器人传感器获取到的信息，有时我们可能需要时时处理，有时可能只是采集数据，事后分析，在ROS中关于数据的留存以及读取实现，提供了专门的工具rosbag，它实现了数据的复用，方便调试和测试。rosbag的本质也是一个ros节点，当录制时，rosbag是一个订阅节点，可以订阅话题消息并将订阅到的数据写入磁盘文件；当重放时，rosbag是一个发布节点，可以读取磁盘文件，发布文件中的话题消息。
①命令行实现：
1>创建rosbag记录：rosbag record -a -O 目标文件路径及名称（建议以.bag作为后缀）
其中-a表示all，即记录所有节点的数据变化；-O指output，即此时rosbag为输出
此时ROS便会开始记录所有节点的变化，当按下ctrl+C时终止并保存数据至.bag
2>查看存在的rosbag信息：rosbag info 目标文件路径及名称
3>读取并回放rosbag的数据：rosbag play 目标文件路径及名称
②代码实现：
1>写入实现：

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

int main(int argc, char *argv[])
{
// 初始化节点和节点句柄
ros::init(argc, argv, "WriteBag");
ros::NodeHandle nh;
// 创建rosbag对象，可以打开，写入和关闭
rosbag::Bag bag;
bag.open("hello.bag", rosbag::BagMode::Write);
std_msgs::String msg;
msg.data = "hello,rosbag!";
// 写入操作用到的话题，时间戳和具体内容
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);
bag.close();
return 0;
}
2>读出实现：
#include "ros/ros.h"
#include "rosbag/bag.h"
#include "rosbag/view.h"
#include "std_msgs/String.h"
#include "std_msgs/Int32.h"

int main(int argc, char *argv[])
{
// 初始化节点和节点句柄
ros::init(argc, argv, "ReadBag");
ros::NodeHandle nh;
// 创建rosbag对象
rosbag::Bag bag;
bag.open("hello.bag", rosbag::BagMode::Read);
// 读数据，rosbag::View以集合的方式返回一条消息，可以被迭代器迭代
for (auto &&m : rosbag::View(bag))
{
std::string topic = m.getTopic();
ros::Time time = m.getTime();
std_msgs::StringPtr p = m.instantiate<std_msgs::String>();
if (p != nullptr)
{
ROS_INFO("Topic:%s,Stamp:%.2f,Message:%s",
topic.c_str(),
time.toSec(),
p->data.c_str());
}
}

bag.close();
return 0;
}

3>结果：读出文件会输出写入文件的内容

RQT工具箱：

之前，在 ROS 中使用了一些实用的工具,比如: ros_bag 用于录制与回放、tf2_tools 可以生成 TF 树 ….. 这些工具大大提高了开发的便利性，但是也存在一些问题: 这些工具的启动和使用过程中涉及到一些命令操作，应用起来不够方便，在ROS中，提供了rqt工具箱，在调用工具时以图形化操作代替了命令操作，应用更便利，提高了操作效率。
呼出RQT工具箱：键入命令rqt
使用rqt提供的相关功能：既可以直接键入子命令（例如rqt_graph等），也可以在rqt的plugins中选择对应的功能。

机器人系统仿真：

ROS中提供了系统的机器人仿真实现，通过仿真，可以实现大部分需求。利用ROS仿真，我们可以学会创建并显示机器人模型，搭建仿真环境，实现机器人模型与仿真环境的交互。
机器人的仿真涉及三个模块，URDF、Rviz和Gazebo。
1.模块简介：
①URDF：URDF是Unified Robot Description Format的缩写，直译为统一机器人描述格式，是一种可以以XML的方式描述机器人的结构和关节的文件。该文件可以被C++内置的解释器转换成可视化的机器人模型。
②Rviz：Rviz是ROS Visualization Tool的缩写，直译为ROS的三维可视化工具，它的主要目的是以三维方式显示ROS消息，可以将数据进行可视化表达。
③Gazebo：Gazebo是一款3D动态模拟器，用于显示机器人模型并创建仿真环境,能够在复杂的室内和室外环境中准确有效地模拟机器人。
④总结：机器人的系统仿真是一种集成实现，其中URDF用于创建机器人模型，Rviz用于以图形化的方式显示机器人各种传感器感知到的环境信息，Gazebo用于搭建仿真环境。
三者应用中，只是创建URDF意义不大，一般需要结合Gazebo或Rviz使用，在Gazebo或Rviz中可以将URDF 文件解析为图形化的机器人模型，一般的使用组合为:如果非仿真环境，那么使用URDF结合Rviz直接显示感知的真实环境信息；如果是仿真环境，那么需要使用URDF结合Gazebo搭建仿真环境，并结合Rviz显示感知的虚拟环境信息。
2.代码实现URDF集成Rviz：
①导入依赖和搭建架构：创建新的功能包并导入依赖包urdf与xacro，新建目录:
1>urdf：再下分目录urdf和xacro，其中urdf存储urdf文件，xacro存储xacro文件
2>meshes：存储机器人模型渲染文件stl
3>config：存储配置文件rviz
4>launch：存储launch启动文件
②编写URDF文件：URDF 文件是一个标准的 XML 文件，在 ROS 中预定义了一系列的标签用于描述机器人模型，具体可以分为以下四类：
1>robot标签：根标签，类似于launch文件中的launch标签
a)name属性: 指定机器人模型的名称
b)子标签：其他标签都是robot子级标签
2>link标签：连杆标签，用于描述机器人某个部件(也即刚体部分)的外观和物理属性，比如:机器人底座、轮子、激光雷达、摄像头等都对应了一个link。
一个连杆由以下几部分组成：连杆参考的基坐标，连杆的视觉形象（连杆的形状，连杆基于基坐标产生的偏移和旋转，连杆的颜色和透明度），连杆的碰撞属性和惯性矩阵。

a)name属性: 指定连杆的名称
b)visual子标签：指定连杆的外观(对应的数据是可视的)
geometry子标签：设置连杆的形状（注意其下只能同时存在以下一个标签）
box子标签：设置连杆的形状为立方体
size属性：设置立方体的长(x) 宽(y) 高(z)
cylinder子标签：设置连杆的形状为圆柱
radius属性：设置圆柱的半径
length属性：设置圆柱的高度
sphere子标签：设置连杆的形状为球
radius属性：设置球的半径
mesh子标签：自定义设置连杆的形状
filename属性：设置自定义文件所在的路径(格式:package://…)
origin子标签：设置连杆相对于坐标原点的偏移量与倾斜弧度
xyz属性:设置坐标偏移x y z
rpy属性:设置坐标旋转r p y (弧度)
material子标签：设置连杆的颜色和透明度
name属性：指定material的名称
color子标签：设置连杆的颜色和透明度
rgba属性：红绿蓝权重值与透明度[0,1]
c)collision子标签：指定连杆的碰撞属性，即碰撞体积的大小
d)Inertial子标签：指定连杆的惯性矩阵
3>joint子标签：关节标签，用于连接机器人的两个连杆部件(分别称之为parent link与child link)，不同的关节有不同的运动形式: 他可以是任意旋转的云台、用螺丝螺母固定的结构、或是旋转受限的俯仰机构，这些都可以在joint标签下设置。需要注意的是，joint标签对应的数据在模型中是不可见的。
一个关节由以下几部分组成：父连杆与子连杆，关节相对于父连杆基坐标的偏移，关节的类型与旋转轴。需要注意的是此时关节的位置就是子连杆基坐标的位置。

a)name属性: 指定关节的名称
b)type 属性：指定关节的类型，分别可以设置为：
continuous: 旋转关节，可以绕单轴无限旋转
revolute: 旋转关节，类似于 continuous,但是有旋转角度限制
prismatic: 滑动关节，沿某一轴线移动的关节，有位置极限
planer: 平面关节，允许在平面正交方向上平移或旋转
floating: 浮动关节，允许进行平移、旋转运动
fixed: 固定关节，不允许运动的特殊关节
c)parent子标签：
link属性：父级连杆的名字
d)child子标签：
link属性：子级连杆的名字
e)origin子标签：
xyz属性: 关节相对于父连杆基坐标各轴线上的偏移量
rpy属性：关节相对于父连杆基坐标各轴线上的偏移弧度
f)axis子标签：
xyz属性: 设置关节围绕哪个关节轴（用向量表示）运动
4>gazebo标签：集成gazebo需要使用的标签，会在之后详细解释
③URDF工具：
1>命令check_urdf urdf文件可以检查复杂的urdf文件是否存在语法问题。如果不抛出异常，说明文件合法,否则非法。
2>命令urdf_to_graphiz urdf文件可以查看urdf关节和连杆的树形模型结构
④编写launch文件解析URDF文件：
之前提到过URDF需要被Rviz解析才能出现图形化的机器人模型，这一过程可以通过编写launch文件实现，具体的：

<launch>
//1.寻找urdf文件
<param name="xxx" textfile="$(find 包名)/urdf/urdf/urdf文件" />
//2.启动rviz节点，调用上次存储的rviz状态配置文件.rviz
//第一次载入时不加args，进入rviz后点击Add->RobotModel后Save Config
<node pkg="rviz" type="rviz" name="rviz" 
args="-d $(find 包名)/config/xxx.rviz"/>
//3.启动机器人状态和关节状态发布节点
<node pkg="robot_state_publisher" type="robot_state_publisher" 
name="robot_state_publisher" />
<node pkg="joint_state_publisher" type="joint_state_publisher" 
name="joint_state_publisher" />
//4.启动图形化的控制关节运动节点 
<node pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" 	name="joint_state_publisher_gui" />
</launch>

⑤编写Xacro文件优化URDF文件的实现过程：
前面URDF文件构建机器人模型的过程中，存在一些问题：
在设计关节的位置时，需要按照一定的公式计算，公式是固定的，但是在URDF中依赖于人工计算，存在不便，容易计算失误，且当某些参数发生改变时，还需要重新计算。
URDF 中的部分内容是高度重复的，驱动轮与支撑轮的设计实现，不同轮子只是部分参数不同，形状、颜色、翻转量都是一致的，在实际应用中，构建复杂的机器人模型时，更是易于出现高度重复的设计，按照一般的编程涉及到重复代码应该考虑封装。
在编程语言中，可以通过变量结合函数直接解决上述问题，ROS则选择引入了Xacro。Xacro 是一种 XML 宏语言，是可编程的 XML。Xacro 可以声明变量，可以通过数学运算求解，使用流程控制控制执行顺序，还可以通过类似函数的实现，封装固定的逻辑，将逻辑中需要的可变的数据以参数的方式暴露出去，从而提高代码复用率以及程序的安全性。
由于Xacro也是一种XML格式文件，因而其基本编写逻辑与URDF无异，但是Xacro文件额外提供了属性、宏和文件包含的XML语句，具体的：
1>属性：类似于变量定义，用于封装URDF中的一些字段，比如PI值，小车的尺寸等
属性定义：<xacro:property name="xxxx" value="yyyy" />
属性调用：${属性名称}
算数运算：${数学表达式}
2>宏：类似于函数实现，提高代码复用率
宏定义：<xacro:macro name="宏名称" params="参数列表(空格分隔)">
    .....
    参数调用格式: ${参数名}
</xacro:macro>
宏调用：<xacro:宏名称 参数1=xxx 参数2=xxx/>
3>文件包含：类似于include，将不同部件封装为单独的xacro文件再将它们集成。
文件包含语句：<xacro:include filename=”xxx.xacro" />
需要注意的是文件的包含是有顺序的，当某一文件需要调用另一文件的东西时，该文件	在文件包含中必须位于另一文件之后。一般来说会有一个总文件专门整合各部分Xacro。
需要注意的是Xacro文件本身是不能被Rviz直接解析的，需要先将Xacro文件编译为URDF文件，为此Xacro文件的根标签robot中必须包含命名空间声明属性的语句:
xmlns:xacro="http://wiki.ros.org/xacro"
此外，还需要修改launch文件的param属性为：
<param name="robot_description" command="$(find xacro)/xacro 
$(find 包名)/urdf/xacro/xxx.xacro" />
⑥使用Arbotix在Rviz中实现控制机器人模型运动：
通过 URDF 结合 rviz 可以创建并显示机器人模型，不过当前实现的只是静态模型，而使用 Arbotix 可以实现对机器人模型的运动控制。Arbotix 是一款控制电机、舵机的控制板，并提供相应的 ros 功能包，这个功能包的功能不仅可以驱动真实的 Arbotix 控制板，它还提供一个差速控制器，通过接受速度控制指令更新机器人的 joint 状态，从而帮助我们实现机器人在 rviz 中的运动。这个差速控制器在 arbotix_python 程序包中，完整的 arbotix 程序包还包括多种控制器，分别对应 dynamixel 电机、多关节机械臂以及不同形状的夹持器。
使用Arbotix的方法是在原有静态机器人模型的基础上添加 Arbotix 相关的配置文件和节点。在config文件夹下添加配置文件WheelController.yaml如下：
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 #加速限制
    }
}
这段配置文件是一个差速控制器，规定了控制器的相关参数设置，例如配置差速轮的间距和PID控制参数。
而后在原静态模型launch文件中添加配置 arbotix 节点如下：
<node name="arbotix" pkg="arbotix_python" type="arbotix_driver" 
output="screen">
<rosparam command="load" 
file="$(find 包名)/config/WheelController.yaml" />
<param name="sim" value="true" />
</node>
可以看到Arbotix是一个节点，在节点中载入之前配置的文件WheelController.yaml，同时设置环境为仿真环境。
启动该launch文件，在rviz里选择坐标系Fixed Frame为odom（里程计），Add添加odometry，选择odometry的话题Topic为/odom，/odom会自动呼出速度控制话题/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}}'
现在，小车就可以运动起来了。
3.代码实现URDF集成Gazebo：
①导入依赖和搭建架构：创建新的功能包并导入依赖包 urdf、xacro、gazebo_ros、gazebo_ros_control、gazebo_plugins，新建目录:
1>urdf：再下分目录urdf、xacro和Gazebo，其中urdf存储urdf文件，xacro存储xacro文件，Gazebo存储连杆配置文件（虽然也是Xacro文件，但前两者是建立模型时使用的文件，后者则是配置连杆功能时使用的文件）
2>launch：存储launch启动文件
3>worlds：存储在Gazebo中构建的世界模型
②编写建立模型的URDF和Xacro文件：整体过程与URDF集成Rviz时大体一致，但由于Gazebo基于仿真环境，因此连杆link必须启用碰撞检测collision和惯性力学计算标签inertial ，此外，Gazebo下的颜色设置也需要使用专门的标签gazebo：
<gazebo reference="base_link">
<material>Gazebo/Black</material>	#设置颜色为黑色
</gazebo>
1>collision标签：如果机器人link是标准的几何体形状，那么collision的设置只需要和 visual的设置一致即可（除了不需要设置颜色）。
2>inertial标签：惯性矩阵是一个非常专业的概念，在Gazebo中如果胡乱设置会导致仿真出现各种问题，基本形状的惯性矩阵可以直接使用下述Xacro文件的宏计算获得：
<!-- 球体惯性矩阵：m为质量，r为半径-->
    <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>
<!-- 圆柱惯性矩阵：m为质量，r为半径，h为高度-->
<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>
<!-- 立方体惯性矩阵：m为质量，l为长度，w为宽度，h为高度-->
 <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外，机器人的每个刚体部分都需要设置惯性矩阵，且惯性矩阵必须经上述计算严谨得出。具体在调用时，只需要使用对应的宏即可，例如：
<xacro:cylinder_inertial_matrix m=xxx r=xxx h=xxx />
即可返回一个正确的惯性标签。
3>Gazebo标签：Gazebo是一种修饰标签，它依赖于其他已有的标签存在，其属性reference的值就是依赖标签的名字，它只起修饰reference所指向的标签的作用。例如上例就是修饰名字为base_link的连杆颜色为黑色，之后还会提到修饰某个连杆为雷达、摄像头、里程计等。
③编写控制机器人运动的URDF和Xacro文件：至此gazebo 中已经可以显示静态机器人模型了，那么如何像在rviz中一样控制机器人运动呢？在此，需要涉及到ros中的组件: ros_control。ros_control是一组软件包，它包含了控制器接口，控制器管理器，传输和硬件接口。它是一套机器人控制的中间件，是一套规范，不同的机器人平台只要按照这套规范实现，那么就可以保证与ROS 程序兼容，通过这套规范，实现了一种可插拔的架构设计，大大提高了程序设计的效率与灵活性。gazebo 已经实现了 ros_control 的相关接口，如果需要在 gazebo 中控制机器人运动，直接调用相关接口即可。编写Xacro文件如下：
<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
</hardware	Interface>
            </joint>
            <actuator name="${joint_name}_motor">
                <hardwareInterface>hardware_interface/VelocityJointInterface
</hardwareInterface>
                <mechanicalReduction>1</mechanicalReduction>
            </actuator>
        </transmission>
    </xacro:macro>
    <!-- 每一个驱动轮都需要配置传动装置 -->
    <xacro:joint_trans joint_name="left_wheel2base_link" />
    <xacro:joint_trans joint_name="right_wheel2base_link" />
    <!-- 控制器 -->
    <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>left_wheel2base_link</leftJoint>
            <rightJoint>right_wheel2base_link</rightJoint> 
            <wheelSeparation>${base_link_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>

一些重要标签：xacro:joint_trans宏用于绑定左右轮的关节，leftJoint和rightJoint用于绑定左右轮的连杆名称，wheelSeparation用于设置车轮间距，wheelDiameter用于设置车轮直径，commandTopic用于设置运动控制话题名，odometryTopic用于设置里程计话题名，robotBaseFrame用于设置根坐标系名。

④编写修饰模型的URDF和Xacro文件：
1>里程计：里程计用于获取机器人相对出发点坐标系的位姿状态(XYZ坐标以及朝向)
可以通过 Rviz 显示机器人的里程计信息以及运动朝向

<launch>
    <!-- 启动 rviz -->
    <node pkg="rviz" type="rviz" name="rviz" />

    <!-- 关节以及机器人状态发布节点 -->
    <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
</launch>

2>雷达：和里程计直接与机器人控制节点集成不同，ROS中设置雷达的方式是把一个连杆组件修饰为仿真雷达，也就是使用gazebo reference的方法，具体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>

一些重要标签：sensor type为传感器类型，pose为偏移，update_rate为数据更新频率，
samples为光线数，resolution为分辨率，min/max_angle为雷达非死区范围，noise为噪声（高斯噪声，均值和标准差），topicName为话题名称。
3>摄像头：ROS中设置摄像头的方式与雷达一致，即把一个连杆组件修饰为仿真摄像头：

<robot name="my_sensors" xmlns:xacro="http://wiki.ros.org/xacro">
  <gazebo reference="camera（想要修饰为摄像头的连杆名）">
    <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>

⑤整合各部分Xacro文件：
分析我们需要包含的文件，首先是惯性矩阵计算的Xacro文件InertialAlgorithm.xacro，然后是形成机器人静态模型的各Xacro文件，例如底盘、摄像头与雷达等，最后是修饰机器人模型所需的Xacro文件。然后创建一个Xacro文件将它们全部包含进来，如下所示：

<robot name="my_car_camera"
xmlns:xacro="http://wiki.ros.org/xacro">
<!-- 引入建立机器人模型所需的Xacro文件 -->
<xacro:include filename="InertialAlgorithm.xacro"/>
<xacro:include filename="XacroExerciseCarBase1.xacro" />
<xacro:include filename="XacroExerciseCamera1.xacro" />
<xacro:include filename="XacroExerciseLaser1.xacro" />
<!-- 引入控制机器人运动所需的Xacro文件 -->
<xacro:include filename="gazebo/WheelMove.xacro"/>
<!-- 引入修饰机器人模型所需的Xacro文件 -->
<xacro:include filename="gazebo/Laser.xacro"/>
<xacro:include filename="gazebo/Camera.xacro"/>
<xacro:include filename="gazebo/Kinect.xacro"/>
</robot>
⑥编写launch文件解析URDF和Xacro文件，并启动Gazebo：
<launch>
<!-- 将 Urdf 文件的内容加载到参数服务器 -->
<param name="robot_description" 
command="$(find xacro)/xacro $(find 包名)/urdf/Xacro整合文件.xacro" />
<!-- 启动 gazebo，方法是启动对应launch文件并载入world文件-->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" value="$(find 包名)/worlds/xxx.world" />
</include>
<!-- 在 gazebo 中显示机器人模型，需要结合第一步Urdf的param名字 -->
<node pkg="gazebo_ros" type="spawn_model" name="model" 	
args="-urdf -model mycar -param robot_description" />
</launch>
⑦编写launch文件启动Rviz监测机器人在Gazebo仿真环境下的状态：
之前的文件都是为了启动Gazebo仿真小车的，而状态的监测需要使用Rviz，为此编写专门启动Rviz用来检测机器人状态的launch文件：
<launch>
<node pkg="rviz" type="rviz" name="rviz" 
args="-d $(find UrdfTest)/config/ShowMyCar.rviz"/>
<node name="joint_state_publisher" pkg="joint_state_publisher" 
type="joint_state_publisher" />
<node name="robot_state_publisher" pkg="robot_state_publisher" 
type="robot_state_publisher" />
</launch>

⑧Gazebo仿真文件.world搭建：
之前提到过world文件可以在Gazebo中载入已经搭建好的一个仿真环境。
那么怎么创建一个新的仿真环境呢？实现方式有三种:分别是直接添加内置组件创建仿真环境，手动绘制仿真环境和直接下载使用官方或第三方提供的仿真环境插件，我们分别来看看
1>直接添加内置组件：在Gazebo上方有基本形状和光照，直接导入即可。
2>手动绘制仿真环境：点击Edit/Building Editor即可自定义设计仿真环境。
3>使用插件：下载官方库git clone https://github.com/osrf/gazebo_models并将得到的gazebo_models文件夹内容复制到 /usr/share/gazebo-*/models中，此时重启 Gazebo，选择左侧菜单栏的insert就可以选择并插入相关道具了。
绘制好新的仿真环境后，选择File/Save World As即可得到一个world文件了。
4.总结：主要介绍了ROS中仿真实现涉及的三大知识点:URDF(Xacro)，Rviz和Gazebo
其中URDF 是用于描述机器人模型的 xml 文件，可以使用不同的标签具代表不同含义，xacro 则是对 URDF 的优化实现。容易混淆的是Rviz与Gazebo，在此我们着重比较以下二者的区别:rviz是三维可视化工具，强调通过插件把已有的数据可视化显示，也就是说它需要已有数据的支持，这些数据可以通过话题、参数的形式发布后被Rviz订阅，而后Rviz对其完成可视化的渲染，以此让开发者更容易理解数据的意义；gazebo是三维物理仿真平台，强调的是创建一个虚拟的仿真环境，不需要数据，而是创造数据。我们可以在gazebo中创建一个仿真机器人的运动功能和传感器数据的虚拟世界，而这些数据就可以放到rviz中显示，所以gazebo和rviz经常配合使用。综上，如果你手上已经有机器人硬件平台，并且在上边可以完成需要的功能，用rviz应该就可以满足开发需求。如果你手上没有机器人硬件，或者想在仿真环境中做一些算法、应用的测试，gazebo+rviz应该是你需要的。

机器人导航：

amcl通过传感器获取的信息，将这些特征点与全局地图相匹配，从而大致获取机器人的位姿信息。

rosrun nodelet nodelet load nodelet_tutorial_math/Plus mymanager name:=n1 _value:=100
rosrun nodelet nodelet load nodelet_tutorial_math/Plus mymanager name:=n2 _value:=-50 /n2/in:=/n1/out

Action通信：

假设要求机器人导航到某个目标点,此过程需要一个节点A发布目标信息，然后一个节点B接收到请求并控制移动，最终响应目标达成状态信息。乍一看，这好像是服务通信，因为需求中要A发送目标，B执行并返回结果，是一个典型的基于请求响应的应答模式，不过如果只是使用基本的服务通信实现，存在一个问题：导航是一个过程，是耗时操作，如果使用服务通信，那么只有在导航结束时，才会产生响应结果，而在导航过程中，节点A是不会获取到任何反馈的，从而可能出现程序”假死”的现象，过程的不可控意味着不良的用户体验，以及逻辑处理的缺陷(比如:导航中止的需求无法实现)。更合理的方案应该是:导航过程中，可以连续反馈当前机器人状态信息，当导航终止时再返回最终的执行结果。这个过程在ROS中被action通信优化。action通信一般适用于耗时的请求响应场景,用以获取连续的状态反馈，是一种特殊的服务通信。在ROS中提供了actionlib功能包集，用于实现 action 通信。action通信是一种类似于服务通信的实现，其实现模型也包含请求和响应，但是不同的是，在请求和响应的过程中，服务端还可以连续的反馈当前任务进度，客户端可以接收连续反馈，并且还可以取消任务。

goal:目标任务;
cacel:取消任务;
status:服务端状态;
result:最终执行结果(只会发布一次);
feedback:连续反馈(可以发布多次)。
尝试用代码实现下述Action通信过程：创建两个ROS节点作为服务器和客户端，客户端可以向服务器发送目标数据N，服务器会计算 1 到 N 之间所有整数的和并返回给客户端，假设服务器每累加一次耗时0.1s，为了良好的用户体验，需要服务器在计算过程中，每累加一次就给客户端响应一次百分比格式的执行进度。
类似于话题和服务通信，当使用自定义通讯格式进行action通讯时，需要进行以下操作：
1.定义action文件：在小src同级下创建action文件夹并创建xxx.action文件，在其中以类似结构体的格式输入需要定义的自定义通讯格式构成。在action文件中，数据组成分为三部分:请求目标值、最终响应结果、连续反馈值，三者之间使用—-分割，例如:

int32 num
---
int32 result
---

float64 progress_bar
2.编辑package.xml：
这里简单介绍一下package文件的构成，package的核心代码如下：
①依赖的构建工具

catkin
②指定构建此软件包所需的软件包 （编译依赖）

roscpp

rospy

std_msgs
③指定根据这个包构建库所需要的包（编译外部依赖）

roscpp

rospy

std_msgs
④运行该程序包中的代码所需的程序包（执行依赖）

roscpp

rospy

std_msgs
我们要在编译依赖中添加message_generation
在执行依赖中添加message_runtime
3.编辑CMakeLists.txt：
L10：取消注释并修改编译依赖为：

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  message_generation
)

L51：取消注释并配置msg源文件：

add_message_files(
  FILES
  xxx.msg
)

L71：取消注释使自定义消息格式依赖于 std_msgs：

generate_messages(
  DEPENDENCIES
  std_msgs
)

L108：取消注释并修改执行依赖为：

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

4.编译：此时C++调用的中间文件位于(…/工作空间/devel/include/包名/xxx.h)
Python调用的中间文件位于(…/工作空间/devel/lib/python3/dist-packages/包名/msg/xxx.py)，之后要想使用这一自定义通讯格式就需要包含这个头文件。

代码实现C++话题通讯：

1.如果使用自定义格式，需要添加c_cpp_properties.json 的 includepath属性如下：
“/xxx/yyy工作空间/devel/include/*“ //配置 head 文件的路径
2.包含通讯所需格式的头文件：
对于ROS官方提供的基本格式需要包含#include “std_msgs/xxx.h”
对于自定义的格式需要包含#include “包名/xxx.h”（即自定义格式经编译得到的头文件）
3.发布方实现：
①初始化ros节点并创建节点句柄：
ros::init(argc, argv, “Publisher”);
ros::NodeHandle nh;
②创建发布者对象：（发布者和订阅者的话题topic需要一致）
官方格式：ros::Publisher pub = nh.advertise(“topic”, 10);
自定义格式：ros::Publisher pub = nh.advertise<包名::xxx>(“topic”, 10);
③编辑要发送的数据：
创建对象：std_msgs::xxx/包名::xxx msg;
4.订阅方实现：
①初始化ros节点并创建节点句柄：
ros::init(argc, argv, “Publisher”);
ros::NodeHandle nh;
②创建订阅者对象：
官方：ros::Subscriber sub = nh.subscribe(“topic”,10,doMsg);
自定义：ros::Subscriber sub = nh.subscribe<包名::xxx>(“topic”,10,doMsg);
③循环调用回调函数：ros::spin();
④搭建回调函数doMsg：void doMsg(const 包名::xxx::ConstPtr &msg){}

动态参数：

pluginlib的使用：

nodelet的使用：