ROS理论与实践(以移动机器人为例)连载(三) ——ROS常用组件工具

---

重点： gazebo、tf坐标变换、ros::launch

---

1.Launch启动文件

上一讲，我们讲的都是使用rosrun命令实现节点的创建，rostopic话题消息的发布，rosservice服务的搭建等等。但是，复杂又乱七八糟的shell窗口必然成了心腹大患，每次运行一个节点都要新开一个命令窗口，气的抠掉“Ctrl+Alt+T”。
那么，这一讲我们就来学习新的组件launch，挽救大家的键盘。

Launch文件： 通过XML文件实现多节点的配置和启动(可自动启动ROS Master，省下每次另开一个终端运行roscore)。

先来段吓人的代码~

<launch>
	<!-- local machine already has a definition by default.
		This tag overrides the default definition with
		specific ROS_ROOT and ROS_PACKAGE_PATH values -->
	<machine name="local_alt" address="localhost" default="true" ros-root="/u/user/ros/ros/" ros-package-path="/u/user/ros/ros-pkg" />
	<!-- a basic listener node -->
	<node name="listener-1" pkg="rospy_tutorials" type="listener" />
	<!-- pass args to the listener node -->
	<node name="listener-2" pkg="rospy_tutorials" type="listener" args="-foo arg2" />
	<!-- a respawn-able listener node -->
	<node name="listener-3" pkg="rospy_tutorials" type="listener" respawn="true" />
	<!-- start listener node in the 'wg1' namespace -->
	<node ns="wg1" name="listener-wg1" pkg="rospy_tutorials" type="listener" respawn="true" />
	<!-- start a group of nodes in the 'wg2' namespace -->
	<group ns="wg2">
		<!-- remap applies to all future statements in this scope. -->
		<remap from="chatter" to="hello" />
		<node pkg="rospy_tutorials" type="listener" name="listener" args="--test" respawn="true" />
		<node pkg="rospy_tutorials" type="talker" name="talker" />
			<!-- set a private parameter for the mode -->
			<param name="talker_1_param" value="a value" />
			<!-- nodes can have their own remap args -->
			<remap from="chatter" to="hello-1" />
			<!-- you can set environment variables for a node -->
			<env name="ENV_EXAMPLE" value="some value" />
		</node>
	</group>
</launch>

• <launch>
  • launch文件中的根目录用<launch>标签定义。
  • 意味着所有语法都要写在<launch>和</launch>中间

---

• <node>
  • 启动节点

<node pkg="package-name" type="executable-name" name="node-name" />

• 参数：
  • pkg：节点所在的功能包名称
  • type：节点的可执行文件名称
  • name：节点运行时的名称
    • 也就是ros::init那个名字，这个name可以覆盖executable里面init的节点名，也就是可以重命名，当然也可以使用原来的
  • output(日志是否要输出到终端中output="screen")、respawn(如果节点运行失败，会重新启动)、required(表明这个节点是必须的，如果这个节点运行失败，其他节点就不能运行)、ns(设置一个命名空间)、args(就是rosrun后跟的参数[argc和argv]用来传递给接受者)

---

• <param>/<rosparam>
  • 设置ROS系统运行中的参数，存储到参数服务器中(ROS Master的一块区域)
  • param一次只能传一个参数，rosparam一次可以传多个参数
    • name：参数名
    • value：参数值

<param name="output_frame" value="odom" />

• 加载参数文件的多个参数(将多个参数存入params.yaml内[在config文件夹]，通过load命令加载进去，ns设置了一个名为params的命名空间)：

<rosparam file="params.yaml" command="load" ns="params" />

---

• <arg>
  • launch文件内部的局部变量，仅限于launch文件使用
    • name：参数名
    • value：参数值

<arg name="arg-name" default="arg-value" />

• 调用：

<param name="foo" value="$(arg arg-name)" />
<node name="node" pkg="package" type="type" args="$(arg arg-name)" />

---

• <remap>
  • 重映射计算图资源的命名
    • from：原命名
    • to：映射之后的命名

<remap from="/turtlebot/cmd_vel" to="/cmd_vel" />

---

• <include>
  • 包含其他launch文件，类似C语言中的头文件包含
    • file：包含的其他launch文件路径

<include file="$(dirname)/other.launch" />

更多标签可见：http://wiki.ros.org/roslaunch/XML

Tips:

• rosnode list 在终端打印出当前运行中的节点名
• rosservice list 在终端打印出当前运行中的所有服务名
• rostopic list 在终端打印出当前运行中的所有话题名
• rosparam list 在终端中打印出当前运行中ROS Master内的所有参数
• rosparam get 在终端中打印出选定参数的值

