[Notas de estudio ROS 13] Transformación de coordenadas TF en ROS

Language 2023-04-09 02:39:30 views: null

[Notas de estudio ROS 13] Transformación de coordenadas TF en ROS

Directorio de artículos

Escrito en el frente, esta serie de notas se refiere al tutorial de AutoLabor, la dirección específica del proyecto está aquí

prefacio

Hay algunas herramientas prácticas integradas en ROS. A través de estas herramientas, una determinada función o programa de depuración se puede realizar de forma cómoda y rápida, a fin de mejorar la eficiencia del desarrollo. Este capítulo presenta principalmente los siguientes componentes integrados en ROS:

  • Transformación de coordenadas TF, realice la conversión entre diferentes tipos de sistemas de coordenadas;
  • rosbag se usa para registrar el proceso de ejecución de los nodos ROS y puede reproducir el proceso;
  • La caja de herramientas rqt integra una variedad de herramientas gráficas de depuración.

Objetivos de aprendizaje que se espera alcanzar en este capítulo:

  • Comprender el concepto y los escenarios de aplicación de la transformación de coordenadas TF;
  • Capaz de completar el caso TF de forma independiente: la pequeña tortuga sigue;
  • La grabación y la reproducción se pueden realizar utilizando el comando o la codificación rosbag;
  • Ser capaz de utilizar hábilmente las herramientas gráficas en rqt.

Demostración del caso: la pequeña tortuga sigue la implementación, este caso es un caso incorporado en ros , ingrese el comando de inicio en la terminal

roslaunch turtle_tf2 turtle_tf2_demo_cpp.launch

roslaunch turtle_tf2 turtle_tf2_demo.launch

El teclado puede controlar el movimiento de una tortuga y la otra sigue el movimiento.

Resultado de ejemplo:

1. Transformación de coordenadas estáticas

La llamada transformación de coordenadas estática significa que la posición relativa entre dos sistemas de coordenadas es fija.

Descripción del requisito:

Hay un modelo de robot existente. La estructura central incluye el cuerpo principal y el radar, cada uno correspondiente a un sistema de coordenadas. El origen del sistema de coordenadas se encuentra en el centro físico del cuerpo principal y el radar, respectivamente. La relación de desplazamiento conocida entre el origen del radar y el origen del cuerpo principal es el siguiente: x 0.2 y0.0 z0.5. El radar actual detecta un obstáculo. Las coordenadas del obstáculo en el sistema de coordenadas del radar son (2.0 3.0 5.0). ¿Cuál es la coordenada del obstáculo en relación con el sujeto?

Demostración de resultados:

Realizar análisis:

  1. La relación relativa del sistema de coordenadas puede ser publicada por el editor
  2. Suscriptor, suscríbase a la relación relativa del sistema de coordenadas publicado, y luego pase la información del punto de coordenadas (puede codificarse de forma rígida), y luego realice la transformación de coordenadas con la ayuda de tf, y emita el resultado

**Proceso de implementación:**C++ es consistente con el proceso de implementación de Python

  1. Cree un nuevo paquete de funciones y agregue dependencias
  2. Escribir la implementación del editor
  3. Escribir la implementación del suscriptor
  4. Ejecutar y ver los resultados

Solución A: implementación de C++

1. Cree un paquete de características

Cree un paquete de características del proyecto que dependa detf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs

2. Editor

/* 
    静态坐标变换发布方:
        发布关于 laser 坐标系的位置信息 

    实现流程:
        1.包含头文件
        2.初始化 ROS 节点
        3.创建静态坐标转换广播器
        4.创建坐标系信息
        5.广播器发布坐标系信息
        6.spin()
*/


// 1.包含头文件
#include "ros/ros.h"
#include "tf2_ros/static_transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"

int main(int argc, char *argv[])
{
    
    
    setlocale(LC_ALL,"");
    // 2.初始化 ROS 节点
    ros::init(argc,argv,"static_brocast");
    // 3.创建静态坐标转换广播器
    tf2_ros::StaticTransformBroadcaster broadcaster;
    // 4.创建坐标系信息
    geometry_msgs::TransformStamped ts;
    //----设置头信息
    ts.header.seq = 100;
    ts.header.stamp = ros::Time::now();
    ts.header.frame_id = "base_link";
    //----设置子级坐标系
    ts.child_frame_id = "laser";
    //----设置子级相对于父级的偏移量
    ts.transform.translation.x = 0.2;
    ts.transform.translation.y = 0.0;
    ts.transform.translation.z = 0.5;
    //----设置四元数:将 欧拉角数据转换成四元数
    tf2::Quaternion qtn;
    qtn.setRPY(0,0,0);
    ts.transform.rotation.x = qtn.getX();
    ts.transform.rotation.y = qtn.getY();
    ts.transform.rotation.z = qtn.getZ();
    ts.transform.rotation.w = qtn.getW();
    // 5.广播器发布坐标系信息
    broadcaster.sendTransform(ts);
    ros::spin();
    return 0;
}

El archivo de configuración se omite aquí.

Efecto de ejemplo:

usar

rviz

Inicie la interfaz gráfica y luego podrá ver los resultados:

3. Suscriptor

/*  
    订阅坐标系信息，生成一个相对于 子级坐标系的坐标点数据，转换成父级坐标系中的坐标点

    实现流程:
        1.包含头文件
        2.初始化 ROS 节点
        3.创建 TF 订阅节点
        4.生成一个坐标点(相对于子级坐标系)
        5.转换坐标点(相对于父级坐标系)
        6.spin()
*/
//1.包含头文件
#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" //注意: 调用 transform 必须包含该头文件

int main(int argc, char *argv[])
{
    
    
    setlocale(LC_ALL,"");
    // 2.初始化 ROS 节点
    ros::init(argc,argv,"tf_sub");
    ros::NodeHandle nh;
    // 3.创建 TF 订阅节点
    tf2_ros::Buffer buffer;
    tf2_ros::TransformListener listener(buffer);

    ros::Rate r(1);
    while (ros::ok())
    {
    
    
    // 4.生成一个坐标点(相对于子级坐标系)
        geometry_msgs::PointStamped point_laser;
        point_laser.header.frame_id = "laser";
        point_laser.header.stamp = ros::Time::now();
        point_laser.point.x = 2.0;
        point_laser.point.y = 3.0;
        point_laser.point.y = 5.0;
    // 5.转换坐标点(相对于父级坐标系)
        //新建一个坐标点，用于接收转换结果  
        //--------------使用 try 语句或休眠，否则可能由于缓存接收延迟而导致坐标转换失败------------------------
        try
        {
    
    
            geometry_msgs::PointStamped point_base;
            point_base = buffer.transform(point_laser,"base_link");
            ROS_INFO("转换后的数据:(%.2f,%.2f,%.2f),参考的坐标系是:%s",point_base.point.x,point_base.point.y,point_base.point.z,point_base.header.frame_id.c_str());

        }
        catch(const std::exception& e)
        {
    
    
            // std::cerr << e.what() << '\n';
            ROS_INFO("程序异常.....");
        }


        r.sleep();  
        ros::spinOnce();
    }


    return 0;
}

