missing odom to base tf link - dropped 100% odom messages
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 ...
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?