Hokuyo LiDAR misses scan

Question

I am working on a robocar project. The car has Nvidia jetson TX2, LiDAR, camera, IMU, etc and the ROS node is started with 4 thread

ros::MultiThreadedSpinner spinner(4)
spinner.spin(); 

The LiDAR is Hokuyo UST-10LX. I used the following code to subscribe to the scan data:

_subScan = _nh.subscribe("/scan", 4, &scanCallback, this, ros::TransportHints().tcpNoDelay());

In the callback function, the car processes the scan to localize itself and then send out Ackermann drive command to move along a pre-defined path. Hokyyo UST-10LX is 40hz which provides very dense data for the car to run smoothly.

However, I did found that in some cases, my ROS node missed quite some scan frames. It could miss as many as 40 scans. So the car paused for about a second, did not know where to go since the callback function was not called.

My questions are:

1. Why the scan frames were missed and callback funciton was not called?
2. If this could happen, what is the best way to handle this? Since all my localization and drive code are inside the LiDAR callback, if I missed the scan, the car will stop.

Thanks,

---

Originally posted by AutoCar on ROS Answers with karma: 102 on 2019-05-20

Post score: 0

---

Original comments

Comment by stevemacenski on 2019-05-21:
check to make sure its not your application that uses scanCallback that is actually taking too long, then blocking the rest. It's unlikely the laser is not reporting data for 1 full second, or that there was a pipe delay. With your multithreaded master, you can process 4 measurement at a time (hope your not having mutex contention or fighting over a static member), and your queue size of 4 means if there's more than 4 messages in the queue it will drop them.

Are you seeing in rostopic hz that the scan message is dropping for a second vs. the application running slower?

Comment by gvdhoorn on 2019-05-21:\

what is the best way to handle this? Since all my localization and drive code are inside the LiDAR callback, if I missed the scan, the car will stop.

Combining @stevemacenski's comment with your own observation, the best way to avoid this would be to not run your control law inside the callback. If your CPU isn't fast enough to process everything in 25ms (preferably less), then messages will be missed, dropped or processed with a delay.

Comment by AutoCar on 2019-05-21:
@stevemacenski Jetson TX2 has 4~6 CPU cores, so I hope the way I started the ROS will make sure the 4 measurements will be processed at different cores. Is that correct?

Comment by stevemacenski on 2019-05-21:
4 of those 6 cores are very weak. The A57's aren't exactly powerhouses. And if you're doing stuff on the other cores (like running any other process) then you're not going to get all that power at once. You're leaving it up to the scheduler.

Answer 1

I found the problem. In the scanCallback, for every LiDAR frame, the latest LiDAR, camera image, IMU, odom data was saved to a file so I can debug offline. I did not use ROS bag file because I believe my own format is more compact. Regular fopen, fwrite were used when saving this data. I found sometimes, it took a long time (a couple seconds) to save even a very small amount of data, odom for example. I do not understand why...

---

Originally posted by AutoCar with karma: 102 on 2019-05-22

This answer was ACCEPTED on the original site

Post score: 0

---

Original comments

Comment by gvdhoorn on 2019-05-22:\

I do not understand why...

because file IO is slow, you don't have a real-time (ie: deterministic) system and inserting a potentially blocking operation in an event handler is not a good idea in general.

That is exactly why rosbag listens in on the message traffic: it does not insert itself into the critical path of your application, avoiding these kinds of delays while still recording the data.

Comment by stevemacenski on 2019-05-22:
Glad you found your issue. Please mark the answer as correct to get off the unanswered questions queue :-)

Comment by AutoCar on 2019-05-22:
Thanks. What is the correct way to save the data...

Comment by stevemacenski on 2019-05-22:
Add your data to a queue or something and have a thread saving the queued messages to file. In general you don't want "long blocking" operations in your callbacks if you care about making sure you see all the data.