ros教程TF

catkin_create_pkg learning_tf tf roscpp rospy turtlesim

1 广播器
This tutorial teaches you how to broadcast coordinate frames to tf. In this case, we want to broadcast the changing coordinate frames of the turtles, as they move around.

turtle_tf_broadcaster.cpp

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

std::string turtle_name;



void poseCallback(const turtlesim::PoseConstPtr& msg){
  // Here, we create a TransformBroadcaster object that we'll use later to send the transformations over the wire.
  static tf::TransformBroadcaster br;
  // Here we create a Transform object, and copy the information from the 2D turtle pose into the 3D transform.
  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);
  br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
}

int main(int argc, char** argv){
  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;
};

br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));

这是完成实际工作的地方。使用TransformBroadcaster发送转换需要四个参数。

1. 首先，我们传递转换本身。

2. 现在我们需要给要发布的转换加上时间戳，我们只需要用当前时间ros :: Time :: now（）标记它。

3. 然后，我们需要传递所创建链接的父框架的名称，在本例中为“ world”

4. 最后，我们需要传递正在创建的链接的子框架的名称，在这种情况下，这就是乌龟本身的名称。

注意：sendTransform和StampedTransform的父级和子级顺序相反。

turtle_name坐标系与world坐标系的变换关系是transform。
turtle_name指向world。可以理解为turtle_name的原点在world坐标系下，关系位transform。

CMakeLists.txt

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

start_demo.launch

  <launch>
    <!-- Turtlesim Node-->
    <node pkg="turtlesim" type="turtlesim_node" name="sim"/>

    <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>
    <!-- Axes -->
    <param name="scale_linear" value="2" type="double"/>
    <param name="scale_angular" value="2" type="double"/>

    <node pkg="learning_tf" type="turtle_tf_broadcaster"
          args="/turtle1" name="turtle1_tf_broadcaster" />
    <node pkg="learning_tf" type="turtle_tf_broadcaster"
          args="/turtle2" name="turtle2_tf_broadcaster" />

  </launch>

检查rosrun tf tf_echo /world /turtle1## 标题

At time 1597672836.017
- Translation: [7.560, 5.544, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.000, 1.000]
            in RPY (radian) [0.000, -0.000, 0.000]
            in RPY (degree) [0.000, -0.000, 0.000]
At time 1597672837.009
- Translation: [7.560, 5.544, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.000, 1.000]
            in RPY (radian) [0.000, -0.000, 0.000]
            in RPY (degree) [0.000, -0.000, 0.000]
At time 1597672838.018
- Translation: [7.560, 5.544, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.000, 1.000]
            in RPY (radian) [0.000, -0.000, 0.000]
            in RPY (degree) [0.000, -0.000, 0.000]

2 侦听器
turtle_tf_listener.cpp

#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::init(argc, argv, "my_tf_listener");

    ros::NodeHandle node;

    ros::service::waitForService("spawn");
    ros::ServiceClient add_turtle =
            node.serviceClient<turtlesim::Spawn>("spawn");
    turtlesim::Spawn srv;
    add_turtle.call(srv);

    ros::Publisher turtle_vel =
            node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10);

    tf::TransformListener listener;

    ros::Rate rate(10.0);
    while (node.ok()){
        tf::StampedTransform transform;
        try{
            listener.lookupTransform("/turtle2", "/turtle1",
                                     ros::Time(0), transform);
        }
        catch (tf::TransformException &ex) {
            ROS_ERROR("%s",ex.what());
            ros::Duration(1.0).sleep();
            continue;
        }

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

listener.lookupTransform("/turtle2", "/turtle1",
                                     ros::Time(0), transform);

在这里，实际的工作已经完成，我们向侦听器查询特定的转换。让我们看一下四个参数：

1. 我们希望从框架/ turtle1转换为框架/ turtle2。

2. 我们想要转变的时间。提供ros :: Time（0）只会为我们提供最新的可用转换。

3. 我们在其中存储结果转换的对象。

所有这些都包装在try-catch块中，以捕获可能的异常。

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

这里把算出来的速度发给turtle2.

CMakeLists.txt

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

start_demo.launch

  <launch>
    ...
    <node pkg="learning_tf" type="turtle_tf_listener"
          name="listener" />
  </launch>

[ERROR] [1597675028.782350227]: "turtle2" passed to lookupTransform argument target_frame does not exist. 

这里因为创建turtle2需要时间，这里提示turtle2不存在。

3 添加 frame
当前，我们的tf树包含三个框架：world，turtle1和turtle2。
frame_tf_broadcaster.cpp

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

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

  tf::TransformBroadcaster br;
  tf::Transform transform;

  ros::Rate rate(10.0);
  while (node.ok()){
    transform.setOrigin( tf::Vector3(0.0, 2.0, 0.0) );
    transform.setRotation( tf::Quaternion(0, 0, 0, 1) );
    br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "turtle1", "carrot1"));
    rate.sleep();
  }
  return 0;
};

该代码与tf广播器中的示例非常相似。

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

  <launch>
    ...
    <node pkg="learning_tf" type="frame_tf_broadcaster"
          name="broadcaster_frame" />
  </launch>

尝试修改侦听节点

            listener.lookupTransform("/turtle2", "/carrot1",
                                     ros::Time(0), transform);

第二只乌龟在第一只乌龟左边两米处

我们在本教程中发布的额外框架是固定框架，相对于父框架，框架不会随时间变化。但是，如果要发布移动框架，则可以将广播者更改为随时间变化。
Let’s modify the /carrot1 frame to change relative to /turtle1 over time.

    transform.setOrigin( tf::Vector3(2.0*sin(ros::Time::now().toSec()), 2.0*cos(ros::Time::now().toSec()), 0.0) );
    transform.setRotation( tf::Quaternion(0, 0, 0, 1) );

carrot1绕turtle1转圈，turtle2跟着carrot1