El archivo de configuración se omite aquí.

4. Ejecución

Puede usar la línea de comando o el archivo de inicio para iniciar el nodo de publicación y el nodo de suscripción respectivamente.Si no hay anomalías en el programa, la consola generará el resultado de la conversión de coordenadas.

El resultado de la ejecución:

Opción B: implementación de Python

1. Cree un paquete de características

Cree un paquete de características del proyecto que dependa detf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs

2. Editor

#! /usr/bin/env python
"""  
    静态坐标变换发布方:
        发布关于 laser 坐标系的位置信息 
    实现流程:
        1.导包
        2.初始化 ROS 节点
        3.创建 静态坐标广播器
        4.创建并组织被广播的消息
        5.广播器发送消息
        6.spin
"""
# 1.导包
import rospy
import tf2_ros
import tf
from geometry_msgs.msg import TransformStamped

if __name__ == "__main__":
    # 2.初始化 ROS 节点
    rospy.init_node("static_tf_pub_p")
    # 3.创建 静态坐标广播器
    broadcaster = tf2_ros.StaticTransformBroadcaster()
    # 4.创建并组织被广播的消息
    tfs = TransformStamped()
    # --- 头信息
    tfs.header.frame_id = "world"
    tfs.header.stamp = rospy.Time.now()
    tfs.header.seq = 101
    # --- 子坐标系
    tfs.child_frame_id = "radar"
    # --- 坐标系相对信息
    # ------ 偏移量
    tfs.transform.translation.x = 0.2
    tfs.transform.translation.y = 0.0
    tfs.transform.translation.z = 0.5
    # ------ 四元数
    qtn = tf.transformations.quaternion_from_euler(0,0,0)
    tfs.transform.rotation.x = qtn[0]
    tfs.transform.rotation.y = qtn[1]
    tfs.transform.rotation.z = qtn[2]
    tfs.transform.rotation.w = qtn[3]


    # 5.广播器发送消息
    broadcaster.sendTransform(tfs)
    # 6.spin
    rospy.spin()

Aquí se omiten los ajustes de permisos y los archivos de configuración.

Resultado de ejemplo:

3. Suscriptor

#! /usr/bin/env python
"""  
    订阅坐标系信息，生成一个相对于 子级坐标系的坐标点数据，
    转换成父级坐标系中的坐标点

    实现流程:
        1.导包
        2.初始化 ROS 节点
        3.创建 TF 订阅对象
        4.创建一个 radar 坐标系中的坐标点
        5.调研订阅对象的 API 将 4 中的点坐标转换成相对于 world 的坐标
        6.spin

"""
# 1.导包
import rospy
import tf2_ros
# 不要使用 geometry_msgs,需要使用 tf2 内置的消息类型
from tf2_geometry_msgs import PointStamped
# from geometry_msgs.msg import PointStamped

if __name__ == "__main__":
    # 2.初始化 ROS 节点
    rospy.init_node("static_sub_tf_p")
    # 3.创建 TF 订阅对象
    buffer = tf2_ros.Buffer()
    listener = tf2_ros.TransformListener(buffer)

    rate = rospy.Rate(1)
    while not rospy.is_shutdown():    
    # 4.创建一个 radar 坐标系中的坐标点
        point_source = PointStamped()
        point_source.header.frame_id = "radar"
        point_source.header.stamp = rospy.Time.now()
        point_source.point.x = 10
        point_source.point.y = 2
        point_source.point.z = 3

        try:
    #     5.调研订阅对象的 API 将 4 中的点坐标转换成相对于 world 的坐标
            point_target = buffer.transform(point_source,"world")
            rospy.loginfo("转换结果:x = %.2f, y = %.2f, z = %.2f",
                            point_target.point.x,
                            point_target.point.y,
                            point_target.point.z)
        except Exception as e:
            rospy.logerr("异常:%s",e)

    #     6.spin
        rate.sleep()

Aquí se omiten los ajustes de permisos y los archivos de configuración.

PD: en la implementación de Python de tf2, tf2 ha encapsulado algunos tipos de mensajes, y los tipos en geometric_msgs.msg no se pueden usar

4. Ejecución

Puede usar la línea de comando o el archivo de inicio para iniciar el nodo de publicación y el nodo de suscripción respectivamente.Si no hay anomalías en el programa, la consola generará el resultado de la conversión de coordenadas.

Resultado de ejemplo:

Suplemento 1:

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 /baselink /laser

También se recomienda utilizar este método para realizar directamente la liberación de información relativa del sistema de coordenadas estáticas.

Suplemento 2:

Puede usar rviz para mostrar la relación del sistema de coordenadas, la operación específica:

  • Ingrese el comando en la nueva ventana: rviz;
  • Establezca Marco fijo en base_link en el rviz iniciado;
  • Haga clic en el botón Agregar en la esquina inferior izquierda y seleccione el componente TF en la ventana emergente para mostrar la relación de coordenadas.

2. Transformación dinámica de coordenadas

La llamada transformación dinámica de coordenadas significa que la posición relativa entre dos sistemas de coordenadas cambia.

Descripción del requisito:

Inicie turtlesim_node, la ventana en este nodo tiene un sistema de coordenadas mundial (la esquina inferior izquierda es el origen del sistema de coordenadas), y la tortuga es otro sistema de coordenadas. El teclado controla el movimiento de la tortuga y publica dinámicamente las posiciones relativas de los dos sistemas de coordenadas.

Demostración de resultados:

Realizar análisis:

  1. La tortuga en sí no solo puede considerarse un sistema de coordenadas, sino también un punto de coordenadas en el sistema de coordenadas mundial.
  2. Suscríbase a turtle1/pose, puede obtener la coordenada x, la coordenada y, el desplazamiento, la velocidad lineal y la velocidad angular de la tortuga en el sistema de coordenadas mundial
  3. Convierta la información de pose en información relativa al sistema de coordenadas y suéltela

