Serie ROS: Capítulo 5 Componentes comunes (2)

Cinco, componentes comunes de ROS

Algunas herramientas relativamente útiles están integradas en ROS; este capítulo presenta principalmente los siguientes componentes integrados de ROS:

Transformación de coordenadas TF, realice la conversión entre diferentes tipos de sistemas de coordenadas;

rosbag registra el proceso de ejecución de los nodos ROS y se da cuenta del efecto de reproducir el proceso;

rqt toolbox, una herramienta de depuración gráfica;

1. Transformación de coordenadas TF

El sistema de robot tiene múltiples sensores para percibir la información del entorno circundante, pero es incorrecto equiparar directamente la información de coordenadas del objeto en relación con el sensor con la información de orientación de coordenadas entre el sistema de robot y el objeto, y necesita pasar por un proceso de conversión.

La conversión entre diferentes coordenadas es relativamente complicada y engorrosa, y los módulos relacionados se encapsulan directamente en ROS: Coordinate Transformation (TF);

**Concepto: **tf: transformación de coordenadas;

**Sistema de coordenadas: **ROS calibra el objeto a través del sistema de coordenadas y calibra a través del sistema de coordenadas de la mano derecha;

[Falló la transferencia de la imagen del enlace externo, el sitio de origen puede tener un mecanismo anti-leeching, se recomienda guardar la imagen y cargarla directamente (img-v0jRw5FA-1668434092634) (C:\Users\Haotian\AppData\Roaming\Typora\ typora-user-images \1668314087211.png)]

**Función: **Utilizado en ROS para realizar la conversión de puntos o vectores entre diferentes sistemas de coordenadas;

**Descripción:**tf está obsoleto y ahora se usa tf2, que es simple y eficiente, y los paquetes de funciones correspondientes son:

tf2_geometry_msgs: el mensaje ROS se convierte en un mensaje TF2;

tf2: encapsula mensajes comunes de cambios de coordenadas;

tf2_ros: proporciona enlaces roscpp y rospy para tf2 y encapsula API comunes para la transformación de coordenadas;

1.1 Mensaje de mensaje de coordenadas


El mensaje de portadora de transmisión de datos comúnmente utilizado en la transformación de coordenadas es:

geometry_msgs/TransformStamped
geometry_msgs/PointStamped

El primero se usa para transmitir la información de posición relacionada con el sistema de coordenadas, y el segundo se usa para transmitir la información de puntos de coordenadas en un determinado sistema de coordenadas;

  • geometric_msgs/TransformStamped

Terminal de entrada:

rosmsg info geometry_msgs/TransformStamped 
std_msgs/Header header   
  uint32 seq
  time stamp
  string frame_id						#坐标id
string child_frame_id					#子坐标系的id
geometry_msgs/Transform transform		#坐标信息
  geometry_msgs/Vector3 translation  #偏移量
    float64 x
    float64 y
    float64 z
  geometry_msgs/Quaternion rotation   #四元数
    float64 x
    float64 y
    float64 z
    float64 w
    

explicar:

  1. A y B dos sistemas de coordenadas, discuta la relación entre el sistema de coordenadas A y el sistema de coordenadas B, en este momento A es child_frame_id y B es frame_id.
  2. El cuaternión es otra forma de ángulo de Euler (volteo, inclinación, rotación), pero hay una singularidad en el ángulo de Euler y habrá errores al escribir el código, así que elige un cuaternión que parezca un número complejo.
  • geometric_msgs/PointStamped

Terminal de entrada:

rosmsg info geometry_msgs/PointStamped 
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
geometry_msgs/Point point
  float64 x
  float64 y
  float64 z

1.2 Transformación de coordenadas estáticas


Transformación de coordenadas estáticas, la posición relativa entre dos sistemas de coordenadas es fija.

Lógica básica de implementación:

  1. La relación relativa del sistema de coordenadas, enviada por el editor.
  2. De acuerdo con la relación del sistema de coordenadas del editor de suscripción, el suscriptor pasa la información del punto de coordenadas (puede codificarse), realiza la transformación de coordenadas con la ayuda de tf y genera el resultado.

