本系列文章是与大家分享一下，我在学习ROS入门过程中所做的笔记，目前主要的学习资料是古月老师（胡春旭老师）的 ROS入门21讲（文章中部分图片来自于21讲课件），以及其编著的《ROS机器人开发实践》，当然其中也加入了我自己的理解和其他的相关资料，本系列共五篇文章，本篇文章是本系列第四篇文章，本篇文章主要介绍参数的使用与编程方法、ROS中的坐标系管理系统、tf坐标系广播与监听的编程实现
十一、参数的使用与编程方法
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 <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;
}
（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 <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;
};
（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;
（8）接下来通过sendTransform把位置关系广播出去，然后在后台运行的tf树就会把这样的位置关系插入到树中，(transform, ros::Time::now(), “world”, turtle_name)这一部分是时间戳，其中ros::Time::now()是当前的时间，描述的是”world”和turtle_name之间的关系transform
4、TF监听器c++代码的实现
/**
* 该例程监听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;
};
（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文件所要包含的内容
开心洋葱 , 版权所有丨如未注明 , 均为原创丨未经授权请勿修改 , 转载请注明ROS入门学习笔记（四）—–参数的使用与编程方法、ROS中的坐标系管理系统、tf坐标系广播与监听的编程实现！
