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::Odometry>("odom", 50);   
    tf::TransformBroadcaster odom_broadcaster;


    //ros::Time current_time, last_time;
    //current_time = ros::Time::now();
    //last_time = ros::Time::now();
    ros::Time current_time(angle_timestamp1);
    ros::Time last_time(angle_timestamp1);


    //ros::Rate r(100.0);

    //while(n.ok())

    //{

            //current_time = ros::Time::now();
        //ros::Time current_time(angle_timestamp1);

            //compute odometry in a typical way given the velocities of the robot
            double dt = (current_time - last_time).toSec();
            double delta_th = vth * dt;
            th += delta_th;
            double delta_x = V * cos(th) * dt;
            double delta_y = V * sin(th) * dt;  
        // double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
        // double delta_y = (vx * sin(th) + vy * cos(th)) * dt;


            x += delta_x;
            y += delta_y;


            //since all odometry is 6DOF we'll need a quaternion created from yaw
            geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);

            //first, we'll publish the transform 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 = x;
            odom_trans.transform.translation.y = y;
            odom_trans.transform.translation.z = 0.0;
            odom_trans.transform.rotation = odom_quat;

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

            //next, we'll publish the odometry message over ROS
            nav_msgs::Odometry odom;
            odom.header.stamp = current_time;
            odom.header.frame_id = "odom";

            //set the position (actuele positie van de robot)
            odom.pose.pose.position.x = x;
            odom.pose.pose.position.y = y;
            odom.pose.pose.position.z = 0.0;
            odom.pose.pose.orientation = odom_quat;

            //set the velocity
            odom.child_frame_id = "base_link";
            odom.twist.twist.linear.x = V;  //snelheid in de x-richting
            odom.twist.twist.linear.y = 0; //snelheid in de y-richting
            odom.twist.twist.angular.z =W;  // hoeksnelheid robot

            //publish the message
            odom_pub.publish(odom);

            last_time = current_time;
/*      
            //ros::spinOnce();
            //r.sleep();
    //}

*/
        }

        ROS_INFO("End of file");
    }
        else 

    {
        ROS_INFO("file not found!");
            ros::shutdown();
    }




}

Thanks!