-
Notifications
You must be signed in to change notification settings - Fork 79
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
No IMU data with OS0-64 at 1024x20 or 2048x10 #55
Comments
We do not technically support the OS0 sensor because Tom nor I have one to work with. We can though evaluate if we see that behavior on our OS1's and move forward from there if there's a shared problem. My guess is its a shared problem, just wanted to be up front about that. What is the commit hash of the ROS1 driver that you're using? They did some updates recently to their UDP client library that aren't yet reflected here (#42), but if you saw it with a commit hash before that, then we should be able to fix it I imagine. If its after, then I think perhaps it would be helpful for you to port that work here to use. If you're using the newer version, then it would be great if you can see if you see that behavior with the old version. If so, then we have a clear answer of how to fix it, if not, then me and/or Tom (probably me) can look into it a bit with the hardware we have. Let me know what you think. |
Commit hash of the ROS1 version I am using is 68ad03c, which is the latest version. Moving back one commit to 1cf6faf, it still works just fine. I have just tested with an OS1-64 and I see the same behavior as with the OS0 in both ROS1 and ROS2. |
Works just fine meaning that you get IMU? So the issue is/was fixed by that update to the UDP client? |
With ROS1, the IMU data works as expected on both 68ad03c and 1cf6faf. Meaning that the update to the UDP client changed nothing with respect to this issue. |
To make it clearer:
|
I can check with an OS1-16 later today, but I am not surprised that the problematic modes are
Not surprised by this either. Changing the My hypothesis is that the way in which the incoming packets are handled, buffered, and processed by the driver is the root of the issue. When I observed what I saw with the Inside of the driver, there is a timer callback instantiated when the lifecycle node First, using a timer callback is a form of polling (with extra overhead). What would be preferred is a dedicated thread that reacts to incoming data on the socket buffer via a rising edge trigger. This would allow the driver to react to data on-demand as it is flowing into the host computer. This is also the lowest latency approach. Second, all the work mentioned that is done in There are other optimizations that I would make as well, related to this, but, I don't want to get too far off topic or come off as prescriptive (not my intention). It is just, I have worked extensively with the Ousters in ROS2 under real work loads using this driver and I believe I have zoned in on the fundamental challenges. Bottom-line, I have seen similar instabilities with the LiDAR data as you are seeing with the IMU data at the higher data rate modes. I do not believe these to be unrelated topics. I do not believe that the issue is inside the LiDAR hardware. I think the fix is in the driver and it is not simple if it is to be done correctly. |
Simple test of listening for ~20s on
Two consecutive runs, listening for ~20s, in
|
Thank you both very much for the reply's. The behavior you see with the /imu topic on the OS1-16 is exactly the behavior I am experiencing with the OS0-64 and OS1-64. I have just received beta ROS1 drivers from Ouster for the OS0-128 and it appears they work perfectly. I can run at 1024x20 with both IMU and point cloud data published at the expected rates (100Hz and 20 Hz respectively). What are the differences in the implementation of the Ouster ROS1 drivers and your ROS2 drivers? Is there a fundamental design difference that causes this performance gap? It seems strange to me that the ROS1 system can handle an OSx-128 at 1024x20 while the ROS2 system cannot properly handle an OSx-64 at 1024x20. That's more than a 2x performance difference between the two systems. Don't get me wrong, I'm not trying to criticize what you guys have done, just trying to understand the limitations and potential improvements that can be made to your ROS2 system. |
@tpanzarella 100% agree, I wanted to do event based rather than polling but I couldn't find anywhere in their public docs a reference to find if a new packet is available. Their ROS1 drivers polled as well without a throttle, which is a big no-no in robotics since that will essentially try to consume a core as a We could certainly buffer the data and have a separate thread process them, that would work, but I'm still wanting to avoiding a A trivial way to test this would be to increase the timer rate by something stupid high and see if the IMU packets come through. If they do, we know were we need to fix things. |
The data from the LiDAR are straight UDP, so, this is done with
Yeah, there is no need for a |
So, I double the rate at which the timer fires. Specifically: void OusterDriver::onActivate()
{
DataProcessorMapIt it;
for (it = _data_processors.begin(); it != _data_processors.end(); ++it) {
it->second->onActivate();
}
// speed of the Ouster lidars is 1280 hz
_process_timer = this->create_wall_timer(
390625ns, std::bind(&OusterDriver::processData, this));
//781250ns, std::bind(&OusterDriver::processData, this));
} Running in
Running my performance benchmarks in the same mode of operation yields the following: Raw jitter: And a quantile plot of the same: |
@tpanzarella what's the CPU load difference though? If that's not much higher, sure, lets do it. I'd much prefer to actually use the |
@SteveMacenski I agree. I think for now, we should jack this up. I did not do anything scientific here, I just doubled it. Let me do a little bit of analysis to look at the effect a bit more analytically. But, I think in the near term, we will want to ship something like this. |
Here is a look at running in 1280 HzRaw jitter:Quantile plot:CPU Utilization:NOTE: The dashed orange trace here is a plot of rxKB/s on the network interface connected to the LiDAR (see right y-axis). I've plotted that time correlated to the CPU core utilization (8 cores, hyperthreaded quad core machine) so that it "frames" the relevant time window on the plot -- i.e., when the bytes were flowing in from the LiDAR to the host PC. Summary stats on CPU idle time:
2560 HzRaw jitter:Quantile plot:CPU Utilization:NOTE: The dashed orange trace here is a plot of rxKB/s on the network interface connected to the LiDAR (see right y-axis). I've plotted that time correlated to the CPU core utilization (8 cores, hyperthreaded quad core machine) so that it "frames" the relevant time window on the plot -- i.e., when the bytes were flowing in from the LiDAR to the host PC. Summary stats on CPU idle time:
|
Ok, that seems pretty open and closed to me that this is a great option for now. |
When running the OS0-64 at either 1024x20 or 2048x10 modes, IMU data is not properly published. At lidar startup there is a burst of about 20 IMU messages published, then no more. Changing the os1_proc_mask parameter to only 'IMU' does not change the behavior.
When running in 1024x10, 512x20 and 512x10 modes, IMU data is published at the correct rate (100 Hz).
When using the Ouster provided ROS1 driver, IMU data is correctly published in 1024x20 and 2048x10 modes which suggests this is not an issue on the lidar side, but on the host side.
Additional information:
The text was updated successfully, but these errors were encountered: