include文件:

#include "tf/transform_broadcaster.h"
#include "tf/transform_listener.h"
#include "tf/message_filter.h"
#include "tf/tf.h"
#include "message_filters/subscriber.h"

某时刻机器人在地图上的位置:

当机器人在移动过程中,tf会不断接收 base_link->odom 的位置关系信息,这些信息是根据时间不断变化并被记录下来的。当其它节点需要获取某个时间点上的 base_link的位置时就可以通过下面的方法查询:

x, y, yaw 就是base_link 在t 时刻在地图上的位置:

bool getOdomPose(tf::Stamped<tf::Pose>& odom_pose,
double& x, double& y, double& yaw,
const ros::Time& t, const std::string& base_link)
{
// Get the robot's pose
tf::Stamped<tf::Pose> ident (tf::Transform(tf::createIdentityQuaternion(),
tf::Vector3(0,0,0)), t, base_link );
try
{
tf_ = new tf::TransformListener();
tf_->transformPose(odom_frame_id_, ident, odom_pose);
}
catch(tf::TransformException e)
{
ROS_WARN("Failed to compute odom pose, skipping scan (%s)", e.what());
return false;
}
x = odom_pose.getOrigin().x();
y = odom_pose.getOrigin().y();
double pitch,roll;
odom_pose.getBasis().getEulerYPR(yaw, pitch, roll); return true;
}

  

机器人某个位置相对map的位置关系:
机器人是矩形,四个角儿相对中心的位置已知,获取四个角相对map的位置

tf::Stamped<tf::Pose> corner1(
tf::Pose(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.30, -0.45, 0.0)),
ros::Time(0), "base_link");
tf::Stamped<tf::Pose> corner2(
tf::Pose(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.30, 0.45, 0.0)),
ros::Time(0), "base_link");
tf::Stamped<tf::Pose> corner3(
tf::Pose(tf::Quaternion(0, 0, 0, 1), tf::Vector3(-0.30, -0.45, 0.0)),
ros::Time(0), "base_link");
tf::Stamped<tf::Pose> corner4(
tf::Pose(tf::Quaternion(0, 0, 0, 1), tf::Vector3(-0.30, 0.45, 0.0)),
ros::Time(0), "base_link");
transform_listener = new tf::TransformListener();
tf::Stamped<tf::Pose> transformed_corner_1;
transform_listener.transformPose("map", corner_1, transformed_corner_1);
tf::Stamped<tf::Pose> transformed_corner_2;
transform_listener.transformPose("map", corner_2, transformed_corner_2);
tf::Stamped<tf::Pose> transformed_corner_3;
transform_listener.transformPose("map", corner_3, transformed_corner_3);
tf::Stamped<tf::Pose> transformed_corner_1;
transform_listener.transformPose("map", corner_4, transformed_corner_4);

  

随机器人移动点在t+1时刻的位置

已知 t 时刻的位置是 pose_old,求t+1 时刻的位置 pose_new

tf_ = new tf::TransformListener();
tf::StampedTransform tx_odom;
try
{
tf_->lookupTransform(base_frame_id_, ros::Time::now(),
base_frame_id_, msg.header.stamp,
global_frame_id_, tx_odom);
}
catch(tf::TransformException e)
{
ROS_WARN("Failed to transform initial pose in time (%s)", e.what());
tx_odom.setIdentity();
} tf::Pose pose_old, pose_new;
tf::poseMsgToTF(msg.pose.pose, pose_old);
pose_new = tx_odom.inverse() * pose_old; // Transform into the global frame
ROS_INFO("Setting pose (%.6f): %.3f %.3f %.3f",
ros::Time::now().toSec(),
pose_new.getOrigin().x(),
pose_new.getOrigin().y(),
getYaw(pose_new));

这里认为global_frame_id是不动的,pose_old和pose_new都是在global_frame_id坐标系下的坐标。但是pose_old描述的物体是随着base_frame_id同步移动的

关于fixed frame的解释:2.3 Transforms in Time

相对角度的转换Quaternion

