程序员经验分享-hwhale

ROS1学习笔记：tf坐标系广播与监听的编程实现（ubuntu20.04）

2023-05-16

参考B站古月居ROS入门21讲：tf坐标系广播与监听的编程实现
基于VMware Ubuntu 20.04 Noetic版本的环境

文章目录

  • 一、创建功能包
  • 二、创建代码
    • 2.1 以C++为例
      • 2.1.1 配置代码编译规则
      • 2.1.2 编译整个工作空间
      • 2.1.2 配置环境变量
      • 2.1.4 执行代码
    • 2.2 以Python为例
      • 2.2.1 配置代码编译规则
      • 2.2.2 编译整个工作空间
      • 2.2.3 配置环境变量
      • 2.2.4 执行代码

一、创建功能包

我们在src目录下创建learning_tf功能包：

cd ~/catkin_ws/src
catkin_create_pkg learning_tf roscpp rospy tf turtlesim

二、创建代码

如何实现一个TF广播器？

  • 定义TF广播器（TransformBroadcaster）
  • 创建坐标变换值
  • 发布坐标变换（sendTransform）

如何实现一个TF监听器？

  • 定义TF监听器（TransformListener）
  • 查找坐标变换（waitForTransform、lookupTransform）

2.1 以C++为例

turtle_tf_broadcaster.cpp和turtle_tf_listener.cpp拷贝进src目录下，
turtle_tf_broadcaster.cpp代码为：

/***********************************************************************
Copyright 2020 GuYueHome (www.guyuehome.com).
***********************************************************************/

/**
 * 该例程产生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;
};

turtle_tf_listener.cpp代码为：

/***********************************************************************
Copyright 2020 GuYueHome (www.guyuehome.com).
***********************************************************************/

