Skip to content
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

feat: add robosense support #77

Merged
merged 89 commits into from
Dec 7, 2023

Conversation

mebasoglu
Copy link
Collaborator

@mebasoglu mebasoglu commented Sep 14, 2023

PR Type

  • New Feature

Related Links

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
Copy link

codecov-commenter commented Sep 14, 2023

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.

@drwnz drwnz requested review from mojomex, amc-nu and drwnz September 15, 2023 06:01
@mebasoglu
Copy link
Collaborator Author

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)

image

@mebasoglu mebasoglu force-pushed the memin/dev/robosense-support branch from 103fcae to 7389081 Compare September 19, 2023 15:29
@drwnz
Copy link
Collaborator

drwnz commented Sep 20, 2023

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 mebasoglu force-pushed the memin/dev/robosense-support branch from 62b03aa to 71ab11f Compare September 26, 2023 08:05
@mebasoglu mebasoglu marked this pull request as ready for review September 26, 2023 14:40
@mebasoglu
Copy link
Collaborator Author

mebasoglu commented Sep 26, 2023

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

Copy link
Collaborator

@mojomex mojomex left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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
Copy link
Collaborator Author

mebasoglu commented Sep 28, 2023

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 mebasoglu force-pushed the memin/dev/robosense-support branch from 420094b to f65280e Compare October 2, 2023 12:31
@mebasoglu
Copy link
Collaborator Author

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
Copy link
Collaborator

mojomex commented Oct 3, 2023

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
Copy link
Collaborator

mojomex commented Oct 5, 2023

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.

Copy link
Collaborator

@mojomex mojomex left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

@mebasoglu mebasoglu force-pushed the memin/dev/robosense-support branch from 2fbcf7a to c12e45c Compare October 5, 2023 16:30
@mebasoglu
Copy link
Collaborator Author

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
Copy link
Collaborator Author

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.

@mebasoglu mebasoglu force-pushed the memin/dev/robosense-support branch 2 times, most recently from 9360bbc to 5a4017d Compare October 26, 2023 09:17
@mojomex mojomex self-requested a review November 20, 2023 05:39
@mebasoglu mebasoglu force-pushed the memin/dev/robosense-support branch 2 times, most recently from 45439d7 to 34a7509 Compare November 22, 2023 11:35
Copy link
Collaborator

@mojomex mojomex left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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)
Screenshot from 2023-11-27 10-53-15 Screenshot from 2023-11-27 10-51-13

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
Copy link
Collaborator Author

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

@mebasoglu
Copy link
Collaborator Author

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?

Copy link
Collaborator

@mojomex mojomex left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

@mebasoglu mebasoglu force-pushed the memin/dev/robosense-support branch from 931fc70 to ad004c8 Compare December 6, 2023 10:44
@mebasoglu
Copy link
Collaborator Author

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.
image

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
Copy link
Collaborator Author

mebasoglu commented Dec 6, 2023

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 mojomex self-requested a review December 7, 2023 04:31
Copy link
Collaborator

@mojomex mojomex left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

LGTM! 🚀
Thank you again for your extensive work!

@mojomex mojomex merged commit 810849e into tier4:main Dec 7, 2023
9 checks passed
mojomex added a commit to mojomex/nebula that referenced this pull request Dec 26, 2023
This commit adds Robosense M1 support based on tier4#77 (add robosense support) and tier4#104 (composable sensor behavior).

The driver is feature-complete, the hardware interface cannot set return modes and other settings on the sensor though.
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

Enhancement: Add Robosense Lidar Support
4 participants