Firmware 2.0 compatibility

MaxandreOgeret commented 3 years ago

Big thanks to Ouster for lending me a sensor to continue this development !

fixes #66

Brought client directory from https://github.com/ouster-lidar/ouster_example This PR still needs a lot of cleaning.

Include

ros2_ouster
- client
  - client.h
  - types.h
  - version.h
  - impl
  - netcompat.h
  - parsing.h

src

client
- client.cpp
- netcompat.cpp
- types.cpp

I followed the idea in this comment : https://github.com/ros-drivers/ros2_ouster_drivers/issues/63#issuecomment-742027046. But a lot of changes had to be made nonetheless. For example ClientState became client_state and Metadata became client_info. Also the client_state can now have several values that can be determined with a bitwise operation. (See toString in conversions.h)

It compiles BUT it's not functionnal. It doesn't publish anything at the moment. I reaches the process function in the processor, for example pointcloud_processor.hpp but never actually publishes anything. It seems that the _batch_and_publish function fails somehow and that we never reach it.

And at this point I'm kinda stuck, I don't know what is wrong. Could someone have a look at it ? @SteveMacenski ?

Also, should the class LidarScan be brought to the repo ? Because it seems that how pointclouds are currently computed is deprecated somehow. In the ouster repo they compute scans first and then pointclouds from scans. The files OS1_packet.hpp, OS1_sensor.hpp, OS1_util.hpp

If we are required to compute the pointclouds from scans how do you think we should proceed because at the moment we compute it from the packet_data. But maybe there's still a way to do it.

Also, In the ouster repo there is no mention of OS1 sensor, does it still make sense to have a OS1 class and not something like sensor ?

Thanks for your help, I will continue to work on this when I have time again.

anderwm commented 3 years ago

Echo that this is more complicated than meets the eye. There are types in the V2 example that conflict with types from V1x and don't necessarily do the same thing. With only a shallow understanding of the code here, it is hard to piece it all back together as the writer intended.

SteveMacenski commented 3 years ago

@MaxandreOgeret please explain where you've tracked the data down to (e.g. where's the last place in the code you know its right or where's the first part of the code you know something's not triggering).

@anderwm do you see something wrong here or a function we're misusing?

Also, should the class LidarScan be brought to the repo ?

I don't understand the question, please provide links :smile: .

I reaches the process function in the processor, for example pointcloud_processor.hpp but never actually publishes anything.

Did you check that the packets its given are "real"? I'd try just publishing data each iteration to the terminal and soft sanity check the ranges / values look "real". The batch function didn't change it doesn't look so the only thing that could have changed are the inputs or how the inputs are formatted.

Also, In the ouster repo there is no mention of OS1 sensor, does it still make sense to have a OS1 class and not something like sensor ?

We can rename it once we get it working. I'd rather focus on functionality and then go back over and make sure that we make use of all the right things and named right.

anderwm commented 3 years ago

Also, should the class LidarScan be brought to the repo ? Because it seems that how pointclouds are currently computed is deprecated somehow. In the ouster repo they compute scans first and then pointclouds from scans. The files OS1_packet.hpp, OS1_sensor.hpp, OS1_util.hpp If we are required to compute the pointclouds from scans how do you think we should proceed because at the moment we compute it from the packet_data. But maybe there's still a way to do it.

@MaxandreOgeret 's point above is kind of what I'm struggling to understand as well. I freely admit I am nowhere close to understanding all you have done here. However, I can't see how the functionality exposed in their LidarScan class can be replicated. Or even if it can be, if it would be prudent to do so. Particularly, the way ScanBatcher makes a LidarScan from a scan of data seems like a significant shift from the paradigm they used before. On the other hand, bringing in LidarScan opens up a whole different sack of ferrets (mostly about what gets stripped out/ where it hooks into the ros2_ouster code).

It could also be that I don't understand the code well enough to see the easy way out...

SteveMacenski commented 3 years ago

