[ROS] TF 坐标转换，小乌龟跟随案例（调试成功，课件代码）

最新推荐文章于 2024-03-26 23:39:05 发布
最新推荐文章于 2024-03-26 23:39:05 发布
阅读量1.3k 收藏 18
点赞数 7
分类专栏： 智能机器人学习
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/weixin_37834269/article/details/110850479
版权

[ROS] TF 坐标转换，小乌龟跟随案例（调试成功，课件代码）

文章目录

1. 完整代码（cpp）

0. 配置

package.xml

添加 TF 依赖

    <build_depend>tf</build_depend>
    <build_export_depend>tf</build_export_depend>
    <exec_depend>tf</exec_depend>

cmakelist.txt

# 1. 找到 TF 库文件包
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
        roscpp
        rosmsg
        rospy
        tf
        )

# 2. 生成可执行文件
add_executable(${PROJECT_NAME}_demo01_turtle1
        src/demo01_turtle1.cpp)


add_executable(${PROJECT_NAME}_demo01_turtle2
        src/demo01_turtle2.cpp)


# 3. 为可执行文件链接库文件
target_link_libraries(${PROJECT_NAME}_demo01_turtle1
        ${catkin_LIBRARIES}
        )

target_link_libraries(${PROJECT_NAME}_demo01_turtle2
        ${catkin_LIBRARIES}
        )

1. 项目结构

在这里插入图片描述

2. demo01.launch 文件

<launch>

    <!--
     1. 界面上需要显示两只小乌龟
     2. 小乌龟1是怎么运动的：通过键盘操作控制移动
     3. 小乌龟2是怎么运动的：通过publisher发送topic消息控制移动的
     4. 小乌龟2目标位置的确定
     -->

      <!-- 一个界面， 一个乌龟 -->
     <node pkg="turtlesim" type="turtlesim_node" name="turtlesim"></node>

     <!-- 键盘操控小乌龟 -->
     <node pkg="turtlesim" type="turtle_teleop_key" name="turtle_teleop_key"></node>

     <!-- 小乌龟1对应的节点，用来发布实时位置 -->
     <node pkg="demo_tf" type="demo_tf_demo01_turtle1" name="turtle1" output="screen"></node>

     <!-- 显示小乌龟2，用来接收乌龟1位置，并通过 TF 得到实时的与乌龟1的相对位置信息，再计算距离，角度 -->
     <node pkg="demo_tf" type="demo_tf_demo01_turtle2" name="turtle2" output="screen"></node>


</launch>

3. demo01_turtle1.cpp

//获取turtle1的实时位置
//将实时位置上传个TF工具


#include <iostream>
#include <ros/ros.h>
#include <turtlesim/Pose.h>

#include <tf/transform_broadcaster.h>


using namespace std;


void poseCallback(const turtlesim::Pose::ConstPtr &msg, tf::TransformBroadcaster broadcaster);
//void poseCallback(const turtlesim::Pose::ConstPtr &msg);  // test without TF


// tf使用步骤:
// 1. 映入依赖 tf
// 2. 添加头文件
// 3. 同broadcaster.sendTransform来上传

int main(int argc, char **argv) {
    // 初始化节点
    string nodeName = "demo01_turtle1_node";
    ros::init(argc, argv, nodeName);

    ros::NodeHandle node;

    //将实时位置上传个TF工具(broadcaster)
    tf::TransformBroadcaster broadcaster;

    //获取turtle1的实时位置
    string pose_topic_name = "/turtle1/pose";
    const ros::Subscriber &subscriber = node.subscribe<turtlesim::Pose>(
            pose_topic_name,
            1000,
            boost::bind(poseCallback, _1, broadcaster)
            );

//    // test without TF
//    const ros::Subscriber &subscriber = node.subscribe<turtlesim::Pose>(
//            pose_topic_name,
//            1000,
//            poseCallback
//    );

    //阻塞
    ros::spin();

    return 0;
}


