ROS入门（五）

一、参数使用与编程

1.1参数模型

在ROS系统中有一个参数服务器（Parameter Server），其作为一个全局字典，保存各个节点的配置参数

可以将Parameter Server理解成一个存储全局变量的存储空间

1.2创建功能包

1.3Parameter命令行使用

如果参数比较多的情况，会使用一个YAML文件用于保存参数，避免了一个个对参数进行设定

后续直接读取该文件即可以一次性保存所有参数进入系统，如下所示：

rosparam命令行工具的使用

• 列出系统当前多个参数

rosparam list

• 显示某个参数

rosparam get ***(参数名)

• 设置某个参数

rosparam set ***(参数名) ***(设定参数)

完成设定后，参数进行修改，如本案例中的海龟模拟器背景颜色发生改变：

• 保存参数到文件

rosparam dump ***(文件名)

使用该命令后则在命令行当前文件目录下生成一个保存参数的文件

• 从文件读取参数

rosparam load ***(文件名)

• 删除参数

rosparam delete ***(参数名)

1.4编程实现

/**
 * 该例程设置/读取海龟例程中的参数
 */
#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("/background_r", red);
	ros::param::get("/background_g", green);
	ros::param::get("/background_b", blue);
	ROS_INFO("Get Backgroud Color[%d, %d, %d]", red, green, blue);
	// 设置背景颜色参数
	ros::param::set("/background_r", 255);
	ros::param::set("/background_g", 255);
	ros::param::set("/background_b", 255);
	ROS_INFO("Set Backgroud Color[255, 255, 255]");
    // 读取背景颜色参数
	ros::param::get("/background_r", red);
	ros::param::get("/background_g", green);
	ros::param::get("/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;
}

程序较为简单，调用一个API接口即可以实现对参数的get和set

• 通过参数名获取参数值并保存至变量：ros::param::get("/background_r", red);
• 配置参数：ros::param::set("/background_r", 255);
• 刷新服务：

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

1.5配置编译规则

add_executable(parameter_config src/parameter_config.cpp)
target_link_libraries(parameter_config ${catkin_LIBRARIES})

1.6编译运行

结果如下所示：

二、ROS中坐标管理系统TF

2.1概念

根据机器人导论，在一个机器人中存在着很多个坐标系，这些坐标系之间都有距离上的差别

如果需要机器人进行运转就要计算坐标系之间的距离和位置计算

TF功能包的作用就是管理机器人中存在的所有坐标系，通过这个功能包即可以通过查询的方式来解决坐标系与坐标系之间的关系和计算

每个Ros节点只需要把自己已知的任何相对坐标发布出去，那么就把已知信息通过TF的函数计算后发布出去

TF功能包的实现机制：广播和监听

2.2基本功能演示

• roslaunch指令用于启动ROS系统中的一个launch文件，launch文件可以理解成一个脚本文件启动多个节点

也就是使用该命令可以启动多个节点

使用结果如下：

• 使用rosrun tf view_frames命令，可以可视化的看到系统中所有坐标之间的关系

使用该命令后，默认会在终端当前路径下生成一个pdf文件，以展示节点关系

这个pdf文件展示了坐标系之间的关系

如图所示，这个系统中存在三个坐标系分别是：world、turtle1、turtle2

其中world坐标系是不会变的，始终作为整个系统中的零点位置

再次案例中可以抽象为turtle2坐标系向turtle1坐标系不断作变化而靠近的关系

• 另外，使用tf_echo工具可以直观的看到两个坐标系之间的关系

rosrun tf tf_echo ***(坐标系名)

如上所示：

Translation栏展示的是在x/y/z三轴上的平移[x, y, z]

Rotation栏展示的是旋转关系，分为三种方式来进行描述

Quaternion栏通过的是四元树的方式

RPY (radian)栏表示分别围绕x/y/z三轴旋转，通过弧度单位进行描述

RPY (degree)栏表示分别围绕x/y/z三轴旋转，通过角度单位进行描述

• 还可以使用rosrun rviz rviz -d 'rospack find turtle_tf' /rviz/turtle_rviz.rviz来观察两个坐标之间的可视化关系

三、TF坐标系广播与监听

3.1创建功能包

3.2TF广播器的创建

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

std::string turtle_name;
void poseCallback(const turtlesim::PoseConstPtr& msg)
{
	// 创建tf的广播器
	static tf::TransformBroadcaster br;
	// 初始化tf数据
	tf::Transform transform;
	transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );
	tf::Quaternion q;
	q.setRPY(0, 0, msg->theta);
	transform.setRotation(q);
	// 广播world与海龟坐标系之间的tf数据
	br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
}

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;
	ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);
    // 循环等待回调函数
	ros::spin();
	return 0;
};

3.2.1代码解读

• 通过输入参数来判断海龟与世界坐标的关系

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

• 创建话题订阅者，接收由海龟不断发布过来的位置消息

// 订阅海龟的位姿话题
ros::NodeHandle node;
ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);

• 回调函数传入参数就是海龟的位置

void poseCallback(const turtlesim::PoseConstPtr& msg) {}

• 创建广播器，不断将位置信息发布出去

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

• 填充坐标系位置映射关系

// 初始化tf数据
tf::Transform transform;
transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );
tf::Quaternion q;
q.setRPY(0, 0, msg->theta);
transform.setRotation(q);

