feat: add robosense support

[mebasoglu]

PR Type

• New Feature

Related Links

• The issue: Enhancement: Add Robosense Lidar Support #76

Description

This change proposes Robosense Helios and Bpearl support for nebula.
Closes #76.

Review Procedure

It is still in progress.

Remarks

Pre-Review Checklist for the PR Author

PR Author should check the checkboxes below when creating the PR.

• Assign PR to reviewer

Checklist for the PR Reviewer

Reviewers should check the checkboxes below before approval.

• Commits are properly organized and messages are according to the guideline
• (Optional) Unit tests have been written for new behavior
• PR title describes the changes

Post-Review Checklist for the PR Author

PR Author should check the checkboxes below before merging.

• All open points are addressed and tracked via issues or tickets

CI Checks

• Build and test for PR: Required to pass before the merge.

[codecov-commenter]

Codecov Report

Attention: 1122 lines in your changes are missing coverage. Please review.

Comparison is base (9b239c6) 8.28% compared to head (eac1c8b) 8.23%.

Files	Patch %	Lines
...os/src/robosense/robosense_decoder_ros_wrapper.cpp	0.00%	179 Missing ⚠️
...s/nebula_decoders_robosense/decoders/bpearl_v3.hpp	0.00%	133 Missing ⚠️
...ders/nebula_decoders_robosense/decoders/helios.hpp	0.00%	131 Missing ⚠️
...robosense_hw_interfaces/robosense_hw_interface.cpp	0.00%	115 Missing ⚠️
...c/robosense/robosense_hw_interface_ros_wrapper.cpp	0.00%	102 Missing ⚠️
...src/robosense/robosense_hw_monitor_ros_wrapper.cpp	0.00%	101 Missing ⚠️
...s/nebula_decoders_robosense/decoders/bpearl_v4.hpp	0.00%	94 Missing ⚠️
..._decoders_robosense/decoders/robosense_decoder.hpp	0.00%	76 Missing ⚠️
...a_decoders_robosense/decoders/robosense_packet.hpp	0.00%	53 Missing ⚠️
...src/nebula_decoders_robosense/robosense_driver.cpp	0.00%	36 Missing ⚠️
... and 7 more

❗ Your organization needs to install the Codecov GitHub app to enable full functionality.

Additional details and impacted files

@@           Coverage Diff            @@
##            main     #77      +/-   ##
========================================
- Coverage   8.28%   8.23%   -0.05%     
========================================
  Files         87      67      -20     
  Lines       8462    8074     -388     
  Branches     854     844      -10     
========================================
- Hits         701     665      -36     
+ Misses      7185    6835     -350     
+ Partials     576     574       -2     

Flag	Coverage Δ
differential	8.23% <0.00%> (?)
total	?

Flags with carried forward coverage won't be shown. Click here to find out more.

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

[mebasoglu]

Hello, I have been working on this for some time. I have managed to get a point cloud with Helios 5515. It is still work in progress and I wanted to share an update. I tried to follow Hesai part.

Robosense has a different byte order than Hesai, it is big-endian. So, direct memcpy into a struct doesn't work. I have used Boost.Endian library to cope with byte order issue.

There are some hard coded parts for timestamps and return modes for now, I am working on them.

Helios 5515 and Bpearl 3.0 support configuration only via web interface, so it seems not possible to implement setup sensor part.