IMO anything in this directory https://github.com/ros-drivers/ros2_ouster_drivers/tree/foxy-devel/ros2_ouster/include/ros2_ouster/OS1 is fair game to nuke. We need to have some time of abstraction (https://github.com/ros-drivers/ros2_ouster_drivers/blob/foxy-devel/ros2_ouster/include/ros2_ouster/OS1/OS1_sensor.hpp) for the specific sensor so that we separate the driver from the sensor and I want to make sure we continue to follow the processor (https://github.com/ros-drivers/ros2_ouster_drivers/tree/foxy-devel/ros2_ouster/include/ros2_ouster/OS1/processors) design for throwing a packet from the sensor at these processors so that you can create different data structures (XYZ point cloud, 2D laser scan, IMU, XYZI Pointcloud, etc). Having the 3 nodes like ROS 1 is incredibly inefficient and a bad idea, and that's why we have the processors so that a single packet from the hardware can be given to essentially all 3 processing modes all at once, if enabled (and allows users to make even more of them like the 2D scan).

How that data is buffered and what functions we use (e.g. remove the batch iterator lambdas) is totally fair game to remove if the V2 code demands it. The ROS 1 driver changes how it batches packets into complete pointclouds to return (looks like it does https://github.com/ouster-lidar/ouster_example/blob/master/ouster_ros/src/os_cloud_node.cpp#L60) I'm not married to keeping it in the V1.x styling at all. That batch iteration function is all from the v1 code and I don't care at all if we keep it, my goal in fact would be to make sure we have the most minimum (if any) changes to the ouster client code so its a near-mirror and the processor functions update appropriately with the new styling.

MaxandreOgeret commented 3 years ago

Thanks both of you for this analysis. I need to wrap my head around all this and continue working on it. I should have some time this weekend.

please explain where you've tracked the data down to (e.g. where's the last place in the code you know its right or where's the first part of the code you know something's not triggering).

For me the connection with the lidar is OK, polling is OK, reading the packets is OK. When I output the _lidar_packet as hex I get data so it should be fine.

I manage to get into the process function of the processor but then nothing happens, it seems that we never get into the _batch_and_publish lambda. And the lambda is very dense and I'm not sure what is happening there.

For example I cannot output anything with cout in the lambda.

Did you check that the packets its given are "real"?

They seem to be ! Here is the output in hex of one packet : https://pastebin.com/WwR39rEV

It seems like it would be a good idea imitate the way packets are batched in the ROS1 client in the processors. But as you can see here they are computing scans and then later pointclouds.

What I see from :

https://github.com/ouster-lidar/ouster_example/blob/master/ouster_client/src/lidar_scan.cpp (Where scans are generated)
https://github.com/ouster-lidar/ouster_example/blob/master/ouster_ros/src/ros.cpp#L67 (Where scans are turned into PC2)

It seems that once you have a scan message you are very close from having a pointcloud2 message. Does it make sense to have two processors computing approximately the same thing but outputting data in a different format ?
It could be worth it to merge the scan and pointcloud processor into one, what do you think ?

BTW, what was the idea behind using lambdas ?

SteveMacenski commented 3 years ago

I manage to get into the process function of the processor but then nothing happens, it seems that we never get into the _batch_and_publish lambda. And the lambda is very dense and I'm not sure what is happening there.

So maybe part of the disconnect here, that lambda is not mine, that's from the V1.x client from ouster itself. So it might be that things look so different now that that just doesn't work anymore. you can just remove that lambda and use whatever the new batching system is for the V2.x client instead.

It seems like it would be a good idea imitate the way packets are batched in the ROS1 client in the processors. But as you can see here they are computing scans and then later pointclouds.

Sure but look at that function: https://github.com/ouster-lidar/ouster_example/blob/9352730127c3e2bc8ce1e778be4ae8b0a3b40f3c/ouster_ros/src/ros.cpp#L67, its trivial. All it does is iterate over the data to populate the pointcloud. That can just be removed if the pointcloud processor is doing that in parallel. Maybe there's some word choice issue here, in this driver "scan" is the 2D laser scan like from a 2D lidar. In the new ouster updates, I think its just a full rotation of the 3D lidar accumulating the packets to then populate the pointcloud.

