ROS学习(Intermediate Level 5): Adding a frame (C++)

In the previous tutorials we recreated the turtle demo by adding a tf broadcaster and a tf listener. This tutorial will teach you how to add an extra frame to the tf tree(tf tools). This is very similar to creating the tf broadcaster, and will show some of the power of tf.

Why adding frames

For many tasks it is easier to think inside a local frame, e.g. it is easier to reason about a laser scan in a frame at the center of the laser scanner. tf allows you to define a local frame for each sensor, link, etc in your system. And, tf will take care of all the extra frame transforms that are introduced.

Where to add frames

1. a frame only has one single parent, but it can have multiple children. 树的结构
2. If we want to add a new frame to tf, one of the three existing frames needs to be the parent frame, and the new frame will become a child frame. 新增加的 frame 是前面的子frame

tf builds up a tree structure of frames; it does not allow a closed loop in the frame structure. This means that a frame only has one single parent, but it can have multiple children. Currently our tf tree contains three frames: world, turtle1 and turtle2. The two turtles are children of world. If we want to add a new frame to tf, one of the three existing frames needs to be the parent frame, and the new frame will become a child frame.

How to add a frame

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


// add a new frame
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;
}

#include <ros/ros.h>

// to help make the task of receiving transforms easier.
#include <tf/transform_listener.h>
// #include <turtlesim/Velocity.h> is replaced by <geometry_msgs/Twist.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);

    // 监听
    listener.lookupTransform("/turtle2", "/carrot1",
                                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;
}

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

<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" />

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

ros::Duration(3.0): timeout, don't wait for longer than this maximum duration

waitForTransform()

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