Proceso de implementación: El proceso de implementación de C++ y Python es consistente

  1. Cree un nuevo paquete de funciones y agregue dependencias
  2. Cree un editor de relaciones relativas coordinadas (necesita suscribirse a la información de pose de tortuga al mismo tiempo)
  3. Crear un suscriptor de relación relativa coordinada
  4. implementar

Solución A: implementación de C++

1. Cree un paquete de características

Cree un paquete de características del proyecto que dependa de

 tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs、turtlesim

2. Editor

/*  
    动态的坐标系相对姿态发布(一个坐标系相对于另一个坐标系的相对姿态是不断变动的)

    需求: 启动 turtlesim_node,该节点中窗体有一个世界坐标系(左下角为坐标系原点)，乌龟是另一个坐标系，键盘
    控制乌龟运动，将两个坐标系的相对位置动态发布

    实现分析:
        1.乌龟本身不但可以看作坐标系，也是世界坐标系中的一个坐标点
        2.订阅 turtle1/pose,可以获取乌龟在世界坐标系的 x坐标、y坐标、偏移量以及线速度和角速度
        3.将 pose 信息转换成 坐标系相对信息并发布

    实现流程:
        1.包含头文件
        2.初始化 ROS 节点
        3.创建 ROS 句柄
        4.创建订阅对象
        5.回调函数处理订阅到的数据(实现TF广播)
            5-1.创建 TF 广播器
            5-2.创建 广播的数据(通过 pose 设置)
            5-3.广播器发布数据
        6.spin
*/
// 1.包含头文件
#include "ros/ros.h"
#include "turtlesim/Pose.h"
#include "tf2_ros/transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"

void doPose(const turtlesim::Pose::ConstPtr& pose){
    
    
    //  5-1.创建 TF 广播器
    static tf2_ros::TransformBroadcaster broadcaster;
    //  5-2.创建 广播的数据(通过 pose 设置)
    geometry_msgs::TransformStamped tfs;
    //  |----头设置
    tfs.header.frame_id = "world";
    tfs.header.stamp = ros::Time::now();

    //  |----坐标系 ID
    tfs.child_frame_id = "turtle1";

    //  |----坐标系相对信息设置
    tfs.transform.translation.x = pose->x;
    tfs.transform.translation.y = pose->y;
    tfs.transform.translation.z = 0.0; // 二维实现，pose 中没有z，z 是 0
    //  |--------- 四元数设置
    tf2::Quaternion qtn;
    qtn.setRPY(0,0,pose->theta);
    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-3.广播器发布数据
    broadcaster.sendTransform(tfs);
}

int main(int argc, char *argv[])
{
    
    
    setlocale(LC_ALL,"");
    // 2.初始化 ROS 节点
    ros::init(argc,argv,"dynamic_tf_pub");
    // 3.创建 ROS 句柄
    ros::NodeHandle nh;
    // 4.创建订阅对象
    ros::Subscriber sub = nh.subscribe<turtlesim::Pose>("/turtle1/pose",1000,doPose);
    //     5.回调函数处理订阅到的数据(实现TF广播)
    //        
    // 6.spin
    ros::spin();
    return 0;
}

Darse cuenta del efecto:

El archivo de configuración se omite aquí.

3. Suscriptor

//1.包含头文件
#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" //注意: 调用 transform 必须包含该头文件

int main(int argc, char *argv[])
{
    
    
    setlocale(LC_ALL,"");
    // 2.初始化 ROS 节点
    ros::init(argc,argv,"dynamic_tf_sub");
    ros::NodeHandle nh;
    // 3.创建 TF 订阅节点
    tf2_ros::Buffer buffer;
    tf2_ros::TransformListener listener(buffer);

    ros::Rate r(1);
    while (ros::ok())
    {
    
    
    // 4.生成一个坐标点(相对于子级坐标系)
        geometry_msgs::PointStamped point_laser;
        point_laser.header.frame_id = "turtle1";
        point_laser.header.stamp = ros::Time();
        point_laser.point.x = 1;
        point_laser.point.y = 1;
        point_laser.point.z = 0;
    // 5.转换坐标点(相对于父级坐标系)
        //新建一个坐标点，用于接收转换结果  
        //--------------使用 try 语句或休眠，否则可能由于缓存接收延迟而导致坐标转换失败------------------------
        try
        {
    
    
            geometry_msgs::PointStamped point_base;
            point_base = buffer.transform(point_laser,"world");
            ROS_INFO("坐标点相对于 world 的坐标为:(%.2f,%.2f,%.2f)",point_base.point.x,point_base.point.y,point_base.point.z);

        }
        catch(const std::exception& e)
        {
    
    
            // std::cerr << e.what() << '\n';
            ROS_INFO("程序异常:%s",e.what());
        }


        r.sleep();  
        ros::spinOnce();
    }


    return 0;
}

El archivo de configuración se omite aquí.

4. Ejecución

Puede usar la línea de comando o el archivo de lanzamiento para iniciar el nodo de publicación y el nodo de suscripción respectivamente.Si el programa es normal, el resultado es similar a la demostración.

Puede usar rviz para ver la relación relativa del sistema de coordenadas.

Resultado de ejemplo:

Opción B: implementación de Python

1. Cree un paquete de características

Cree un paquete de características del proyecto que dependa de

 tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs、turtlesim

2. Editor

#! /usr/bin/env python
"""  
    动态的坐标系相对姿态发布(一个坐标系相对于另一个坐标系的相对姿态是不断变动的)

    需求: 启动 turtlesim_node,该节点中窗体有一个世界坐标系(左下角为坐标系原点)，乌龟是另一个坐标系，键盘
    控制乌龟运动，将两个坐标系的相对位置动态发布

    实现分析:
        1.乌龟本身不但可以看作坐标系，也是世界坐标系中的一个坐标点
        2.订阅 turtle1/pose,可以获取乌龟在世界坐标系的 x坐标、y坐标、偏移量以及线速度和角速度
        3.将 pose 信息转换成 坐标系相对信息并发布
    实现流程:
        1.导包
        2.初始化 ROS 节点
        3.订阅 /turtle1/pose 话题消息
        4.回调函数处理
            4-1.创建 TF 广播器
            4-2.创建 广播的数据(通过 pose 设置)
            4-3.广播器发布数据
        5.spin
"""
# 1.导包
import rospy
import tf2_ros
import tf
from turtlesim.msg import Pose
from geometry_msgs.msg import TransformStamped