I haven't looked at the scan class that closely, but it seems like its just the laser scans from the sensor (e.g. the pointcloud) in an internal data structure that's easier to store into and then converted into a PC2 type for publication. It doesn't seem to me that it does anything worth keeping. It seems like the packets are "batched" to a full rotation "scan" which is then converted into a "pointcloud" https://github.com/ouster-lidar/ouster_example/blob/master/ouster_client/src/example.cpp. The only reason that I think the scan is useful is if it is used to convert into the depth / reflectivity / intensity images too, but I don't see that happening in the code.

MaxandreOgeret commented 3 years ago

you can just remove that lambda and use whatever the new batching system is for the V2.x client instead.

Yep, will do that.

Maybe there's some word choice issue here (...) I think its just a full rotation of the 3D lidar accumulating the packets to then populate the pointcloud.

Indeed it seems so, things seem to be a bit clearer now, thanks for catching this.
So we would need to 'skip' the scan. I will try to see what I can do.

SteveMacenski commented 3 years ago

Got it, sounds good! The 2D scan thing is something I added to the ROS2 drivers (and more of the image topics) because alot of people just want the 2D laser scan as another topic for localization / mapping on embedded systems but use the full 3D pointcloud for collision avoidance. You could always extract it later, but its more efficient to do it at this point.

MaxandreOgeret commented 3 years ago

Today I got the first publishings from the sensor, it' s not working yet and I suspect the packages are wrong. I need to have a look at the _pf variable again, as it might be the cause of the problems.

But I' m on the right path ! :smile:

I will clean the https://github.com/ros-drivers/ros2_ouster_drivers/tree/foxy-devel/ros2_ouster/include/ros2_ouster/OS1 directory as I adapt each processor.
I also got rid of the lambda and implemented a standard function. I think it' s way cleaner like that,
It would be nice if someone could have a look at the point processor already.

Also I am a bit slower because I had to bring the sensor back to the lab, and I work on this on my free time at home and test it at work.

Invalid pointcloud2 message :

``` --- header: stamp: sec: 5269 nanosec: 860104870 frame_id: laser_data_frame height: 128 width: 1024 fields: - name: x offset: 0 datatype: 7 count: 1 - name: y offset: 4 datatype: 7 count: 1 - name: z offset: 8 datatype: 7 count: 1 - name: intensity offset: 16 datatype: 7 count: 1 - name: t offset: 20 datatype: 6 count: 1 - name: reflectivity offset: 24 datatype: 4 count: 1 - name: ring offset: 26 datatype: 2 count: 1 - name: ambient offset: 28 datatype: 4 count: 1 - name: range offset: 32 datatype: 6 count: 1 is_bigendian: false point_step: 48 row_step: 49152 data: - 0 - 0 - 0 - 128 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 128 - 63 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 128 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 128 - 63 - 0 - 0 - 0 - 0 - 90 - 69 - 220 - 3 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 128 - 63 - 0 - 0 - 0 - 0 - 136 - 71 - 34 - 204 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - 0 - '...' is_dense: true --- ```

SteveMacenski commented 3 years ago

I also got rid of the lambda and implemented a standard function. I think it' s way cleaner like that,

Awesome! Yeah that lambda was a real pain to work through myself when I ported that from the ROS 1 drivers. I think the PC2 processor is the easiest to start with (easiest to visually verify). Then IMU, laser scan, then the image processors.

Unfortunately, I'm still not on V2 yet to be able to test any changes :( I'm also slammed with IROS deadlines so I can't really take a look at this right now.

MaxandreOgeret commented 3 years ago

Thanks for your comment ! No problem I understand, I'm also not very fast myself. I should get a second sensor in a few weeks dedicated only to development so it will go faster starting from there. I tested today and I have a problem somewhere between getting the packet data and turning it into a pc2. It's not fast but I'm on it ! :)

SteveMacenski commented 3 years ago

No worries, you're on it more than I am right now, so all the credit to you :laughing:. Q1 of this year hasn't gone at all how I expected it to - sorry, this is by no means the only thing I've not been as active on as I would like due to other things that have exploding deadlines on my side!