/***-------------------- callback function area --------------------***/
void poseCallback(const turtlesim::Pose::ConstPtr &msg, tf::TransformBroadcaster broadcaster) {
//void poseCallback(const turtlesim::Pose::ConstPtr &msg) {
    //获取turtle1的实时位置 给定的坐标是相对于显示面板(世界坐标系)
    float x = msg->x;
    float y = msg->y;
    float theta = msg->theta;

    ROS_INFO_STREAM("turtle_1: " << x << " " << y << " " << theta);

    //实时上传
    // 位置和姿态信息
    tf::Transform input;
    //设置位置 (x, y, z)
    input.setOrigin(tf::Vector3(x, y, 0));
    //设置姿态(以四元素建立姿态描述) （欧拉角（roll, pitch, yaw）） 四元素(x, y, z, w) https://quaternions.online/
    tf::Quaternion quat;
    quat.setRPY(0, 0, theta);
    input.setRotation(quat);

    tf::StampedTransform transform(input, ros::Time::now(), "world", "turtle1");
    broadcaster.sendTransform(transform);
}

4. demo02_turtle2.cpp


// 小乌龟2

#include <iostream>

#include <ros/ros.h>
#include <turtlesim/Pose.h>
#include <turtlesim/Spawn.h>
#include <geometry_msgs/Twist.h>

#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>

using namespace std;


void poseCallback(const turtlesim::Pose::ConstPtr &msg, tf::TransformBroadcaster broadcaster);


int main(int argc, char **argv) {
   // 初始化节点
   string nodeName = "demo01_turtle2";
   ros::init(argc, argv, nodeName);

   ros::NodeHandle node;


   ros::AsyncSpinner spinner(1);
   spinner.start();


    产生小乌龟2 
   //将小乌龟2显示到界面上去
   //通过 service client通讯创建小乌龟
   string service_name = "/spawn";
   ros::ServiceClient client = node.serviceClient<turtlesim::Spawn>(service_name);

   //等待启动
   client.waitForExistence();

   turtlesim::Spawn service;
   service.request.x = 1.0;
   service.request.y = 1.0;
   service.request.theta = 0;
   service.request.name = "turtle2";
   client.call(service);


    获取小乌龟的位置 
   //将实时位置上传个TF工具(broadcaster)
   tf::TransformBroadcaster broadcaster;

   //获取turtle1的实时位置
   string pose_topic_name = "/turtle2/pose";
   const ros::Subscriber &subscriber = node.subscribe<turtlesim::Pose>(
           pose_topic_name,
           1000,
           boost::bind(poseCallback, _1, broadcaster)
   );

//    // test without TF
//    const ros::Subscriber &subscriber = node.subscribe(
//            pose_topic_name,
//            1000,
//            poseCallback
//    );


    发布速度 
   // publisher控制小乌龟动
   string vel_topic_name = "/turtle2/cmd_vel";
   ros::Publisher vel_publisher = node.advertise<geometry_msgs::Twist>(vel_topic_name, 1000);

   //tf的监听工具，可以帮助我们实时的拿到相对位置信息
   tf::TransformListener listener;

   ros::Rate rate(10);
   while (ros::ok()) {
       //实时查看turtle1 在 turtle2的坐标系中的位置

       // target_frame: 坐标系，参考坐标系
       // source_frame: 求解的坐标系
       // 想知道 source_frame在 target_frame的位置信息
       // time: 0获得最近我和你的相对位置

       tf::StampedTransform transform;

       try {
           listener.lookupTransform("turtle2", "turtle1", ros::Time(0), transform);
       } catch (exception e) {
           //不执行
           continue;
       }

       //获得 位置
       tf::Vector3 &vector3 = transform.getOrigin();
       double x = vector3.x();
       double y = vector3.y();
       double z = vector3.z();

       ROS_INFO_STREAM("xyz(" << x << ", " << y << ", " << z << ")");

       double distance = sqrt(pow(x, 2) + pow(y, 2));
       double angular = atan2(y, x);

       // 之前的pid是做匀速控制的pid

       // time值不好确定
       // kp系数 t又不确定 kp/t 当成一种系数 k (常量)

       // vel = distance / t
       // vel = k * distance  - 常量

       // distance / t;
       // angular / t;

       geometry_msgs::Twist twist;
       twist.linear.x = 0.6 * distance;
       twist.angular.z = 6 * angular;
       vel_publisher.publish(twist);

       //获得 姿态
       // const tf::Quaternion &quaternion = transform.getRotation();


       //知道位置后，距离就好计算了

       rate.sleep();
   }


   //阻塞
   ros::waitForShutdown();
//    ros::spin();

   return 0;
}