(1)例一

1)turtlesim_parameter_config.launch

<launch>
	<!--set turtle_number=2 in ROS master-->
	<param name="/turtle_number" value="2" />
	<!--temporary value only can use in this launch-->
	<arg name="TurtleName1" default="Tom" />
	<arg name="TurtleName2" default="Jerry" />

	<!--run the node turtlesim_node-->
	<node pkg="turtlesim" type="turtlesim_node" name="turtlesim_node">
		<!--use temporary value to set a parameter in ROS master-->
		<param name="turtle_name1" value="$(arg TurtleName1)" />
		<param name="turtle_name2" value="$(arg TurtleName2)" />
		
		<rosparam file="$(find learning_launch)/config/param.yaml" command="load" />
	</node>
	
	<!--run the node can ctrl turtle by keyboard-->
	<node pkg="turtlesim" type="turtle_teleop_key" name="turtle_teleop_key" output="screen" />

</launch>

$ roslaunch learning_launch turtlesim_parameter_config.launch

这条命令可以自行启动roscore(ROS Master)，并执行.launch内定义的指令，将定义了output="screen"的指令输出到命令行窗口。

2)param.yaml

A: 123
B: "hello"

group:
	C: 456
	D: "hello"

变量A、B、C、D，C和D输入命名空间group，A和C是整型，B和D是字符串。

.launch文件内部param/rosparam所在位置不同，rosparam list中的命名空间也不同：

扫描二维码关注公众号，回复： 10847273 查看本文章

• 在<node>和</node>内部定义的参数，会默认将节点名做为命名空间，表现出：node名/参数名
• 在外部即仅在.launch文件中时，则没有命名空间，表现出：参数名
• 用<rosparam>将config文件夹内的param.yaml以文件形式load。
  • 如果.yaml内没有用命名空间，rosparam list表现出：参数名；
  • 使用了命名空间，表现出：命名空间/参数名；
  • 在<node>和</node>内部有在.yaml中定义了命名空间，表现出：node名/命名空间/参数名

(2)例二

turtlesim_remap.launch

<launch>
	<!--include other .launch files-->
	<include file="$(find learning_launch)/launch/simple.launch" />
	
	<node pkg="turtlesim" type="turtlesim_node" name="turtlesim_node" />
		<!--change the old name to new name"/cmd_vel"-->
		<remap from="/turtle1/cmd_vel" to="/cmd_vel" />
	<node/>

</launch>

改名后"turtle1/cmd_vel" $\to$"/cmd_vel"原先turtle_teleop_key键盘控制海龟移动就不可以使用了，因为话题名已经修改了。

今后的工作中会经常使用到.launch文件的修改，并不会写很多代码，.launch文件的各部分功能一定要熟练。

$(find PACKAGE_NAME)会返回该功能包所在的路径地址。

2.TF坐标变换(TransForm)

