Problem publishing from txt file to topic

edit

Hi

I have a txt file with encoder data that looks like this:

...
1 277.87333332 269.94188565 2111.27302100 2111.27305200 2.40910042 0.58907051
1 277.88407118 269.94457012 2111.27810100 2111.27812500 2.33276239 0.58943359
1 277.89595953 269.94725459 2111.28316700 2111.28318600 2.33464865 0.57762184
1 277.90746439 269.94993905 2111.28825800 2111.28828900 2.32527232 0.56519989
1 277.91935274 269.95262352 2111.29334100 2111.29336900 2.30780781 0.54064191
1 277.93315857 269.95569148 2111.29842600 2111.29847000 2.36224113 0.54073054
1 277.94658090 269.95875944 2111.30351200 2111.30353700 2.38172210 0.55344243
1 277.96153721 269.96182740 2111.30858400 2111.30860500 2.42912526 0.56616211
1 277.97572653 269.96451187 2111.31365500 2111.31367500 2.46379925 0.56604012
1 277.98953236 269.96796333 2111.31872300 2111.31873900 2.50533659 0.59181053
...

I want to publish this data to an odometry publisher. In the original program a while loop is used for publishing the topics. I had to remove this loop to come back into my for loop, but now the /odom and /tf topic are not constantly published. What can I do to solve this?

Afterwards I want to record the /tf topic, together with a sensordata topic (with the same timestamps). But the publishing of these topics is not equal with the timestamps, does de rosbag arrange the topics with help from the timestamps I set in the publisher (coming from the txt file).

This is my code: int main(int argc, char **argv) {

    ros::init(argc, argv, "odometry_from_text_publisher");
    ros::NodeHandle n;
    ros::Rate r(100.0);
    //ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 50);   
    //tf::TransformBroadcaster odom_broadcaster;
    std::ifstream file("/home/bert/catkin_ws/src/mastering_ros_demo_pkg/src/WheelEncoders1.txt");
    std::vector<int> data;

    if(file.is_open())
    {
        for(int i=0;i<36747 && file.good() ;i++)
        {
            double index;
            //Created a message
            //mastering_ros_demo_pkg::angle_msg msg;
            file >> index;
            file >> angle_left;
            file >> angle_right;
            file >> angle_timestamp1;
            file >> angle_timestamp2;
            file >> index2;
            file >> index3;

            //Printing message data
            //ROS_INFO("%f",index);
            ROS_INFO("%f",angle_left);
            ROS_INFO("%f",angle_right);
            ROS_INFO("%f",angle_timestamp1);

            //ROS_INFO("%f",msg.angle_timestamp2);
            //ROS_INFO("%f",index2);
            //ROS_INFO("%f",index3);



    //ros::init(argc, argv, "odometry_from_text_publisher");
    //ros::NodeHandle n;

    //tijd van de meting 
    ros::Time current_time_encoder(angle_timestamp1);


    //verschil in hoek tov vorige berekening 
    deltaLeft = angle_left - _PreviousLeftEncoderAngle;
    deltaRight = angle_right - _PreviousRightEncoderAngle;

    //snelheid links (x) en rechts (y)
    vx = deltaLeft * DistancePerRadian / (current_time_encoder - last_time_encoder).toSec();
    vy = deltaRight * DistancePerRadian / (current_time_encoder - last_time_encoder).toSec();

    //als snelheid links = snelheid rechts -> snelheid = snelheid links en hoeksnelheid = 0
    if(abs(vx-vy)<0.00001)  
    //if (vx == vy)
    {
        V = vx;
        W = 0;
    }


    else
    {
        // Assuming the robot is rotating about point A   
        // W = vel_left/r = vel_right/(r + d), see the image below for r and d
            double r = (vx * d) / (vy - vx); // Anti Clockwise is positive
            W = vx/r; // Rotational velocity of the robot
            V = W * (r + d/2); // Translation velocity of the robot
    }

    //vth = hoeksnelheid
    vth= W;

    //hoek onthouden 
    _PreviousLeftEncoderAngle = angle_left;
    _PreviousRightEncoderAngle = angle_right;

    //tijd van de laatste meting onthouden
    last_time_encoder = current_time_encoder;


    ros::Publisher odom_pub = n.advertise<nav_msgs ...