Ask Your Question
2

Problems with converting pose to tf

asked 2014-06-25 08:47:34 -0600

andreas.gerken gravatar image

Hi ROS-community,

i'm pretty new to ROS but got some things working by now:

I'm working with the asctec pelican with a hokuyo laser onboard. To get all the data from the quadrotor im using the asctec_mav_framework and the laserdata comes with the urg_node. I did many tutorials but now i want to program the my first usefull node which should get the quadrotor pose and turn it into a tf transformation. I hope to see the laserpoints in rviz in "3D" and not flat on the ground.

The Quadrotor pose is published with the topic /fcu/current_pose in a geometry_msgs::PoseStamped message, which i try to convert to a tf.

Since i'm a starter i tried to combine the code from several tutorials (subscriber node, tf publish frame and a tutorial to get from a Pose msg to a tf ( I'm not allowed to publish links http:// answers.ros.org/question/10323/from-posestamped-message-to-tf-stampedtransform/ ) It looked all pretty good to me, it's compiling too without a problem but sadly it's not working.

I tried to make it simpler by using static values for the transform but thats not working either.

#include <ros/ros.h>
#include <ros/console.h>
#include <tf/transform_broadcaster.h>
#include "geometry_msgs/PoseStamped.h"

int counter = 0;

void poseCallback(const geometry_msgs::PoseStamped::ConstPtr& msg)
{
    // use only every 50th message to slow down the debugging output
    if(counter++ % 50 != 0){
        return;
    }

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

    geometry_msgs::Pose pose = msg->pose;
    geometry_msgs::Point position = pose.position;
    geometry_msgs::Quaternion orientation = pose.orientation;


    //ROS_DEBUG("poseCallback called");
    //transform.setPose(msg->pose);

    transform.setOrigin( tf::Vector3(0.0, 2.0, 0.0) );
    transform.setRotation( tf::Quaternion(0, 0, 0, 1) );

    //For later use
    //transform.setOrigin( tf::Vector3(position.x, position.y, position.z) );
    //transform.setRotation( tf::Quaternion( orientation.x, orientation.y, orientation.z, orientation.w) );

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


    ROS_INFO("Broadcasted transformation laser_tf_broadcaster");
}


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

    ros::Subscriber sub = n.subscribe("/fcu/current_pose", 1000, poseCallback);

    ros::spin();

    return 0;
}

To see what's not working, i looked at the Debugging information, which i didn't realy understood.

[ INFO] [1403703363.048784719]: Broadcasted transformation laser_tf_broadcaster
[DEBUG] [1403703363.048859931]: Publisher on '/tf' deregistering callbacks.
[DEBUG] [1403703363.284203709]: Accepted connection on socket [6], new socket [10]
[DEBUG] [1403703363.284337646]: Adding tcp socket [10] to pollset
[DEBUG] [1403703363.284425414]: TCPROS received a connection from [10.250.100.4:39856]
[DEBUG] [1403703363.284552639]: Connection: Creating TransportSubscriberLink for topic [/tf] connected to [callerid=[/fcu] address=[TCPROS connection to [10.250.100.4:39856 on socket 10]]]
[DEBUG] [1403703363.284754772]: received a connection for a nonexistent topic [/tf] from [TCPROS connection to [10.250.100.4:39856 on socket 10]] [/fcu].
[DEBUG] [1403703363.285001269]: Connection::drop(1)
[DEBUG] [1403703363.285090360]: TCP socket [10] closed
[DEBUG] [1403703363.285156511]: Connection::drop(0)
[DEBUG] [1403703363.285220992]: Connection::drop(2)

The output of

rosrun tf tf_echo world laser1

is

Failure at ...
(more)
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
3

answered 2014-06-25 12:17:06 -0600

tfoote gravatar image

updated 2014-06-26 13:27:41 -0600

You should post your listener code. there's no instance of a lookupTransform which is what's failing. Since the list of frames is empty. I'm guessing that you have not giving the TransformListener class time to fill its buffer. I suggest you review the tf Tutorials in particular writing a listener where you should always use a try catch statement and git it time to retry.

Edit: Sorry I didn't see that the output was from tf_echo.

Your problem is that you're constructing the broadcaster inside your callback. This does not give it enough time to establish connections before destructing. You should make sure your broadcaster persists between callbacks.

edit flag offensive delete link more

Comments

Hi tfoote, i don't have a listener, i only want to show the transformation in RVIZ first or in tf_echo which is a listener right? And can't i start a publisher without a listener? In the broadcaster example (http://wiki.ros.org/tf/Tutorials/Writing%20a%20tf%20broadcaster%20(C%2B%2B)) they don't have a listener either. I found a misstake but it didn't change anything: ros::Subscriber sub = n.subscribe("/fcu/current_pose", 1000, poseCallback); should be ros::Subscriber sub = n.subscribe("/fcu/current_pose", 1000, &poseCallback); according to the tutorial.

andreas.gerken gravatar imageandreas.gerken ( 2014-06-26 05:28:03 -0600 )edit

I thought of that too but too but the tutorial did it the same way. I tried it and it works! Thank you so much i was stuck with that problem for nearly a week now. If anybody has the same problem: If you create the broadcaster and the transformation as global variables they will be created before the init call and it will fail. So i just created global pointers and instanciated the classes in the main function. Global: tf::Transform* transform_ptr; In Main: transform_ptr = new tf::Transform(); In Callback: transform_ptr->setOrigin( tf::Vector3(0.0, 2.0, 0.0) ); The working Code is at http://codepaste.net/5tjg1d .

andreas.gerken gravatar imageandreas.gerken ( 2014-06-27 03:21:56 -0600 )edit

Andreas, the link to your working code is gone if you still have the code could you link it again or send it to drepreller@gmail.com please? Thanks

Andre_Preller_3000 gravatar imageAndre_Preller_3000 ( 2016-03-03 19:31:50 -0600 )edit

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: 2014-06-25 08:47:34 -0600

Seen: 4,933 times

Last updated: Jun 26 '14