对于刚体而言，存在着六个自由度，其中三个用来描述质心位置( $x, y, z$)，而另外三个自由度用来描述整个刚体绕质心的旋转( $\epsilon_x ,\epsilon_y,\epsilon_z$。

实际上TF坐标变换就是在一个机器人实体上有需要不同的装置或者叫模块，它们都有自己的坐标系，但是为了满足某种需求，需要将不同坐标系之间联系起来，这里就用到了TF坐标变换，下面具体讲一讲：

参考自《机器人学导论》这里放一个博主的心得链接，写的很详细，我都看不懂。
补充参考：旋转矩阵/三维旋转：欧拉角、四元数、旋转矩阵、轴角之间的转换。

$^AP=^A_BR^BP+^AP_{BORG}$
$^AP=^A_BT^BP$
这两个式子就可以变成这样的矩阵相乘：
$\begin{bmatrix} ^AP \\ 1 \\ \end{bmatrix}=\begin{bmatrix} ^A_BR & ^AP_{BORG} \\ 0 & 1 \\ \end{bmatrix}\begin{bmatrix} ^BP \\ 1 \\ \end{bmatrix}$
如果看了上面那篇博文，又遇到了这个图

是不是想死的心都有了，赶紧抓起线性代数猛K，结果成功被K.O。其实做为一个机械系的学生(咳咳~虽然偏自控，但是都是主修了)我也不想看那么繁杂的矩阵推导。你想想斯坦福都是大神啊！他们会做这么无聊的数学推倒，每次去写坐标变换算法效率低下吗？当然不可能，于是有了TF坐标变换这个功能包，能够便捷的实现所需的坐标变换，如：机器人中心坐标系相对于全局坐标系的位置在哪？机械臂抓取的物体相对于机器人中心坐标系的位置在哪？等等。

TF坐标变换的实现包括：

• 广播TF变换： 把任意两个坐标系的关系让ROS TF知道，TF-Tree能保存10s内的坐标信息
• 监听TF变换： 查询任意坐标系之间的关系

移动机器人的本体坐标系与雷达坐标系：
base_link——底盘，base_laser——激光雷达

坐标系之间的数据变化：

看图就能发现这个例子只涉及到了移动，所有很容易就能推出来，但是涉及了十个数十个坐标的移动和旋转，恐怕就头大了…
扩展阅读：ROS探索总结（二十二）——设置机器人的tf变换

• 使用定时器 ros::NodeHandle::createTimer()
  • 第一个参数ros::Duration(1.0)表示每秒执行一次
  • 第二个参数boost::bind(&transformPoint, boost::ref(listener))表示每秒执行一次
    void transformPoint(const tf::TransformListener& listener)并且将main函数中定义的listener当做参数传入(boost::ref是将listener以引用形式传入)
    • bind的其他用法如bind(function, placeholders::_2)用第二个参数绑定到第一个参数，具体可参考https://blog.csdn.net/qq_44455588/article/details/104474038#stdbind_338

//main函数内部
tf::TransformListner listener(ros::Duration(10));
//等待10s,如果10s之后都还没收到消息,那么之前的消息就被丢弃掉。

ros::Timer timer = n.createTimer(ros::Duration(1.0), boost::bind(&transformPoint, boost::ref(listener)));

• geometry_msgs::PointStamped可以通过这个创建一个虚拟点类型。该类型包含标准的header消息结构，这样就可以在消息中加入发布数据的时间戳和参数系的id。

void transformPoint(const tf::TransformListener& listener)
{
	geometry_msg::PointStamped laser_point;
	laser_point.header.frame_id = "base_laser";
	
	laser_point.header.stamp = ros::Time();
	
	laser_point.point.x = 1.0;
	laser_point.point.y = 0.2;
	laser_point.point.z = 0.0;
}

• TransformListener的transformPoint()函数，可以用来将第二个参数中的原始数据转换到第一个参数的参考系下并存入第三个参数中。

try{
	geometry_msgs::PointStamped link_point;
	listener.transformPoint("base_link", laser_point, link_point);
	ROS_INFO("base_laser: (%.2f, %.2f. %.2f) -----> base_link: (%.2f, %.2f, %.2f) at time %.2f",
        laser_point.point.x, laser_point.point.y, laser_point.point.z,
        link_point.point.x, link_point.point.y, link_point.point.z, link_point.header.stamp.toSec());
}

(1)先试试手

第一步:

$ sudo apt-get install ros-melodic-turtle-tf
$
$ roslaunch turtle_tf turtle_tf_demo.launch
$ rosrun turtlesim turtle_teleop_key

生成两只小海龟，一只通过咱们自己的键盘控制移动turtle_teleop_key这个节点；另一只跟踪第一只一直爬，还会抄近道。

第二步:

$ rosrun tf view_frames

在当前目录下生成一个.pdf文件，里面会有一张如下的图片。

其中的world代表全局坐标系，turtle1和turtle2分别是两只乌龟中心坐标系，一只乌龟广播它相对于全局坐标系的关系，另一只乌龟监视这个关系去做跟踪。同时可以通过欧氏距离判断两海龟之间的距离，从而改变跟踪海龟的速度随着距离增大而加速。

tf_echo

关于两只小海龟的TF信息的关系可以使用tf功能包内的tf_echo节点，如下形式打印到终端：

$ rosrun tf tf_echo turtle1 turtle2

$T_{turtle1\_turtle2}=T_{turtle1\_world}*T_{world\_turtle2}$
通过几组信息描述turtle1和turtle2两个中心坐标系的关系

• Translation：两坐标系之间的位置关系
• Rotation：两坐标系之间的旋转关系
  • Quaternion四元数：可以参考四元数和旋转(Quaternion & rotation)。
    • 四元数：由实数和三个虚数单位 $i$， $j$， $k$组成表示为 $a+bi+cj+dk$。对于 $i$， $j$， $k$的意义可以理解为几何旋转。
      • 其中 $i$代表 $X$轴与 $Y$轴相交平面中 $X$轴正向向 $Y$轴正向的旋转；
      • $j$代表 $X$轴与 $Z$轴相交平面中 $X$轴正向向 $Z$轴正向的旋转；
      • $k$代表 $Y$轴与 $Z$轴相交平面中 $Y$轴正向向 $Z$轴正向的旋转；
  • RPY(radian)：弧度数，描述的三周旋转的欧拉角
  • RPY(degree)：角度，描述的三轴旋转欧拉角

补充：左手坐标系和右手坐标系。

rviz

也可以使用可视化工具rviz把两只小乌龟的可视化图打印出来

$ rosrun rviz rviz -d `rospack find turtle_tf` /rviz/turtle_rviz.rviz

这里的rospack find可以用来查找功能包的绝对路径，如:/opt/ros/melodic/share/turtle_tf

rviz GLOBAL STATUS ERROR出错参考http://www.freesion.com/article/4310157574/和https://blog.csdn.net/hitgavin/article/details/51997379

(2)广播TF变换

turtle_tf_broadcaster.cpp

/*
 *该例程产生tf数据，并计算、发布turtle2的速度指令
 */
#include <ros/ros.h>
#include <turtlesim/Pose.h>
#include <tf/transform_broadcaster.h>

std::string turtle_name;

void poseCallback(const turtlesim::Pose::ConstPtr& msg)
{
	//创建tf的广播器
	static tf::TransformBroadcaster br;

	//初始化tf数据
	tf::Transform transform;
	transform.setOrigin(tf::Vector3(msg->x,msg->y,0.0));	//x,y,z三个坐标
	tf::Quaternion q;	//四元数
	q.setRPY(0, 0, msg->theta);	//x,y,z三个方向的旋转角
	transform.setRotation(q);

	//广播world与海龟坐标系之间的tf数据
	br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));	//传递的信息是transform，当前时间下world和海龟的位姿关系
}
int main(int argc, char** argv)
{
	//初始化ROS节点
	ros::init(argc, argv, "my_tf_broadcaster");

	//输入参数作为海龟的名字
	if(argc != 2)
	{
		ROS_ERROR("need turtle name as argument");
		return -1;
	}
	turtle_name = argv[1];

	//订阅海龟的位姿话题
	ros::NodeHandle node;
	node.subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);	
	//这里因为”/turtle/pose“随着小乌龟的运动一直在更新，所以不断调用回调函数，更新tf坐标

	//循环等待回调函数
	ros::spin();
	
	return 0;
}