• 通过sendTransform方法广播海龟坐标与world坐标之间的关系

// 广播world与海龟坐标系之间的tf数据
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));

3.2.2流程总结

如何创建一个广播器？

• 定义一个TF广播器
• 创建坐标变换值
• 发布坐标变换

3.3TF监听器的创建

/**
 * 该例程监听tf数据，并计算、发布turtle2的速度指令
 */
#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())
   {
      // 获取turtle1与turtle2坐标系之间的tf数据
      tf::StampedTransform transform;
      try
      {
         listener.waitForTransform("/turtle2", "/turtle1", ros::Time(0), ros::Duration(3.0));
         listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), transform);
      }
      catch (tf::TransformException &ex) 
      {
         ROS_ERROR("%s",ex.what());
         ros::Duration(1.0).sleep();
         continue;
      }
      // 根据turtle1与turtle2坐标系之间的位置关系，发布turtle2的速度控制指令
      geometry_msgs::Twist vel_msg;
      vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(),
                                    transform.getOrigin().x());
      vel_msg.linear.x = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) +
                                  pow(transform.getOrigin().y(), 2));
      turtle_vel.publish(vel_msg);

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

3.3.1代码解读

• 创建监听器

// 创建tf的监听器
tf::TransformListener listener;

• 保存平移和旋转关系

// 获取turtle1与turtle2坐标系之间的tf数据
tf::StampedTransform transform;

• 等待变换和查询变换

listener.waitForTransform("/turtle2", "/turtle1", ros::Time(0), ros::Duration(3.0)); // 等待变化
listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), transform); // 查询变换

• 根据距离查询结果移动海龟2向海龟1进行移动

// 根据turtle1与turtle2坐标系之间的位置关系，发布turtle2的速度控制指令
geometry_msgs::Twist vel_msg;
vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(), transform.getOrigin().x());
vel_msg.linear.x = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) + pow(transform.getOrigin().y(), 2));
turtle_vel.publish(vel_msg);

3.3.2流程总结

如何实现一个TF监听器？

• 定义TF监听器（TransformListener）
• 查找坐标变换（waitForTransform、lookupTransform）

3.4编译

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.5重映射机制

为避免两个节点调用同一个方法而导致初始化节点时节点重名问题

如上代码中使用了__name:=turtle1_tf_broadcaster代码就是用来解决该问题的，这种方式被称为重映射机制

注意：是两个下划线！

重映射机制的原理就是对节点的重新命名，用于取代掉程序里面初始化时的命名

四、Launch启动文件

4.1概念

Launch文件：通过XML文件实现多节点的配置和启动（可自动启动ROS Mater）

Launch通过XML这种文件形式实现多个节点之间的配置和启动，使用lauch后可以不用启动多个终端来进行调试

一个Launch文件就能启动多个节点

4.2语法

Launch文件中所有的语法都是通过xml语言进行描述的，主要由以下这些标签：

4.2.1<launch>标签

launch文件中的根元素

4.2.2<node>标签

表示启动节点，格式：<node pkg="package-name" type="executable-name" name="node-name">

• pkg：节点所在的功能包名称
• type：节点的可执行文件名称
• name：节点运行时的名称
• output（可选）：控制节点是否是要将日志信息打印到当前终端所在目录
• respawn（可选）：控制节点控制如果节点出现问题是否需要重启
• required（可选）：规定节点是否一定要启动
• ns（可选）：命名空间避免命名冲突
• args（可选）：给每个节点输出参数

4.2.3<param>、<rosparam>标签

设置ROS系统运行中的参数，储存在参数服务器中，格式：<param name="output_frame" value="odom"/>

• name：参数名
• value：参数值

加载参数文件中的多个参数：<rosparam file="params.yaml" command="load" ns="params">

4.2.4<arg>标签

launch文件内部的局部变量，仅限于launch文件使用，格式：<arg name="arg-name" default="arg-value"/>

• name：参数名
• value：参数值

调用：

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

4.2.5<remap>标签

重映射ROS计算图资源的命名，格式：<remap from="turtlebot/cmd_vel" to="/cmd_vel" />

• from：原命名
• to：映射之后的命名

4.2.6<include>标签

包含其他launch文件，类似C语言中的头文件包含，格式：

• file：包含的其他launch文件路径

4.3使用方法

4.3.1Launch文件创建

和创建其他功能包一样，在src文件下创建一个专用于存放launch相关文件的功能包

进入该文件夹后创建一个名为launch的文件夹，用于存放launch文件

文件范例1：

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

文件范例2：

<launch>

	<param name="/turtle_number"   value="2"/>

    <node pkg="turtlesim" type="turtlesim_node" name="turtlesim_node">
		<param name="turtle_name1"   value="Tom"/>
		<param name="turtle_name2"   value="Jerry"/>

		<rosparam file="$(find learning_launch)/config/param.yaml" command="load"/>
	</node>

    <node pkg="turtlesim" type="turtle_teleop_key" name="turtle_teleop_key" output="screen"/>

</launch>

4.3.2Launch文件启动

首先还是需要对文件进行相关的编译

使用roslaunch命令，用于专门启动launch文件，命令格式：

roslaunch ***(功能包名) ***(launch文件名)