ROS学习笔记（四）-----参数的使用与编程方法、ROS中的坐标系管理系统、tf坐标系广播与监听的编程实现

   本系列文章是与大家分享一下，我在学习ROS过程中所做的笔记，目前主要的学习资料是古月老师（胡春旭老师）的 ROS入门21讲，以及其编著的《ROS机器人开发实践》，当然其中也加入了我自己的理解和其他的相关资料，本系列的各篇文章将处于不断的更新完善中，本篇文章是本系列第四篇文章

---

十一、参数的使用与编程方法

   1、在ROS Master 里面有一个参数服务器Parameter Server，他是一个全局字典，各个节点都可以全局访问，各个节点可以在不同的PC里面，只要在同样的ROS环境里面都可以通过Parameter Server去查询全局字典的参数

   2、首先我们在工作空间的src文件夹下创建一个新的功能包，取名为learning_parameter 添加依赖roscpp 、 rospy 、 std_srvs

cd ~/catkin_ws/src
catkin_create_pkg learning_parameter roscpp rospy std_srvs

   3、YAML参数文件：当我们的参数较多时，我们将其放到YAML参数文件中，在用的时候可以一次性将其加载到Parameter Server中

   4、rosparam命令可以完成大部分的参数的查询、设置、保存到文件、读取文件等

⚫ 列出当前多有参数
$ rosparam list
⚫ 显示某个参数值
$ rosparam get param_key
⚫ 设置某个参数值
$ rosparam set param_key param_value
⚫ 保存参数到文件
$ rosparam dump file_name
⚫ 从文件读取参数
$ rosparam load file_name
⚫ 删除参数
$ rosparam delete param_key

   5、我们利用小海龟来学习该部分内容，先运行节点管理器，再运行海龟仿真器，再在一个新的终端里输入rosparam，按回车便可以显示出他后面可以跟的参数

   （1）rosparam list可以查看当前所有参数

   （2）rosparam get 可以获取参数的值，后面跟rosparam list所显示出来的参数，如rosparam get /turtlesim/background_b

   （3）rosparam set可以重新设定参数的值，后面跟的第一个参数是参数的名字，第二个参数是新的值。如rosparam set /turtlesim/background_b 255 将/turtlesim/background_b 的值设定为255，这时可以运用rosparam get来查看现在/turtlesim/background_b的值是否变为了255，/turtlesim/background_b 、/turtlesim/background_r 、/turtlesim/background_g 这三个参数用来设定仿真器的背景色，但是经过上面的改动后，背景色并没有改变，因为仿真器还需要发送一个请求rosservice call /clear “{}” 它会去查询背景的rgb值有没有发生改变，若改变会刷新背景的颜色，如下所示

   （4）rosparam dump命令可以将参数保存为文件，后面跟的参数是文件名如 rosparam dump param.yaml 默认路径会存放在主文件夹下

   （5）我们可以在以上的文件中对参数进行修改，保存后再利用rosparam load命令将其中的参数导入，后面跟的参数是文件名，如rosparam load param.yaml （同样需要运行rosservice call /clear "{}"背景色才会变）

   （6）rosparam delete命令可以删除某个参数，后面跟的是参数名，如rosparam delete /turtlesim/background_b

   6、接下来我们看一下如何利用c++代码实现以上的功能（以下代码是古月老师写的例程，有些地方比如参数名需要我们进行修改才能用，下文中会进行介绍，我们先来看一下他的整体结构）

/**

 * 该例程设置/读取海龟例程中的参数

 */

#include 
#include 
#include 

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

   （1）在主函数里面，依旧是先进行节点的初始化，创建句柄

   （2）接下来的读取背景颜色参数部分，通过ros::param::get来读取背景的颜色，比如ros::param::get("/background_r", red); 读取背景颜色/background_r的值，存放到变量red中，注意此处要修改路径，路径可以通过在终端输入rosparam list来查看，如下所示我的background_r这个变量的路径是/turtlesim/background_r 那么相应的程序中该语句应该改为ros::param::get("/turtlesim/background_r", red); 同样后面的涉及background_r、background_g、background_b的路径都要修改

   （3）接下来ROS_INFO(“Get Backgroud Color[%d, %d, %d]”, red, green, blue);输出一个日志信息，告诉我们修改前这几个参数的值是什么

   （4）接下来就是通过ros::param::set来设定新的参数值，并输出一个日志信息，告诉我们新的参数值

   （5）接下来就是再重新读取这几个参数值，看一下是否改变了

   （6）接下来的代码就是调用clear服务将海龟仿真器的背景刷新

   7、接下来同样是在CMakeLists.txt文件中设置编译的规则，将以下语句复制到CMakeLists文件中

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

   8、接下来就是返回到工作空间目录下，对其进行编译了，编译完后，由于之前我们已经在bashrc文件中添加了工作空间下的环境变量的配置，这里就不需要再配置了，依次运行节点管理器、海龟仿真器和我们刚才编写好的节点parameter_config，会发现海龟仿真器的背景颜色由蓝色变为白色.

