simulating sensor data, problem with rosbag and clock

edit

Hello

I wrote a simple simulator that generates sensor data (ideal values plus some noise). My goal is to test my localization algorithms (EKF). It seems to work, but I am having problems with recording a bag file.

Here is how it works: given an acceleration profile, I integrate it to obtain ground truth (position, orientation, velocity, etc.). I then generate encoder and IMU data from that ground truth. Once all the data has been generated, I publish it sequentially on respective topics, together with clock messages.

Here some part of the code:

ros::init(argc, argv, "sensorSimulator");
bool realtime = false; //value obtained from command line or parameters...
float period = 0.01; //period of the simulation

// Generate the data
// (...)
vector<StampedData *> data = profile.generateData();

// Create publishers on the following topics
// sensor_msgs::Imu on /ms/imu/data
// rosgraph_msgs::Clock on /clock
// lowlevel::Encoders (custom) on /encoders
// fmutil::SimpleOdo (custom) on /ground_truth_linear
// nav_msgs::Odometry on /ground_truth
// tf::TransformBroadcaster
// (...)


// Publish the data
for( unsigned i=0; i<data.size() && ros::ok(); i++ )
{
    rosgraph_msgs::Clock clockmsg;
    clockmsg.clock = ros::Time(data[i]->time);
    clockPub.publish( clockmsg );

    if( data[i]->type == ground_truth_t ) {
        // publish the ground truth odometry and TF
        // (...)
        if( realtime )
            ros::WallDuration(period).sleep();
    }
    else if( data[i]->type == imu_t ) {
        //publish IMU data
        // (...)
    }
    else if( data[i]->type == encoders_t ) {
        // publish encoders data
        // (...)
    }
}

This works: when I start rviz (with use_sim_time=true) and run my code with the real time option on, I can see the simulated vehicle moving on the desired trajectory. I can record all the data in a bag file as well.

However, if I want to generate the data and record it in a bag it does not work: roscore rosbag record -a -O sensorSimulation rosrun mypack sensorSimulation Once this is done, I CTRL-C the rosbag process. I can then inspect the content of the bag with rosbag info sensorSimulation.bag. Data is missing: only a few of the topics are recorded, and duration is wrong (too short).

I have an hypothesis to explain what's happening. I am publishing too fast on the topics, and some data gets lost. So I am wondering: is there a way to monitor the status of the publishers and associate mechanisms, and slow down the publishing process accordingly? Btw, I should mention that my publishers are created with a message queue of size 0 (infinite).