/***-------------------- callback funciton area --------------------***/

void poseCallback(const turtlesim::Pose::ConstPtr &msg, tf::TransformBroadcaster broadcaster) {
//void poseCallback(const turtlesim::Pose::ConstPtr &msg) {
   //获取turtle2的实时位置 给定的坐标是相对于显示面板(世界坐标系)
   float x = msg->x;
   float y = msg->y;
   float theta = msg->theta;

   ROS_INFO_STREAM("turtle_2: " << x << " " << y << " " << theta);

   //实时上传
   // 位置和姿态信息
   tf::Transform input;
   //设置位置 (x, y, z)
   input.setOrigin(tf::Vector3(x, y, 0));
   //设置姿态(以四元素建立姿态描述) （欧拉角（roll, pitch, yaw）） 四元素(x, y, z, w) https://quaternions.online/
   tf::Quaternion quat;
   quat.setRPY(0, 0, theta);
   input.setRotation(quat);

   tf::StampedTransform transform(input, ros::Time::now(), "world", "turtle2");
   broadcaster.sendTransform(transform);
}

2. 调试验证

在这里插入图片描述

3.完整代码（python）

1. 项目结构

在这里插入图片描述

2. py01.launch

<launch>

    <!--
    python版本的
     1. 界面上需要显示两只小乌龟
     2. 小乌龟1是怎么运动的：通过键盘操作控制移动
     3. 小乌龟2是怎么运动的：通过publisher发送topic消息控制移动的
     4. 小乌龟2目标位置的确定
     -->

      <!-- 一个界面， 一个乌龟 -->
     <node pkg="turtlesim" type="turtlesim_node" name="turtlesim"></node>

     <!-- 键盘操控小乌龟 -->
     <node pkg="turtlesim" type="turtle_teleop_key" name="turtle_teleop_key"></node>

     <!-- 小乌龟1对应的节点 -->
     <node pkg="demo_tf" type="demo01_turtle1.py" name="turtle1" output="screen"></node>

     <!-- 显示小乌龟2 -->
     <node pkg="demo_tf" type="demo01_turtle2.py" name="turtle2" output="screen"></node>


    <!-- rviz可视化调试节点 -->
    <!-- 路径必须写绝对路径 -->
    <node pkg="rviz"
          type="rviz"
          name="rviz"
          args="-d $(find demo_tf)/rviz/demo01.rviz">
    </node>

</launch>

3. demo01_turtle1.py

#! /usr/bin/env python
# coding: utf-8

import rospy
from turtlesim.msg import Pose
from tf.broadcaster import TransformBroadcaster
from tf.transformations import quaternion_from_euler


def pose_callback(msg):
    if not isinstance(msg, Pose):
        return

    x = msg.x
    y = msg.y
    # 小乌龟Z轴转动的角度, roll: x   pitch: y    yaw:z
    theta = msg.theta

    # 实时发布位置信息到TF工具
    # 位置
    translation = (x, y, 0)
    # 姿态 tf工具是用四元素来描述姿态信息 将 rpy欧拉角描述方式转换为 四元素描述方式
    rotation = quaternion_from_euler(0, 0, theta)

    broadcaster.sendTransform(translation, rotation, rospy.Time().now(), "turtle1", "world")


