Laser scan from ROS 2 driver is not properly aligned with point cloud

[tanelikor]

Describe the bug
The laser scan and point cloud provided by the ROS 2 driver are misaligned by about 2-3 degrees.

This issue exists also on the community driver, but I believe it is not yet known what causes it there: ros-drivers/ros2_ouster_drivers#88

To Reproduce
Steps to reproduce the behavior (steps below are just an example):

1. source ros environment
2. compile and source the project workspace
3. ros2 launch ouster_ros sensor.composite.launch with proc_mask parameter that includes both SCAN and PCL options and with scan_ring as one of the "middle rings" (e.g. 15 or 16 with a 32 ring lidar)
4. visualize the point cloud (/ouster/points) and scan (/ouster/scan) in Rviz
5. observe the misalignment between the scan and pointcloud

Screenshots
Here are a couple of screenshots about what the problem looks like.

Platform (please complete the following information):

• Ouster Sensor: OS-0 32 Rev D (we also have some devices with newer hw revisions, but I have not yet gotten to test this on them)
• Ouster Firmware Version: Tested on 2.0.0 and 2.5.2.
• ROS version/distro: Humble
• Operating System: Ubuntu 20.04 running (Ubuntu 22.04 based) ros:humble Docker image
• Machine Architecture: x64
• git commit hash: 556dbd0

[Samahu]

Hi @tanelikor, I will take a close look at the issue and address ASAP. Thanks for reporting it.

[Samahu]

@tanelikor I have created two pull requests that should fix the issue. Could you pull any of the two branches and let me know if this resolves the issue for you please. Thanks!

[tanelikor]