Proceso básico de implementación:

  1. Paquete de funciones, agregar dependencia tf
  2. Escribe al editor
  3. Escribe el suscriptor

Opción A: C++

experimento:

1. Cree un nuevo paquete de funciones demo04_ws y cree un nuevo archivo .cpp en su archivo src que debe contener los siguientes paquetes dependientes:

roscpp rospy std_msgs tf2 tf2_ros tf2_geometry_msgs geometry_msgs

2. Configure el nuevo código para compilar rápidamente el archivo tasks.json:

{
    
    
// 有关 tasks.json 格式的文档,请参见
    // https://go.microsoft.com/fwlink/?LinkId=733558
    "version": "2.0.0",
    "tasks": [
        {
    
    
            "label": "catkin_make:debug", //代表提示的描述性信息
            "type": "shell",  //可以选择shell或者process,如果是shell代码是在shell里面运行一个命令,如果是process代表作为一个进程来运行
            "command": "catkin_make",//这个是我们需要运行的命令
            "args": [],//如果需要在命令后面加一些后缀,可以写在这里,比如-DCATKIN_WHITELIST_PACKAGES=“pac1;pac2”
            "group": {
    
    "kind":"build","isDefault":true},
            "presentation": {
    
    
                "reveal": "always"//可选always或者silence,代表是否输出信息
            },
            "problemMatcher": "$msCompile"
        }
    ]
}
  • Editor
#include "ros/ros.h"
#include "tf2_ros/static_transform_broadcaster.h"
#include "geometry_msgs/TwistWithCovarianceStamped.h"
#include "tf2/LinearMath/Quaternion.h"//欧拉角

/**
 * @brief 需求:该节点需要发布两个坐标系之间的关系
 * 
 * 流程:
 * 1、包含头文件
 * 2、设置编码 节点初始化  nodehandle
 * 3、创建发布对象
 * 4、组织被发布的消息
 * 5、发布数据
 * 6、spin()
 *
 */
int main(int argc, char* argv[])
{
    
    
    //  * 2、设置编码 节点初始化  nodehandle
    setlocale(LC_ALL,"");
    ros::init(argc, argv, "static_pub");
    ros::NodeHandle nh;
    //  * 3、创建发布对象
    tf2_ros::StaticTransformBroadcaster pub;

    //  * 4、组织被发布的消息
    geometry_msgs::TransformStamped tfs;
    tfs.header.stamp = ros::Time::now();
    tfs.header.frame_id = "base_link";//相对关系之中被参考的那个  参考坐标系
    tfs.child_frame_id = "laser";
    tfs.transform.translation.x = 0.2;
    tfs.transform.translation.y = 0.0;
    tfs.transform.translation.z = 0.5;
    //需要参考欧拉角转换
    tf2::Quaternion qtn;//创建 四元数 对象
    //向该对象设置欧拉角 这个对象可以将欧拉角转化为四元数
    qtn.setRPY(0, 0, 0); //目前研究对象雷达相对于小车是固定的,所以没有翻滚,俯仰,偏航 等因此设为0,0,0  欧拉角的单位是弧度
    tfs.transform.rotation.x = qtn.getX();
    tfs.transform.rotation.y = qtn.getY();
    tfs.transform.rotation.z = qtn.getZ();
    tfs.transform.rotation.w = qtn.getW();

    //  * 5、发布数据
    pub.sendTransform(tfs);
    //  * 6、spin()
    ros::spin();
    return 0;
}
rostopic list
/rosout
/rosout_agg
/tf_static
rostopic echo /tf_static 
transforms: 
  - 
    header: 
      seq: 0
      stamp: 
        secs: 1668322909
        nsecs: 686407571
      frame_id: "base_link"
    child_frame_id: "laser"
    transform: 
      translation: 
        x: 0.2
        y: 0.0
        z: 0.5
      rotation: 
        x: 0.0
        y: 0.0
        z: 0.0
        w: 1.0
---

Use rviz para mostrar:

rviz

[La transferencia de la imagen del enlace externo falló, el sitio de origen puede tener un mecanismo anti-leeching, se recomienda guardar la imagen y cargarla directamente (img-YxcJHdjc-1668434092636) (C:\Users\Haotian\AppData\Roaming\Typora\ typora-user-images \1668324095841.png)]

  • Abonado