当base_link代表机器人时,激光扫描仪laser_scan安装的角度与base_link不平行,即激光数据的零度不对应机器人的正前方零度。已知 laser_scan->angle_min  和 laser_scan->angle_increment 为激光数据信息,转换角度到base_link的位置代码如下,该算法可以考虑到激光器上下颠倒安装的情况导致angle_increment为负:

tf::Quaternion q;
q.setRPY(0.0, 0.0, laser_scan->angle_min);
tf::Stamped<tf::Quaternion> min_q(q, laser_scan->header.stamp,
laser_scan->header.frame_id);
q.setRPY(0.0, 0.0, laser_scan->angle_min + laser_scan->angle_increment);
tf::Stamped<tf::Quaternion> inc_q(q, laser_scan->header.stamp,
laser_scan->header.frame_id);
try
{
tf_->transformQuaternion(base_frame_id_, min_q, min_q);
tf_->transformQuaternion(base_frame_id_, inc_q, inc_q);
}
catch(tf::TransformException& e)
{
ROS_WARN("Unable to transform min/max laser angles into base frame: %s",
e.what());
return;
} double angle_min = tf::getYaw(min_q);
double angle_increment = tf::getYaw(inc_q) - angle_min; //考虑到了激光器上下颠倒安装的情况导致为负数

 

已知 W->B 和B->A的坐标转换,求W->A的坐标转换

ROS 主动蒙特卡罗粒子滤波定位算法 AMCL 解析-- map与odom坐标转换的方法

有时间差的lookupTransform

ros上的详细教程

turtle1和turtle2都是 world 的child frame. turtle1->world 和turtle2->world 的tf都不断发布的,现在需要知道这样的一个transform转换关系:

5秒中之前turtle1相对与现在的turtle2的位置关系

