ROS学习笔记（二）ROS常用功能--参数，TF与Launch

写在前面

笔者在参加机器狗比赛，故写此博客记录下学习过程。

关于ROS的学习，我主要是根据古月居ROS入门21讲的顺序和理念，加上自己查阅一些资料来学习的，如果有什么问题欢迎来交流~

相关链接：ROS学习笔记（一）从0开始的ROS的安装以及初步使用-CSDN博客

---

参数的使用

        在上一节中，我们提到了节点管理器提供了参数服务器的功能，用来记录全局变量的变量名和变量值，供所有节点使用，这节我们就来体验一下，以小海龟仿真程序为例，输入如下指令。

roscore
rosrun turtlesim turtlesim_node

        之后，我们再打开一个终端，输入

rosparam

        这时，就会显示很多指令，如下所示

        我们来看下，根据字面意思也不难理解，set是设置参数值，get是获取参数值，load是从文件中加载参数值，dump是把参数值写入文件（字典形式，键值对），delete是删除参数，list是展示参数名，我们来用一下。

rosparam list
rosparam set /turtlesim/background_b 10
rosservice call /clear "{}"

        执行完第二条语句后，会发现并没有什么变化，那是因为还没有请求服务，我们执行请求服务操作，按下回车，就会发现仿真界面的背景颜色发生了改变，如下所示：

        背景颜色由原来的蓝色变为了现在的墨绿色，再输入如下语句：

rosparam dump param.yaml

         这之后，会在终端路径下生成一个名为param.yaml的文件存储参数信息，如下所示：

        在文件中修改参数background的rgb值，再执行：

rosparam load param.yaml
rosservice call /clear "{}"

         执行之后，就会发现背景颜色被成功改变了。下面是用C++代码实现换背景颜色的功能，代码源自古月居。

#include <string>
#include <ros/ros.h>
#include <std_srvs/Empty.h>

int main(int argc, char **argv)
{
	int red, green, blue;

    // ROS节点初始化
    ros::init(argc, argv, "parameter_config");

    // 创建节点句柄
    ros::NodeHandle node;

    // 读取背景颜色参数
	ros::param::get("/turtlesim/background_r", red);
	ros::param::get("/turtlesim/background_g", green);
	ros::param::get("/turtlesim/background_b", blue);

	ROS_INFO("Get Backgroud Color[%d, %d, %d]", red, green, blue);

	// 设置背景颜色参数
	ros::param::set("/turtlesim/background_r", 255);
	ros::param::set("/turtlesim/background_g", 255);
	ros::param::set("/turtlesim/background_b", 255);

	ROS_INFO("Set Backgroud Color[255, 255, 255]");

    // 读取背景颜色参数
	ros::param::get("/turtlesim/background_r", red);
	ros::param::get("/turtlesim/background_g", green);
	ros::param::get("/turtlesim/background_b", blue);

	ROS_INFO("Re-get Backgroud Color[%d, %d, %d]", red, green, blue);

	// 调用服务，刷新背景颜色
	ros::service::waitForService("/clear");
	ros::ServiceClient clear_background = node.serviceClient<std_srvs::Empty>("/clear");
	std_srvs::Empty srv;
	clear_background.call(srv);
	
	sleep(1);

    return 0;
}

        编译后运行，会发现海龟仿真程序背景颜色成功被改变为了白色。

坐标管理系统

        首先我们要了解ROS当中的坐标变换（TransForm,简称TF），它包括了位置和姿态两个方面的变换，表现为同一位置在不同坐标系下的表示的转换。

ROS中机器人模型包含大量的部件，每一个部件统称之为link（比如手部、头部、某个关节、某个连杆）,每一个link上面对应着一个frame（坐标系）, 用frame表示该部件的坐标系，frame和link是绑定在一起的。

TF是一个通俗的名称，实际上它有很多含义：

1. 可以被当做是一种标准规范，这套标准定义了坐标转换的数据格式和数据结构．tf本质是树状的数据结构，即"tf tree"。
2. tf也可以看成是一个话题/tf，话题中的消息保存的就是tf tree的数据结构格式。维护了整个机器人的甚至是地图的坐标转换关系。维持并更新机器人整个坐标系的话题是/tf，/tf话题表示的内容是整个机器人的tf树，而非仅仅是某两个坐标系的转换关系，这样的话，/tf话题是需要很多的节点来维护的，每一个节点维护两个frame之间的关系。
3. tf还可以看成是一个package,它当中包含了很多的工具．比如可视化，查看关节间的tf,debug tf等等．
4. tf含有一部分的API接口，用来节点程序中的编程。TF对发布器与订阅器进行了封装，使开发者通过TF的接口更加简单地建立对TF树中某些坐标系转换关系的维护与订阅。

       （注：上述来源：ROS中TF(坐标系转换)原理与使用_sendtransform-CSDN博客） 

         至于TF的原理我们不需要了解太多，只需知道，TF是使用树的数据结构，它的建立和维护是基于话题通讯机制的。TF通过广播和监听来实现，下面我们以一个例子来帮助我们理解。