#include <ros/ros.h>
#include "tf2_ros/transform_listener.h"//订阅数据
#include "tf2_ros/buffer.h"//缓存
#include "geometry_msgs/PointStamped.h"//坐标点信息
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"

/**
 * @brief 订阅方
 * 需求:
 *订阅发布的坐标系相对关系,传入一个坐标点,调用tf实现转换
头发》订阅坐标系相对关系
 * 4、组织一个坐标点的数据
 * 5、转换算法实现   调用tf内置实现
 * 6、输出转换结果
 */
int main(int argc, char* argv[])
{
    
    
    //  * 2、编码 初始化 nodehandle 必须有nodehandle的
    setlocale(LC_ALL,"");
    ros::init(argc, argv, "static_sub");
    ros::NodeHandle nh;
    //  * 3、创建一个订阅对象   ----》订阅坐标系相对关系
    //3-1 创建一个buffer缓存
    tf2_ros::Buffer buffer;

    // 3-2 创建一个监听对象 (监听对象可以将被订阅的的数据存入缓存buffer之中)
    tf2_ros::TransformListener listener(buffer);

    //  * 4、组织一个坐标点的数据
    geometry_msgs::PointStamped ps;
    ps.header.frame_id = "laser";
    ps.header.stamp = ros::Time::now();
    ps.point.x = 2.0;
    ps.point.y = 3.0;
    ps.point.z = 5.0;
    //添加休眠  等待发布方
    // ros::Duration(2).sleep();
    //  * 5、转换算法实现   调用tf内置实现
    ros::Rate rate(10);
    while(ros::ok())
    {
    
    
        //核心代码  ----将 ps 转换成相对于 base——link的坐标点
        geometry_msgs::PointStamped ps_out;
        /*
            调用了buffer的转换函数 transform
            参数1:被转换的坐标点
            参数2:目标坐标系
            返回值:输出的坐标点

            注意:调用是必须包含头文件 tf2_geometry_msgs/
            注意:运行时候出现报错,提示base_link不存在 
                 原因分析:是因为发布方可能还没有发布 ,订阅数据是一个耗时操作,可能调用转换时,坐标系的相对关系还没有订阅
                 解决办法:方案1:调用转换函数的前面加一个休眠
                         方案2:异常处理 (建议使用)
        */
       try
       {
    
    
           ps_out = buffer.transform(ps, "base_link");
           //  * 6、输出转换结果
           ROS_INFO(
               "转换后的坐标值:(%.2f,%.2f,%.2f),参考的坐标系:%s",
               ps_out.point.x,
               ps_out.point.y,
               ps_out.point.z,
               ps_out.header.frame_id.c_str());
       }
       catch(const std::exception& e)
       {
    
    
            //std::cerr << e.what() << '\n';
            ROS_INFO("异常消息:%s", e.what());
       }
    //    ps_out = buffer.transform(ps, "base_link");
    //    //  * 6、输出转换结果
    //    ROS_INFO(
    //        "转换后的坐标值:(%.2f,%.2f,%.2f),参考的坐标系:%s",
    //        ps_out.point.x,
    //        ps_out.point.y,
    //        ps_out.point.z,
    //        ps_out.header.frame_id.c_str());
       rate.sleep();
       ros::spinOnce();
    }

    return 0;
}

Resultados de la:

[ INFO] [1668329057.735620630]: 异常消息:"base_link" passed to lookupTransform argument target_frame does not exist. 
[ INFO] [1668329057.835439384]: 转换后的坐标值:(2.20,3.00,5.50),参考的坐标系:base_link

Reponer:

Cuando la posición relativa entre los sistemas de coordenadas es fija, los parámetros requeridos también son fijos: nombre de la coordenada principal, nombre del sistema de coordenadas secundario, desplazamiento x, desplazamiento y, desplazamiento z, ángulo de balanceo x, ángulo de cabeceo y, ángulo de guiñada z, la lógica de implementación es el mismo, pero los parámetros son diferentes, entonces el sistema ROS ya ha empaquetado un nodo especial, y el método de uso es el siguiente:

