Ask Your Question
1

how to use tf2 to transform PoseStamped messages from one frame to another?

asked 2019-06-05 20:13:26 -0500

GabrielBagon44 gravatar image

updated 2019-06-11 13:14:43 -0500

Hi I am new to ROS so I apologize if my questions seem all over the place. I am simulating an autonomous quadcopter in gazebo, using aruco marker detection for landing. As pictured below, I have a map frame which is connected to the camera frame (base_link) which is then linked to the marker_frame. My goal is to transform the PoseStamped messages from the marker frame to the frame of the quadcopter model. And so, I have a few general inquries.

TF_Tree

  • [fcu] <---> [fcu_frd] (connected to each other)
  • [map]--->[base_link]--->[marker_frame]
  • [local_origin]--->[local_origin_ned]

(For some reason there are 3 separate trees)

Questions

  1. I am confused as to how I am to transform the PoseStamped message from the Marker frame into the quadcopter frame. Correct me if I am wrong but is the local_origin_ned the frame for the quadcopter?
  2. I have created a broadcaster node that transforms the posestamped message from the marker frame to the camera frame however, I am not sure I am transforming the information right. Is this even the right way to start? I am quite confused. (pasted below)

I am using ros kinetic on ubuntu 16.04. Again, sorry for the unorganized thoughts, and thanks in advance.

#include <ros/ros.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/transform_broadcaster.h>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/PoseStamped.h>
void callback(const geometry_msgs::PoseStamped& msg){
    static tf2_ros::TransformBroadcaster br; 
    geometry_msgs::TransformStamped transformStamped; 
    transformStamped.header.stamp = ros::Time::now(); 
    transformStamped.header.frame_id = "base_link" ;
    transformStamped.child_frame_id = "marker_frame";

    transformStamped.transform.translation.x = msg.pose.position.x; 
    transformStamped.transform.translation.y = msg.pose.position.y; 
    transformStamped.transform.translation.z = msg.pose.position.z; 
        transformStamped.transform.rotation.x = msg.pose.orientation.x;
        transformStamped.transform.rotation.y = msg.pose.orientation.y;
        transformStamped.transform.rotation.z = msg.pose.orientation.z;
        transformStamped.transform.rotation.w = msg.pose.orientation.w;

    br.sendTransform(transformStamped); 
}

int main(int argc, char** argv){
    ros::init(argc,argv,"broadcast_cm");
    ros::NodeHandle nh; 
    ros::Subscriber sub = nh.subscribe("aruco_single/pose", 100, &callback);
    ros::spin();
    return 0; 
}

Listener Node

#include <ros/ros.h>
#include <tf2_ros/transform_listener.h>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/PoseStamped.h>
#include <tf2_ros/buffer_interface.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
geometry_msgs::PoseStamped poseIn;
geometry_msgs::PoseStamped poseOut;
bool received; 

ros::Duration timeout; 
void callback(const geometry_msgs::PoseStamped& msg){
    poseIn = msg; 
    received= true; 
}

int main(int argc, char** argv){
    ros::init(argc, argv, "listener_test");
    ros::NodeHandle nh;
    ros::Subscriber sub = nh.subscribe("aruco_single/pose", 100, &callback); 
    tf2_ros::Buffer tfBuffer;
    tf2_ros::TransformListener tfListener(tfBuffer);

    ros::Rate rate(10.0); //rate 10 
    sleep(1);

    while (nh.ok()){
        try{
            if(received){poseOut = tfBuffer.transform(poseIn, "map", ros::Duration(0.0));}
        }
        catch (tf2::TransformException &ex) {
            ROS_WARN("%s",ex.what());
            ros::Duration(1.0).sleep();
                continue;
        }
    rate.sleep();
    ros::spinOnce();
    }
    return 0;
}
edit retag flag offensive close merge delete

Comments

Just making sure: have you seen the tf2/Tutorials?

gvdhoorn gravatar imagegvdhoorn ( 2019-06-06 02:50:41 -0500 )edit

yes, I have.

GabrielBagon44 gravatar imageGabrielBagon44 ( 2019-06-07 09:55:07 -0500 )edit

2 Answers

Sort by » oldest newest most voted
0

answered 2019-06-06 07:04:48 -0500

I'll try and clear up things for you, transforming a pose into a different frame is a standard feature of TF2, you don't have to implement any of the mathematics yourself.

In your code snippet above you're not actually transforming anything, it just publishes the pose data as a TF between two different frames. Since the pose is an inverse of a transform this may well be doing the opposite of what you expect it to. What frame are are the pose messages initially defined in? I would expect it to be the frame of the camera, which frame is this?

The recommended method for your task is to create a tf2_ros::Listener, then to use its attached tf2_ros::Buffer object to transform the pose directly. The Buffer object already has a transform method that will take care of the transformation mathematics for you.

If you use the tf2 listener example here as a starting point, then you replace the tfBuffer.lookupTransform statement within the try catch block with a call to tfBuffer::transform to transform your pose into the frame you want. NOTE this will only work if the original frame of the pose and the new frame are part of the same TF tree.

Also you mention you have three isolated TF trees. This is a problem and is caused either because frames are not names correctly or there are some missing transforms. You'll either want to add some static transform publishers or add a robot model to join these together.

Hope this makes sense.

edit flag offensive delete link more

Comments

Hi, thanks for the response. I took your advice and tried to make my own listener. However, when I run it, I am getting an error.

The line is:

transformStamped = tf2_ros::BufferInterface::transform(poseIn, poseOut, "map", ros::Time(0));

The Error is:

