• 欢迎访问开心洋葱网站,在线教程,推荐使用最新版火狐浏览器和Chrome浏览器访问本网站,欢迎加入开心洋葱 QQ群
  • 为方便开心洋葱网用户,开心洋葱官网已经开启复制功能!
  • 欢迎访问开心洋葱网站,手机也能访问哦~欢迎加入开心洋葱多维思维学习平台 QQ群
  • 如果您觉得本站非常有看点,那么赶紧使用Ctrl+D 收藏开心洋葱吧~~~~~~~~~~~~~!
  • 由于近期流量激增,小站的ECS没能经的起亲们的访问,本站依然没有盈利,如果各位看如果觉着文字不错,还请看官给小站打个赏~~~~~~~~~~~~~!

ROS TF :使用 TF 设置机器人 发布坐标变换 使用坐标变换 将传感器数据转换为机器人坐标系下

人工智能 月照银海似蛟龙 1908次浏览 0个评论

1、变换设置 设计一个传感器在机器人上的场景

许多ROS 功能包 通过利用TF2 软件 库 去 发布 机器人的 坐标变换树在抽象层面上,坐标变换树 定义了 每个 不同的坐标系间的 偏移和旋转。

为了更加具体一些,举个例子,

例如一个简单的机器人,是一个可移动的小车底盘在顶部安装着一个单线激光测距仪。

在这个简单机器人中,定义两个坐标系 : 一个小车底盘的中心(base_link),另一个激光测距仪的中心(base_laser)

假设激光测距有一些数据,是目标到激光测距仪中心的距离。即 有一些数据在base_laser坐标系下。

现在我们想用这些数据帮助小车移动,来避免撞到东西。

我们需要将 在 base_laser 下的数据转换到 base_link下。换句话说就是我们需要base_laser坐标系和base_link坐标系的转换关系。

在这里插入图片描述
现在假设有这种关系, 雷达安装在距离小车中心点前10cm,高20cm的位置。

这样就有了两个坐标系的偏移转换关系。(两个坐标系之间没有旋转关系)

由 base_link 的 数据 转换到 base_laser 的坐标系下 需要 (x: 0.1m, y: 0.0m, z: 0.2m) 这样的转换

相反 base_laser 的 数据 转换到 base_link 的坐标系下 需要 (x:- 0.1m, y: 0.0m, z:- 0.2m) 这样的转换

我们可以选择自己管理这种关系,这意味着在必要时在坐标系之间存储和应用适当的转换,但是随着坐标框架数量的增加,这变得非常困难。 幸运的是,我们不必自己做这项工作。 相反,我们将使用TF一次定义“ base_link”和“ base_laser”之间的关系,并让它为我们管理两个坐标系之间的转换。

要使用tf定义和存储“ base_link”和“ base_laser”坐标系之间的关系,我们需要将它们添加到坐标变换树中。 坐标变换树中的每个节点都对应于一个坐标系,每个坐标变换是从当前节移动到其子节点的变换。 TF使用树结构来确保所有的坐标系都在这个树结构中。

在这里插入图片描述

对上面的示例 创建一个 坐标变换树 ,需要创建两个节点,一个用于“ base_link”坐标系,一个用于“ base_laser”坐标系。

需要确定哪个是父坐标系,哪个是子坐标系。这种区别很重要,因为tf假定所有转换都从父级移动到子级。

选择“ base_link”坐标系作为父坐标系,因为随着其他零件/传感器被添加到机器人中,通过遍历“ base_link”框架将它们与“ base_laser”框架联系起来是最有意义的连接“ base_link”和“ base_laser”相关的变换应为(x:0.1m,y:0.0m,z:0.2m)。

通过设置 这样的转换树 ,把 base_laser 坐标系下的数据 转换为 base_link 坐标系下的数据 将变的像调用tf库一样简单。

2、发布坐标变化(TF) 代码 (发布传感器坐标系与机器人坐标系关系)

TF分成两个版本 TF和TF2
下面这个是TF

需要创建一个节点 完成广播 base_laser到base_link TF 通过ROS.

创建 src/tf_broadcaster.cpp 文件

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

int main(int argc, char** argv){
  ros::init(argc, argv, "robot_tf_publisher");
  ros::NodeHandle n;

  ros::Rate r(100);

  tf::TransformBroadcaster broadcaster;

  while(n.ok()){
    broadcaster.sendTransform(
      tf::StampedTransform(
        tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.1, 0.0, 0.2)),
        ros::Time::now(),"base_link", "base_laser"));
    r.sleep();
  }
}