MaxandreOgeret commented 3 years ago

I have some questions @SteveMacenski.

Sorry for the flow development time, it's hard to test what I do as I have to run to the lab after working hours as I am remote working because of the covid.

I'm still not having a working solution yet, I get a low publishing rate (approx 3Hz) so I guess something is wrong with the batcher somehow and I am debugging this at the moment.

My questions are :

What is happening here : https://github.com/ros-drivers/ros2_ouster_drivers/blob/2a2a7db5aebe17ea05b6ad6e04047ad5cfc0f0ad/ros2_ouster/src/ouster_driver.cpp#L208 With the std::pair and the iterators. And why to we always take the second to call process ? It's a bit fuzzy to me. Is this part of the code documented or could you explain ?
- How is it defined what type of package (imu or lidar) does a processor needs to get ? For example I guess the pointcloud processor should get only lidar packets and not imu. Am I right ? How is it managed ?

Thanks for your help !

SteveMacenski commented 3 years ago

https://en.cppreference.com/w/cpp/algorithm/equal_range, because the second is the data processor pointer https://github.com/ros-drivers/ros2_ouster_drivers/blob/5f2435dd9443ead4e2ea51786b398e1d8bbf0d65/ros2_ouster/include/ros2_ouster/ouster_driver.hpp#L49 the first is the ClientState.

https://github.com/ros-drivers/ros2_ouster_drivers/blob/2a2a7db5aebe17ea05b6ad6e04047ad5cfc0f0ad/ros2_ouster/include/ros2_ouster/OS1/processor_factories.hpp#L132 the processor factory sets this

MaxandreOgeret commented 3 years ago

I just got the sensor, kindly lent by Ouster, to allow me to finish this project.

There is something I noticed that I need to figure out now. It's possible now that when asking for the client state, it will return LIDAR_DATA AND IMU_DATA.

Here's the output of this piece of code :

uint8_t * OS1Sensor::readPacket(const ouster::sensor::client_state & state)
{
  if (state & ouster::sensor::client_state::LIDAR_DATA) {
    std::cout << "LIDAR_DATA" << std::endl;
  }
  if (state & ouster::sensor::client_state::IMU_DATA) {
    std::cout << "IMU_DATA" << std::endl;
  }
  std::cout << "---------------------------------" << std::endl;

[...]

}

[...]
[ouster_driver-1] ---------------------------------
[ouster_driver-1] LIDAR_DATA
[ouster_driver-1] IMU_DATA
[ouster_driver-1] ---------------------------------
[ouster_driver-1] LIDAR_DATA
[ouster_driver-1] IMU_DATA
[ouster_driver-1] ---------------------------------
[ouster_driver-1] LIDAR_DATA
[ouster_driver-1] IMU_DATA
[ouster_driver-1] ---------------------------------
[ouster_driver-1] LIDAR_DATA
[ouster_driver-1] IMU_DATA
[ouster_driver-1] ---------------------------------
[ouster_driver-1] IMU_DATA
[ouster_driver-1] ---------------------------------
[ouster_driver-1] IMU_DATA
[ouster_driver-1] ---------------------------------
[ouster_driver-1] IMU_DATA
[ouster_driver-1] ---------------------------------
[ouster_driver-1] IMU_DATA
[ouster_driver-1] ---------------------------------
[ouster_driver-1] IMU_DATA
[ouster_driver-1] ---------------------------------
[ouster_driver-1] IMU_DATA
[ouster_driver-1] ---------------------------------
[ouster_driver-1] IMU_DATA
[ouster_driver-1] ---------------------------------
[ouster_driver-1] IMU_DATA
[ouster_driver-1] ---------------------------------
[ouster_driver-1] LIDAR_DATA
[ouster_driver-1] ---------------------------------
[...]

We can clearly see that something strange is happening there, I think that's what is causing my problems at the moment. Could it mean that during this time we received 2 packages ? I have tried reducing the wall timer to 1ns but the output is the same.

I'm on it this weekend.

Here's the current status :

https://user-images.githubusercontent.com/22786578/111874764-4f68b680-899f-11eb-81d3-413f10554f01.mp4