小海龟跟随实例

        就像标题那样说的，这个实例会实现一只小海龟一直跟着我们自己的小海龟的功能，原理也十分简单，就是通过TF的广播监听，不断获取两只海龟的位矢，通过向量差得到方向，修改跟随海龟的移动方向即可。首先打开终端，安装功能包：

sudo apt-get install ros-<ros-ver>-turtle-tf 

        注意，这里的<ros-ver>代表的是ROS版本，可通过下面这条语句查询当前ROS版本：

rosversion -d

        之后，使用roslaunch语句（执行脚本文件）实现跟随程序：

roslaunch turtle_tf turtle_tf_demo.launch

        这句话会帮我们运行包括roscore,turtlesim_node等节点，此时会弹出一个窗口，看到一只海龟慢慢靠近另一只海龟，此时我们再用键盘控制窗口中间的小海龟：

rosrun turtlesim turtle_teleop_key

        就能看到一只海龟一直跟着我们控制的小海龟。

        下面我们来使用一下TF给我们提供的工具来参考理解。

view_frames

        这是TF功能包里面给我们提供的工具，可以可视化的看到系统当中的所有tf的关系，监听5s的时间，把5s之内所有坐标系之间的关系都给保存下来。然后生成一个pdf

rosrun tf view_frames

        笔者在运行时出现了下面的报错:

        分析日志，发现是文件/opt/ros/noetic/lib/tf/view_frames出了问题，我们找到该文件，会发现没有权限修改，这时，我们有个简单粗暴的方法，打开一个终端输入如下语句以root身份打开文件资源管理器：

sudo -s
nautilus

         然后，我们再找到view_frames文件，在第89行加上这样一句话：

vstr = str(vstr)

        把vstr强行转化为字符串格式就能解决报错信息啦，是不是很粗暴但有效的方式？保存后退出，我们再试一下view_frames，如下所示，成功解决报错：

         这个图中，world是全局坐标系，零点对应仿真界面左下角，而turtle1和turtle2分别表示两只小海龟上的坐标系。

rqt_tf_tree

rosrun rqt_tf_tree rqt_tf_tree

        指令如上，这是一个实时工具，观察在ROS上被发布的坐标系树，可用刷新按钮来更新树的内容。它与view_frames工具的区别在于：上一个工具连续采样5s获得的树的内容，并存成一个图片；这个工具可以连续的观察树的内容，使用起来更方便。如果你和笔者一样出现了package not found的问题，请用下面的语句安装这个工具：

sudo apt-get install ros-<ros_ver>-rqt-tf-tree

        注意<ros_ver>为版本。然后再次运行，会得到下面这样的可视化界面：

tf_echo

        我们也可以使用命令行工具tf_echo来查看两个坐标系之间的位置关系：

rosrun tf tf_echo turtle1 turtle2

        运行之后，如下所示，会不断显示两者坐标关系（这里展示的是重合后的，所以很多都是0，也没有变化）：

 rviz

         rviz是ROS中一款三维可视化平台，一方面能够实现对外部信息的图形化显示，另外还可以通过rviz给对象发布控制信息，从而实现对机器人的监测与控制。

        我们可以输入如下语句来打开rviz平台并可视化看两只海龟：

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

        同样的，如果显示找不到资源包就sudo安装一下就好了，打开后如下所示：

中间部分为 3D视图显示区，能够显示外部信息
上部为工具栏，包括视角控制、目标设置、地点发布等，还可以添加自定义的一些插件。
左侧为显示项目，显示当前选择的插件，并且能够对插件的属性进行设置。
下侧为时间显示区域，包括系统时间和ROS时间等。
右侧为观测视角设置区域，可以设置不同的观测视角。

 （注：来源：ROS学习笔记——rviz的基本介绍-CSDN博客）

        我们把tf add到里面，就能看到两只小海龟的位置信息了，此时控制海龟移动，会发现显示区的两个海龟坐标系也跟着动了起来。

广播与监听的编程实现

         在前面我们提到过，TF通过广播和监听来实现。为什么要用tf来广播和监听坐标变换，而不是直接进行坐标转换？答案很简单：多个实体多个进程共享坐标转换，降低管理难度。下面是一些基本的tf数据类型和转换函数：

tf::Quaternion 四元数，表示一个姿态。

tf::Vector3 三维向量

tf::Point 位置。

tf::Pose 位姿（位置+姿态）成员getRotation()或getBasis()用于获取旋转矩阵；成员getOffset()用于获取平移向量。

tf::Transform 一个坐标转换。成员有：Matrix3x3 m_basis，用3*3的矩阵表示旋转；Vector3 m_origin，用3*1的向量表示平移。

tf::Quaternion createIdentityQuaternion() 得到单位四元数

tf::Quaternion createQuaternionFromRPY(double roll,double pitch,double yaw) 欧拉角转四元数

geometry_msgs::Quaternion createQuaternionMsgFromRollPitchYaw(double roll,double pitch,double yaw) 欧拉角转四元数

 （注：来源：ROS tf - 知乎 (zhihu.com)）

        下面以一个实例来看，首先创建一个功能包，依赖有roscpp,rospy和tf，如果忘了的话后续在CMakelist.txt文件里添加也可，然后在src中添加下面两个代码：

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>