代码解释

#include <tf/transform_broadcaster.h>

TF功能包 实现了 tf::TransformBroadcaster 来完成 发布 坐标变换
使用 tf::TransformBroadcaster 必须包含 该 头文件 tf/transform_broadcaster.h

====================================================================

tf::TransformBroadcaster broadcaster;

创建TransformBroadcaster 类 的实例,用它来发布 base_link → base_laser 的变换

========================================================================

    broadcaster.sendTransform(
      tf::StampedTransform(
        tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.1, 0.0, 0.2)),
        ros::Time::now(),"base_link", "base_laser"));

这是真正工作的地方,
发送一个坐标变换需要5个参数

  • 第一个是旋转变换 通过四元数
  • 第二个是偏移向量 Vector3 这个例子中设置雷达 的 x偏移 0.1m 和 z的0.2m 相对与 base_link的
  • 第三个是发布的时间戳 用ros::Time::now() 即可
  • 第四个两个坐标变换中的父坐标系的名称 即 “base_link”
  • 第五个两个坐标变换中的子坐标系的名称 即 “base_laser”

这个和之前的
基本一至 其实做的事情就是一样的 。

对比一下

ROS TF :使用 TF 设置机器人 发布坐标变换 使用坐标变换 将传感器数据转换为机器人坐标系下

这篇博客 就是 通过 tf::StampedTransform() 去构造的 geometry_msgs::TransformStamped

之前博客 就是 手动去 配置的 geometry_msgs::TransformStamped

下面介绍下 tf::StampedTransform() 函数
有五个参数

  1. 坐标的变换关系 旋转
  2. 坐标的变换关系 平移
  3. 时间戳
  4. 父坐标系
  5. 子坐标系

3、使用坐标变换 代码 (将传感器数据转换为机器人坐标系下)

上面,创建了一个节点,发布 base_laser 和 base_link 的坐标变换(TF)通过ROS。

现在写一个节点,使用上面的坐标变换(TF),来将base_laser坐标系下的点,转换到base_link坐标系下。

创建 src/tf_listener.cpp 文件 :

#include <ros/ros.h>
#include <geometry_msgs/PointStamped.h>
#include <tf/transform_listener.h>

void transformPoint(const tf::TransformListener& listener){
  //we'll create a point in the base_laser frame that we'd like to transform to the base_link frame
  geometry_msgs::PointStamped laser_point;
  laser_point.header.frame_id = "base_laser";

  //we'll just use the most recent transform available for our simple example
  laser_point.header.stamp = ros::Time();

  //just an arbitrary point in space
  laser_point.point.x = 1.0;
  laser_point.point.y = 0.2;
  laser_point.point.z = 0.0;

  try{
    geometry_msgs::PointStamped base_point;
    listener.transformPoint("base_link", laser_point, base_point);

    ROS_INFO("base_laser: (%.2f, %.2f. %.2f) -----> base_link: (%.2f, %.2f, %.2f) at time %.2f",
        laser_point.point.x, laser_point.point.y, laser_point.point.z,
        base_point.point.x, base_point.point.y, base_point.point.z, base_point.header.stamp.toSec());
  }
  catch(tf::TransformException& ex){
    ROS_ERROR("Received an exception trying to transform a point from \"base_laser\" to \"base_link\": %s", ex.what());
  }
}

int main(int argc, char** argv){
  ros::init(argc, argv, "robot_tf_listener");
  ros::NodeHandle n;

  tf::TransformListener listener(ros::Duration(10));//等待10s,如果10s之后都还没收到消息,那么之前的消息就被丢弃掉

  //we'll transform a point once every second
  ros::Timer timer = n.createTimer(ros::Duration(1.0), boost::bind(&transformPoint, boost::ref(listener)));

  ros::spin();

}

这个 main 函数里有个 调用ROS定时器的功能 就是 这
在这里插入图片描述
这个不是 本 篇 的重点 回来再介绍 ,功能就是 间隔 1 秒 (第一个参数),然后执行 后面的 函数

还有一个 知识点 boost::bind
boost::bind
是标准库函数std::bind1st和std::bind2nd的一种泛化形式。
其可以支持函数对象、函数、函数指针、成员函数指针,并且绑定任意参数到某个指定值上或者将输入参数传入任意位置。