Not yet there but going forward at least.

MaxandreOgeret commented 3 years ago

@SteveMacenski

After some investigation I think I have noticed a problem that will require some work to be solved. It seems that running the full driver in a single thread is not enough. At least on my computer. (Ryzen 3900x)

Here's the tests I made with the pointcloud processor :

Frequency
empty pc (no processing)	`10Hz` and empty pointcloud
`512x10`	`9.1Hz` And incomplete pointcloud
`1024x10`	`4.5Hz` And incomplete pointcloud
`1024x20`	`2.8Hz` And incomplete pointcloud

When publishing empty pointclouds, if I uncomment the scan_to_cloud function, then the frequency drops as well. It shows that the processing made in scan_to_cloud is too heavy to run in a single thread along with the rest of the driver. (At least now).

Could it be why Ouster chose to create 2 nodes and hence 2 processes to query the sensor and build/publish the pointclouds ?

The connection with the sensor is good, as I can visualize the pointcloud perfectly with Ouster studio.

How would you test that ?

Thanks for your help !

[Edit] Nevermind, building in Release mode solved the problem and it works now. I didn't think it could make such a difference !

https://user-images.githubusercontent.com/22786578/111882459-666ad100-89be-11eb-82b6-d724a71d1a30.mp4

[Edit] Current status :

Scan processor working.
Imu processor working.
Pointcloud processor working.
Image processor working.

Left to do :

Clean unused files.
Remove mentions of OS1 sensor.
Code review

MaxandreOgeret commented 3 years ago

Should be mostly done by now. Waiting for comments, code-review and it would be nice to have someone else do some testing ! :)

SteveMacenski commented 3 years ago

I'll review today! What's "mostly done" mean - still some issues to resolve or complete?

Something that would also help a review would be to mention which files, if any, you took directly from the Ouster Example repo without changes. Those I can simply skip over and just assume work. If you made some very light modifications, just letting me know what those were.

On above, it sounds like everything was resolved, anything you want me to specifically comment on from above? Yes, release mode really does matter :wink: (I usually notice serious speed ups in planning and perception tools from optimizer optimizations)

MaxandreOgeret commented 3 years ago

Hi and thanks for your time !

What's "mostly done" mean - still some issues to resolve or complete?

It should mean everything except the timestamp. I haven't tested them and I plan to spend this Saturday on this and on your review.

which files, if any, you took directly from the Ouster Example repo without changes.

All the files under the client dir in src and include :

ros2_ouster_drivers/ros2_ouster/include/ros2_ouster/client/
ros2_ouster_drivers/ros2_ouster/src/client/

anything you want me to specifically comment on from above?

No I think it's good and that those problems are resolved.

Very good overall

Thanks ! I spent a lot of time on this and it's greatly appreciated ! I tried to write clean code but I know it's not perfect and it's priceless for me to get feedback on the code I write. Thanks a lot !

I'd like to know though if there's a mechanism (and if it works) to reject on start up / throw an error / something for if you try to use this with an incompatible firmware version (e.g. 1.X)

I will answer to the remaining comments on saturday :)

Good question, I know it's possible to query the firmware version, but I dont know how how the version 1.X would react if we try with the @.0 client. Needs testing. Maybe we can check using the TCP API and then start the driver if the version is 2.0 ?

SteveMacenski commented 3 years ago

I think its OK if we connect to the sensor to get its firmware, but at the first opportunity, it would be great to throw errors galore or throw an exception to bring everything down if in fact the firmware is incompatible with this driver version (requiring 2.x or whatever the full valid range of this client is). The goal here is to tell people that they need to update their firmware to be compatible if they're not.

MaxandreOgeret commented 3 years ago

I would need a firmware 1.X sensor to test that. Making an API call seems doable to identify the firmware. I would have to look at the software documentation of the firmware 1.X to see if we can use the same API call as firmware 2.0.

Here's the scan working :

Scan + pointcloud
Scan only
Pointcloud only

MaxandreOgeret commented 3 years ago

