rosserial doesn't subscribe to a published Topic

edit

I've written a simple program for my arduino which reads data from an IMU and send it to the PC. As you can see it runs smooth and well and I m able to see the content of the topic through ROS using the rostopic echo /topic

wilhem@MIG-31:~/workspace_ros$ rostopic echo /imu9dof roll: 0.00318404124118 pitch: 0.00714263878763 yaw: -2.9238409996 --- roll: 0.00247799116187 pitch: 0.00720235193148 yaw: -2.92382121086 --- roll: 0.00187148409896 pitch: 0.00657133804634 yaw: -2.92379975319

I wrote also a program running ROS to subscribe on that topic, take the value of the 3 axis and put them in a variable which is used later. The problem is... the subscriber doesn 't listen to the topic and does't save the variables transmitted by the massage. Here the barebone program:

#include <ros/ros.h>
#include <string>
#include <sensor_msgs/JointState.h>
#include <tf/transform_broadcaster.h>
#include "asctec_quad/imu9dof.h"

float roll_ = {0.0};
float pitch_ = {0.0};
float yaw_ = {0.0};

void updateIMU( const asctec_quad::imu9dofPtr& data ) {

    roll_ = data->roll;
    pitch_ = data->pitch;
    yaw_ = data->yaw;

    ROS_INFO( "I heard: [%f]", data->yaw );
}

int main( int argc, char **argv ) {

    ros::init( argc, argv, "state_publisher" );
    ros::NodeHandle nh;

    ROS_INFO( "starting ROS..." );

    ros::Publisher joint_pub = nh.advertise<sensor_msgs::JointState>( "joint_states", 1 );
    ros::Subscriber sub = nh.subscribe( "imu9dof", 10, updateIMU );

    sensor_msgs::JointState joint_state;

    ros::Rate loop( 50 );

    tf::TransformBroadcaster broadcaster;

    geometry_msgs::TransformStamped odom_trans;

    odom_trans.header.frame_id = "odom";
    odom_trans.child_frame_id = "base_footprint";

    while( ros::ok() ) {

        // update position
        odom_trans.header.stamp = ros::Time::now();
        odom_trans.transform.translation.x = 0;
        odom_trans.transform.translation.y = 0;
        odom_trans.transform.translation.z = 0;
        odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw( 0 );

        /* update joint state */
        joint_state.header.stamp = ros::Time::now();
        joint_state.name.resize(3);
        joint_state.position.resize(3);
        joint_state.name[0] = "odom_2_base_footprint";
        joint_state.position[0] = 0.0;
        joint_state.name[1] = "base_footprint_2_base_link";
        joint_state.position[1] = 0.0;
        joint_state.name[2] = "base_link_2_base_frame";
        joint_state.position[2] = yaw_;

        /* send the joint_state position */
        joint_pub.publish( joint_state );

        /* broadcast the transform */
        broadcaster.sendTransform( odom_trans );

        loop.sleep();

    }

    ros::spin();

    return 0;
}

as you can see in the function:

void updateIMU( const asctec_quad::imu9dofPtr& data ) {

    roll_ = data->roll;
    pitch_ = data->pitch;
    yaw_ = data->yaw;

    ROS_INFO( "I heard: [%f]", data->yaw );
}

I put a ROS_INFO just for debugging porpuoses. I start the serialnode for arduino and I can see the output, but launching the ROS programs that function is never called. It hangs all the time:

wilhem@MIG-31:~/workspace_ros$ rosrun asctec_quad my_prog

[ INFO] [1409088374.452412227]: starting ROS...

I did it checkign the tutorials many many times and crosschecking in the Q&A. What can I do? Thanks