rosrun tf2_ros static_transform_publisher x偏移量 y偏移量 z偏移量 z偏航角度 y俯仰角度 x翻滚角度 父级坐标系 子级坐标系

Ejemplo:

rosrun tf2_ros static_transform_publisher 0.2 0 0.5 0 0 0 /base_link /camera 
rviz
rosrun tf2_ros static_transform_publisher 0.1 0.0 0.3 1.0 0 0 /base_link /camera 

El efecto de conversión se puede ver en rviz, y también se recomienda utilizar este método para realizar directamente la liberación de información relativa del sistema de coordenadas estáticas.

1.3 Transformación dinámica de coordenadas


Requiere la versión de transformación de coordenadas de la sección anterior

y suscripción de coordenadas dinámicas

  • Implementos de editor
#include <ros/ros.h>
#include "turtlesim/Pose.h"
#include "tf2_ros/transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"

/*
    发布方:需要订阅乌龟的位资信息,转换成想对于窗体的一个坐标关系,并发布
    准备:
        话题:/turtle1/pose

        消息:turtlesim/Pose
    liucheng流程:
        1、包含头文件
        2、设置编码、初始化、nodehandle
        3、创建订阅对象、订阅/turtle1/pose
        4、回调函数处理订阅的消息。将位资信息转换成坐标相对关系并发布
        5、spin()

*/
void doPose(const turtlesim::Pose::ConstPtr& pose)//const 防止函数修改引用内容   传引用提高效率
{
    
    
    //获取位资信息,转换成坐标系想对关系(核心)并发布
    //创建TF发布对象
    static tf2_ros::TransformBroadcaster pub; // static修饰  使得每次回调函数是哟你pub这个函数;锁定pub
    //组织被发布的数据
    geometry_msgs::TransformStamped ts;
    ts.header.frame_id = "world";
    ts.header.stamp = ros::Time::now();
    ts.child_frame_id = "turtle1";
    //坐标系偏移量
    ts.transform.translation.x = pose->x;
    ts.transform.translation.y = pose->y;
    ts.transform.translation.z = 0;
    //四元数
    //位资信息之中  没有四元数   有个z轴的偏航角度  而乌龟是2D 没有翻滚和府仰角 所以可以得出乌龟的欧拉角为:0 0 theta
    tf2::Quaternion qtn;
    qtn.setRPY(0,0,pose->theta);
    ts.transform.rotation.x = qtn.getX();
    ts.transform.rotation.y = qtn.getY();
    ts.transform.rotation.z = qtn.getZ();
    ts.transform.rotation.w = qtn.getW();

    //发布
    pub.sendTransform(ts);
}
int main(int argc, char* argv[])
{
    
    
    // 2、设置编码、初始化、nodehandle
    setlocale(LC_ALL,"");
    ros::init(argc, argv, "dynamic_pub");
    ros::NodeHandle nh;
    // 3、创建订阅对象、订阅/turtle1/pose
    ros::Subscriber sub = nh.subscribe("turtle1/pose",100,doPose);
    // 4、回调函数处理订阅的消息。将位资信息转换成坐标相对关系并发布
    // 5、spin()
    ros::spin();
    return 0;
}

Mostrar resultados:

  • Implementación de suscriptores
#include <ros/ros.h>
#include "tf2_ros/transform_listener.h" //订阅数据
#include "tf2_ros/buffer.h"             //缓存
#include "geometry_msgs/PointStamped.h" //坐标点信息
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"

/**
 * @brief 订阅方
 * 需求:
 *订阅发布的坐标系相对关系,传入一个坐标点,调用tf实现转换
头发》订阅坐标系相对关系
 * 4、组织一个坐标点的数据
 * 5、转换算法实现   调用tf内置实现
 * 6、输出转换结果
 */