About the timestamp I have decided to make a change on how we enable the timestamp to be overridden by the ROS time. And I think using the Ouster timestamp_mode for this matter is a bad idea.

In the original version of this driver you could bypass the sensor's timestamp with the ros time by setting the timestamp mode to TIME_FROM_ROS_RECEPTION which is not a valid Timestamp Mode which I think is confusing. And at the moment if I try to set this timestamp I will get the following error :

Invalid timestamp mode: TIME_FROM_ROS_RECEPTION.

The solution would be to edit the Ouster client to accept it and send another type of timestamp mode that is valid. But it wouldn't represent what is truly happening within the sensor.
I think we should avoid to edit the ouster client at all cost to make updating easier later.

I will send TIME_FROM_INTERNAL_OSC to the sensor if the timestamp mode in the driver is set to TIME_FROM_ROS_RECEPTION.

another solution would be to create a new boolean parameter : use_ros_time with an explanation in the config yaml

What do you think ? I think it makes more sense that way. If you think it's bad idea we can find something else but I really believe we shouldnt change the ouster client at all.

Thanks !

MaxandreOgeret commented 3 years ago

I'm quite happy with how it goes now. I just have one thing that bothers me. If you look in those processors :

scan_processor.hpp
pointcloud_processor.hpp
image_processor.hpp

You can see that we batch the packets in all of them. It seems to me that this is very redundant as it will batch 3 times erxactly the same thing. I think we should batch only once and all those processors should just use the generated LidarScan.

How would you do that ? Should we create pre-processors that the processors can access ?

Thanks ! :)

SteveMacenski commented 3 years ago

I would need a firmware 1.X sensor to test that.

True, but I'm just asking if there's anything in place to do that in this PR, even if it hasn't been tested yet

Scan stuff looks good! Thanks for showing

I don't understand the time concerns, it was working before, why mess with it? The whole point of the timemodes is so that there arent just a bunch of boolean params for which time mode to use, there's just the one timestamp_mode that @tpanzarella wrote. I think that ROS timestamp propagates correctly, what's the specific issue you're trying to fix?

Edit: Are you just trying to get the sensor to work properly with this different timestamp mode (e.g. nothing changes how the timestamps are assigned, but the sensor is just unaware of it)? If so, that's OK. I understand if the client code would react badly to a non-defined time mode. It doesn't really matter for the ROS time mode what the sensor reports back since we'll override it - as long as those mechanics are still working fine.

You can see that we batch the packets in all of them. It seems to me that this is very redundant as it will batch 3 times erxactly the same thing. I think we should batch only once and all those processors should just use the generated LidarScan.

Before this V2, the V1 software had lambdas that would take in raw packets, assemble them, and then publish on a full rotation. It looks like on V2 they support some kind of assembler for us. If there's a clean way for a global batcher to notify us that 'hey, I have a full scan' that we can then use to all the appropriate processors to populate the appropriate ROS types / data, I'd be OK with that.

MaxandreOgeret commented 3 years ago

True, but I'm just asking if there's anything in place to do that in this PR, even if it hasn't been tested yet

There is a way to get the sensor version, but it happens after the init_client and I think it would fail with a 1.X sensor.

I don't understand the time concerns, it was working before, why mess with it?

Because it didn't work anymore, I didn't 'mess' with it for no reason. This test would fail as ouster::sensor::timestamp_mode_of_string would report an unknown time mode. So something had to be done.

Are you just trying to get the sensor to work properly with this different timestamp mode (e.g. nothing changes how the timestamps are assigned, but the sensor is just unaware of it)?

Yes exactly, and I detailed the two ways of doing that above. I will revert the changes I made, and use timestamp_mode instead.

It looks like on V2 they support some kind of assembler for us

Yes it's the scanbatcher. I can imagine a batcher pre-processor that the processors would poll and eventually get a Lidarscan once it's ready.

What do you think ? Any implementation ideas ?

SteveMacenski commented 3 years ago

There is a way to get the sensor version, but it happens after the init_client and I think it would fail with a 1.X sensor.

I'd still just check, if nothing else it creates the check for later development to check specific version if required.

