0
$\begingroup$

Rosanswers logo

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 1403703435.308840975
Exception thrown:"world" passed to lookupTransform argument target_frame does not exist. 
The current list of frames is:

If anybody would help my i would be realy thankful.

Cheers Andreas


Originally posted by andreas.gerken on ROS Answers with karma: 68 on 2014-06-25

Post score: 2

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

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.


Originally posted by tfoote with karma: 58457 on 2014-06-25

This answer was ACCEPTED on the original site

Post score: 3


Original comments

Comment by andreas.gerken on 2014-06-26:
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.

Comment by andreas.gerken on 2014-06-27:
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 .

Comment by Andre_Preller_3000 on 2016-03-03:
Andreas, the link to your working code is gone if you still have the code could you link it again or send it to [email protected] please? Thanks

$\endgroup$

Your Answer

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