TurtleBot3 - Notes on Lidar HLS-LFCD-LDS

Subscribe Send me a message home page tags


#raspberry-pi  #python  #ros  #turtlebot3  #burger  #HLS-LFCD-LDS  #LDS01 

Introduction

This post contains notes when learning the Lidar on TurtleBot3.

Related Readings

Orientation of the Robot and the Lidar Coordinate System

The data from Lidar is a mapping from theta to distance. The theta is the position of the Lidar in a coordinate system. It's surprisingly hard to figure out this coordinate system. I still don't find any document that formally describes the zero degree axis of the Lidar on the TurtleBot3 robot. Here we will make some education guess based on materials found online. Of course, the only way to figure our the coordinate is to calibrate the Lidar.

First, we assume the photo on the TurtleBot3 website is the front of the robot. In the upper left part of the figure below, we can see that the rubber wheels are in front of the third stainless steel wheel. The bottom left part shows a bot in motion and the bottom right part of the figure shows the position of the lidar when the drive moves forward.

The upper right part of the figure shows the coordinate system of the lidar.

turtlebot3-lidar.jpg

We could reasonably assume this coordinate system is also the coordinate system of the robot.

LaserScan Message API

We digress a little here. TurtleBot3 is controlled by ROS, which defines a set of common APIs. For example, ROS defines the LaserScan message which can be used to report laser distance sensor data.

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
# Single scan from a planar laser range-finder
#
# If you have another ranging device with different behavior (e.g. a sonar
# array), please find or create a different message, since applications
# will make fairly laser-specific assumptions about this data

Header header            # timestamp in the header is the acquisition time of
                         # the first ray in the scan.
                         #
                         # in frame frame_id, angles are measured around
                         # the positive Z axis (counterclockwise, if Z is up)
                         # with zero angle being forward along the x axis

float32 angle_min        # start angle of the scan [rad]
float32 angle_max        # end angle of the scan [rad]
float32 angle_increment  # angular distance between measurements [rad]

float32 time_increment   # time between measurements [seconds] - if your scanner
                         # is moving, this will be used in interpolating position
                         # of 3d points
float32 scan_time        # time between scans [seconds]

float32 range_min        # minimum range value [m]
float32 range_max        # maximum range value [m]

float32[] ranges         # range data [m] (Note: values < range_min or > range_max should be discarded)
float32[] intensities    # intensity data [device-specific units].  If your
                         # device does not provide intensities, please leave
                         # the array empty.

Notes on intensities field found online

The intensities field should contain the measured intensity (i.e. brightness) of the reflected laser beams. This is, however, device specific and thus you cannot really make any assumptions about the values. In general, the higher the value, the brighter the reflected laser beam.

You can use the intensities to make some assumptions about the properties of the scanned material. E.g. it is used to detect reflectors and reflector tape, as there the intensity of the reflected beam is, in general, higher than from other material.

HLS-LFCD-LDS Driver

The source code of the driver can be found here. We are interested in the part where the code publishes the Lidar data to ROS.

This code can be found in the hlds_laser_publisher.cpp file:

1
2
3
4
5
6
7
8
9
10
while (ros::ok())
{
  sensor_msgs::LaserScan::Ptr scan(new sensor_msgs::LaserScan);
  scan->header.frame_id = frame_id;
  laser.poll(scan);
  scan->header.stamp = ros::Time::now();
  rpms.data=laser.rpms;
  laser_pub.publish(scan);
  motor_pub.publish(rpms);
}

How does scan variable get populated? The fields of scan are set in the poll method in the script. We can also see in the code the communication is done through serial communication.

1
2
3
4
5
6
7
8
...

boost::asio::read(serial_,boost::asio::buffer(&raw_bytes[2], 2518))

...

scan->ranges[359-index] = range / 1000.0;
scan->intensities[359-index] = intensity;
serial_communication_lidar.jpg

Becuase the sensor data is published directly, we should be able to get the sensor data without any ROS or TurtleBot3 packages. Here is the python code to read the Lidar data, it's a direct translation from the hlds_laser_publisher.cpp file (Some comments in the source code don't seem accurate so we just ignore them.)

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
import os
import time

def clearScreen():
    os.system('cls' if os.name == 'nt' else 'clear')

def convertByteToInt(byte):
    return int.from_bytes(byte, 'big')

def getIntValueOfByte(byteArray, index):
    return convertByteToInt(byteArray[index])

def getCurrentTimeInSecond():
    return time.time()

class Lidar:
    def __init__(self, byteReader):
        self.byteReader = byteReader
        self.got_scan = False
        self.lastDisplayTime = 0

    def poll(self):
        start_count = 0
        raw_bytes = bytearray(2520)
        good_sets = 0
        motor_speed = 0
        rpms = 0

        while not self.got_scan:
            receivedByte = self.byteReader.read()
            raw_bytes[start_count] = receivedByte[0]

            if start_count == 0:
                if raw_bytes[start_count] == 0xFA:
                    start_count = 1
            elif start_count == 1:
                if raw_bytes[start_count] == 0xA0:
                    start_count = 0
                    self.got_scan = True
                    receivedBytes = self.byteReader.read(2518)
                    raw_bytes[2:] = receivedBytes

                    toPrintResult = getCurrentTimeInSecond() - self.lastDisplayTime > 1

                    if toPrintResult:
                        self.lastDisplayTime = getCurrentTimeInSecond()
                        clearScreen()
                        print(">>> time = {}".format(self.lastDisplayTime))

                    for i in range(0, len(raw_bytes), 42):
                        if raw_bytes[i] == 0xFA and raw_bytes[i+1] == (0xA0 + int(i / 42)):
                            good_sets += 1
                            a = raw_bytes[i+3] << 8
                            b = raw_bytes[i+2]
                            motor_speed += a + b
                            rpms = int((a|b) / 10)

                            for j in range(i+4, i + 40, 6):
                                index = 6 * (int(i / 42)) + int((j - 4 - i) / 6)
                                byte0 = raw_bytes[j]
                                byte1 = raw_bytes[j + 1]
                                byte2 = raw_bytes[j + 2]
                                byte3 = raw_bytes[j + 3]

                                intensity = (byte1 << 8) + byte0
                                distRange = (byte3 << 8) + byte2
                                angle = 359 - index

                                if toPrintResult and distRange > 120 and distRange < 3500 and intensity > 3200:
                                    print("theta = {}, range(m) = {}, intensity = {}, rpms = {}".format(359 - index, distRange / 1000.0, intensity, rpms))
                else:
                    start_count = 0

if __name__ == '__main__':
    import serial
    baudrate = 230400
    ser = serial.Serial('/dev/ttyUSB0', baudrate)
    lidar = Lidar(ser)
    while True:
        lidar.poll()
        lidar.got_scan = False

    # ser.close()

----- END -----

©2019 - 2024 all rights reserved