int main(int argc, char *argv[])
{
    
    
    //  * 2、编码 初始化 nodehandle 必须有nodehandle的
    setlocale(LC_ALL, "");
    ros::init(argc, argv, "static_sub");
    ros::NodeHandle nh;
    //  * 3、创建一个订阅对象   ----》订阅坐标系相对关系
    // 3-1 创建一个buffer缓存
    tf2_ros::Buffer buffer;

    // 3-2 创建一个监听对象 (监听对象可以将被订阅的的数据存入缓存buffer之中)
    tf2_ros::TransformListener listener(buffer);

    //  * 4、组织一个坐标点的数据
    geometry_msgs::PointStamped ps;//被转换的坐标点
    //参考坐标系
    ps.header.frame_id = "turtle1";
    //时间戳
    ps.header.stamp = ros::Time(0.0);//动态坐标转换的时候buffer生成会有时间戳,时间戳不对代表坐标变化,因此报错
    // ps.header.stamp = ros::Time::now();

    ps.point.x = 2.0;
    ps.point.y = 3.0;
    ps.point.z = 0.0;
    //添加休眠  等待发布方
    // ros::Duration(2).sleep();
    //  * 5、转换算法实现   调用tf内置实现
    ros::Rate rate(10);
    while (ros::ok())
    {
    
    
        //核心代码  ----将 ps 转换成相对于 base——link的坐标点
        geometry_msgs::PointStamped ps_out;
        /*
            调用了buffer的转换函数 transform
            参数1:被转换的坐标点
            参数2:目标坐标系
            返回值:输出的坐标点

            注意:调用是必须包含头文件 tf2_geometry_msgs/
            注意:运行时候出现报错,提示base_link不存在
                 原因分析:是因为发布方可能还没有发布 ,订阅数据是一个耗时操作,可能调用转换时,坐标系的相对关系还没有订阅
                 解决办法:方案1:调用转换函数的前面加一个休眠
                         方案2:异常处理 (建议使用)
        */
        try
        {
    
    
            ps_out = buffer.transform(ps, "world");
            //  * 6、输出转换结果
            ROS_INFO(
                "转换后的坐标值:(%.2f,%.2f,%.2f),参考的坐标系:%s",
                ps_out.point.x,
                ps_out.point.y,
                ps_out.point.z,
                ps_out.header.frame_id.c_str());
        }
        catch (const std::exception &e)
        {
    
    
            // std::cerr << e.what() << '\n';
            ROS_INFO("异常消息:%s", e.what());
        }
        //    ps_out = buffer.transform(ps, "base_link");
        //    //  * 6、输出转换结果
        //    ROS_INFO(
        //        "转换后的坐标值:(%.2f,%.2f,%.2f),参考的坐标系:%s",
        //        ps_out.point.x,
        //        ps_out.point.y,
        //        ps_out.point.z,
        //        ps_out.header.frame_id.c_str());
        rate.sleep();
        ros::spinOnce();
    }

    return 0;
}
  • Mostrar resultados

[Falló la transferencia de la imagen del enlace externo, el sitio de origen puede tener un mecanismo anti-leeching, se recomienda guardar la imagen y cargarla directamente (img-qymlplVl-1668434092640) (C:\Users\Haotian\AppData\Roaming\Typora\ typora-user-images \1668426497090.png)]

Resumen: el proceso de implementación del suscriptor es aproximadamente el mismo que el del suscriptor estático en la sección anterior. Solo necesita cambiar el nombre de la coordenada correspondiente y tenga en cuenta que la marca de tiempo del búfer no es la actual ahora, pero debe cambiarse a 0,0;

1.4 Transformación multicoordenada


**Requisito: **Transformación entre múltiples coordenadas, las dos secciones anteriores son transformaciones de posiciones relativas entre dos coordenadas. Esta sección introducirá la transformación entre múltiples coordenadas; el mundo de coordenadas principal, las dos coordenadas secundarias son1, son2, son1 en relación con el mundo y son2 en relación con el mundo son conocidas, encuentre las coordenadas del origen de son1 en son2, o ya Conociendo las coordenadas de un punto en son1, encuentre las coordenadas de su coordenada de referencia son2

analizar:

1. Publicar son1 relativo al mundo y son2 relativo al mundo

2. Suscríbase y publique información, y use tf2 para convertir la relación de coordenadas entre son1 y son2

3. Realizar la conversión entre puntos de coordenadas

proceso:

1. Cree un nuevo paquete de funciones y agregue dependencias relacionadas

2. Cree un publicador de relaciones relativas coordinadas

3. Crear un suscriptor de relación relativa coordinada

  • El editor usa coordenadas estáticas para transformar el archivo de lanzamiento