try{
ros::Time now = ros::Time::now();
ros::Time past = now - ros::Duration(5.0);
listener.waitForTransform("/turtle2", now,
"/turtle1", past,
"/world", ros::Duration(1.0));
listener.lookupTransform("/turtle2", now,
"/turtle1", past,
"/world", transform);

得到的转换结果可以这样理解, ( transform.getOrigin().x(), transform.getOrigin().y() ) 是以turtle2为原点的XY平面上turtle1的坐标。
转载:https://blog.csdn.net/crazyquhezheng/article/details/49124115

ROS代码经验系列-- tf进行位置查询变换的更多相关文章

  1. SLAM+语音机器人DIY系列:(二)ROS入门——7.理解tf的原理

    摘要 ROS机器人操作系统在机器人应用领域很流行,依托代码开源和模块间协作等特性,给机器人开发者带来了很大的方便.我们的机器人“miiboo”中的大部分程序也采用ROS进行开发,所以本文就重点对ROS ...

  2. solr与.net系列课程(四)solr查询参数的讲解与.net如何获取solr数据

    solr与.net系列课程(四)solr查询参数的讲解与.net如何获取solr数据 上一节我们完成了solr连接数据库,细心的朋友会发现一个问题,就是solr其实和语言没有任何关系,配置完成后任何语 ...

  3. 【SqlServer系列】表单查询

    1   概述 如下几个问题,如果你能解决,请继续往下看,若不能解决,请先复习SQL基础知识,再来阅读本篇文章.本篇文章深度中等左右. Q1:表StudentScores如下,用一条SQL语句查询出每门 ...

  4. ROS与Matlab系列:一个简单的运动控制

    ROS与Matlab系列:一个简单的运动控制 转自:http://blog.exbot.net/archives/2594 Matlab拥有强大的数据处理.可视化绘图能力以及众多成熟的算法函数,非常适 ...

  5. (原创)高DPI适配经验系列:(四)高DPI适配示例

    一.前言 光说不练假把式. 原理说再多,也不如一个例子直观明了.所以本篇文章就来通过一个例子演示一下高DPI适配的流程. 相信看完的你,一定会有所收获! 本文地址:https://www.cnblog ...

  6. 【百度地图API】建立全国银行位置查询系统(五)——如何更改百度地图的信息窗口内容?

    原文:[百度地图API]建立全国银行位置查询系统(五)--如何更改百度地图的信息窗口内容? 摘要: 酷讯.搜房.去哪儿网等大型房产.旅游酒店网站,用的是百度的数据库,却显示了自定义的信息窗口内容,这是 ...

  7. 【百度地图API】建立全国银行位置查询系统(四)——如何利用百度地图的数据生成自己的标注

    原文:[百度地图API]建立全国银行位置查询系统(四)--如何利用百度地图的数据生成自己的标注 摘要: 上一章留个悬念,"如果自己没有地理坐标的数据库,应该怎样制作银行的分布地图呢?&quo ...

  8. 【百度地图API】建立全国银行位置查询系统(三)——如何在地图上添加银行标注

    原文:[百度地图API]建立全国银行位置查询系统(三)--如何在地图上添加银行标注 <摘要>你将在第三章中学会以下知识: 如何在地图上添加带银行logo的标注?(你也可以换成商场logo, ...

  9. 【百度地图API】建立全国银行位置查询系统(二)——怎样为地图添加控件

    原文:[百度地图API]建立全国银行位置查询系统(二)--怎样为地图添加控件 <摘要>你将在第二章中学会以下知识: 使用手写代码的利器——notepad++: 如何为地图添加控件——鱼骨. ...

随机推荐

  1. Python中使用百分号占位符的字符串格式化方法中%s和%r的输出内容有何不同?

    Python中使用百分号占位符的字符串格式化方法中%s和%r表示需要显示的数据对应变量x会以str(x)还是repr(x)输出内容展示. 关于str和repr的关系请见: <Python中rep ...

  2. 第8.27节 Python中__getattribute__与property的fget、@property装饰器getter关系深入解析

    一. 引言 在<第7.23节 Python使用property函数定义属性简化属性访问的代码实现>和<第7.26节 Python中的@property装饰器定义属性访问方法gette ...

  3. 第15.27节 PyQt(Python+Qt)入门学习:Model/View架构中的便利类QTreeWidget详解

    老猿Python博文目录 专栏:使用PyQt开发图形界面Python应用 老猿Python博客地址 一.引言 树部件(Tree Widget)是Qt Designer中 Item Widgets(It ...

  4. PyQt(Python+Qt)学习随笔

    老猿Python博文目录 老猿Python博客地址 PyQt学习随笔 PyQt(Python+Qt)帮助文档官网及文档下载 PyQt(Python+Qt)学习随笔:PyQt帮助文档导入assistan ...

  5. PyQt(Python+Qt)学习随笔:formLayout的layoutFormAlignment 属性

    一.引言 Qt Designer的表单布局(formLayout)中,layoutFormAlignment 用于控制表单布局中所有子部件在布局框内的对齐方式(与layoutLabelAlignmen ...

  6. java课堂作业--异常处理

    一. 运行结果: 二. 结果: ArrayIndexOutOfBoundsException/内层try-catch 发生ArithmeticException 三. 结果: ArrayIndexOu ...

  7. 面试阿里,字节,腾讯90%被问到的springboot常见面试题,这次给你讲的明明白白!

    1.什么是Spring Boot? 多年来,随着新功能的增加,spring变得越来越复杂.只需访问https://spring.io/projects页面,我们就会看到可以在我们的应用程序中使用的所有 ...

  8. Leetcode学习笔记(2)

    题目1 ID面试题 01.04 给定一个字符串,编写一个函数判定其是否为某个回文串的排列之一. 回文串是指正反两个方向都一样的单词或短语.排列是指字母的重新排列. 回文串不一定是字典当中的单词. 示例 ...

  9. python中的万能密码

    在php中,我们经常见到这样的语句 if(isset($_GET['id'])) { $id=$_GET['id']; //logging the connection parameters to a ...

  10. caffe源码 全连接层

    图示全连接层 如上图所示,该全链接层输入n * 4,输出为n * 2,n为batch 该层有两个参数W和B,W为系数,B为偏置项 该层的函数为F(x) = W*x + B,则W为4 * 2的矩阵,B ...