missing odom to base tf link - dropped 100% odom messages

asked 2017-01-31 20:23:27 -0600

charlie gravatar image

I have recorded a bag file using a physical robot with odometry and lidar info.

The robot uses arduino controllers to push data up to a PC using rosserial.

All seems to work correctly until I attempt to create a map with gmapping.

Once I have driven the robot around and saved a XXXXXXX.bag file

I start the core, then use "rosparam set use_sim_time true" , then "rosrun gmapping slam_gmapping", then "rosbag play --clock XXXXXXX.bag" ' I see MessageFilter [target=odom] dropped 100% of the messages so far........and mapsaver waits forever the map.......

rqt_bag shows tf and odom data, raw data shows data in both /odom and /tf, everything looks correct.

when I run rosrun tf_view_frames I only see base_link----> laser_frame and broadcaster /tf info.

However, I expeccted to see /odom---->base_link as well, but this is not the case.

I have included the PC code to convert the arduino serial data to odom/tf messaages.

does anyone has any suggestions ?

// ************************************************************ //
// *****                                                  ***** //
// *****  Arduino --> ROS                                 ***** //
// *****                                                  ***** //
// ***** This program process a minimal set of odometry   ***** //
// ***** data from the Arduino "Encoder_Data" topic       ***** //
// ***** and converts it into standard ROS                ***** //
// ***** nav_msgs/Odometry message format                 ***** //
// *****                                                  ***** //
// ***** Arduino program is MotCont_A_0p67                ***** //
// *****                                                  ***** //
// *****                                                  ***** //
//                                                              //
//                                                              //
// ************************************************************ //
// ***** Subscriber to Adruino topic Encoder_Data         ***** //
// ************************************************************ //
// ***** The subscribed data is                           ***** //
// ***** Vx                                               ***** //
// ***** Vy                                               ***** //
// ***** vTh                                              ***** //
// ***** x coordinate total distance                      ***** //
// ***** y coordinate total distance                      ***** //
// ***** theta total coordinate rotated                   ***** //
// ***** vxy linear velocity
// ***** 999                                              ***** //
// *****                                                  ***** //
// ************************************************************ //
// ***** Then publish to nav_msgs/Odometry message        ***** //
// ************************************************************ //
// ***** std_msgs/Header Message                          ***** //
// *****     uint32 seq                                   ***** //
// *****     time stamp                                   ***** //
// *****     string frame_id                              ***** //
// ************************************************************ //
// ***** geometry_msgs/PoseWithCovariance Message         ***** //
// *****     geometry_msgs/Pose pose                      ***** //
// *****     float64 covariance                           ***** //
// ***** geometry_msgs/TwistWithCovariance Message        ***** //
// *****     geometry_msgs/Twist twist                    ***** //
// *****     float64 covariance                           ***** //
// ************************************************************ //
//                                                              //

#include <ros/ros.h>
#include <std_msgs/Float32.h>
#include <geometry_msgs/Vector3.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>

// streamed arduino odometry data 

  int count = 0;
  double A_odom_data[10];

class ExampleCls
{
public:
    ExampleCls();
    void EncoderDataCallback(const std_msgs::Float32::ConstPtr& float_msg);

private:
    ros::NodeHandle nh_;
    ros::Subscriber Float32_sub_;
    ros::Publisher odom_pub;
};

ExampleCls::ExampleCls() 
{
    odom_pub = nh_.advertise<nav_msgs::Odometry>("odom",50);    
    Float32_sub_ = nh_.subscribe<std_msgs::Float32> ("Encoder_Data", 1000, &ExampleCls::EncoderDataCallback, this);
}

void ExampleCls::EncoderDataCallback(const std_msgs::Float32::ConstPtr& float_msg)
{

 //ROS_INFO("I heard:[%f]",float_msg->data);
 //ROS_INFO("count:[%i]",count);

     // Fill the array from the Arduino
     A_odom_data[count] = float_msg-> data;
     count = count + 1;

     //A_odom_data[0] = vx
     //A_odom_data[1] = vy
     //A_odom_data[2] = vth
     //A_odom_data[3] = Total X traveled
     //A_odom_data[4] = Total Y traveled
     //A_odom_data[5] = Total Th
     //A_odom_data[6] = vxy
     //A_odom_data[7] = 999

    if (float_msg-> data == 999)
      {
        //ROS_INFO("publish data");
        ros::Time current_time = ros::Time::now();
        tf::TransformBroadcaster odom_broadcaster;
        geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(A_odom_data[5]);  //Total Th


        // ***************************************
        // **** Publish the transfor over tf *****
        // ***************************************
    geometry_msgs::TransformStamped odom_trans;
    odom_trans.header.stamp = current_time;
    odom_trans.header.frame_id = "odom";
    odom_trans.child_frame_id = "base_link";

    odom_trans.transform.translation.x = A_odom_data[3];  // Linear X
    odom_trans.transform.translation.y = A_odom_data[4];  // Linear Y
    odom_trans.transform.translation.z = 0;
    odom_trans.transform.rotation = odom_quat;

        // Send the transform
    odom_broadcaster.sendTransform(odom_trans);

        // ******************************************
        // **** Publish odom messages over ROS ******
        // ******************************************
    nav_msgs::Odometry odom;
    odom.header.stamp = current_time;
    odom.header.frame_id = "odom";

        // **** Set the position ****
    odom.pose.pose.position.x = A_odom_data[3];  // Linear X
    odom.pose.pose.position.y = A_odom_data[4];  // Linear Y
    odom.pose.pose.position.z = 0.0 ...
(more)
edit retag flag offensive close merge delete

Comments

I used rqt_console and rqt_logger to review the DEBUG data and I found that every time it loops through this code I see "Publisher on '/tf' deregistering callbacks". I suspect I need to make /tf global outide the callback loop. I've tried a couple of options with no success. suggestions?

charlie gravatar image charlie  ( 2017-02-19 20:51:47 -0600 )edit