cd ~/catkin_ws
catkin_make
source devel/setup.bash
roscore
rosrun turtlesim turtlesim_node
rosrun learning_parameter parameter_config

---

十二、ROS中的坐标系管理系统

   1、TF功能包可以记录10秒钟之内的机器人所有坐标系之间的位置关系，TF坐标变换是通过监听和广播的机制实现的，当我们启动节点管理器和TF之后，就会在后台去维护一个TF树

   2、本部分我们要实现的内容是通过键盘控制第一个海龟移动，让另一个海龟通过TF坐标变换自动的跟踪第一个海龟

   3、接下来需要通过以下指令来安装turtle-tf示例的功能包

 sudo apt-get install ros-melodic-turtle-tf

   注意这个地方因安装的ROS的版本不同，安装语句也不同，比如我安装的ROS是noetic版本，而不是上面那条语句中的melodic版本，需要将该语句中的melodic改为noetic，如下所示：

 sudo apt-get install ros-noetic-turtle-tf

   4、接下来用到了ROS中的roslaunch 指令，去启动一个launch文件，launch文件可以理解为一个脚本文件，可以把很多节点的启动都写进去，运行一下的语句（内容后面再做介绍）

 roslaunch turtle_tf turtle_tf_demo.launch

   我的noetic版本的ROS在运行该语句时报错了，比如：ImportError: No module named yaml，只出现了一只海龟，这里的错误多半是因为Python的版本导致的，解决的方法如下

   （1）先运行以下语句，进入usr.bin目录

cd /usr/bin/

   （2）再运行以下语句，删除原默认编译器文件

sudo rm -r python

   （3）再运行以下语句，复制python3编译器设置为原编译器文件

sudo  cp python3 python

   （4）然后再重新运行该roslaunch 命令，就会发现错误解决了

 roslaunch turtle_tf turtle_tf_demo.launch

---

 rosrun turtlesim turtle_teleop_key

   5、接下来运行键盘控制节点，使用↑↓←→键来控制第一个海龟移动，可以发现第二个海龟会跟踪第一个海龟移动

   6、在tf功能包里提供了一个小工具view_frames可以可视化的看见系统中所有tf间的关系，使用 rosrun tf view_frames命令来运行它，它会先监听5s的时间，然后把5s内的所有坐标系之间的关系保存下来，它会生成一个pdf文件，默认放在主文件夹下

 rosrun tf view_frames
 

   由于Python2和Python3语法的不兼容问题，在这一步我又遇到了错误，按照系统给出的提示，修改的办法如下：

   （1）按照错误的提示，找到存放view_frames文件的目录（在ubuntu中打开文件默认是在主文件下，选择其他位置，点击计算机，可以看到opt文件夹，然后按照提示一步步找到view_frames所在的文件夹下）

   （2）在该目录下右键，选择在终端打开，输入以下命令，打开修改文件的权限，然后打开该文件（view_frames），找到报错的第89行

sudo chmod 666 view_frames

   （3）将第89行改成m = r.search(str(vstr))，然后保存

   （4）关闭该文件，再输入以下的指令，把打开的权限关掉

sudo chmod 777 view_frames

   （5）然后再输入 rosrun tf view_frames 就发现可以错误解决了，可以成功产生PDF文件了

---

   7、其中的world坐标系是建立在仿真器左下角的，坐标系turtle1和turtle2是建立在两个海龟上的，若是turtle与world之间的箭头没有则说明坐标系没有建立成功

---

  8、接下来我们看一下另一个tf工具：tf_echo工具，这个工具可以直接查询任意两个节点在树中的坐标关系，如rosrun tf tf_echo turtle1 turtle2 可以查看节点turtle1 到节点turtle2的坐标变换，也就是turtle2坐标系如何变换到turtle1坐标系，我们用键盘控制第一个海龟运动，可以发现数据会变化，其中Translation表示的是平移，Rotation表示的是旋转，在Rotation中，Quaternion行是用四元数表示的，下一行是RPY（通过XYZ轴旋转）是通过的弧度来描述的，下一行的RPY是通过角度来描述的。

rosrun tf tf_echo turtle1 turtle2

---

  9、接下来看一个更加可视化的工具rviz工具，他是一个三维可视化的显示平台，我们通过以下的指令来运行该工具，运行后点击Add添加tf，然后就可以直观的看到坐标系之间的关系，使用键盘指令控制第一只海龟移动可以发现坐标系的移动

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

---

十三、tf坐标系广播与监听的编程实现

   1、在工作空间的src文件夹下创建一个新的功能包learning_tf

cd ~/catkin_ws/src
catkin_create_pkg learning_tf roscpp rospy tf turtlesim

   2、位置关系是通过tf广播出来的，我们首先看一下如何在ros中通过tf去广播任意两个坐标系之间的位置关系，称之为tf广播器

   3、TF广播器c++代码的实现

/**
 * 该例程产生tf数据，并计算、发布turtle2的速度指令
 */