StamedTransform：

StampedTransform (const tf::Transform &input, const ros::Time &timestamp, const std::string &frame_id, const std::string &child_frame_id)

这里TransformBroadcaster用来sendTransform时，还有另一种写法(举的另一个例子的节选)：

tf::TransfromBroadcaster broadcaster;
broadcaster.sendTransform(tf::StampedTransform(
	tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.1, 0.0, 0.2)),
	ros::Time::now(),"base_link","base_laser"));

(3)监听TF变换

turtle_tf_listener.cpp

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/Twist.h>
#include <turtlesim/Spawn.h>

int main(int argc, char** argv)
{
	//初始化ROS节点
	ros::init(argc, argv, "my_tf_listener");
	
	//创建节点句柄
	ros::NodeHandle node;

	//请求产生turtle2
	ros::service::waitForService("/spawn");
	ros::ServiceClient add_turtle = node.serviceClient<turtlesim::Spawn>("/spawn");
	turtlesim::Spawn srv;
	add_turtle.call(srv);

	//创建发布turtle2速度指令的发布者
	ros::Publisher turtle_vel = node.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel", 10);
	
	//创建tf的监听器
	tf::TransformListener listener;

	ros::Rate rate(10.0);
	while(node.ok())	//会执行ros::ok()
	{
		//获取turtle1和turtle2坐标系之间的tf数据
		tf::StampedTransform transform;	//对应这broadcaster的sendTransform(tf::StampedTransform())
		try
		{
			listener.waitforTransform("/turtle2","/turtle1", ros::Time(0), ros::Duration(3.0));	
			//等待3.0s内两个坐标系是否存在
			listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), transform);	
			//查询两个坐标系之间的关系，turtle2相对于turtle1的向量关系保存到transform里
		}
		catch(tf::TransformException &ex)	//try成功，执行后面的位姿信息设定和发布，不成功catch到错误则会执行{}内
		{
			ROS_ERROR("%s", ex.what());
			ros::Duration(1.0).sleep();	//延时，和Rate不同的是，Rate用来平衡每次循环的频率，而duration是延时()内的时间
			continue;
		}

		//根据turtle1与turtle2坐标系之间的位置关系，发布turtle2的速度控制指令
		geometry_msgs::Twist vel_msg;
		vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(), 
										transform.getOrigin().x());
		//atan2(y,x)接受两个数据arctan(y/x)计算角度，atan()接受一个数据计算即y/x
		vel_msg.linear.x = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) +
									  pow(transform.getOrigin().y(), 2);
		//0.5也就是1/2，目的是2s内运动到turtle1的速度
		turtle_vel.publish(vel_msg);
		
		rate.sleep();
	}
	return 0;	
}

