Next gen lidar parts #918

Ezward · 2021-08-15T08:13:58Z

There is a desire to integrate 2D lidar data more completely into the donkeycar pipeline - see issue 910. In particular the idea is to integrate the lidar data with the front-facing camera image. In addition there are some issues with regarding accuracy and latency. This pull request endeavours to tackle both problems by creating new lidar parts for acquiring data from the RPLidar and plotting data.

There a few problems with the current RPLidar and LidarPlot parts.

The RPLidar part would wait for a full scan to be available before updating any data. For a lidar spinning at 5hz, this would incur a 200ms latency between data updates.
The RPLidar part forced angles to be on integer degrees. This is probably ok for near-obstacle avoidance since the slight error won't matter much. However, for longer distances this will cause significant deviation from actual readings if those readings include fractional angles (as a higher-end lidar could do).
The RPLidar spins in a clockwise direction and so readings from it have angles that increase in clockwise direction. However, mathematically we want angles to increase in a counter-clockwise direction.
The LidarPlot part was written to be consistent with this clockwise increasing angles by using pixels coordinates where the y-axis increases going down.
There was no way to easily visualize what these parts were doing without running a donkeycar.

These issues have been addressed by adding new versions of the parts (RPLidar2 and LidarPlot2) which presume mathematical angles that increase in a counter clockwise direction and are plotted on the Cartesian plane where y increases going up. This can be adjusted with parameters. In addition the new RPLidar2 streams measurements and makes them available immediately without waiting for a full scan. A main has been added to visualizing the output of the lidar without having to run a full donkeycar.

The current RPLidar and LidarPlot parts remain for those that have code that depends upon them.

Added RPLidar2 part for streaming measurements from an RPLidar.

