ROS入门学习笔记(四)—–参数的使用与编程方法、ROS中的坐标系管理系统、tf坐标系广播与监听的编程实现

123
0
2020年11月18日 10时31分

   本系列文章是与大家分享一下,我在学习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;

   (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 <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文件所要包含的内容

在这里插入图片描述

发表评论

后才能评论