@Samahu Thanks for the quick action! I will give it a try as soon as I can (shouldn't be later than Monday) and report back to you.

[tanelikor]

@Samahu Turns out that I found a bit of time to check this already today. I built the driver from the humble PR branch, and it looks like the difference between the scan and cloud is definitely much better. Not quite 100% match (the shift in the scan seems to be already a bit too much) but close, see the screenshots below.

Note: I am using our own Rviz config here (instead of the one in this package) because it was faster for me to spin up that one, but I don't think this should make any difference.

[Samahu]

Thanks for the feedback @tanelikor, I didn't have the right setup to properly verify these changes due to the location of the sensor I work with (surrounded by a lot of clutter), but I will take another stab at it and update you.

[Samahu]

@tanelikor I pushed an update to #203 and #204 which should better align the laserscan with the point cloud. Here is a screen shot of my own verification:

There are minor offsets which are not accounted for atm.

[tanelikor]

@Samahu Thanks! I will try the updated version and report back as soon as I can.

[tanelikor]

@Samahu On our device, it looks like one side (left) of the scan is now well aligned with the point cloud, but the other one is off by a lot. See screenshots below, and let me know if you'd like for me to check something else as well.

This was on the humble branch again.

[Samahu]

I will take another look at it. Would you be willing to share a small recording (raw packets) 1 min of your scene. It would be much easier for me to fire up the driver and observe the same mismatch.

[tanelikor]

Sure. Here is a compressed bag recording of packets:
os_scan_cloud-2-compressed.zip

[Samahu]

@tanelikor the file does not contain packet topics, you can did inspect the file using ros2 bag info and notice it only has the metadata but I didn't see any raw packets.

Was the file captured using the provided record launch file?

ros2 launch ouster_ros record.launch.xml bag_file:=bag_file_name

[tanelikor]

@Samahu Oh, this was my mistake. I did record the file using the launch file you mentioned (with some extra arguments due to the setup but still) and did check that it had data. However, bag was/is too big to upload here, so I compressed it with ros2 bag convert. The mistake was that I did the compressing in an environment that didn't have the driver and necessary messages built, so the messages got lost. I also forgot / didn't think to verify the contents of the compressed bag again.

I now tried the compressing again and this time it worked, but the bag is still too large to upload here. So here is a link to the non-compressed bag in drive. I'll probably delete the file from drive once you have downloaded it as it is somewhat sizeable.

[Samahu]

I have downloaded the file if you want to delete it. I was able to verify that the problem actually exist.

[Samahu]

@tanelikor thanks for providing the bag file. Please checkout the updated branches. Based on the bag file and the update I just made here is the result (I nullified other rows from the point cloud for better visibility):

This image was produce using ortho view. You should note that the alignment is correct this time. Note that as you move further from the center of the point cloud there would be some notable differences in the distance between the points of LaserScan vs points of PointCloud this is due to the fact the LaserScan is a planar message while the points produced in the point cloud have some tangent with plane .. so as you move further there is going to be larger distance between the two. However, this shouldn't affect the alignment. Please let me know if this addresses the issue on your end.

[tanelikor]

@Samahu Yes, I'm aware of the distortion caused by not having a 0 degree scan ring, and I agree that it shouldn't affect to this issue. I suppose that a way to get rid of the distortion would be to re-compute the range from x and y coordinates, but that would also change the nature of the scan a bit and is probably an entirely different discussion.

About the fix. I tried it out and also did some additional testing on a couple different devices, and have some mixed news.

On the device that I used to take the previous screenshots and also the bag, it still looks like half of the scan is aligned well while the other half is a bit off. See the couple screenshots below for visual reference. This device is OS-0-32 Rev-D and is (and has been during the investigation of this issue) running the latest firmware version 2.5.2.

Seeing this, I got curious and tested this on a couple other devices of ours that I currently have access to. Both are Rev-06 of the same lidar model (OS-0-32), and were initially running firmware version 2.2.1. On both of them, the situation looked like this: the offset was in the opposite direction and much more than in my opening comment.

I then went and updated one of the Rev-06 devices to the latest firmware version of 2.5.2. After this, the scan and point cloud alignment seemed to be spot on, see image below.

So, I'm not sure whether to consider this issue solved, since I'm getting different results with different devices. In any case, it seems like this also has something to do with the hardware / firmware versions of the devices. I hope this information helps.

[Samahu]

@tanelikor sorry for leaving this issue stale for a while since I had to shift focus to other stuff within ouster org.

I did pushed a fix to the towards the two open PRs which I have tested against three different generations of Ouster sensors

Note that I have only used FW 2.4 and FW 3.0

If you got time to checkout the update and give feedback asap so we can close this issue for good this time.

Note: there is one remaining issue with the dual mode which I am investigating so please avoid this mode for now.

[tanelikor]

@Samahu Thanks for getting back to it. I also wanted to do some more testing on this but couldn't find the time to dig deep enough to find anything notable.

I will try to test this by Monday evening (if I can't get it done by then, the next chance will likely be at the start of December). Since the last try, we did update all our devices to FW 2.5.2, so sadly I think I can't re-test the situation with the older versions, but I will test what I can.

[Samahu]

@Samahu Thanks for getting back to it. I also wanted to do some more testing on this but couldn't find the time to dig deep enough to find anything notable.

I will try to test this by Monday evening (if I can't get it done by then, the next chance will likely be at the start of December). Since the last try, we did update all our devices to FW 2.5.2, so sadly I think I can't re-test the situation with the older versions, but I will test what I can.

@tanelikor Since you reported that you upgrade to 2.5.2 I went ahead and upgraded one of my sensor to it and the last fix works as I expected. With that I have verified that FW 2.4, FW 2.5.2 and FW 3.0 all work fine with the submitted fix. However, out of curiosity, I wanted to try a FW prior to 2.4 so I went ahead and downgraded one of my sensors to 2.3 and to my surprise the LaserScan msg was skewed 🤯!

I have also confirmed that all FW prior to 2.4 behave differently too. I am following up with Ouster FW team to understand what has changed on their part (I have some speculations but will wait a bit before digging further).

[tanelikor]

@Samahu That looks quite bizarre indeed, even weirder than what I saw earlier with FW 2.2.1. 😳 Definitely something has changed quite drastically, hopefully you can find out what.

I don't expect to have enough time to start downgrading back to earlier versions on our side. I'm not expecting problems with 2.5.2, but will give it a try to be sure.

[Samahu]

I amended my changes to the submitted fix such that the driver exhibit the same behavior across all firmware (verified it on FW 2.1 and onward).

[tanelikor]

@Samahu I gave it a try, but it doesn't look right. Half the scan ("left" side) is good, the other half is off. This is on a Rev 6 device with FW 2.5.2, the only one that I had easy access to today. Here's a screenshot:

I also tried the last commit from Wednesday (Nov 22nd), but it looked the same:

Let me know if I can or should try something else. I will be on a work trip until Thursday evening, but can pick this up again on Friday or next week.

[Samahu]

It is weird that I don't get the same output. I will try to re-produce on Rev 6 with FW 2.5.2.

[tanelikor]

Yeah, very weird. I guess it's possible that there is something wrong with the particular device that we have, but we haven't been having any other notable problems with it or the other ones that I have posted results from in this issue. 🤔

[Samahu]

Based on my own tests I was getting correct values when setting the ring with an even value but the misalignment in LaserScan happen when choosing an odd ring value. The latest changes should fix the issue. Please pull and check the latest at your earliest convenience.

[tanelikor]

Oh, wow... I have indeed been testing it with ring 15. I'll give the new version a try on Friday.

[Samahu]

I feel good about the last fix and I think I am going to go ahead and merge it (the merge will probably automatically close this issue but can still comment and give feedback).

[tanelikor]

@Samahu I tested it (with the same device as the previous test), and now it looks great! 😃 Also checked with ring 16 just to be sure, and that one is also good. I'll add screenshots below for confirmation.

[Samahu]

Finally 😄 🎉
(thank you so much for verifying this issue till we had the proper fix, I didn't know that there was some variance based on FW).