0
$\begingroup$

Rosanswers logo

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

Originally posted by GabrielBagon44 on ROS Answers with karma: 60 on 2019-06-05

Post score: 3


Original comments

Comment by gvdhoorn on 2019-06-06:
Just making sure: have you seen the tf2/Tutorials?

Comment by GabrielBagon44 on 2019-06-07:
yes, I have.

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

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.


Originally posted by PeteBlackerThe3rd with karma: 9529 on 2019-06-06

This answer was ACCEPTED on the original site

Post score: 2


Original comments

Comment by GabrielBagon44 on 2019-06-07:
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)’

Comment by PeteBlackerThe3rd on 2019-06-07:
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");

Comment by GabrielBagon44 on 2019-06-07:
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));
                             

                                                  ^

Comment by PeteBlackerThe3rd on 2019-06-07:
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.

Comment by GabrielBagon44 on 2019-06-10:
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

Comment by GabrielBagon44 on 2019-06-10:
aside from this, what does "const T &" mean?

Comment by PeteBlackerThe3rd on 2019-06-10:
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.

Comment by PeteBlackerThe3rd on 2019-06-10:\

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++

Comment by GabrielBagon44 on 2019-06-10:
ah I see, I am going to try and put it into poseOut then. Thanks for the quick replies.

Comment by GabrielBagon44 on 2019-06-10:
Just a quick question for some reason I haven't thought about this, how exactly does this transform function know which frame you are referring to when giving it the raw pose?

Comment by PeteBlackerThe3rd on 2019-06-10:
You're not giving it a raw pose, you're giving it a PoseStamped. The Stamped part means that it has a time and frame_id associated with it. This is why you can just pass that and the new frame_id.

Comment by GabrielBagon44 on 2019-06-11:
so, good news is that I've been able to compile it without errors, and just as you mentioned I was getting runtime errors. For the most part, I think I have solved those issues, however now I am receiving an error shown below. I have rechecked my TF tree and all the frames I am working with are under the same TF [map]-->[base_link]-->[marker_frame], where base_link is the camera frame.

[ WARN] [1560275941.431970740, 1406.020000000]: Lookup would require extrapolation into the past.  Requested time 1405.716000000 but the earliest data is at time 1560275940.551247808, when looking up transform from frame [base_link] to frame [map]

Comment by PeteBlackerThe3rd on 2019-06-12:
Okay, this is actually a different problem now, you've solved your original question. It would be best to ask a new question to keep the threads clean as the solution to this time issue is unrelated to the original question. Can you accept this answer and start a new question, and we'll carry on from there.

Comment by GabrielBagon44 on 2019-06-12:
Okay, thanks a lot for your help on this problem!

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.