• lookupTransform("/turtle2","/turtle1",ros::Time(0),ros::Duration(3)) 实际上得到的是以/turtle2为中心坐标系的/turtle1的位姿;
• ros::Duration的原因是监听tf坐标变换是有延时的并不能实时监听，这里等待了3.0s；
• 并且使用ros::Time(0)而不使用ros::Time::now()的原因是，ros::Time(0)指最近时刻存储的数据，ros::Time::now()则指当下。

(4)launch文件该怎么写？

start_tf_demo_c++.launch

<launch>
	<!--Turtlesim Node-->
	<node pkg="turtlesim" type="turtlesim_node" name="sim" />
	<node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen" />

	<node pkg="learning_tf" type="turtle_tf_broadcaster" args="/turtle1" name="turtle1_tf_broadcaster" />
	<node pkg="learning_tf" type="turtle_tf_broadcaster" args="/turtle2" name="turtle2_tf_broadcaster" />

	<node pkg="learning_tf" type="turtle_tf_listener" name="listener" />

</launch>

由于节点名不能重复，当要在一个launch文件中使用同一个节点时，就要用name将同一个可执行文件中的同样功能的节点命名为两个不同的节点名。

(5)CmakeLists.txt怎么写？

add_executable(turtle_tf_broadcaster src/turtle_tf_broadcaster.cpp)
target_link_libraries(turtle_tf_broadcaster ${catkin_libraries})

add_executable(turtle_tf_listener src/turtle_tf_listener.cpp)
target_link_libraries(turtle_tf_listener ${catkin_libraries})

3.可视化显示与仿真工具

(1)rqt_console——日志输出工具

r $\to$ROS, qt基于Qt开发的一系列图像工具

例如如果海龟撞墙，则会持续输出warning信息，可以在这个console里进行筛选。

(2)rqt_plot——数据绘图工具

可以显示出各个话题的信息绘图

(3)rqt_graph——计算图可视化工具

(4)rqt_image_view——图像渲染工具

(5)Rviz——数据显示平台

Rviz是一款三维可视化工具，可以很好的兼容基于ROS软件框架的机器人平台。

• 在rviz中，可以使用可扩展标记语言XML对机器人以及周围物体等任何实物进行尺寸、质量、位置、材质、关节等属性的描述，并且在界面呈现出来
• 同时，rviz还可以通过图形化的方式，实时显示机器人传感器的信息、机器人的运动状态、周围环境的变化等信息
• 总而言之，rviz通过机器人模型参数、机器人发布的传感信息等数据，为用户进行所有可监测信息的图形化显示。用户和开发者也可以在rviz的控制界面下，通过按钮、滑动条、数值等方式，控制机器人的行为

界面形式

• 0：3D视图区
• 1：工具栏
• 2：显示项列表
• 3：视角设置区
• 4：时间显示区

基于Rviz打造自己的人机交互页面

可以通过Qt插件打造自定义内容

(6)Gazebo——仿真环境(创造数据)

Gazebo是一个三维动态物理仿真器，能准确高效地仿真在复杂的室内外环境下机器人群体。与游戏引擎类似，Gazebo能对一整套传感器进行高度逼真的物理仿真、为程序和用户提供交互接口

• 测试机器人算法
• 机器人的设计
• 现实情境下的回溯测试

下面这个模型可以从https://bitbucket.org/osrf/gazebo_models/downloads/下载，提前放到模型~/.gazebo/models下，需要把文件内的所有文件散开放，每个文件夹都是一个模型，运行：

$ roslaunch gazebo_ros mud_world.launch

• 0：3D视图区
• 1：工具栏
• 2：模型列表
• 3：模型属性项
• 4：时间显示区

如何使用Gazebo进行仿真

• 配置机器人模型urdf
• 创建仿真环境
• 开始仿真

下一讲将从urdf建立机器人模型开始！

---

【补充参考链接】

1. 欧拉角，四元数？晕头转向的空间姿态表示法(上)
2. 欧拉角，四元数？晕头转向的空间姿态表示法(下)
3. 可视化理解四元数，愿你不再掉头发
4. ROS探索总结（七十）—— 古月私房课 | 基于ROS设计一款机械臂控制系统
5. ROS探索总结（六十五）—— 古月私房课 | MoveIt!编程驾驭机械臂运动控制
6. ROS探索总结（六十二）—— 古月私房课 | 如何从零创建一个机器人模型