#     4.回调函数处理
def doPose(pose):
    #         4-1.创建 TF 广播器
    broadcaster = tf2_ros.TransformBroadcaster()
    #         4-2.创建 广播的数据(通过 pose 设置)
    tfs = TransformStamped()
    tfs.header.frame_id = "world"
    tfs.header.stamp = rospy.Time.now()
    tfs.child_frame_id = "turtle1"
    tfs.transform.translation.x = pose.x
    tfs.transform.translation.y = pose.y
    tfs.transform.translation.z = 0.0
    qtn = tf.transformations.quaternion_from_euler(0,0,pose.theta)
    tfs.transform.rotation.x = qtn[0]
    tfs.transform.rotation.y = qtn[1]
    tfs.transform.rotation.z = qtn[2]
    tfs.transform.rotation.w = qtn[3]
    #         4-3.广播器发布数据
    broadcaster.sendTransform(tfs)

if __name__ == "__main__":
    # 2.初始化 ROS 节点
    rospy.init_node("dynamic_tf_pub_p")
    # 3.订阅 /turtle1/pose 话题消息
    sub = rospy.Subscriber("/turtle1/pose",Pose,doPose)
    #     4.回调函数处理
    #         4-1.创建 TF 广播器
    #         4-2.创建 广播的数据(通过 pose 设置)
    #         4-3.广播器发布数据
    #     5.spin
    rospy.spin()

Aquí se omiten los ajustes de permisos y los archivos de configuración.

El efecto del ejemplo es el siguiente:

3. Suscriptor

#! /usr/bin/env python
"""  
    动态的坐标系相对姿态发布(一个坐标系相对于另一个坐标系的相对姿态是不断变动的)

    需求: 启动 turtlesim_node,该节点中窗体有一个世界坐标系(左下角为坐标系原点)，乌龟是另一个坐标系，键盘
    控制乌龟运动，将两个坐标系的相对位置动态发布

    实现分析:
        1.乌龟本身不但可以看作坐标系，也是世界坐标系中的一个坐标点
        2.订阅 turtle1/pose,可以获取乌龟在世界坐标系的 x坐标、y坐标、偏移量以及线速度和角速度
        3.将 pose 信息转换成 坐标系相对信息并发布
    实现流程:
        1.导包
        2.初始化 ROS 节点
        3.创建 TF 订阅对象
        4.处理订阅的数据


"""
# 1.导包
import rospy
import tf2_ros
# 不要使用 geometry_msgs,需要使用 tf2 内置的消息类型
from tf2_geometry_msgs import PointStamped
# from geometry_msgs.msg import PointStamped

if __name__ == "__main__":
    # 2.初始化 ROS 节点
    rospy.init_node("static_sub_tf_p")
    # 3.创建 TF 订阅对象
    buffer = tf2_ros.Buffer()
    listener = tf2_ros.TransformListener(buffer)

    rate = rospy.Rate(1)
    while not rospy.is_shutdown():    
    # 4.创建一个 radar 坐标系中的坐标点
        point_source = PointStamped()
        point_source.header.frame_id = "turtle1"
        point_source.header.stamp = rospy.Time.now()
        point_source.point.x = 10
        point_source.point.y = 2
        point_source.point.z = 3

        try:
    #     5.调研订阅对象的 API 将 4 中的点坐标转换成相对于 world 的坐标
            point_target = buffer.transform(point_source,"world",rospy.Duration(1))
            rospy.loginfo("转换结果:x = %.2f, y = %.2f, z = %.2f",
                            point_target.point.x,
                            point_target.point.y,
                            point_target.point.z)
        except Exception as e:
            rospy.logerr("异常:%s",e)

    #     6.spin
        rate.sleep()

Aquí se omiten los ajustes de permisos y los archivos de configuración.

4. Ejecución

Puede usar la línea de comando o el archivo de lanzamiento para iniciar el nodo de publicación y el nodo de suscripción respectivamente.Si el programa es normal, el resultado es similar a la demostración.

Puede usar rviz para ver la relación relativa del sistema de coordenadas.

Los resultados de ejemplo son los siguientes:

3. Transformación multicoordenada

Descripción del requisito:

El sistema de coordenadas existente, el mundo del sistema de coordenadas principal, tiene dos subsistemas son1, son2, se conoce la relación entre son1 en relación con el mundo y son2 en relación con el mundo, y se conocen las coordenadas del origen de son1 en son2 Se requieren las coordenadas de un punto en son1 para encontrar las coordenadas del punto en son2

Realizar análisis:

  1. Primero, debe publicar los mensajes de coordenadas de son1 en relación con el mundo y son2 en relación con el mundo
  2. Luego, debe suscribirse a las coordenadas para publicar mensajes, sacar los mensajes suscritos y usar tf2 para realizar la conversión entre son1 y son2
  3. Finalmente, es necesario realizar la conversión de puntos de coordenadas

**Proceso de implementación:**C++ es consistente con el proceso de implementación de Python

  1. Cree un nuevo paquete de funciones y agregue dependencias
  2. Cree un editor de relaciones relativas coordinadas (necesita publicar dos relaciones relativas coordinadas)
  3. Crear un suscriptor de relación relativa coordinada
  4. implementar

Solución A: implementación de C++

1. Cree un paquete de características

Crear el paquete de funciones del proyecto depende de tf2, tf2_ros, tf2_geometry_msgs, roscpp rospy std_msgs geometric_msgs, turtlesim

2. Editor

Para mayor comodidad, publique utilizando la transformación de coordenadas estáticas

<launch>
    <node pkg="tf2_ros" type="static_transform_publisher" name="son1" args="5 0 0 0 0 0 /world /son1" output="screen" />
    <node pkg="tf2_ros" type="static_transform_publisher" name="son2" args="3 0 0 0 0 0 /world /son2" output="screen" />
</launch>

Resultado de ejemplo:

3. Suscriptor