/**
 * 该例程监听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;
};

2.1.1 配置代码编译规则

将以下代码拷贝CMakeLists.txt进指定位置：

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

在这里插入图片描述

2.1.2 编译整个工作空间

cd ~/catkin_ws
catkin_make

2.1.2 配置环境变量

之前已经配置过。

2.1.4 执行代码

roscore

rosrun turtlesim turtlesim_node

我们下面直接在命令行传入参数。
第1个参数：我们在turtle_tf_broadcaster.cpp定义节点时使用了"my_tf_broadcaster"的名字，我们使用__name:=传入新的名字取代"my_tf_broadcaster"，这样避免名字重复（因为ROS中节点名字不能重复），这样就可以重复跑程序了。
第2个参数是turtle名称 turtle1 和 turtle2。

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

执行完后就会有一只小海龟生成并跑向中间的第1只小海龟。
在这里插入图片描述
我们可以用键盘控制小海龟，同样可以让第2只小海龟追着我们跑。
在这里插入图片描述

2.2 以Python为例

将turtle_tf_broadcaster.py和turtle_tf_listener.py拷贝进scripts文件夹下(打开执行权限)。
turtle_tf_broadcaster.py代码为：

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

########################################################################
####          Copyright 2020 GuYueHome (www.guyuehome.com).          ###
########################################################################

# 该例程将请求/show_person服务，服务数据类型learning_service::Person

import roslib
roslib.load_manifest('learning_tf')
import rospy

import tf
import turtlesim.msg

def handle_turtle_pose(msg, turtlename):
    br = tf.TransformBroadcaster()
    br.sendTransform((msg.x, msg.y, 0),
                     tf.transformations.quaternion_from_euler(0, 0, msg.theta),
                     rospy.Time.now(),
                     turtlename,
                     "world")

if __name__ == '__main__':
    rospy.init_node('turtle_tf_broadcaster')
    turtlename = rospy.get_param('~turtle')
    rospy.Subscriber('/%s/pose' % turtlename,
                     turtlesim.msg.Pose,
                     handle_turtle_pose,
                     turtlename)
    rospy.spin()

turtle_tf_listener.py代码为：

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

########################################################################
####          Copyright 2020 GuYueHome (www.guyuehome.com).          ###
########################################################################

# 该例程将请求/show_person服务，服务数据类型learning_service::Person

import roslib
roslib.load_manifest('learning_tf')
import rospy
import math
import tf
import geometry_msgs.msg
import turtlesim.srv

if __name__ == '__main__':
    rospy.init_node('turtle_tf_listener')

    listener = tf.TransformListener()

    rospy.wait_for_service('spawn')
    spawner = rospy.ServiceProxy('spawn', turtlesim.srv.Spawn)
    spawner(4, 2, 0, 'turtle2')

    turtle_vel = rospy.Publisher('turtle2/cmd_vel', geometry_msgs.msg.Twist,queue_size=1)

    rate = rospy.Rate(10.0)
    while not rospy.is_shutdown():
        try:
            (trans,rot) = listener.lookupTransform('/turtle2', '/turtle1', rospy.Time(0))
        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
            continue

        angular = 4 * math.atan2(trans[1], trans[0])
        linear = 0.5 * math.sqrt(trans[0] ** 2 + trans[1] ** 2)
        cmd = geometry_msgs.msg.Twist()
        cmd.linear.x = linear
        cmd.angular.z = angular
        turtle_vel.publish(cmd)

        rate.sleep()

2.2.1 配置代码编译规则

打开CMakeLists.txt，配置编译规则
在这里插入图片描述

2.2.2 编译整个工作空间

cd ~/catkin_ws
catkin_make

2.2.3 配置环境变量

之前已经配置过。

2.2.4 执行代码

roscore

rosrun turtlesim turtlesim_node

rosrun learning_tf turtle_tf_broadcaster.py __name:=turtle1_tf_broadcaster _turtle:=turtle1

rosrun learning_tf turtle_tf_broadcaster.py __name:=turtle2_tf_broadcaster _turtle:=turtle2

rosrun learning_tf turtle_tf_listener.py

rosrun turtlesim turtle_teleop_key

在这里插入图片描述

本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)

ROS1学习笔记：tf坐标系广播与监听的编程实现（ubuntu20.04） 的相关文章

随机推荐

  • 指针加1的含义、指针减法运算

    指针加1的含义 一 p 43 1实例运行结果 二 p 43 1 与 p 43 1实例运行结果 三 p 43 43 实例运行结果 四 指针减法运算实例运行结果 五 void 六 指针用来做什么 xff1f 一 p 43 1 p 43 1 表示

  • VMware从零配置安装CentOS 7

    不跳步图文详细安装教程 一 VMware的下载二 VMware的安装三 CentOS7的下载第一步 xff1a 根据自己电脑操作系统的位数点击选择 xff08 大多数都是64位操作系统 xff09 第二步 xff1a 任意挑选一个镜像源进入

  • VMware虚拟机CentOS 7系统：图形模式与文字命令行模式的切换快捷键

    切换快捷键 图形模式切换到文字命令行模式Ctrl 43 Alt 43 F2 F6 文字命令行模式切换到图形模式Ctrl 43 Alt 43 F1 图形模式切换到文字命令行模式 Ctrl 43 Alt 43 F2 F6 文字命令行模式切换到图

  • Linux 常用快捷键与指令总结（不断更新ing）

    命令快捷键含义Tab命令补全 文件补全Ctrl 43 C停掉错误的指令或参数Ctrl 43 D结束离开文字接口Shift 43 Page Up往前翻页Shift 43 Page Down往后翻页 61 61 61 61 61 61 61 6

  • Pytorch关于卷积核（Conv2d）的简单操作与模型修剪

    在使用Pytorch搭建深度学习算法时 xff0c torch nn Conv2d是用得最多的函数之一 Conv2d函数主要是对输入数据做卷积运算 输入参数如下图 xff1a torch nn Conv2d函数所生成的卷积核主要包括weig

  • 小觅相机运行VINS-Fusion（一）

    写在前面的话 xff1a 1 本文基于自己的另一篇博文win10 43 ubuntu16 04 43 ROS Kinetic 2 参考 4 是一篇非常好的帖子 xff0c 但由于小觅托管在github的代码在不断更新 xff0c 故需在某些

  • 正点原子stm32F407学习笔记3——蜂鸣器实验

    一 硬件设计 蜂鸣器为有源蜂鸣器 xff0c 当 PF 8 输出高电平的时候 xff0c 蜂鸣器将发声 xff0c 当 PF 8 输出低电平的时候 xff0c 蜂鸣器停止发声 xff0c 硬件原理图如下 二 软件设计 1 新建beep c文

  • Ubuntu 安装Cmake

    1 安装Cmake 1 Cmake与makefile xff1a Cmake好处是 1 可以自动化生成makefile xff0c 不需要手动编写 2 跨平台 可生成 native 编译配置文件 在 Linux Unix 平台 生成 mak

  • 什么是字节序？

    字节序 字节序 xff0c 顾名思义 xff0c 就是字节组织的顺序 我们可以将其根据其存储时从低位开始还是从高位开始分为两种 xff0c 具体如下 xff1a 类型简写本质大端BE big endian 将高序字节存储在起始地址小端LE

  • PX4开发基础

    文章目录 一 与无人机的缘分二 开发前三 背景知识硬件PIXHAWKpixhawkV1 xff1a pixhawkV2 xff1a pixhawkV3x xff1a PX4 xff08 推荐 xff09 地面站 xff08 GCS xff0

  • 利用蜂鸣器播放音乐

    我们知道 xff0c 蜂鸣器可分为有源蜂鸣器和无源蜂鸣器 xff0c 有源蜂鸣器内置了频率发生电路 xff0c 因此其通电就能够发出声音 xff0c 但是其频率是固定的 xff0c 而无源蜂鸣器内部没有频率发生电路 xff0c 需要外界给予

  • 51单片机之智能小车（避障、跟随、循迹）

    目录 基本概述 硬件组成 功能 关键字 模块介绍 电机模块L9110S 循迹模块 xff08 TCRT5000传感器 xff09 红外避障模块 测速模块 小车 移动小车 xff08 控制电机转动 xff0c 使小车前进 后退 左转 右转 x

  • PX4开发环境搭建（Ubuntu1804+QGC+Qt Creator ）

    文章目录 前言一 安装环境二 PX4环境搭建1 在虚拟机中安装Ubuntu2 安装PX4环境 1 下载PX4源码 xff1a 2 安装工具链 3 jMAVSim仿真 4 编译验证 三 安装QGC四 安装Qt Creator五 镜像分享 前言

  • ESP8266————AT指令+网络透传

    一 AT指令 ESP8266可以通过串口的AT指令和其他设备进行通讯或控制 刚开始我以为AT指令是ESP8266本就封装好的 xff0c 直接就可以用 xff0c 结果不是 xff0c 需要自己安装AT固件 xff0c 安装好固件之后直接用

  • 修改pip安装路径的方法

    当我们需要安装python的第三方库时 xff0c 通常都是打开cmd输入pip install xxx去安装 但是默认安装路径在C盘 xff0c 极大占用空间 xff0c 看看我的C盘空间 xff0c 已经不足了 xff01 xff01

  • 详细介绍如何在华为云调用SDK的Python代码(以文字识别OCR技术实现身份证识别为例)

    文章目录 1 保存信息2 安装SDK3 生成代码 1 保存信息 首先我们在首页的产品一栏输入OCR xff0c 找到文字识别OCR 然后找到证件类 xff0c 并点击 xff0c 然后点击立即使用 进入到文字识别 控制台 xff0c 找到身

  • 还是搞不懂Anaconda是什么?读这一篇文章就够了

    文章目录 1 Anaconda介绍2 conda介绍3 安装Anaconda4 Anaconda的使用配置Anaconda源 5 创建虚拟环境并使用5 1 创建虚拟环境5 2 查看所有环境5 3 激活环境5 4 安装包5 4 1 conda

  • 安装tensorflow的GPU版本（详细图文教程）--CUDA11.6的安装

    文章目录 TensorFlow简介TensorFlow是什么tensorflow版本变迁tensorflow 2 0 架构 安装过程常用IDE安装python3 9的安装Anaconda的安装 CUDA安装cuda软件安装cuDNN神经网络

  • 解决visual studio C++ build tools时安装包缺失或损坏的问题（亲测绝对有用！！！）

    安装visual studio C 43 43 build tools时遇到安装包缺失或损坏的问题 这个问题真的困扰我好久 xff0c 尝试过很多种办法都失败了 xff0c 我真的很无语 我尝试过卸载掉visual studio重新下载 用

  • ROS1学习笔记：tf坐标系广播与监听的编程实现（ubuntu20.04）

    参考B站古月居ROS入门21讲 xff1a tf坐标系广播与监听的编程实现 基于VMware Ubuntu 20 04 Noetic版本的环境 文章目录 一 创建功能包二 创建代码2 1 以C 43 43 为例2 1 1 配置代码编译规则2

热门标签