Here is an image of a Helios point cloud: (I haven't applied correction values yet)

[drwnz]

Awesome @mebasoglu, keep up the great work! Unfortunately we don't have a Robosense device to test with, but hopefully we can assist with testing from pcaps.

[mebasoglu]

Hello @drwnz @amc-nu @mojomex , I have completed for Helios and I think it is ready for review. I have used pandar msgs for packets. I will continue with Bpearl and waiting your feedback.

I will share a pcap file tomorrow.

Thank you

[mojomex]

Thank you for your contribution, the code already looks very good!
Please have a look at the few small comments I left concerning mainly return mode handling and readability.
I will check for functionality when the pcap file(s) is/are available.

@amc-nu has more knowledge of the HW interfaces so I have not reviewed those in depth yet.

[mebasoglu]

Hello @mojomex , thank you for the feedback! I am working on them.
Here is the pcap file: https://drive.google.com/file/d/1xfm1kB3YZQ5n4Ur9A7B76Aw1jeI_r3GJ/view?usp=drive_link

For Helios:

    <arg name="sensor_model" default="Helios" description="Helios|Bpearl"/>

    <arg name="sensor_ip" default="192.168.1.211" description="Lidar Sensor IP"/>
    <arg name="host_ip" default="255.255.255.255" description="Broadcast IP from Sensor"/>
    <arg name="data_port" default="2011" description="LiDAR Data Port"/>
    <arg name="gnss_port" default="3011" description="LiDAR GNSS Port"/>

For Bpearl:

    <arg name="sensor_model" default="Bpearl" description="Helios|Bpearl"/>

    <arg name="sensor_ip" default="192.168.1.210" description="Lidar Sensor IP"/>
    <arg name="host_ip" default="255.255.255.255" description="Broadcast IP from Sensor"/>
    <arg name="data_port" default="2010" description="LiDAR Data Port"/>
    <arg name="gnss_port" default="3010" description="LiDAR GNSS Port"/>

[mebasoglu]

Hello @mojomex @drwnz @amc-nu , I worked on the current feedback and added Bpearl also. The provided pcap file has both Helios and Bpearl.

About the MTU_SIZE and MAX_SCAN_BUFFER_POINTS, I am not sure how to calculate them. Do you have a suggestion for that?

I am waiting your further comments.

Thank you!

[mojomex]

Hi, I just tested the driver with the PCAP file and for Helios it works perfectly. For Bpearl, it outputs the pointclouds but logs [robosense_cloud]: Timestamp error, verify clock source. ~10 times per second, i.e. for every scan. Could you look into this @mebasoglu?

Thank you!

[mojomex]

About the MTU_SIZE and MAX_SCAN_BUFFER_POINTS, I am not sure how to calculate them. Do you have a suggestion for that?

As for MAX_SCAN_BUFFER_POINTS, this number should correspond to the theoretical maximum number of points in one scan. This is usually given in the datasheet under 'specifications' or similar. In case of Bpearl:

Number of Output Points | 576000 pts / s (Single Return Mode), 1152000 pts / s (Dual Return Mode)

The formula would be $N = f \cdot (FoV \div r_H) \cdot n_C \cdot n_R = 10 Hz \cdot (360^{\circ} \div 0.2^{\circ}) \cdot 32 \cdot 2 = 1152000 \frac{1}{s}$
where $N$ is MAX_SCAN_BUFFER_POINTS, $f$ is the frequency the sensor is running at, $FoV$ is the field of view, $r_H$ is the horizontal angular resolution, $n_C$ is the number of channels and $n_R$ is the maximum number of returns the sensor supports.
This number remains constant even for different framerates as the horizontal resolution is inversely proportional to the framerate.

As for MTU_SIZE, all packets sent by the sensor are 1248 B as far as I can tell, so leaving MTU at the default 1500 is fine.

[mojomex]

The return_type field is currently ReturnType::UNKNOWN for all points for both Helios and Bpearl.

[mebasoglu]

Hello, on the last Sensing&Perception WG, we have decided to create a separate Robosense msgs package. The hardware interface ros wrapper publishes both data (MSOP) and info (DIFOP) packets to robosense_packets and robosense_info_packets.

I have also updated MTU_SIZE and MAX_SCAN_BUFFER_POINTS.

Hi, I just tested the driver with the PCAP file and for Helios it works perfectly. For Bpearl, it outputs the pointclouds but logs [robosense_cloud]: Timestamp error, verify clock source. ~10 times per second, i.e. for every scan. Could you look into this @mebasoglu?

Thank you!

For this one, I have tested it with the sensor itself instead of the pcap file and didn't get the warning.

[mebasoglu]

Hello, it seems we have two different Bpearl models, v3.0 and v4.0 at the office. While transitioning to v4.0, some packet definitions have changed such as time stamp. The warning that @mojomex got about timestamps is also because of that. I am working on these different versions of Bpearl. I will let you know when it is completed.

[mojomex]

Hi @mebasoglu, thanks again for your contributions!
Here's my updated review after the newest changes:

• I made some code comments that are mostly style-related and will help readability
• When launching with the provided configurations, the printed sensor config in hw_monitor will be incomplete: (All values are either missing or garbage data):

[robosense_hw_monitor_ros_wrapper_node]: SensorConfig:SensorModel: BPEARL, ReturnMode: Unknown, HostIP: , SensorIP: , FrameID: , DataPort: 0, Frequency: 0, MTU: 0, Use sensor time: 0, GnssPort: 0, ScanPhase:0, RotationSpeed:28416, FOV(Start):28514, FOV(End):25971

• The timestamps are not decoded correctly for Helios (but are for Bpearl):

Bpearl V3 (correct)	Helios (incorrect)

The nanosec field in the header timestamp should be in the [0, 1e9) range, the point timestamps in the [0, 1e8) range, assuming 10 Hz.
I observed these header values:

---
sec: 1695812910
nanosec: 543488
---
sec: 1695812910
nanosec: 643840
---
sec: 1695812910
nanosec: 743680
---
sec: 1695812910
nanosec: 843520
---
sec: 1695812910
nanosec: 943616
---
sec: 1695812910
nanosec: 1043456
---
sec: 1695812910
nanosec: 1143808
---
sec: 1695812910
nanosec: 1243904
---
sec: 1695812910
nanosec: 1343488
---
sec: 1695812910
nanosec: 1443584
---
sec: 1695812911
nanosec: 543488
---

The nanosec field seems to only get up to 1.44e7 ns. The first digit should be increasing by 1 for each output pointcloud (0e8 -- 1e8 -- 2e8 ... 9e8 -- 0e8 etc.).

In the coming days we will try to validate everything on the real sensors 😃

[mebasoglu]

Hello @mojomex , thank you for the feedback.
I fixed the parts excluding timestamp issue, I am working on that currently.

[mebasoglu]

Hello again @mojomex , I have found the problem. I used nanoseconds instead of microseconds while decoding timestamp, although it says microseconds on the manual. It should be fine right now.

Also, for the spellcheck test, Idat and Vdat current and voltage words are failing. Should I add them to the list also or is it okay for them to fail?

[mojomex]

Thank you, looks great! Yes, lease add Idat and Vdat to the .cspell.json file!

[mebasoglu]

Hello @mojomex @drwnz ,

Since correction angles are different for each sensor, it seems the order of the rings are also different. Sorting calibration data descending by vertical angles and creating corrected channels numbers with the new indexes solves the ring problem.

Apart from these, one thing I missed is the PTP synchronization. I only checked utc_time flag for sensor timestamp, and it only represents GPRMC synchronization. I am going to push one more commit to handle PTP sync also.

[mebasoglu]

Hello @mojomex @drwnz ,

I pushed a commit to handle PTP time synchronization also.
About the version question, on the web interface of Bpearl v3, it says "Hardware version" is v3. So it is a hardware improvement also for v4.

So, I think it is mergeable now!

[mojomex]

LGTM! 🚀
Thank you again for your extensive work!