/*

需求:
    现有坐标系统，父级坐标系统 world,下有两子级系统 son1，son2，
    son1 相对于 world，以及 son2 相对于 world 的关系是已知的，
    求 son1 与 son2中的坐标关系，又已知在 son1中一点的坐标，要求求出该点在 son2 中的坐标
实现流程:
    1.包含头文件
    2.初始化 ros 节点
    3.创建 ros 句柄
    4.创建 TF 订阅对象
    5.解析订阅信息中获取 son1 坐标系原点在 son2 中的坐标
      解析 son1 中的点相对于 son2 的坐标
    6.spin

*/
//1.包含头文件
#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#include "geometry_msgs/TransformStamped.h"
#include "geometry_msgs/PointStamped.h"

int main(int argc, char *argv[])
{
    
       setlocale(LC_ALL,"");
    // 2.初始化 ros 节点
    ros::init(argc,argv,"sub_frames");
    // 3.创建 ros 句柄
    ros::NodeHandle nh;
    // 4.创建 TF 订阅对象
    tf2_ros::Buffer buffer; 
    tf2_ros::TransformListener listener(buffer);
    // 5.解析订阅信息中获取 son1 坐标系原点在 son2 中的坐标
    ros::Rate r(1);
    while (ros::ok())
    {
    
    
        try
        {
    
    
        //   解析 son1 中的点相对于 son2 的坐标
            geometry_msgs::TransformStamped tfs = buffer.lookupTransform("son2","son1",ros::Time(0));
            ROS_INFO("Son1 相对于 Son2 的坐标关系:父坐标系ID=%s",tfs.header.frame_id.c_str());
            ROS_INFO("Son1 相对于 Son2 的坐标关系:子坐标系ID=%s",tfs.child_frame_id.c_str());
            ROS_INFO("Son1 相对于 Son2 的坐标关系:x=%.2f,y=%.2f,z=%.2f",
                    tfs.transform.translation.x,
                    tfs.transform.translation.y,
                    tfs.transform.translation.z
                    );

            // 坐标点解析
            geometry_msgs::PointStamped ps;
            ps.header.frame_id = "son1";
            ps.header.stamp = ros::Time::now();
            ps.point.x = 1.0;
            ps.point.y = 2.0;
            ps.point.z = 3.0;

            geometry_msgs::PointStamped psAtSon2;
            psAtSon2 = buffer.transform(ps,"son2");
            ROS_INFO("在 Son2 中的坐标:x=%.2f,y=%.2f,z=%.2f",
                    psAtSon2.point.x,
                    psAtSon2.point.y,
                    psAtSon2.point.z
                    );
        }
        catch(const std::exception& e)
        {
    
    
            // std::cerr << e.what() << '\n';
            ROS_INFO("异常信息:%s",e.what());
        }


        r.sleep();
        // 6.spin
        ros::spinOnce();
    }
    return 0;
}

El archivo de configuración se omite aquí.

4. Ejecución

Puede usar la línea de comando o el archivo de inicio para iniciar el nodo de publicación y el nodo de suscripción respectivamente.Si el programa es normal, se generará el resultado convertido.

Resultado de ejemplo:

Opción B: implementación de Python

1. Cree un paquete de características

Crear el paquete de funciones del proyecto depende de tf2, tf2_ros, tf2_geometry_msgs, roscpp rospy std_msgs geometric_msgs, turtlesim

2. Editor

Para mayor comodidad, publique utilizando la transformación de coordenadas estáticas

<launch>
    <node pkg="tf2_ros" type="static_transform_publisher" name="son1" args="5 0 0 0 0 0 /world /son1" output="screen" />
    <node pkg="tf2_ros" type="static_transform_publisher" name="son2" args="3 0 0 0 0 0 /world /son2" output="screen" />
</launch>

3. Suscriptor

#!/usr/bin/env python
"""  
    需求:
        现有坐标系统，父级坐标系统 world,下有两子级系统 son1，son2，
        son1 相对于 world，以及 son2 相对于 world 的关系是已知的，
        求 son1 与 son2中的坐标关系，又已知在 son1中一点的坐标，要求求出该点在 son2 中的坐标
    实现流程:   
        1.导包
        2.初始化 ROS 节点
        3.创建 TF 订阅对象
        4.调用 API 求出 son1 相对于 son2 的坐标关系
        5.创建一依赖于 son1 的坐标点，调用 API 求出该点在 son2 中的坐标
        6.spin

"""
# 1.导包
import rospy
import tf2_ros
from geometry_msgs.msg import TransformStamped
from tf2_geometry_msgs import PointStamped

if __name__ == "__main__":

    # 2.初始化 ROS 节点
    rospy.init_node("frames_sub_p")
    # 3.创建 TF 订阅对象
    buffer = tf2_ros.Buffer()
    listener = tf2_ros.TransformListener(buffer)

    rate = rospy.Rate(1)
    while not rospy.is_shutdown():

        try:
        # 4.调用 API 求出 son1 相对于 son2 的坐标关系
            #lookup_transform(self, target_frame, source_frame, time, timeout=rospy.Duration(0.0)):
            tfs = buffer.lookup_transform("son2","son1",rospy.Time(0))
            rospy.loginfo("son1 与 son2 相对关系:")
            rospy.loginfo("父级坐标系:%s",tfs.header.frame_id)
            rospy.loginfo("子级坐标系:%s",tfs.child_frame_id)
            rospy.loginfo("相对坐标:x=%.2f, y=%.2f, z=%.2f",
                        tfs.transform.translation.x,
                        tfs.transform.translation.y,
                        tfs.transform.translation.z,
            )
        # 5.创建一依赖于 son1 的坐标点，调用 API 求出该点在 son2 中的坐标
            point_source = PointStamped()
            point_source.header.frame_id = "son1"
            point_source.header.stamp = rospy.Time.now()
            point_source.point.x = 1
            point_source.point.y = 1
            point_source.point.z = 1

            point_target = buffer.transform(point_source,"son2",rospy.Duration(0.5))

            rospy.loginfo("point_target 所属的坐标系:%s",point_target.header.frame_id)
            rospy.loginfo("坐标点相对于 son2 的坐标:(%.2f,%.2f,%.2f)",
                        point_target.point.x,
                        point_target.point.y,
                        point_target.point.z
            )

        except Exception as e:
            rospy.logerr("错误提示:%s",e)


        rate.sleep()
    # 6.spin    
    # rospy.spin()

Aquí se omiten los ajustes de permisos y los archivos de configuración.

4. Ejecución

Puede usar la línea de comando o el archivo de inicio para iniciar el nodo de publicación y el nodo de suscripción respectivamente.Si el programa es normal, se generará el resultado convertido.

Resultado de ejemplo:

4. Ver la relación del sistema de coordenadas

