【ROS学习9】参数的使用与编程及tf坐标管理系统的基本了解

参数的使用与自定义实现

Parameter Server是Ros中自带的参数服务端，由于其对参数的定义类型为一个全局字典，因此RosMaster下的各个节点均可全局访问，可以将Parmeter Server简单理解成全局变量的存储空间。

参数服务端的命令行使用：

⚫ 列出rosmaster下所有节点包括的所有参数rosparam list
⚫ 显示某个参数值rosparam get param_key
⚫ 设置某个参数值rosparam set param_key param_value
⚫ 保存参数到文件rosparam dump file_name
⚫ 从文件读取参数rosparam load file_name
⚫ 删除参数rosparam delete param_key

以小海龟仿真器为例：

其中/background_b_g_r是turtlesim_node特有的参数

使用rosparam get显示参数：

使用rosparam set更改参数：

需要发送一个服务请求使turtlesim_node查询参数是否发生变化并刷新，请求内容为空。

刷新背景颜色：

若参数数据十分庞大，可以生成一个.yaml文件保存当前参数，保存在当前路径，以便下次使用：


使用rosparam load读取参数文件：

获取/设置参数的编程实现：

创建功能包：

cd /catkin_ws/src
catkin_create_pkg learning_parameter roscpp rospy std_srvs

如何获取/设置参数:
• 初始化ROS节点;
• get函数获取参数;
• set函数设置参数;

代码实现：

/*
该例程设置/读取海龟例程中的参数 
*/
#include<iostream>
#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);
    
    // 设置背景颜色参数	
    int r,g,b;	
    std::cin>>r>>g>>b;  //读取参数	
    ros::param::set("/background_r", r);
    ros::param::set("/background_g", g);
    ros::param::set("/background_b", b); 	
    ROS_INFO("Set Backgroud Color[%d, %d, %d]",r, g, b);     
    
    // 读取背景颜色参数	
    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;
}

CMakeLists：

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

编译
运行：

roscore
rosrun turtlesim turtlesim_node
rosrun learning_parameter parameter_config

Client端：

实现：

（ros官方教程）

Ros坐标系管理系统

机器人中的坐标变换

(描述两个不同坐标系之间坐标的转换）：
基本原理可以参考《视觉SLAM十四讲》或其他一些机器人学书籍。

在实际的机器人运动中，机器人各个组件的坐标系是独立的，因此我们需要求出各个部件坐标系之间的联系来更好的控制机器人的运动，在实际应用中一个机器人可能含有几十个不同的坐标系，对应的计算也较为繁杂.

对此ros中的TF功能包为我们提供了坐标管理的功能，同时也封装了底层的运算函数，帮助开发者进行机器人坐标的管理与查询。

TF功能包能干什么?
•五秒钟之前,机器人头部坐标系相对于全局坐标系的关系是什么样的?
•机器人夹取的物体相对于机器人中心坐标系的位置在哪里?
•机器人中心坐标系相对于全局坐标系的位置在哪里？

TF坐标变换如何实现（广播与监听机制）
• 广播TF变换
• 监听TF变换

Ros节点中的所有坐标系都是通过TFTree（TF树）的数据结构存储的

图中
机器人的雷达和底盘各有一个独立的坐标系base_link和base_laser,雷达和底盘的坐标变换是确定的，当雷达检测障碍物后得出障碍物距离雷达的相对距离，通过坐标变换便可以得出障碍物距底盘坐标系的相对距离。

小海龟跟随实验：

(控制两海龟坐标系最终重叠以实现跟随）

安装tf功能包：（kinetic版本）

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

启动tf中提供的launch文件:

roslaunch turtle_tf turtle_tf_demo.launch

launch文件相当于一个脚本，将启动，初始化各个节点的操作一起执行，这样便简化里我们一开始的繁琐操作。

启动键盘控制节点：

rosrun turtlesim turtle_teleop_key

实现：

实用命令行工具：

1.查看rosmaster下所有坐标之间的关系：

rosrun tf view_frames

执行后tf view_frames会监听5s，并记录所有坐标之间的关系，生成pdf文件。

生成的pdf文件：

其中
world相当于世界坐标系（恒定不变的全局坐标系）
在海龟仿真器中就是左下角的坐标原点
turtle1和tuotle2是两只海龟自身的坐标系
world与turtle1和turtle2构成一个有三个节点的TFTree。

2.查询TFTree中任意两个坐标系之间的变换关系：

rosrun tf tf_echo turtle1 turtle

其中：
Transform表示两坐标系之间在XYZ方向上的平移变换
Quaternion以四元数方式的描述
RPY(radian)欧拉角xyz轴的旋转（弧度）
RPY(degree)欧拉角xyz轴的旋转（角度度）

重合：

3.rviz可视化：

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

其中
turtle1和turtle2两者间的坐标变换关系可以由turtle1和world，turtle2和world计算得出：

代码实现小海龟跟随实验：

首先创建一个功能包：

cd catkin_ws/src
catkin_create_pkg learning_tf roscpp rospy tf turtlesim

广播：

广播的内容为海龟坐标系与世界坐标系的坐标变换关系

如何实现一个tf广播器：
• 定义TF广播器(TransformBroadcaster)• 创建坐标变换值;
• 发布坐标变换(sendTransform)

广播器代码实现：

/*
该例程产生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的广播器
    brstatic 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);
    //设置旋转（只关于z轴的旋转） 
    // 广播world与海龟坐标系之间的tf数据并存入tf树//ros::Time::now()定义当前时间戳（实时更新）
    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);//创建订阅者，订阅海龟Pose 
    // 循环等待回调函数
    ros::spin(); 
    return 0;
}

监听器：

监听的内容为广播发布的数据，并以此计算跟随者与被跟随者之间的参数，并发布速度指令给跟随者

如何实现一个TF监听器
•定义TF监听器;(TransformListener)
•查找坐标变换;(waitForTransform、 lookupTransform)

监听器代码实现：

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

#include<iostream>
#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{
    
    
            //等待是否存在turtle1和turtle2两个坐标系
            //ros::Duration(3.0)等待超过三秒就超时
            listener.waitForTransform("/turtle2", "/turtle1", ros::Time(0), ros::Duration(3.0));
            //计算变换结果，保存在变量
            transformlistener.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());
         //计算线速度，距离用两点坐标公式表示，×0.5表示完成这段距离所需要的时间是2秒，得到速度
         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;
 }

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

编译
运行：

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.turtle1_tf_broadcaster的名称将取代：

// 初始化ROS节点
ros::init(argc, argv, "my_tf_broadcaster"); //重映射

2./turtle1 ：海龟名

事实上，根据这一原理，我们可以创造多个节点来实现多海龟跟随：

查看计算图：