Problems with converting pose to tf

edit

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