En el sistema de robot, hay múltiples sistemas de coordenadas involucrados. Para facilitar la visualización, ros proporciona una herramienta especial que se puede usar para generar un archivo pdf que muestra la relación del sistema de coordenadas, que contiene un mapa del sistema de coordenadas con estructura de árbol.

1. Prepárate

Primero llame rospack find tf2_toolspara verificar si el paquete de características está incluido, si no, use el siguiente comando para instalar:

sudo apt install ros-noetic-tf2-tools

2. uso

Después de iniciar el programa de transmisión del sistema de coordenadas, ejecute el siguiente comando para generar un archivo pdf

rosrun tf2_tools view_frames.py

Se generará un mensaje de registro similar al siguiente:

[INFO] [1592920556.827549]: Listening to tf data during 5 seconds...
[INFO] [1592920561.841536]: Generating graph in frames.pdf file...

Ver el directorio actual generará un archivo frames.pdf

Puede ingresar directamente al directorio para abrir el archivo, o llamar al comando para ver el archivo: evince frames.pdf, para ver el archivo

En el interior como se muestra en la figura:

5. Práctica de transformación de coordenadas TF

Descripción del requisito:

Al comienzo del programa: Se generan dos tortugas, la tortuga del medio (A) y la tortuga inferior izquierda (B), B correrá automáticamente a la posición de A, y cuando se controla el teclado, solo controla el movimiento de A, pero B puede seguir a A para correr

Demostración de resultados:

Realizar análisis:

El núcleo de la implementación de seguimiento de tortugas es que tanto las tortugas A como las B publican información de coordenadas relativa al sistema de coordenadas mundial. Luego, para suscribirse a esta información, debe convertir y obtener la información de A relativa al sistema de coordenadas de B. Finalmente, generar información de velocidad y control B. deportivo.

  1. Inicie el nodo de visualización de tortugas.
  2. Genera una nueva tortuga en el formulario de visualización de tortugas (requiere servicio)
  3. Escriba un nodo para que dos tortugas publiquen información de coordenadas
  4. Escriba un nodo de suscripción para suscribirse a la información de coordenadas y generar una nueva relación relativa para generar información de velocidad

**Proceso de implementación:**C++ es consistente con el proceso de implementación de Python

  1. Cree un nuevo paquete de funciones y agregue dependencias
  2. Escriba un cliente de servicio para generar una nueva tortuga.
  3. Escriba al editor para publicar la información de coordenadas de las dos tortugas.
  4. Escribe un suscriptor, suscríbete a la información de dos tortugas, genera información de velocidad y publícala
  5. correr

Preparación:

1. Aprende a crear una segunda tortuga que no sea controlada por el teclado

Para crear una segunda tortuga, debe usar rosservice y el tema usa spawn

rosservice call /spawn "x: 1.0
y: 1.0
theta: 1.0
name: 'turtle_flow'" 
name: "turtle_flow"

El teclado no puede controlar el movimiento de la segunda tortuga, porque el tema usado: /nombre de la segunda tortuga/cmd_vel, se debe publicar el mensaje del tema correspondiente para controlar el movimiento de la tortuga

2. Aprende a obtener las coordenadas de dos tortugas

se 话题 /乌龟名称/pose obtiene a través

x: 1.0 //x坐标
y: 1.0 //y坐标
theta: -1.21437060833 //角度
linear_velocity: 0.0 //线速度
angular_velocity: 1.0 //角速度

Solución A: implementación de C++

1. Cree un paquete de características

Cree un paquete de características del proyecto que dependa de

tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs、turtlesim

2. Cliente de servicio (generar tortuga)

/* 
    创建第二只小乌龟
 */
#include "ros/ros.h"
#include "turtlesim/Spawn.h"

int main(int argc, char *argv[])
{
    
    

    setlocale(LC_ALL,"");

    //执行初始化
    ros::init(argc,argv,"create_turtle");
    //创建节点
    ros::NodeHandle nh;
    //创建服务客户端
    ros::ServiceClient client = nh.serviceClient<turtlesim::Spawn>("/spawn");

    ros::service::waitForService("/spawn");
    turtlesim::Spawn spawn;
    spawn.request.name = "turtle2";
    spawn.request.x = 1.0;
    spawn.request.y = 2.0;
    spawn.request.theta = 3.12415926;
    bool flag = client.call(spawn);
    if (flag)
    {
    
    
        ROS_INFO("乌龟%s创建成功!",spawn.response.name.c_str());
    }
    else
    {
    
    
        ROS_INFO("乌龟2创建失败!");
    }

    ros::spin();

    return 0;
}

El archivo de configuración se omite aquí.

3. El editor (publica la información de coordenadas de las dos tortugas)

Puede suscribirse a la información de pose de la tortuga y luego convertirla en información de coordenadas. La lógica de implementación de las dos tortugas es la misma, pero el nombre del tema suscrito y la información de coordenadas generada son ligeramente diferentes. La diferencia se puede pasar en a través de parámetros:

  • El nodo debe iniciarse dos veces.
  • Debe pasar el nombre del nodo de tortuga cada vez que lo inicie (la primera vez es tortuga1 y la segunda es tortuga2)
/*  
    该文件实现:需要订阅 turtle1 和 turtle2 的 pose，然后广播相对 world 的坐标系信息

    注意: 订阅的两只 turtle,除了命名空间(turtle1 和 turtle2)不同外,
          其他的话题名称和实现逻辑都是一样的，
          所以我们可以将所需的命名空间通过 args 动态传入

    实现流程:
        1.包含头文件
        2.初始化 ros 节点
        3.解析传入的命名空间
        4.创建 ros 句柄
        5.创建订阅对象
        6.回调函数处理订阅的 pose 信息
            6-1.创建 TF 广播器
            6-2.将 pose 信息转换成 TransFormStamped
            6-3.发布
        7.spin

*/
//1.包含头文件
#include "ros/ros.h"
#include "turtlesim/Pose.h"
#include "tf2_ros/transform_broadcaster.h"
#include "tf2/LinearMath/Quaternion.h"
#include "geometry_msgs/TransformStamped.h"
//保存乌龟名称
std::string turtle_name;