if __name__ == '__main__':
    # 创建node
    node_name = "demo01_turtle1"
    rospy.init_node(node_name)

    # 实时发布小乌龟的坐标位置信息
    broadcaster = TransformBroadcaster()

    # 订阅小乌龟1的位置信息
    pose_topic_name = "/turtle1/pose"
    rospy.Subscriber(pose_topic_name, Pose, pose_callback)

    rospy.spin()

4. demo01_turtle2.py

#! /usr/bin/env python
# coding: utf-8

import rospy

from turtlesim.msg import Pose
from tf.broadcaster import TransformBroadcaster
from tf.transformations import quaternion_from_euler

from turtlesim.srv import Spawn, SpawnRequest, SpawnResponse
from geometry_msgs.msg import Twist

from tf.listener import TransformListener
from math import pow, sqrt, atan2


def pose_callback(msg):
    if not isinstance(msg, Pose):
        return

    x = msg.x
    y = msg.y
    # 小乌龟Z轴转动的角度, roll: x   pitch: y    yaw:z
    theta = msg.theta

    # 实时发布位置信息到TF工具
    # 位置
    translation = (x, y, 0)
    # 姿态 tf工具是用四元素来描述姿态信息 将 rpy欧拉角描述方式转换为 四元素描述方式
    rotation = quaternion_from_euler(0, 0, theta)

    broadcaster.sendTransform(translation, rotation, rospy.Time().now(), "turtle2", "world")


if __name__ == '__main__':
    # 创建node
    node_name = "demo01_turtle2"
    rospy.init_node(node_name)

    # 产生小乌龟2
    service_name = "/spawn"
    client = rospy.ServiceProxy(service_name, Spawn)
    client.wait_for_service()

    request = SpawnRequest()
    request.x = 1.0
    request.y = 1.0
    request.theta = 0
    request.name = "turtle2"
    client.call(request)

    # 实时发布小乌龟的坐标位置信息
    broadcaster = TransformBroadcaster()

    # 订阅小乌龟2的位置信息
    pose_topic_name = "/turtle2/pose"
    rospy.Subscriber(pose_topic_name, Pose, pose_callback)

    # 发布速度，控制位置
    vel_topic_name = "/turtle2/cmd_vel"
    publisher = rospy.Publisher(vel_topic_name, Twist, queue_size=1000)

    # 获取相对位置信息的listener
    listener = TransformListener()

    rate = rospy.Rate(10)
    while not rospy.is_shutdown():
        # 实时查看turtle1 在 turtle2的坐标系中的位置
        #
        # target_frame: 坐标系，参考坐标系
        # source_frame: 求解的坐标系
        # 想知道 source_frame在 target_frame的位置信息
        # time: 0获得最近我和你的相对位置

        # 使用 try - catch 防止启动时没有数据导致的异常
        try:
            transform = listener.lookupTransform("turtle2", "turtle1", rospy.Time())
        except:
            rate.sleep()
            continue

        # 获取位置
        x, y, z = transform[0]
        # # 获取姿态 (四元素)
        # quat = transform[1]

        distance = sqrt(pow(x, 2) + pow(y, 2))
        angular = atan2(y, x)

        twist = Twist()
        twist.linear.x = 0.6 * distance
        twist.angular.z = 6 * angular
        publisher.publish(twist)

        rate.sleep()

    rospy.spin()

4. 调试验证

在这里插入图片描述

*. 参考

*. 问题解决

*. rough 知识点

在这里插入图片描述
在这里插入图片描述

创建小乌龟1，使用 turtlesim + 键盘控制节点，通过 launch 文件启动
创建小乌龟2，使用 api 写入 turtlesim，通过 turtlesim 的 service 通信 /spawn

在这里插入图片描述

可以看到 turtulesim 节点中有两个小乌龟订阅 turtle1 跟 bbb

在这里插入图片描述
在这里插入图片描述

小乌龟2运动，publisher控制小乌龟运动
在这里插入图片描述

小乌龟2目标位置确定
在这里插入图片描述
在这里插入图片描述
turtle1 实时上传
创建源码文件，实时获取 turtle1 当前位置，使用 topic 通信

在这里插入图片描述
在这里插入图片描述
在这里插入图片描述

