Ask Your Question
1

simulating sensor data, problem with rosbag and clock

asked 2011-11-08 16:34:26 -0500

brice rebsamen gravatar image

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

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
4

answered 2011-11-08 19:58:52 -0500

Patrick Mihelich gravatar image

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.

edit flag offensive delete link more

Comments

Perfect. Using the rosbag API was very easy and it worked like a charm. Thanks.
brice rebsamen gravatar image brice rebsamen  ( 2011-11-09 12:45:14 -0500 )edit
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?
brice rebsamen gravatar image brice rebsamen  ( 2011-11-09 18:36:17 -0500 )edit
(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."
brice rebsamen gravatar image brice rebsamen  ( 2011-11-09 19:16:02 -0500 )edit
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: <next comment>
William gravatar image William  ( 2011-11-09 21:36:36 -0500 )edit
[c++ tf broadcaster] (http://www.ros.org/wiki/tf/Tutorials/Writing a tf broadcaster (C++))
William gravatar image William  ( 2011-11-09 21:36:41 -0500 )edit
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.
brice rebsamen gravatar image brice rebsamen  ( 2011-11-09 23:44:54 -0500 )edit
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.
William gravatar image William  ( 2011-11-09 23:52:58 -0500 )edit
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.
brice rebsamen gravatar image brice rebsamen  ( 2011-11-10 00:13:12 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

Stats

Asked: 2011-11-08 16:34:26 -0500

Seen: 4,059 times

Last updated: Nov 08 '11