int main(int argc, char** argv){
    // 初始化ROS节点
    ros::init(argc, argv, "robot_tf_broadcaster");

    // 创建节点句柄
    ros::NodeHandle node;

    // 创建tf的广播器
    static tf::TransformBroadcaster br;

    while(node.ok()){
        // 初始化tf数据
        tf::Transform transform;
        transform.setOrigin( tf::Vector3(0.1, 0.0, 0.2) );
        transform.setRotation( tf::Quaternion(0,0,0,1) );

        // 广播base_link与base_laser坐标系之间的tf数据
        br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "base_link", "base_laser"));
    }
    return 0;
};

     

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/PointStamped.h>

int main(int argc, char** argv){

    // 初始化ROS节点
    ros::init(argc, argv, "robot_tf_listener");

    // 创建节点句柄
    ros::NodeHandle node;

    // 创建tf的监听器
    tf::TransformListener listener;
    ros::Rate rate(10.0);
    while (node.ok()){

        //我们将在base_laser帧中创建一个要转换为base_link帧的点
        geometry_msgs::PointStamped laser_point;
        laser_point.header.frame_id = "base_laser";
        
        //我们将在我们的简单示例中使用最近可用的转换
        laser_point.header.stamp = ros::Time();
        
        //laser_point检测点获取
        laser_point.point.x = 0.3;
        laser_point.point.y = 0.0;
        laser_point.point.z = 0.0;     
        try{
            // 等待获取监听信息base_link和base_laser
            listener.waitForTransform("base_link", "base_laser", ros::Time(0), ros::Duration(3.0));
            geometry_msgs::PointStamped base_point;
            listener.transformPoint("base_link", laser_point, base_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,
                base_point.point.x, base_point.point.y, base_point.point.z, base_point.header.stamp.toSec());
        }
        catch(tf::TransformException& ex){
            ROS_ERROR("Received an exception trying to transform a point from \"base_laser\" to \"base_link\": %s", ex.what());
        }

        rate.sleep();
    }
    return 0;
};

        这两段代码实现了link和laser之间的坐标转换，并发送了两者的实时信息。

         编译之后运行如下语句：

roscore
rosrun learning_tf listener
rosrun learning_tf broadcaster

        在执行完第二个语句后由于没有广播会显示我们的ERROR信息，而在运行了广播者之后就可以正常显示两者的实时坐标了，如下所示：

Launch文件的使用

       诶，还记得我们前面用roslaunch语句跑的那个小海龟跟随程序吗？下面我们来详细讲一下launch。那么使用launch可以同时启动多个ros节点，包括master节点。其基本思想是在一个XML格式的文件内将需要同时启动的一组节点罗列出来。

        关于launch的节点属性有很多，包括param,node,remap,machine,include,env等等，我们这里讲一下node和rosparam,更多的可以参考下面这篇文章来学习写法：

ROS学习：launch文件编写https://blog.csdn.net/wwyklnh/article/details/102722698

node

        node的基础属性也是不可少的属性有三个：pkg,type和name，其中pkg表示节点所在的功能包，相当于rosrun后的第一个参数，type则是rosrun后的第二个参数，表示一个可执行文件，name则是节点的名字，相当于我们代码当中ros::init当中的name，这里的name会替换掉代码当中的name，此外，node还有其他可加属性，比如：

• args:启动参数，和程序中的对应。
• machine:在指定的计算机上启动节点。
• respawn:退出后重启动节点。
• output:把节点的信息发送的位置，就是ros_info和ros_err那些，有screen和log两种，前者是屏幕，后者是日志文件，默认为log。

         还有好多这里不一一列举，详情请看上面的参考链接。

rosparam

        rosparam一次性将多个参数加载到参数服务器中. 我们将需要设置的参数放到 .yaml 文件中。它还可以用于删除参数。rosparam标记可以放在node标记中，在这种情况下，该参数被视为私有名称。它的属性有：command,file,param,ns等等。

---

        那么关于具体用法，我们以上面那个简单的广播和监听为例来写一个launch文件，代码如下：

<launch>
    <node pkg="learning_tf" type="broadcaster" name="broadcaster" output="screen"/>

    <node pkg="learning_tf" type="listener" name="listener" output="screen"/>
</launch>

        把它放在我们功能包下新创建的launch文件夹下，编译后roslaunch文件，就会得到和上面类似的结果：

---

参考资料：

https://blog.csdn.net/hxxjxw/article/details/108571143 

https://blog.csdn.net/qq_51963216/article/details/123462952

https://blog.csdn.net/m0_53316604/article/details/127661582?utm_medium=distribute.pc_relevant.none-task-blog-2~default~baidujs_baidulandingword~default-0-127661582-blog-88090914.235^v43^pc_blog_bottom_relevance_base4&spm=1001.2101.3001.4242.1&utm_relevant_index=3

 https://blog.csdn.net/YiYeZhiNian/article/details/105667292