Your comments on timestamp make sense, but just verifying that you're still using the ROS time in that situation, regardless of what the actual lidar sends back in this case?

Without reviewing the nitty-gritty specifics of the scan batcher, I assume there's some way that the processors today know when the complete data is received in order to populate and send out the ROS messages? If so, then you could move the batcher up one level of abstraction and check if its complete there. If so, then call the N processors in the processor map that depend on the laser data.

The one issue here is if someone wants to actually do something live with the data rather than waiting for the full scan. I envisioned a future desire to potentially batch in smaller chunks (like 10 packets) for low latency incomplete pointcloud sending to help de-alias while operating in high speed environments. This would stop that from working. Letting each processor handle everything themselves leaves that flexibility for new processors to do specific things with the packets rather than only after getting all of the data batched up in a full rotation.

MaxandreOgeret commented 3 years ago

Now using the timestamp_mode parameter to set time from ROS. It will initialize the sensor with TIME_FROM_INTERNAL_OSC and stamp the messages with ROS time. (I have updated the readme)
The scan angle_increment is now positive. I iterate through the lidarscan in the reverse direction.

It would be really amazing is someone else could test this with a f2.0 sensor !

About the pre-processors, we can totally imagine something like this :

Untitled Diagram

Where each processor receives a ref to the packet data so we are still free to do what we want. But we can optimize processor using the same data !

The scanbatcher returns true when a batch is complete, we can have processors polling the pre-processors for data. And the polling would also activate the preprocessor.

~~But maybe this can be for a next PR ? (I have a working proof of concept ready on the branch foxy_devel_f2.0_preproc but Im not very proud of the implementation thats why its not included here).~~

See below for detail about the preprocessors

Implementation of preprocessors

I found a way that is clean enough for me to propose it here, I added it in the last commit.
Files created :

preprocessor_manager.hpp
- This class manages the creation of preprocessors. IT is also passed to the processors to access the preprocessors.
data_preprocessor_interface.hpp
- Interface for the preprocessors.
lidarscan_preprocessor.hpp
- Preprocessor creating Lidarscan. It is used by : image_processor, pointcloud_processor, scan_processor

The preprocessors are activated only when a processor is asking for preprocessed data with the isDataReady() function.

I first wanted to create a vector or map of preprocessors and pass this to the processors. But because preprocessors are templated classes it is not possible. Hence the preprocessor manager !

So the processors have access to the preprocessor they need but also to the packetdata ! So you still can create all the processors you want !

some processors that don't use the packetdata still get them in their constructor. I think it's not a big problem as it's only a pointer.

I think this preprocessor logic is a huge improvement ! I'd love to hear your feedback about the implementation.

MaxandreOgeret commented 3 years ago

@SteveMacenski Your last remarks have been solved.

Waiting for your feedback !

MaxandreOgeret commented 3 years ago

@SteveMacenski Your last remarks have been solved.

Waiting for your feedback !

About this :

I'd like to know though if there's a mechanism (and if it works) to reject on start up / throw an error / something for if you try to use this with an incompatible firmware version (e.g. 1.X) to let someone know they need to update firmware to function properly.

Without a sensor running fw 1.x I don't think I can do it.

MaxandreOgeret commented 3 years ago

How will we proceed ? It would be sad for the PR to be stuck at this point.

For the version compatibility a log on startup is the easiest thing to do., also it's very easy to log the sensor version once the driver is connected to it.

I think it would be nice to release it for foxy still, I believe it has a lot of users still.

MaxandreOgeret commented 3 years ago

Found a bug that is also present in the current Ouster client.

On this line : https://github.com/MaxandreOgeret/ros2_ouster_drivers/blob/114406409ce40415e3b05fd1d2c4e75b0d6faa71/ros2_ouster/include/ros2_ouster/conversions.hpp#L268 We should set the RANGE and not REFLECTIVITY.

According to the definition of the Point : https://github.com/MaxandreOgeret/ros2_ouster_drivers/blob/foxy_devel_f2.0/ros2_ouster/include/ros2_ouster/client/point.h

I will correct that very soon.

SteveMacenski commented 3 years ago

