Code refactor and Angle Compensation Enabled

[ManuelMeraz]

Decided to use this repo as a good starting point for ros2 and using my RPLidar A1. After trying to use it with slam_toolbox I discovered that angle compensation wasn't actually being used.

Went through the code refactored it and enabled angle compensation. Honestly the original logic was pretty bad and was very difficult to understand, so I went through it on paper and implemented it better than the original. Originally angle compensation would only work on multiples of 365, but it should work a bit better than that now.

Got rid of a bunch of out dated code and updated it to modern C++ and also removed a bunch of warnings from the SFK.

With angle compensation enabled you should be able to get consistent laser scan sizes that are evenly distributed which will make slam packages happy.

Refactored cmake as well and made it a bit more modern (got rid of the globs)

A few other things:

• Removed dead files
• Renamed classes, files and members to be consistent
• pulled out the parameters from the launch files and made a config directory with the

[allenh1]

@ManuelMeraz This got lost in my notifications somehow (I've not paid much attention to this repo), but I'm starting to pay attention to it again and just bloom released the current state for ROS 2 Foxy.

Went through the code refactored it and enabled angle compensation. Honestly the original logic was pretty bad and was very difficult to understand, so I went through it on paper and implemented it better than the original. Originally angle compensation would only work on multiples of 365, but it should work a bit better than that now.

I held off from refactoring too much because I wanted to preserve behavior with the original, should they do more development there. I'm going to start to move away from that, since they have not reached out to me in order to keep things there.

Concerning this patch, I'd prefer it be chunked out quite a bit. This is very big. I'd be happy to consider these changes individually, but there's a few things I'm not particularly fond of (like the spacing changes in the CMakeLists.txt file and whatnot).

I very much appreciate you taking the time to do this, I'm excited to give it a go!

[chrisl8]

FYI: I am eager to see where this goes, as I would like to use this driver, but I'm unsure at this point which fork is going to "win".

[allenh1]

@chrisl8 this fork was released into Foxy in the last sync, though I'll be more than happy to merge it into the upstream if they ever feel the need to take ownership of it

[chrisl8]

@ManuelMeraz I took a look at this and something that bothers me is that the angle_min and angle_max change on every frame.

I think that most mapping tools only check the angle_min and angle_max once, and after expect it to always be the same.

I worry that this will cause issues for various mapping packages.

Thoughts?

[ManuelMeraz]

@chrisl8 Sorry it's been a while since I worked on this. This is actually mostly a refactor of the code that was already there (meaning that variable angle_min and angle_max is already happening with the original code), but made much clearer and easy to read.

From what I remember, the RPLidar driver sends us a bunch of scan points as nodes with outer ranges where the nodes are invalid. This check is performed with:

constexpr auto is_valid_node = [](const auto& node) -> bool { return node.dist_mm_q2 != 0; };

Basically, it says if the distance detetced is 0, it's invalid.

I don't necessarily think that angle_min and angle_max changing makes a difference; well at least from my experience.

The reason I originally made this refactor was because I was trying to use this with this SLAM package: https://github.com/SteveMacenski/slam_toolbox

I forgot which library that package uses, but what it really hates is variable sized lidar scans; irregardless of the min and max angle.

What was happening is that using the default scan option, the lidar scan would vary in size, due to the variable outer bounds.

Example:

Scan data example {0, 0, 0, 1, 2, 3, 2, 5, 5, 1, 0}
                            ^                 ^
                    angle_min        angle_max

These bounds may vary. Now, angle compensate did somewhat solve this, but the original way it was written it would only create a fixed size array in multiples of 365, which was annoying because my RPLidar produced around 500~, so if i used angle compensate it would reduce it to 365, but i wanted was the closest number available.

My fix basically added that and I also read through the complicated loops and figured out what they were doing, and used stl aglorithms to show what was actually happening. I also saw a lot of antipatterns and spaghetti code that I cleaned up. For example, wrapping the raw driver pointer into a unique_ptr to use RAII.

FYI I'm currently not working on this, since this was good enough for my project.

[chrisl8]

@ManuelMeraz I'm also using Slam Toolbox! :-D

When I run mine with the old code, the angle_max and angle_min never change. I guess I need to review the code and the output again.

Anyway, did your final replacement code for the angle compensated output work well with Slam Toolbox?
If so, then that alone is very good to know.

I've tried both the original (current) code and yours, and they both seem to work with Slam Toolbox, but whenever anything fails with Slam Toolbox, the maintainer always blames the RPLidar driver, so I've been digging into this PR to see if it appears to be better.

Perhaps I should just leave it alone, since the existing code appears to work fine, but I'm always interested in improvements.

I also wondered if you, or @allenh1 or anyone else has had time to look at and/or test your changes more.

Thank you for the response!

[ManuelMeraz]

@chrisl8 Yes mine worked well with it. It worked much better with the angle compensation enabled in my version since it had more data points to work with.

Also, as far as how much the min/max angles shifted, I don't know. The they probably have invalid data points that don't change because it ignroed points that are too close i believe. Put your hand close to the rp lidar and watch the values potentially change for the min and max. If your measuring in an open room it'd probably be less clear. Anyways, here's the original code:

  angle_min = deg_2_rad(0.0f);
  angle_max = deg_2_rad(359.0f);
  if (op_result == RESULT_OK) {
    if (angle_compensate_) {
     ...
    } else {
      ....
      angle_min = deg_2_rad(getAngle(nodes[start_node]));
      angle_max = deg_2_rad(getAngle(nodes[end_node]));
      ...
}

Line 370 you see it basically doing the same thing. It's in there if you look, it's just buried under all the code.

[chrisl8]

@ManuelMeraz In the original code, as you posted, the angle_min and angle_max are only recalculated on every frame if NOT in angle_compensate mode. In angle_compensate mode, it just uses the generic 0.0f and 359.0f based min/max.

In your code the min/max are recalculated every time in both angle_compensate and non angle_compensate modes.

   m_angle_min = degreesToRadians(0.0f);
   m_angle_max = degreesToRadians(359.0f);

   if (op_result == RESULT_OK) {
...
      m_angle_min = degreesToRadians(getAngleInDegrees(*start_node));
      m_angle_max = degreesToRadians(getAngleInDegrees(*end_node));
...
      if (m_angle_compensate) {

That is the difference. I've always used angle_compensate mode, so when I switch to your version, the min/max change on every frame.

However, as you said, it works well for you in Slam Toolbox, so that might not matter. I'm mostly just trying ensure I understand it, and that it is on purpose.

I appreciate the discussion. It helps me sort out the details and what and why.

[ManuelMeraz]

Ah, yeah you're right. That must have been an oversight on my part. Good catch, feel free to move it back to the correct location. But yeah, I never ran into issues with slam tool box.

[chrisl8]

Ok, I will look at this some more as I have time. I see this repository already has some changes and PRs that will conflict with this, and there are probably some things here the maintainer just doesn't want to change, like the CMake updates, so I'll fiddle with it a bit and see if there is a compromise that seems helpful.

I'm also not sure if I can "edit" this PR, or if I need to just cherry pick stuff from it for a new PR?

[ManuelMeraz]

Easiest thing to do would be to fork off my branch and, then pull and merge the new changes in to fix the merge conflicts. Then apply your changes.