void doPose(const turtlesim::Pose::ConstPtr& pose){
    
    
    //  6-1.创建 TF 广播器 ---------------------------------------- 注意 static
    static tf2_ros::TransformBroadcaster broadcaster;
    //  6-2.将 pose 信息转换成 TransFormStamped
    geometry_msgs::TransformStamped tfs;
    tfs.header.frame_id = "world";
    tfs.header.stamp = ros::Time::now();
    tfs.child_frame_id = turtle_name;
    tfs.transform.translation.x = pose->x;
    tfs.transform.translation.y = pose->y;
    tfs.transform.translation.z = 0.0;
    tf2::Quaternion qtn;
    qtn.setRPY(0,0,pose->theta);
    tfs.transform.rotation.x = qtn.getX();
    tfs.transform.rotation.y = qtn.getY();
    tfs.transform.rotation.z = qtn.getZ();
    tfs.transform.rotation.w = qtn.getW();
    //  6-3.发布
    broadcaster.sendTransform(tfs);

} 

int main(int argc, char *argv[])
{
    
    
    setlocale(LC_ALL,"");
    // 2.初始化 ros 节点
    ros::init(argc,argv,"pub_tf");
    // 3.解析传入的命名空间
    if (argc != 2)
    {
    
    
        ROS_ERROR("请传入正确的参数");
    } else {
    
    
        turtle_name = argv[1];
        ROS_INFO("乌龟 %s 坐标发送启动",turtle_name.c_str());
    }

    // 4.创建 ros 句柄
    ros::NodeHandle nh;
    // 5.创建订阅对象
    ros::Subscriber sub = nh.subscribe<turtlesim::Pose>(turtle_name + "/pose",1000,doPose);
    //     6.回调函数处理订阅的 pose 信息
    //         6-1.创建 TF 广播器
    //         6-2.将 pose 信息转换成 TransFormStamped
    //         6-3.发布
    // 7.spin
    ros::spin();
    return 0;
}

El archivo de configuración se omite aquí.

4. Suscriptor (analizar información de coordenadas y generar información de velocidad)

/*  
    订阅 turtle1 和 turtle2 的 TF 广播信息，查找并转换时间最近的 TF 信息
    将 turtle1 转换成相对 turtle2 的坐标，在计算线速度和角速度并发布

    实现流程:
        1.包含头文件
        2.初始化 ros 节点
        3.创建 ros 句柄
        4.创建 TF 订阅对象
        5.处理订阅到的 TF
        6.spin

*/
//1.包含头文件
#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "geometry_msgs/TransformStamped.h"
#include "geometry_msgs/Twist.h"

int main(int argc, char *argv[])
{
    
    
    setlocale(LC_ALL,"");
    // 2.初始化 ros 节点
    ros::init(argc,argv,"sub_TF");
    // 3.创建 ros 句柄
    ros::NodeHandle nh;
    // 4.创建 TF 订阅对象
    tf2_ros::Buffer buffer;
    tf2_ros::TransformListener listener(buffer);
    // 5.处理订阅到的 TF

    // 需要创建发布 /turtle2/cmd_vel 的 publisher 对象

    ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel",1000);

    ros::Rate rate(10);
    while (ros::ok())
    {
    
    
        try
        {
    
    
            //5-1.先获取 turtle1 相对 turtle2 的坐标信息
            geometry_msgs::TransformStamped tfs = buffer.lookupTransform("turtle2","turtle1",ros::Time(0));

            //5-2.根据坐标信息生成速度信息 -- geometry_msgs/Twist.h
            geometry_msgs::Twist twist;
            twist.linear.x = 0.5 * sqrt(pow(tfs.transform.translation.x,2) + pow(tfs.transform.translation.y,2));
            twist.angular.z = 4 * atan2(tfs.transform.translation.y,tfs.transform.translation.x);

            //5-3.发布速度信息 -- 需要提前创建 publish 对象
            pub.publish(twist);
        }
        catch(const std::exception& e)
        {
    
    
            // std::cerr << e.what() << '\n';
            ROS_INFO("错误提示:%s",e.what());
        }



        rate.sleep();
        // 6.spin
        ros::spinOnce();
    }

    return 0;
}

El archivo de configuración se omite aquí.

5. correr

Use el archivo de inicio para organizar los nodos que deben ejecutarse, el ejemplo de contenido es el siguiente:

<!--
    tf2 实现小乌龟跟随案例
-->
<launch>
    <!-- 启动乌龟节点与键盘控制节点 -->
    <node pkg="turtlesim" type="turtlesim_node" name="turtle1" output="screen" />
    <node pkg="turtlesim" type="turtle_teleop_key" name="key_control" output="screen"/>
    <!-- 启动创建第二只乌龟的节点 -->
    <node pkg="demo_tf2_test" type="Test01_Create_Turtle2" name="turtle2" output="screen" />
    <!-- 启动两个坐标发布节点 -->
    <node pkg="demo_tf2_test" type="Test02_TF2_Caster" name="caster1" output="screen" args="turtle1" />
    <node pkg="demo_tf2_test" type="Test02_TF2_Caster" name="caster2" output="screen" args="turtle2" />
    <!-- 启动坐标转换节点 -->
    <node pkg="demo_tf2_test" type="Test03_TF2_Listener" name="listener" output="screen" />
</launch>

Opción B: implementación de Python

1. Cree un paquete de características

Cree un paquete de características del proyecto que dependa de

tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs、turtlesim

2. Cliente de servicio (generar tortuga)

#! /usr/bin/env python
"""  
    调用 service 服务在窗体指定位置生成一只乌龟
    流程:
        1.导包
        2.初始化 ros 节点
        3.创建服务客户端
        4.等待服务启动
        5.创建请求数据
        6.发送请求并处理响应
"""
#1.导包
import rospy
from turtlesim.srv import Spawn, SpawnRequest, SpawnResponse

if __name__ == "__main__":
    # 2.初始化 ros 节点
    rospy.init_node("turtle_spawn_p")
    # 3.创建服务客户端
    client = rospy.ServiceProxy("/spawn",Spawn)
    # 4.等待服务启动
    client.wait_for_service()
    # 5.创建请求数据
    req = SpawnRequest()
    req.x = 1.0
    req.y = 1.0
    req.theta = 3.14
    req.name = "turtle2"
    # 6.发送请求并处理响应
    try:
        response = client.call(req)
        rospy.loginfo("乌龟创建成功，名字是:%s",response.name)
    except Exception as e:
        rospy.loginfo("服务调用失败....")

Aquí se omiten los ajustes de permisos y los archivos de configuración.

3. El editor (publica la información de coordenadas de las dos tortugas)