另一个知识点 boost::ref
boost库中ref用于包装一个对象,使其看起来像别名一样。(不是很理解,先不管它)

boost::bind(&transformPoint, boost::ref(listener))

那么就相当于

transformPoint(listener);
  ros::Timer timer = n.createTimer(ros::Duration(1.0), boost::bind(&transformPoint, boost::ref(listener)));

这行代码也就是,1秒钟,执行transformPoint(listener) 1次

代码解释

main函数里的代码基本解释清楚了

下面主要看下回调函数

 void transformPoint(const tf::TransformListener& listener)

里面的

========================================================================

#include <tf/transform_listener.h>

包含tf/transform_listener.h 头文件

因为需要创建一个 tf::TransformListener 类 实例

TransformListener 对象会自动订阅 坐标变换 信息 通过ROS 并且管理 数据转换

========================================================================

void transformPoint(const tf::TransformListener& listener){

创建一个回调函数, 一个形参TransformListener

在 base_laser 坐标系中 设置一个点, 转换到 base_link 坐标系中。

这个函数 由 main()函数中的定时器,一秒中回调一次。

========================================================================

  //we'll create a point in the base_laser frame that we'd like to transform to the base_link frame
  geometry_msgs::PointStamped laser_point;
  laser_point.header.frame_id = "base_laser";

  //we'll just use the most recent transform available for our simple example
  laser_point.header.stamp = ros::Time();

  //just an arbitrary point in space
  laser_point.point.x = 1.0;
  laser_point.point.y = 0.2;
  laser_point.point.z = 0.0;

创建一个 geometry_msgs::PointStamped 点的变量,

结尾的Stamped表示包含header。可以设置 timestamp 和 frame_id

设置 timestamp 为 ros::Time() 即 查询 TransformListener 中 最新可用的 坐标变换。

frame_id 设置为 “base_laser” 即 创建的点 是在 base_laser 坐标系下的

最后设置 这个点 的 x,y ,z 的 具体的 值

========================================================================

  try{
    geometry_msgs::PointStamped base_point;
    listener.transformPoint("base_link", laser_point, base_point);

    ROS_INFO("base_laser: (%.2f, %.2f. %.2f) -----> base_link: (%.2f, %.2f, %.2f) at time %.2f",
        laser_point.point.x, laser_point.point.y, laser_point.point.z,
        base_point.point.x, base_point.point.y, base_point.point.z, base_point.header.stamp.toSec());
  }

现在有了 base_laser 坐标系下的点, 想转成 base_link 坐标系下。 实现这个用

TransformListener 对象的 transformPoint() 函数

有三个参数

  • 第一个:想要转换到的坐标系下的 坐标系名称 这个例子中就是(base_link)
  • 第二个:需要转换的点
  • 第三个:转换后的点

所以经过 listener.transformPoint(“base_link”, laser_point, base_point); 之后

base_point 就有了在 base_link 坐标系下的 对应的点

========================================================================

  catch(tf::TransformException& ex){
    ROS_ERROR("Received an exception trying to transform a point from \"base_laser\" to \"base_link\": %s", ex.what());
  }

如果由于某种原因base_laser→base_link转换不可用(也许tf_broadcaster未运行),则当调用transformPoint()时,TransformListener可能会引发异常。 为了确保能正常处理,将捕获异常并打印错误。

========================================================================

4、 编译

在 CMakeLists.txt 加入如下

add_executable(tf_broadcaster src/tf_broadcaster.cpp)
add_executable(tf_listener src/tf_listener.cpp)
target_link_libraries(tf_broadcaster ${catkin_LIBRARIES})
target_link_libraries(tf_listener ${catkin_LIBRARIES})

编译

cd 工作空间 路径
catkin_make

5、运行代码

运行 roscore

roscore

运行 tf广播者

rosrun robot_setup_tf tf_broadcaster

运行tf监听者

rosrun robot_setup_tf tf_listener

6、结果

ROS TF :使用 TF 设置机器人 发布坐标变换 使用坐标变换 将传感器数据转换为机器人坐标系下


开心洋葱 , 版权所有丨如未注明 , 均为原创丨未经授权请勿修改 , 转载请注明ROS TF :使用 TF 设置机器人 发布坐标变换 使用坐标变换 将传感器数据转换为机器人坐标系下
喜欢 (0)

您必须 登录 才能发表评论!

加载中……