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 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; itime);
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).
↧