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"

这个 和 之前 博客

基本一至 其实做的事情就是一样的 。

对比一下

这篇博客 就是 通过 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、结果

7、Done