error: no matching function for call to ‘tf2_ros::BufferInterface::transform(geometry_msgs::PoseStamped&, geometry_msgs::PoseStamped&, const char [4], ros::Time)’
GabrielBagon44 gravatar imageGabrielBagon44 ( 2019-06-07 09:54:17 -0500 )edit

It's hard to help you without seeing all of your code, if would help if you added it to your original question.

However you shouldn't be calling tf2_ros::BufferInterface::transform it's a method is an interface class, this is inherited by the Buffer class, and you can only call transform on an instance. So your call should look like my_tf2_buffer_instance.transform(poseIn, poseOut, "map");

PeteBlackerThe3rd gravatar imagePeteBlackerThe3rd ( 2019-06-07 10:16:28 -0500 )edit

Sorry about that, it is added now, and I changed the call to tfBuffer.transform saved and rebuilt, but it seems I am getting a similar error

Errors     << mavros:make /home/totomyl/catkin_ws/logs/mavros/build.make.106.log
/home/totomyl/catkin_ws/src/mavros/mavros/src/listener.cpp: In function ‘int main(int, char**)’:
/home/totomyl/catkin_ws/src/mavros/mavros/src/listener.cpp:25:83: error: no matching function for call to ‘tf2_ros::Buffer::transform(geometry_msgs::PoseStamped&, geometry_msgs::PoseStamped&, const char [4], ros::Time)’
         transformStamped = tfBuffer.transform(poseIn, poseOut, "map", ros::Time(0));


                                                  ^
GabrielBagon44 gravatar imageGabrielBagon44 ( 2019-06-07 10:48:48 -0500 )edit

You only pass the poseIn to the transform method. The result is returned. You can check the documentation to make sure you're call matches.

PeteBlackerThe3rd gravatar imagePeteBlackerThe3rd ( 2019-06-07 18:06:43 -0500 )edit

I've updated the listener code again, once with passing only poseIn and once while passing both poseIn and "map". The first time around yielded the same error as above, while the second one gave me the error below. Is this saying I must remove the "=" ?

/home/totomyl/catkin_ws/src/mavros/mavros/src/listener.cpp: In function ‘int main(int, char**)’:
/home/totomyl/catkin_ws/src/mavros/mavros/src/listener.cpp:26:26: error: no match for ‘operator=’ (operand types are ‘geometry_msgs::TransformStamped {aka geometry_msgs::TransformStamped_<std::allocator<void> >}’ and ‘geometry_msgs::PoseStamped_<std::allocator<void> >’)
         transformStamped = tfBuffer.transform(poseIn, "map");
                          ^
In file included from /opt/ros/kinetic/include/tf2_msgs/TFMessage.h:18:0,
                 from /opt/ros/kinetic/include/tf2_ros/transform_listener.h:36,
                 from /home/totomyl/catkin_ws/src/mavros/mavros/src/listener.cpp
GabrielBagon44 gravatar imageGabrielBagon44 ( 2019-06-10 07:40:03 -0500 )edit

aside from this, what does "const T &" mean?

GabrielBagon44 gravatar imageGabrielBagon44 ( 2019-06-10 07:53:08 -0500 )edit

The error message is telling you that you're trying to copy a geometry_msgs::PoseStamped into a geometry_msgs::TransformStamped. Which won't work because they're different types. You're transforming poseIn into a new pose in a different coordinate frame, so the result is still a pose, not a transform.

There are a couple of other run-time issues with your code. Firstly the tfListener will not work instantly I'd add a one second sleep after you create it so that it can build up a buffer of TF messages. Secondly it's probable that your code will attempt to transform a pose before the callback function has been executed. This will result in a lookup error because the blank poseIn will have no frame ids set. You'll need an extra boolean to flag when a value poseIn value has been received.

PeteBlackerThe3rd gravatar imagePeteBlackerThe3rd ( 2019-06-10 08:05:04 -0500 )edit

what does "const T &" mean?

I strongly recommend working through some C++ tutorials. ROS uses fairly complex C++ and is not intended to be used by beginners or as a way to learn C++

PeteBlackerThe3rd gravatar imagePeteBlackerThe3rd ( 2019-06-10 08:11:31 -0500 )edit
0

answered 2019-06-06 11:33:18 -0500

clyde gravatar image

Wrt the 3 transform trees, it's worth spending some time designing and carefully naming the coordinate frames that you'll be using in your robot. Once you have a plan, you can make sure that each transformation is provided by some node. roswtf can be helpful to find problems.

I'm making assumptions based on your use case, but I would suggest something like the following. Caveat: this is may not work for your use case or the code you're using.

  • map
  • base_link. map -> base_link should be provided the marker detection code. Note that the marker detector is actually computing camera_optical -> marker_frame, but using the TF2 code to calculate and publish map -> base_link instead. The origin of base_link should be in the center of the drone at the lowest point, so that when the drone is on the ground the the drone pose z is 0. In the body frame, +x should be forward along the drone, +y should be left (port), +z should be up. See REP 103 if this is confusing.
  • camera_link. base_link -> camera_link should be specified in the URDF (assuming it's static) and published by robot_state_publisher. The assumption that most people make is that cameras face forward (+x along the body). If the camera is facing down, apply the pitch rotation here.
  • camera_optical. camera_link -> camera_optical should also be specified in the URDF. See rep 103 for the rotation.
  • marker_frame. map -> marker_frame should be provided by whatever mapping code you're using. N.B. if you're using OpenCV and ArUco markers you may need 2 frames per marker: one that follows rep 103, and one that is rotated into the position that OpenCV expects.

I assume that fcu is another name for base_link? I'm also guessing that frd is a different coordinate frame in mavros -- maybe NED vs ENU? I'm not entirely sure what local_origin is.

I hope this helps.

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2019-06-05 20:13:26 -0500

Seen: 571 times

Last updated: Jun 11