I created a new ROS2 branch, lets retarget this PR to that so that it will be used as the basis of the next distribution release.

For the version compatibility a log on startup is the easiest thing to do., also it's very easy to log the sensor version once the driver is connected to it.

Lets add that here. Also, it would be good if the README in the first section said that it requires V2.x firmware to work, that way you can save alot of people problems and headaches that they know off the bat this version requires it.

I think it would be nice to release it for foxy still, I believe it has a lot of users still.

Its not ABI compatible since it requires a hard firmware change, this couldn't be released to foxy under the same name (because it has many users is exactly why we can't release it). We could release it for the foxy distribution, but it would have to be under a new package name on a different branch (e.g. ouster_drivers_v2)

The steps I think are pretty clear

Get someone else with a sensor to sanity check us that this works for them -- @anderwm can you evaluate?
Merge into ros2, once Galactic is released, release this to galactic
Decide if we want to release also to Foxy for a v2 version to use

anderwm commented 3 years ago

Sorry, I was re-vectored onto another effort so I haven't been following too closely. I will try to give it a look.

MaxandreOgeret commented 3 years ago

@SteveMacenski I have changed the target of this PR to the branch ros2.

anderwm commented 3 years ago

It is definitely working. Screenshot from 2021-04-23 07-44-51

I don't have time at the moment to investigate the more complicated pieces I will ultimately need (using as component). I am using an OS0-128-U and I also haven't investigated if it is using all the scans/being used to it's full potential. However I noticed a couple of things (Not saying it's a problem yet, just that I noticed it).

I have to set the Reliability Policy to Best Effort in RViz to view anything
Only the range_image renders in RViz. ambient_image and intensity_image get dropped for not have a FrameID [INFO] [1619182373.389459026] [rviz]: Message Filter dropping message: frame '' at time 649.826 for reason 'EmptyFrameID'

If there is anything specific you worry about/want tested shoot me a message, otherwise thanks for you work on this.

SteveMacenski commented 3 years ago

That's just a ROS2 thing, sensors should be best effort by default
That's a problem @MaxandreOgeret did it not populate the frames for those?

But overall, looks great! Lets get that patched up and we can merge this into ROS2 (and go from there about foxy release)

MaxandreOgeret commented 3 years ago

Hey, sorry for the late replay ! I'll be more available from now :)

I didn't get this error with the images :

But indeed the frame ID was only set for the range image. I corrected that.

I also added the message about the firmware requirements in the readme : https://github.com/ros-drivers/ros2_ouster_drivers/blob/1dba63270ec609f4b51b082c0e466418cdeab1a9/README.md#L3

And the log : https://github.com/ros-drivers/ros2_ouster_drivers/blob/1dba63270ec609f4b51b082c0e466418cdeab1a9/ros2_ouster/src/ouster_driver.cpp#L61

SteveMacenski commented 3 years ago

Awesome, if @anderwm or someone can verify that, I'm happy to merge (though it looks clear to me that it should be fixed now, just another sanity check is always good)

anderwm commented 3 years ago

Screenshot from 2021-05-03 13-15-59 hz_data

Better. I'm not sure how to verify it is using all 128 scans, but looks good so far.

SteveMacenski commented 3 years ago

Awesome! @MaxandreOgeret anything further before we merge this in?

MaxandreOgeret commented 3 years ago

Nothing to add at the moment ! Thanks a lot @SteveMacenski for your advice and your guidance. Thanks @anderwm for your test ! If you have other interesting projects that need some coding it's a pleasure !

SteveMacenski commented 3 years ago

Merged, ros2 now set to the default branch, and will be released with Galactic!

I spend by far the largest chunks of my time on Nav2 in ROS2 https://github.com/ros-planning/navigation2, where there are a ton of different projects to be worked on! We also have a community slack (https://join.slack.com/t/navigation2/shared_invite/zt-pv43supx-hfN_Q3RfaKkn9f8i3Wt~OQ) that might be easy to discuss through as well. I'd be more than happy to have you involved on something else!!

ros-drivers / ros2_ouster_drivers

Firmware 2.0 compatibility #67