<?xml version="1.0"?>
<launch>
<!-- 发布son1 相对于world 以及 son2相对于world 的坐标关系  -->
    <node name="son1" pkg="tf2_ros" type="static_transform_publisher" args="5 0 0 0 0 0 /world /son1" output="screen"/>
    <node name="son2" pkg="tf2_ros" type="static_transform_publisher" args="3 0 0 0 0 0 /world /son2" output="screen"/>
    
</launch>
  • Abonado
#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "geometry_msgs/PointStamped.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#include "geometry_msgs/TransformStamped.h"

/**
 * @brief
 * 订阅方实现:1.计算son1与2之间的相对关系2.计算son1的某个坐标点对于son2的坐标值
 * 流程:
 * 1、包含头文件
 * 2、编码初始化nodehandle
 * 3、创建订阅对象,
 * 4、编写解析逻辑
 * 5、spinOnce
 *
 */
int main(int argc, char *argv[])
{
    
    
    //  * 2、编码初始化nodehandle
    setlocale(LC_ALL,"");

    ros::init(argc, argv, "tfs_sub");
    ros::NodeHandle nh;
    
    //  * 3、创建订阅对象,
    tf2_ros::Buffer buffer;
    tf2_ros::TransformListener sub(buffer);

    //  * 4、编写解析逻辑
    //创建坐标点
    geometry_msgs::PointStamped psAtson1;
    psAtson1.header.stamp = ros::Time::now();
    psAtson1.header.frame_id = "son1";
    psAtson1.point.x = 1.0;
    psAtson1.point.y = 2.0;
    psAtson1.point.z = 3.0;

    ros::Rate rate(10);
    while (ros::ok())
    {
    
       
        //核心
        try
        {
    
    
            //1.计算son1与son2的相对关系
            /*
                A 相对 B 坐标关系

                参数1:目标坐标系     B
                参数2:源坐标系       A
                参数3:ros::Time(0.0)  取时间间隔最短的两个坐标关系计算相对关系
                返回值:geometry_msgs::TransformStamped 源坐标系相对于目标坐标系的相对关系

            */
            geometry_msgs::TransformStamped son1ToSon2 = buffer.lookupTransform("son2","son1",ros::Time(0));
            ROS_INFO("son1相对son2相对关系信息:父级:%s,子级:%s 偏移量(%.2f,%.2f,%.2f)",
                     son1ToSon2.header.frame_id.c_str(), // son2
                     son1ToSon2.child_frame_id.c_str(),  // son1
                     son1ToSon2.transform.translation.x,
                     son1ToSon2.transform.translation.y,
                     son1ToSon2.transform.translation.z


            );
            // 2.计算son1坐标点在son2的坐标值
            geometry_msgs::PointStamped psAtson2= buffer.transform(psAtson1,"son2");
            ROS_INFO(
                "坐标点在son2中的值为(%.2f,%.2f,%.2f)",psAtson2.point.x,psAtson2.point.y,psAtson2.point.z
            );
        }
        catch(const std::exception& e)
        {
    
    
            // std::cerr << e.what() << '\n';

        }

        //  * 5、spinOnce
        rate.sleep();
        ros::spinOnce();
    }
    

    return 0;
}

Revise nuevamente el parámetro args en la etiqueta del nodo bajo el archivo de inicio y genere un punto de coordenadas y una nueva función buffer.lookupTraansform("", "", ros::Time(0.0))

1.5 Comprobar la relación del sistema de coordenadas


  1. Compruebe si tf2_tools está instalado: 'rospack encuentra tf2_tools'
  2. Comando de instalación: 'sudo apt install ros-noetic-tf2-tools'
  3. roslaunch tf03_tfs tfs_c.lanzamiento
  4. rosrun tf2_tools view_frames.py genera un archivo frame.pdf
  5. evidencia marco.pdf
  6. Efecto:
    inserte la descripción de la imagen aquí

1.6 Funcionamiento práctico de la transformación de coordenadas TF

1.7 TF2 y TF

1.8 Resumen

Supongo que te gusta

Origin blog.csdn.net/TianHW103/article/details/127856536
Recomendado
Clasificación