pythonpython-3.xmathsensorslidar

How to extract x and y coordinates from LIDAR sensor(LMS511) data in Python?

I have a LIDAR sensor, specifically the SICK LMS511, which outputs continuous data in the following format:

sRA LMDscandata 0 1 151FDC8 0 0 EC4B EDD5 85BF655E 85BF9621 0 0 3E 0 0 2710 21C 0 1 DIST1 3F800000 00000000 4A0E5 1A0B B5 305 30F 312 315 320 325 328 32F 0 0 B62 B35 B16 AD8 AC9 742 753 767 779 791 7A8 7B8 764 782 793 7B1 7C8 7E5 7FF 817 807 806 834 824 816 802 7F8 7E8 7DA 7C9 7B0 7AF 797 789 780 771 767 781 7AB 7A6 796 788 77F 77E 771 76B 769 751 74A 742 73A 732 731 724 71C 71C 716 70F 707 701 701 6FC 6F2 6F2 6E9 6EC 6E7 6E5 6E3 6E4 6DA 6D6 6D5 6D5 6D6 6D4 6D8 6D7 6D2 6CE 6D2 6D4 6D4 6D4 6CE 6D0 6D8 6E3 6DC 6E1 6E4 6E4 6E9 6E9 6FA 6ED 6F7 6F7 702 70A 707 712 710 71A 720 726 728 730 73C 740 74A 751 759 765 76D 770 787 78A 796 7A3 7A9 7B2 7C6 7D5 7E2 7E9 7FC 808 809 828 837 848 85B 86B 87B 88C 89B 8B3 8D1 8E8 8F8 90F 91C 93E 957 971 989 96A 94E 974 992 9B9 9CC 9E5 A11 A88 AD7 B09 B2F B59 B8A BB5 BE8 C1E C54 C85 CBD D07 D3A D81 DC6 0 0 0 0 0 0

I want to extract the x and y coordinates from this data. How can I do this in Python?

Note: Please note that this is just one sample data from the sensor, and it outputs continuous data in this format.

I have tried to extract the x and y coordinates from the LIDAR data using Python. Specifically, I attempted to parse the data using Python and extract the coordinates. However, I have not been successful in extracting the coordinates.

Solution

  • Telegram Structure

    The telegram in question is documented in Telegram Listing reference, starting on pages 97-115.

    The telegram consists of space separated tokens, the numbers are generally hexadecimal. First, there is what you could call a header, of 18 tokens. I've briefly annotated the header from your example:

    Command type           sRA
Command                LMDscandata
Version                0
Device number          1
Serial number          151FDC8
Device status          0 0
Telegram counter       EC4B
Scan counter           EDD5
Time since startup     85BF655E
Time of transmit       85BF9621
Digital inputs         0 0
Digital outputs        3E 0
Reserved/layer angle   0
Scanning frequency     2710          (100 Hz)
Measurement frequency  21C           (54 kHz)

    Next comes the variable part of the telegram, consisting of 8 sections. Each section starts with a count token, signifying how many blocks of data (if any) that section consists of. When the sensor is not configured to provide particular type of data, the count will be 0, and the next section immediately follows.

    # of encoder blocks         0
[<encoder info>]
# of 16-bit channel blocks  1
[<channel blocks>]
# of 8-bit channel blocks   0
[<channel blocks>]
Position                    0
[<position info>]
Device name                 0
[<name text>]
Comment                     0
[<comment text>]
Time                        0
[<timestamp>]
Events                      0 
[<event info>]

    In your case, the situation is simple, as there is only 1 block of 16-bit channel data. The layout of this block is:

    Content                 DIST1     (Distance values of first pulse)
Scaling factor          3F800000  (1x)
Scale factor offset     00000000
Start angle             4A0E5     (30.3333 deg)
Angular step size       1A0B      (0.6667 deg)
Value count             B5        (181)
Data                    305 ... DC6

    Parsing the Telegram

    With that out of the way, we can come up with a rudimentary parser for your specific sensor configuration:

    A crude implementation of this in plain Python might look like this:

    def parse_telegram(telegram):
    tokens = telegram.split(' ')
    assert(len(tokens) > (18 + 8)) # Minimum valid length
    header = tokens[:18]
    assert(header[0] == 'sRA') # Correct command type
    assert(header[1] == 'LMDscandata') # Correct command
    sections = tokens[18:]
    assert(int(sections[0]) == 0) # No encoder data
    assert(int(sections[1]) == 1) # Exactly 1 16-bit channel block
    assert(sections[2] == 'DIST1') # Expected distance data
    assert(sections[3] in ['3F800000', '40000000'])
    scale_factor = 1 if sections[3] == '3F800000' else 2
    assert(sections[4] == '00000000')
    start_angle = int(sections[5], 16) / 10000.0
    angle_step = int(sections[6], 16) / 10000.0
    value_count = int(sections[7], 16)
    values = list(map(lambda x: int(x, 16) * scale_factor, sections[8:8+value_count]))
    # The following could be cached to avoid recalculation
    # since it will be the same until sensor config is changed...
    angles = [start_angle + angle_step * n for n in range(value_count)]
    return (values, angles)

    The function returns a tuple of two lists -- first containing the distances, second the corresponding beam angles. We can use matplotlib to plot this result on a polar plot to see if it makes sense:

    Values and angles plotted on polar plot

    Polar to Cartesian

    Converting the polar coordinates to Cartesian is just a matter of applying some basic trigonometry:
    x = r × cos(θ)
    y = r × sin(θ)

    In plain Python:

    def to_cartesian(distances, angles):
    x = list(map(lambda r, t: r * math.cos(math.radians(t)), distances, angles))
    y = list(map(lambda r, t: r * math.sin(math.radians(t)), distances, angles))
    return (x, y)

    Again, a quick plot to check if the result makes sense:

    Cartesian coordinates plotted