added a new part that streams measurements rather than scans in order to provide the freshest data possible with low latency.
angles are no longer forced into a 360 element bucket; we simply return what the lidar provides (after filtering). The lidar seems to return about 258 measurements per scan, but the part will actually adapt to whatever the lidar is providing for a full scan, which can actually change from scan to scan. This has the advantage of returning less data if less data is returned. Further, if the lidar provides denser data, then this will support that.
The part also will filter measurements by angle and distance on the fly for efficiency.
The part no longer attempts to sort the measurements (I can't see any reason we need to sort, but any client of the part can do the sort if they want).
The part will also adjust angles so that they increase in a counter-clockwise direction as is expected generally. The default is still angles increasing in a clockwise direction, since that is what the RPLidar does naturally.
The part will also adjust the zero-heading. This will adjust for different mounting of the lidar such that the physical front of the lidar is not longer pointing in the logical front direction. By default this is zero and the most common mounting of the RPlidar on a vehicle would use the default.

Also added LidarPlot2, which fixes a few issues in the old plotter.

It now assumes Cartesian coordinates where y increases going up. The old LidarPlot (which remains untouched) used the pixels coordinate system where y increased going down. This combined with the fact that the RPLidar was producing angles that increased in a clockwise direction, allowed the plotting of RPLidar work correctly. However, since we are now assumin that angles increase in a counter-clockwise direction, we need to plot in cartesian coordinates.
This will also take an argument for rotating the plot. By default the zero heading is on the positive x axis, which is 'East'. The rotating 90 degrees, the zero heading can point 'North' for instance. This is useful when aligning to an image take by a FPV camera.
This draws a line along the zero heading.

Added a main so the part can be run from the command line

This will draw the scans to a window.
Scan rate can be specified.
total number of scans can be specified
filtering for angle and distance can be specified.
The plot can be rotate.
In the future I hope to make this support other lidars.

DocGarbanzo

This looks good to me, only some minor comments.

donkeycar/parts/lidar.py

Ezward · 2021-08-21T07:46:48Z

@DocGarbanzo thank you very very much for the code review. It made me go back and look more deeply at the code. If found a couple of bugs while doing so.

DocGarbanzo · 2021-09-07T19:52:26Z

donkeycar/parts/lidar.py

+    """
+    angle = angle
+    min_angle = min_angle
+    max_angle = max_angle


These 3 lines are obsolete.

Thank you, I remove the redundant lines

DocGarbanzo · 2021-09-07T19:55:04Z

donkeycar/parts/lidar.py

+                        self.full_scan_index += 1
+
+            except serial.serialutil.SerialException:
+                logger.info('SerialException from RPLidar.')


This probably should be a

logger.error(...)

And so it should be, and so it is.

donkeycar/parts/lidar.py

DocGarbanzo · 2021-09-07T20:00:54Z

donkeycar/parts/lidar.py

+
+        # background
+        draw.rectangle(bounds, fill=self.background_color)
+


additional blank line

eliminated. Thanks.

setup.py

- added a new part that streams measurements rather than scans in order to reduce the latency between retrieving full scans. - The part also will filter measurements by angle and distance. - The part will also adjust angles so that they increase in a counter-clockwise direction as is expected generally. The default is still angles increasing in a clockwise direction, since that is what the RPLidar does naturally. - The part will also adjust the zero-heading. This will adjust for different mounting of the lidar such that the physical front of the lidar is not longer pointing in the logical front direction. By default this is zero and the most common mounting of the RPlidar on a vehicle would use the default. Also added LidarPlot2, which fixes a few issues in the old plotter - It now assumes cartesian coordinates where y increases going up. The old LidarPlot (which remains untouched) used the pixels coordinate system where y increased going down. This combined witht the fact that the RPLidar was producing angles that increased in a clockwise direction, allowed the plotting of RPLidar work correctly. However, since we are now assuming that angles increase in a counter-clockwise direction, we need to plot in cartesian coordinates. - This will also take an argument for rotating the plot. By default the zero heading is on the positive x axis, which is 'East'. The rotating 90 degrees, the zero heading can point 'North' for instance. This is useful when aligning to an image take by a FPV camera. - This draws a line along the zero heading. Added a __main__ so the part can be run from the command line - This will draw the scans to a window. - Scan rate can be specified. - total number of scans can be specified - filtering for angle and distance can be specified. - The plot can be rotate. - In the future I hope to make this support other lidars.

- factored out funcs for drawing points and drawing a point - created funcs to draw polar bounds and zero heading

- for instance, if you want the 180 degrees in front of the lidar, then specify min -> max of 270 -> 90. That then is broken into between 270 -> 360 and 0 -> 90 for testing. - In comparison, specifying 90 -> 270 would be the 180 degrees behind the lidar

- shorten long lines - use one space between members of a class - use two spaces between classes

- refactored to create a poll() method that reads a single measurment. update() method calls poll in a loop. - added a run() method that will grab measurements in batches - refactored main to allow it to run in threaded or non-threaded mode. In non-threaded mode the user can quit by pressing the 'q' or escape key.

- single threaded operation used batch_ms to decide how long to loop getting measurements in the run() method.

- rebased against main 4.3.10

Ezward · 2022-04-10T22:44:02Z

Rebased against main.

setup.py

DocGarbanzo · 2022-04-26T20:21:22Z

setup.py

@@ -24,7 +24,7 @@ def package_files(directory, strip_leading):
    long_description = fh.read()

 setup(name='donkeycar',
-      version="4.3.10",
+      version="4.3.11",


I think this needs to be 4.3.14 now - don't know why GitHub doesn't show it as conflict.

DocGarbanzo

I merged from main and resolved the version conflict.

DocGarbanzo reviewed Aug 16, 2021

View reviewed changes

DocGarbanzo reviewed Sep 7, 2021

View reviewed changes

donkeycar/parts/lidar.py Show resolved Hide resolved

DocGarbanzo reviewed Sep 7, 2021

View reviewed changes

setup.py Show resolved Hide resolved

DocGarbanzo and others added 8 commits April 10, 2022 14:59

update setup.py to include adafruit-circuitpython-rplidar

12f2255

Refactored graphics routines to be reusable

7e81c7b

- factored out funcs for drawing points and drawing a point - created funcs to draw polar bounds and zero heading

Little reformatting

f3438f6

- shorten long lines - use one space between members of a class - use two spaces between classes

Modified single-threaded operation with batch time

9f02e24

- single threaded operation used batch_ms to decide how long to loop getting measurements in the run() method.

version="4.3.11"

b67066f

- rebased against main 4.3.10

Ezward force-pushed the next-gen-lidar-parts branch from f56d0d3 to b67066f Compare April 10, 2022 22:23

Add back-in rplidar driver for RPi and Nano

038ef6d

Ezward requested a review from DocGarbanzo April 10, 2022 22:44

Ezward marked this pull request as ready for review April 10, 2022 22:45

small changes based on PR feedback

407c8de

DocGarbanzo reviewed Apr 26, 2022

View reviewed changes

Merge branch 'main' into next-gen-lidar-parts

bdec6bd

DocGarbanzo approved these changes Apr 26, 2022

View reviewed changes

DocGarbanzo merged commit 8eb2705 into main Apr 26, 2022

Ezward mentioned this pull request Feb 18, 2023

Empirical testing suggests that the lidar.py part range data output may not be accurate #914

Closed

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Next gen lidar parts #918

Next gen lidar parts #918

Ezward commented Aug 15, 2021

DocGarbanzo left a comment

Ezward commented Aug 21, 2021

DocGarbanzo Sep 7, 2021

Ezward Apr 10, 2022

DocGarbanzo Sep 7, 2021 •

edited

Loading

Ezward Apr 10, 2022

DocGarbanzo Sep 7, 2021

Ezward Apr 10, 2022

Ezward commented Apr 10, 2022

DocGarbanzo Apr 26, 2022

DocGarbanzo left a comment


		# background
		draw.rectangle(bounds, fill=self.background_color)

Next gen lidar parts #918

Next gen lidar parts #918

Conversation

Ezward commented Aug 15, 2021

DocGarbanzo left a comment

Choose a reason for hiding this comment

Ezward commented Aug 21, 2021

DocGarbanzo Sep 7, 2021

Choose a reason for hiding this comment

Ezward Apr 10, 2022

Choose a reason for hiding this comment

DocGarbanzo Sep 7, 2021 • edited Loading

Choose a reason for hiding this comment

Ezward Apr 10, 2022

Choose a reason for hiding this comment

DocGarbanzo Sep 7, 2021

Choose a reason for hiding this comment

Ezward Apr 10, 2022

Choose a reason for hiding this comment

Ezward commented Apr 10, 2022

DocGarbanzo Apr 26, 2022

Choose a reason for hiding this comment

DocGarbanzo left a comment

Choose a reason for hiding this comment

DocGarbanzo Sep 7, 2021 •

edited

Loading