ROS bag synchronization problem with laser scan messages

asked 2016-12-17 17:31:00 -0500

zkytony
178 ●17 ●27 ●33

updated 2016-12-18 14:41:42 -0500

I have a raw csv data sequence of laser scans and odometry data. Here are a few data samples:

Laser scans:

25 1205853395.41 181  0.5429999828338623 0.5389999747276306 ... 
26 1205853395.51 181  0.5450000166893005 0.5429999828338623 ... 
27 1205853395.62 181  0.5450000166893005 0.5350000262260437 ...
28 1205853395.73 181  0.5400000214576721 0.5400000214576721 ...
29 1205853395.83 181  0.5479999780654907 0.5360000133514404 ...

Odometry:

901 1205853437.119040 0 -3.102520660503609 3.963312253867039 1.153641139388319 ...
902 1205853437.163038 0 -3.102512280308661 3.963330776419583 1.138202487770184 ...
903 1205853437.211029 0 -3.102511007946354 3.963333473265386 1.121727579355471 ...
904 1205853437.263017 0 -3.102505770651651 3.963344116867601 1.105313585686352 ...

In these data samples, the first column refers to the id for the data samples. the second column refers to the time stamp, in seconds.

I am trying to write a ROS-bag file that contains these data samples, and play the bag file, and use AMCL to do localization. The way I currently generate the ROS-bag file is by iterating through all data samples, and write a message (either LaserScan, or Odometry) for each data sample to the appropriate topics, with the timestamp directly obtained from these data samples.

The problem I observed is that, while playing the ROS-bag file, with AMCL running, the localization was pretty bad - the laser scan readings shown in RVIZ completely misaligned with the obstacles in the map, after 5 seconds or so.

After some investigation, I found that the source of the problem could be due to the time stamps written to the bag file. It appears that the time stamps that these messages are published are different from the time stamps when they were written to the bag file. Furthermore, the difference between laser messages' "published time stamp" and "true time stamp" is way larger than that for the odom messages, by around 5-7 seconds.

For example, for laser scan message with id=27, the publish time is 1205853412.16, but the true time for this message, as shown above, is 1205853395.62. On the other hand, for odometry message with id=902, the publish time is 1205853437.163038015, and the true time is 1205853437.163038, as shown above.

I believe that it's because of such difference in the delay caused the synchronization problem.

Although this phenomenon seems reasonable, because the LaserScan message is larger than Odometry due to the "ranges" array, it is something very undesirable. I haven't come up with a way to deal with this effectively. Has anyone encountered this issue before, or know something about how to resolve this? I really appreciate your help!

EDIT: 5-7 seconds should be over 10 seconds.

answered 2016-12-17 23:24:16 -0500

ahendrix

47576 ●179 ●367 ●662 http://namniart.com/

1205853412.16 - 1205853395.62 is about 16.5 seconds - that's quite a while. A medium-sized laser scan message absolutely should not cause a 16-second delay.

rosbag should be more than capable of playing these bag files back at a reasonable speed, without introducing a 5-15 second delay for a laser scan message. (rosbag can play back much higher bandwidth bag files in real time without issues).

There are often two timestamps for each message that is written to the bag file; the bag timestamp (which rosbag uses when replaying) and the timestamp in the Header, if the message contains a Header. The Header timestamp represents the actual time that the message was captured, AMCL will use the Header timestamp to help it correct for any delay in receiving the LaserScan message. You must write the Header timestamp correctly, and having the bag timestamp match the header timestamp is ideal if you're generating the bag file from another dataset.

I'm curious where the "publish time" that you report is coming from. Which tool or which message field are you getting the publish time from?

edit flag offensive delete link

Comments

Thanks for the insight. The published time is from the result of echoing /scan, and /mobile_base/odom, topics that the messages are written to. I did actually write the true time stamp to the header of each message. I also used that time as the third param for the bag.write() function.

zkytony ( 2016-12-18 00:44:45 -0500 )edit

@ahendrix, if you have time, check out this video: https://www.youtube.com/watch?v=LHc1n... for the problem I observed.

zkytony ( 2016-12-18 09:59:33 -0500 )edit

I agree with you that the size of the message shouldn't be the problem. I tested with a rosbag file recorded from data in simulation, and the laser ranges array has length of 921, much larger than what the length in the data sequences here. There is no synchronization problem for that.

zkytony ( 2016-12-18 10:08:18 -0500 )edit

Here is my code for writing the laser scan message:

# write laser scan message
bag.write("/scan", lsmsg,
            rospy.Time.from_sec(scan['time_stamp']))

Here, scan is a dict made by parsing the raw csv data. lsmsg is a LaserScan message, with header.stampassigned to ...

zkytony ( 2016-12-18 10:13:21 -0500 )edit

rospy.Time.from_sec(scan['time_stamp']) as well

zkytony ( 2016-12-18 10:14:21 -0500 )edit

It looks like you're writing timestamps to the bag file correctly. The localization in your video definitely looks poor. rostopic echo doesn't print timestamps by default, so I'm still don't understand what you're running to observe this timestamp issue.

ahendrix ( 2016-12-19 14:14:23 -0500 )edit

I hope you're not trying to use this to do localization in real time, while the robot is driving.

ahendrix ( 2016-12-19 14:15:39 -0500 )edit

Thanks @ahendrix. LaserScan message has a header field, and rostopic echo can display that, isn't it? Because each individual reading is almost unique (lots of decimal digits), I basically used a reading to search for the row for that reading in the csv file. (I read those reading from shell screen)

zkytony ( 2016-12-19 15:46:31 -0500 )edit

see more comments

ROS bag synchronization problem with laser scan messages

1 Answer

Comments

Question Tools

Stats

Related questions

ROS bag synchronization problem with laser scan messages edit

1 Answer

Comments

Question Tools

Stats

Related questions

ROS bag synchronization problem with laser scan messages