turtle1 集成 TF 功能
添加依赖，package.xml，cmakelist 添加 tf 包

在这里插入图片描述
在这里插入图片描述
在这里插入图片描述

4元素

在这里插入图片描述
在这里插入图片描述
在这里插入图片描述

turtle2 获取位置实时上传（与 turtle1 代码几乎相同）
产看关系信息：rosrun rqt_tf_tree rqt_tf_tree

在这里插入图片描述
在这里插入图片描述
在这里插入图片描述
在这里插入图片描述

turtle2 中位置在主线程中接收，对于回调函数
主线程卡了3秒，接收不到
在这里插入图片描述
在这里插入图片描述
创建一个异步 spinner 用于在子线程接收数据

在这里插入图片描述

报错，主线程中的 ros::spin 与 spinner 冲突

在这里插入图片描述
在这里插入图片描述
在这里插入图片描述

turtle2 查看相对位置信息
实时查看 turtle1 在 turtle2 的坐标系中的位置

在这里插入图片描述
在这里插入图片描述
在这里插入图片描述

turtle2 代码做频率操作，降低频率

在这里插入图片描述
在这里插入图片描述

turtle2 速度计算与发布

在这里插入图片描述

飞天牛牛
关注
  • 7
    点赞
  • 踩
  • 18
    收藏
    觉得还不错? 一键收藏
  • 1
    评论