#! /usr/bin/env python
"""  
    该文件实现:需要订阅 turtle1 和 turtle2 的 pose，然后广播相对 world 的坐标系信息

    注意: 订阅的两只 turtle,除了命名空间(turtle1 和 turtle2)不同外,
          其他的话题名称和实现逻辑都是一样的，
          所以我们可以将所需的命名空间通过 args 动态传入

    实现流程:
        1.导包
        2.初始化 ros 节点
        3.解析传入的命名空间
        4.创建订阅对象
        5.回调函数处理订阅的 pose 信息
            5-1.创建 TF 广播器
            5-2.将 pose 信息转换成 TransFormStamped
            5-3.发布
        6.spin
"""
# 1.导包
import rospy
import sys
from turtlesim.msg import Pose
from geometry_msgs.msg import TransformStamped
import tf2_ros
import tf_conversions

turtle_name = ""

def doPose(pose):
    # rospy.loginfo("x = %.2f",pose.x)
    #1.创建坐标系广播器
    broadcaster = tf2_ros.TransformBroadcaster()
    #2.将 pose 信息转换成 TransFormStamped
    tfs = TransformStamped()
    tfs.header.frame_id = "world"
    tfs.header.stamp = rospy.Time.now()

    tfs.child_frame_id = turtle_name
    tfs.transform.translation.x = pose.x
    tfs.transform.translation.y = pose.y
    tfs.transform.translation.z = 0.0

    qtn = tf_conversions.transformations.quaternion_from_euler(0, 0, pose.theta)
    tfs.transform.rotation.x = qtn[0]
    tfs.transform.rotation.y = qtn[1]
    tfs.transform.rotation.z = qtn[2]
    tfs.transform.rotation.w = qtn[3]

    #3.广播器发布 tfs
    broadcaster.sendTransform(tfs)


if __name__ == "__main__":
    # 2.初始化 ros 节点
    rospy.init_node("sub_tfs_p")
    # 3.解析传入的命名空间
    rospy.loginfo("-------------------------------%d",len(sys.argv))
    if len(sys.argv) < 2:
        rospy.loginfo("请传入参数:乌龟的命名空间")
    else:
        turtle_name = sys.argv[1]
    rospy.loginfo("///乌龟:%s",turtle_name)

    rospy.Subscriber(turtle_name + "/pose",Pose,doPose)
    #     4.创建订阅对象
    #     5.回调函数处理订阅的 pose 信息
    #         5-1.创建 TF 广播器
    #         5-2.将 pose 信息转换成 TransFormStamped
    #         5-3.发布
    #     6.spin
    rospy.spin()

Aquí se omiten los ajustes de permisos y los archivos de configuración.

4. Suscriptor (analizar información de coordenadas y generar información de velocidad)

#! /usr/bin/env python
"""  
    订阅 turtle1 和 turtle2 的 TF 广播信息，查找并转换时间最近的 TF 信息
    将 turtle1 转换成相对 turtle2 的坐标，在计算线速度和角速度并发布

    实现流程:
        1.导包
        2.初始化 ros 节点
        3.创建 TF 订阅对象
        4.处理订阅到的 TF
            4-1.查找坐标系的相对关系
            4-2.生成速度信息，然后发布
"""
# 1.导包
import rospy
import tf2_ros
from geometry_msgs.msg import TransformStamped, Twist
import math

if __name__ == "__main__":
    # 2.初始化 ros 节点
    rospy.init_node("sub_tfs_p")
    # 3.创建 TF 订阅对象
    buffer = tf2_ros.Buffer()
    listener = tf2_ros.TransformListener(buffer)
    # 4.处理订阅到的 TF
    rate = rospy.Rate(10)
    # 创建速度发布对象
    pub = rospy.Publisher("/turtle2/cmd_vel",Twist,queue_size=1000)
    while not rospy.is_shutdown():

        rate.sleep()
        try:
            #def lookup_transform(self, target_frame, source_frame, time, timeout=rospy.Duration(0.0)):
            trans = buffer.lookup_transform("turtle2","turtle1",rospy.Time(0))
            # rospy.loginfo("相对坐标:(%.2f,%.2f,%.2f)",
            #             trans.transform.translation.x,
            #             trans.transform.translation.y,
            #             trans.transform.translation.z
            #             )   
            # 根据转变后的坐标计算出速度和角速度信息
            twist = Twist()
            # 间距 = x^2 + y^2  然后开方
            twist.linear.x = 0.5 * math.sqrt(math.pow(trans.transform.translation.x,2) + math.pow(trans.transform.translation.y,2))
            twist.angular.z = 4 * math.atan2(trans.transform.translation.y, trans.transform.translation.x)

            pub.publish(twist)

        except Exception as e:
            rospy.logwarn("警告:%s",e)

Aquí se omiten los ajustes de permisos y los archivos de configuración.

5. correr

Use el archivo de inicio para organizar los nodos que deben ejecutarse, el ejemplo de contenido es el siguiente:

<launch>
    <node pkg="turtlesim" type="turtlesim_node" name="turtle1" output="screen" />
    <node pkg="turtlesim" type="turtle_teleop_key" name="key_control" output="screen"/>

    <node pkg="demo06_test_flow_p" type="test01_turtle_spawn_p.py" name="turtle_spawn" output="screen"/>

    <node pkg="demo06_test_flow_p" type="test02_turtle_tf_pub_p.py" name="tf_pub1" args="turtle1" output="screen"/>
    <node pkg="demo06_test_flow_p" type="test02_turtle_tf_pub_p.py" name="tf_pub2" args="turtle2" output="screen"/>
    <node pkg="demo06_test_flow_p" type="test03_turtle_tf_sub_p.py" name="tf_sub" output="screen"/>

</launch>

El efecto que se consigue es el siguiente:

Referencia

http://www.autolabor.com.cn/book/ROSTutorials/di-2-zhang-ros-jia-gou-she-ji/23-fu-wu-tong-xin/224-fu-wu-tong- xin-zi-ding-yi-srv-diao-yong-b-python.html

Supongo que te gusta

Origin blog.csdn.net/qq_44940689/article/details/129381668
Recomendado
Clasificación
Diario
Más
2024-05-20(5)
2024-05-19(0)
2024-05-18(30)
2024-05-17(4)
2024-05-16(22)
2024-05-15(5)
2024-05-14(10)
2024-05-13(7)
2024-05-12(22)
2024-05-11(31)

Code World

© 2018 codetd.com