0
$\begingroup$

Rosanswers logo

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).


Originally posted by brice rebsamen on ROS Answers with karma: 1001 on 2011-11-08

Post score: 1

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

Probably the biggest issue is that rosbag does not subscribe to your topics in time. When recording all topics (rosbag -a), rosbag has to poll the master periodically to discover new topics, so it may miss initial messages on any topic. In this case, you're publishing in a quick burst, so it may even miss all the messages on some topics.

Your hypothesis is also possible. rosbag uses an internal buffer of 256MB by default, which you might be filling up before rosbag can write everything to disk. You can use an infinite buffer instead (rosbag --buffize=0), though that's not recommended for any live system.

So in theory, you could make your setup work by giving rosbag an infinite buffer and explicitly telling it which topics to record. You'd probably also want your simulator to sleep for a couple seconds after creating the publishers, to give the socket connections the chance to form. Even then you're not 100% guaranteed to get a bag containing every single generated message.

Fortunately, there's a better way! Instead of using the rosbag command-line tool, generate your bag directly from your simulator using the rosbag API. No need to worry about network transport and buffering, you can just write data to a file.

Final note: when using the rosbag API, you don't need to write the /clock messages. When you write a message, you also give a time. On playback, use rosbag play --clock mydata.bag. The rosbag tool will set /use_sim_time=true and publish /clock for you.


Originally posted by Patrick Mihelich with karma: 4336 on 2011-11-08

This answer was ACCEPTED on the original site

Post score: 4


Original comments

Comment by brice rebsamen on 2011-11-10:
I started a new question more specific to the TF issue: http://answers.ros.org/question/2858/how-to-write-tf-broadcasts-in-a-bag-with-the

Comment by brice rebsamen on 2011-11-10:
$ rostopic type /tf gives tf/tfMessage

Comment by Patrick Mihelich on 2011-11-10:
http://www.ros.org/doc/electric/api/tf/html/msg/tfMessage.html. It's just a vector of geometry_msgs/TransformStamped. While running your simulation in live mode, you can do 'rostopic echo /tf' to see just what tfBroadcaster is publishing.

Comment by William on 2011-11-10:
Experimentally: https://gist.github.com/1354948 . As for its formal definition, http://www.ros.org/doc/api/tf/html/c++/classtf_1_1tfMessage.html. I don't really know the reason for this setup, but maybe it will give you a lead.

Comment by brice rebsamen on 2011-11-10:
According to the tutorial on writing a broadcaster, I should broadcast a tf/StampedTransform. Actually I could not find a definition for tfMessage. I tried writing both a tf/StampedTransform and geometry_msgs/TransformStamped. The first cannot be serialized, and the later results in the above error.

Comment by William on 2011-11-10:
Ah I miss read your comment, so the data you inserted into the bag file is on one message type and rviz is expecting a different msg type from that topic? As far as I know if you write something to /tf it is always expected to be of the type tf/tfMessage. Sorry if this isn't helpful.

Comment by brice rebsamen on 2011-11-10:
Thanks William. I've read that tutorial. However, I don't want to publish a tf broadcast, I want to write it in the bag. I am using the rosbag API here.

Comment by William on 2011-11-09:
[c++ tf broadcaster] (http://www.ros.org/wiki/tf/Tutorials/Writing a tf broadcaster (C++))

Comment by William on 2011-11-09:
That message means that you are publishing a msg of one type to a topic and rviz is subscribed to the topic with a different type. I think You need to use a tf broadcaster not just a normal publisher for this. This is a basic example of that:

Comment by brice rebsamen on 2011-11-09:
(continued) I tried to write a geometry_msgs/TransformStamped to /tf but I got this error when playing the bag and visualizing in rviz: "Client [/rviz_1...] wants topic /tf to have datatype/md5sum [tf/tfMessage/9...], but our version has [geometry_msgs/TransformStamped/b...]. Dropping connection."

Comment by brice rebsamen on 2011-11-09:
Hmm... when replaying my bag and visualizing with rviz, it complains that my frame "ground_truth" does not exist. I think this is because the transform broadcast is missing in the bag. How do I write stamped transforms in the bag?

Comment by brice rebsamen on 2011-11-09:
Perfect. Using the rosbag API was very easy and it worked like a charm. Thanks.

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.