#include 
#include 
#include 

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

   （1）在main函数里，首先还是初始化节点，然后根据main函数的输入参数argv来判断是哪只海龟与world建立关系，让两只海龟都与world建立关系，这个程序可以运行两遍，但是在ros中节点名是不能重复的，如果该程序执行两遍节点名会冲突，我们需要想办法将其换一个名字，就可以执行两遍了，也就是重映射

   （2）turtle_name = argv[1]是来获取节点的名字，是个字符串

   （3）接下来创建一个句柄，然后通过句柄创建一个话题的订阅者，来订阅海龟仿真器里不断发布的海龟位置消息，他默认发布的就是"/turtle1/pose"这样的话题，由于这里我们的海龟名字有多个用变量turtle_name来表示，写作ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);

   （4）接下来的ros::spin();就是循环等待，一旦有海龟的位置在发布时，有消息进来，就会跳到poseCallback回调函数中

   （5）在回调函数里我们传入的数据就是海龟的位置信息（包括XY坐标，角度等）turtlesim::PoseConstPtr& msg

   （6）在回调函数里我们要先创建一个tf的广播器，创建名为br的广播器，通过这个广播器我们要把turtle1和turtle2相对于world坐标系的位置关系发出去

static tf::TransformBroadcaster br;

   （7）接下来的一断就是填充坐标系间的位置关系，创建一个Transform类型的数据transform; （Transform就是机器人坐标变换中的4X4的坐标变换矩阵），通过setOrigin来设置平移参数，这里的旋转参数是利用四元数来建立的tf::Quaternion q; q.setRPY(0, 0, msg->theta)；transform.setRotation(q);

   （8）接下来通过sendTransform把位置关系广播出去，然后在后台运行的tf树就会把这样的位置关系插入到树中，(transform, ros::Time::now(), “world”, turtle_name)这一部分是时间戳，其中ros::Time::now()是当前的时间，描述的是"world"和turtle_name之间的关系transform

   4、TF监听器c++代码的实现

/**
 * 该例程监听tf数据，并计算、发布turtle2的速度指令
 */

#include 
#include 
#include 
#include 

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

   （1）在main函数中初始化节点，产生函数句柄后，然后发布"/spawn"请求去产生一只新的海龟

   （2）接下来我们要创建一个速度的发布者，我们让海龟2动的话，需要去发布"/turtle2/cmd_vel"这样一个话题，其消息类型是Twist,海龟2就会根据我们发布的速度指令动起来

   （3）接下来利用tf::TransformListener listener;命令来创建一个名为listener的tf监听器来监听tf坐标器里面任意两个节点之间的位置关系

   （4）接下来的ros::Rate rate(10.0);用于设定while循环里的频率

   （5）在while循环里，我们要获取turtle1与turtle2坐标系之间的tf数据，首先我们创建了一个transform用来保存平移和旋转之间的位置关系

   （6）接下来的语句中waitForTransform是等待变换，看一下系统中是否存在turtle1和turtle2这两个坐标系，如果存在就进行下一行语句，其中ros::Time(0)是查询当前时间， ros::Duration(3.0)是设定的等待时间3s；lookupTransform是查询两个坐标系之间的关系，结果放到transform之中

   （7）在我们得到两个坐标系之间的位置信息后，就可以通过发布速度指令控制海龟2向海龟1移动了，创建一个Twist类型的数据 vel_msg，然后设定线速度和角速度让其完整，sqrt(pow(transform.getOrigin().x(), 2) + pow(transform.getOrigin().y(), 2))是计算两只海龟间的欧氏距离，然后我们期望它在2秒内完成这段移动，位移/时间2秒=0.5*位移，也就是线速度。

   （8）接下来通过刚才创建的速度的发布者把速度发布出去turtle_vel.publish (vel_msg);这时候海龟2就会朝着海龟1去运动了

   5、接下来依旧是配置CMakeLists.txt中的编译规则，把以下的语句添加到CMakeLists文件中

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

   6、接下来就是回到工作空间的根目录下进行编译，并运行了

 cd ~/catkin_ws
 catkin_make
 source devel/setup.bash
 roscore
 rosrun turtlesim turtlesim_node
 rosrun learning_tf turtle_tf_broadcaster __name:=turtle1_tf_broadcaster /turtle1
 rosrun learning_tf turtle_tf_broadcaster __name:=turtle2_tf_broadcaster /turtle2
 rosrun learning_tf turtle_tf_listener
 rosrun turtlesim turtle_teleop_key

   （1）编译完成后，我们先运行roscore和turtlesim_node

   （2）接下来就去运行我们编译生成的可执行文件，先运行发布turtle1和world之间关系的指令，这里用到了重映射，进入broadcaster后会读取海龟的名字， /turtle1就是我们输入的海龟的名字（坐标系名字） ， __name:=turtle1_tf_broadcaster就是ros里面重映射的机制，两个下划线加name再加：=后面的内容就会取代我们在程序中初始化的节点名my_tf_broadcaster，同样再运行发布turtle2和world之间关系的指令

   （3）接下来运行我们编好的tf监听器turtle_tf_listener，最后一句就是运行键盘控制节点了

   （4）上面的第（1）步到第（3）步运行的内容其实就是在第十二部分我们运行的launch文件所要包含的内容