专栏目录
【从零开始学习SLAM】分解ros小海龟跟随代码
小秋 AI SLAM 入门实战教程
12-15 2193
视频教程请关注同名订阅号和哔哩哔哩
ROS学习笔记之小乌龟跟随
Bin_0902的博客
08-14 2584
欢迎理性讨论，共同进步
ROS学习（十四）--TF坐标变换
最新发布
2201_75563228的博客
03-26 1375
TF（Transform）坐标变换是一种在机器人学和计算机视觉领域常用的技术，用于在不同坐标系之间进行转换和变换操作。TF是ROS（Robot Operating System）中的一个模块，提供了强大的坐标变换功能。在TF中，坐标变换是通过维护一个坐标变换树（Coordinate Frame Tree）来实现的。坐标变换树是由多个坐标系组成的层次结构，每个坐标系都有一个唯一的名称，称为Frame ID。树的根节点是一个固定的全局参考坐标系，通常称为"world"或"map"
Ros入门学习003——小乌龟跟随
l434503227的博客
10-01 640
小乌龟跟随实验
【ROS入门】小乌龟跟随示例
博客标题不能为空我也没办法
09-17 2712
ROS小乌龟跟随示例
ROS TF坐标变化，小乌龟跟随案例
ck784101777的博客
06-02 6349
ROS TF的基础，四元数、欧拉角理论见该篇文章，讲解非常详细 https://blog.csdn.net/zhanghm1995/article/details/84644984 1.程序说明 本实例创建两个乌龟，乌龟1乌龟2，控制乌龟1随意移动，保证乌龟2实时跟随乌龟1。想要达到这个效果，程序设计思路为： 1）我们需要获取乌龟1的位姿信息，本程序只考虑x,y，以及欧拉角的偏航角（Yaw），将乌龟1的位姿变换到世界坐标系（tf变换），并发布乌龟1的位姿信息 2）有了乌龟1的位姿信息，我们还需要.
ROS学习-tf学习-简单例程代码
04-20
ROS-tf学习的简单例程代码，具体教程见：https://blog.csdn.net/qq_39779233/article/details/105638335
ros机械臂仿真，课件和代码
12-20
ros机械臂仿真，课件和代码。包括机械臂的各个知识点，从机械臂的轨迹规划，到视觉感知都包括，提供了仿真代码，不需要实际的机械臂，也可以操作仿真机械臂。
ROS控制小乌龟程序
12-11
文章：ROS：编写节点，让ROS小乌龟画圆和矩形 作者：ID：IMBA_09
learning_ros_tf.zip
04-09
ros_tf示例代码，learning_ros_tf学习代码，有注释
ROS机器人系统学习理论与实践（完整版课件）
07-05
内容包含如下： 机器人SLAM与自主导航 课件 ...ROS理论与实践 8.ROS机器人综合应用 课件 ROS理论与实践 9.ROS2.0 课件 ROS基础 该课件通俗易懂，适合新手自学研究或教师上课使用。 课件配套语言：python或c++
ros小海龟定点移动（程序）
10-06
此文件包为控制ros中小海龟的定点移动，其中有两个程序，大同小异。一个需要输入目标点的xy坐标，一个是在程序中固定了xy的坐标。具体情感博客：note-ros-控制小海龟到达指定点 （https://blog.csdn.net/qq_33168256/article/details/82950222）
ROS tf变换之小龟跟随
Young的博客
07-06 2595
广播turtle1和turtle2相对于世界坐标系的坐标变换，收听turtle1和turtle2之间的坐标变换，然后用键盘控制turtle1运动，turtle2跟随turtle1运动。 １、turtle_tf_broadcaster.cpp #include <ros/ros.h> #include <tf/transform_broadcaster.h> #incl...
ROS -- 坐标变换实操--两只乌龟坐标变换
ping96的博客
03-23 1279
ROS -- 坐标变换实操--两只乌龟坐标变换
ROS TF 变换实现小乌龟跟随
m0_46259216的博客
02-21 3020
实现分析： 核心是生成两只乌龟（采用服务的形式在窗体中生成一只新的乌龟），乌龟A和乌龟B都要发布相对于世界坐标系的坐标信息，然后订阅到该消息转换获取A相对于B坐标系的信息，最后生成速度信息，发布并控制B运动。 创建功能包 相关依赖：tf2、tf2_ros、tf2_geometry_msgs、roscpp、rospy、std_msgs、geometry_msgs、turtlesim。（tf2、tf2_ros、tf2_geometry_msgs是与tf2坐标变换有关的依赖） mkdir -p turt
ROS备忘录(3)-ROS QT界面开发之控制小乌龟运动
hexuyangguang的博客
05-20 1210
本文基于之前搭建 /turtle1/cmd_vel为话题，geometry_msgs/Twist 为数据类型。linear为线速度，angular为角速度，单位是m/s 和 rad/s. 线速度linear，x是前后，y是左右，z是上下。角速度angular，x是沿X轴逆时针转动，y是沿y轴逆时针转动，z是沿Z轴逆时针转动。 感谢 ROS小乌龟turtlesim详解 ...
ROS学习记录（二）玩转海龟——海龟保姆级教程
weixin_54729896的博客
12-30 3938
环境变量的设置，海龟的系列实验（画圆、跟随）
服务调用--生成2只乌龟
ping96的博客
03-11 2645
服务调用相关
ros多个小乌龟_ROS 机器人技术 - 添加一个 TF 帧！
weixin_39883194的博客
01-02 188
在实际的机器人中往往有很多个传感器，比如我们组目前用的小车上就有相机，雷达，IMU 等，为了能够在 TF 系统中找到传感器之间的转换，就需要把每个传感器的坐标系加到系统的 TF 树中，方法很简单，下面一起来学习下。一、TF 树的注意事项在实际使用和调试 TF 的时候一定要时刻记住：TF 树中的一个节点可以有多个子节点，但是只能有一个父节点，并且 TF 树中不能出现回环！一个典型的 TF 树如下：这...
ros小乌龟跟随不用TF
11-27
根据提供的引用内容，似乎没有提到如何不使用TF实现ROS小乌龟的跟随。一般情况下，使用TF是实现小乌龟跟随的常见方法。TF是ROS中的一个重要功能包，用于维护坐标系之间的转换关系。通过TF，我们可以轻松地获取小乌龟之间的相对位置和姿态信息，从而实现小乌龟的跟随。如果不使用TF，可能需要手动计算小乌龟之间的相对位置和姿态信息，实现起来比较麻烦。

“相关推荐”对你有帮助么？

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 1
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值