For a school project, I needed to learn ROS, and I wanted to use the Turtlebot3 to do this. I ended up creating my own visualisation of the data of the LDS-01 LIDAR on the bot. The raw notes for this are available in The Library.

I ended up making two nodes:

First off, I ran the LDS-01 node together with Rviz to see what kind of nodes and topics would be started. Running rqt_graph yielded the following:

rosgraph

You can see that the node of the LIDAR is called /hlds_laser_publisher and it publishes to a topic called /scan. If i want to read the data from the sensor, this is the topic I would need to subscribe to. I made a node called scan_values that reads the messages from the LIDAR node, converts them to a point cloud and publishes them. First off, I just wanted to read the data from the sensor and visualize it without a point cloud. However, I could not get the visualization to work as I couldn't properly interpret the data. The visualization looked like this:

lds01 raw map

The code can be found in scan.py. When running rqt_graph again, we now get this:

rosgraph laser values

You can see the node /scan_values is now subscribed to the /scan topic. After this, I started on the python script to retrieve the point cloud and visualize the data. The code for this van be found in pointcloud.py. When checking the graph, we can see this:

pointcloud

You can see that I called the node /lds01_pointcloud. However, the /scan_values node is not yet subscribed to it. Thus, I modified the code to publish the pointcloud to a topic called /lds01_pointcloud and made the /scan_values node subscribe to this. The graph now looks like this:

full nodes graph

You can also see that the visualisation is working in This video. I added some other info about the message and a disco mode.

Pointcloud.py

#! /usr/bin/env python

import sensor_msgs.point_cloud2 as pc2
import rospy
from sensor_msgs.msg import PointCloud2, LaserScan
import laser_geometry.laser_geometry as lg

rospy.init_node("lds01_pointcloud")

lp = lg.LaserProjection()
pc_pub = rospy.Publisher("lds01_pointcloud", PointCloud2, queue_size=1)

def scan_callback(msg):
    # convert the message of type LaserScan to a PointCloud2
    pc2_msg = lp.projectLaser(msg)

    pc_pub.publish(pc2_msg)


rospy.Subscriber("/scan", LaserScan, scan_callback, queue_size=1)
rospy.spin()
            

Scan.py

#! /usr/bin/env python

from curses.ascii import CAN
from dis import disco
from doctest import master
import random
from turtle import update
import rospy
from sensor_msgs.msg import LaserScan
from sensor_msgs.msg import PointCloud2
import sensor_msgs.point_cloud2 as pc2
import tkinter as tk
import math


CANVAS_SIZE = 1000

min_range = 1000
max_range = -1
min_intens = 1000
max_intens = -1


class Application(tk.Frame):
    def __init__(self, master=None):

        # --- tkinter init ---
        print("setting up tkinter")
        # --- ros init ---
        print("setting up ros")
        rospy.init_node('scan_values')
        self.sub = rospy.Subscriber(
            '/lds01_pointcloud', PointCloud2, self.callback)

        super().__init__(master)
        self.pack()
        self.setup_widgets()
        self.points = []
        self.canvas_points = []
        self.pc2_msg = None

        self.running = True
        self.received = False  # we need to check if we received a message from the lidar
        self.text = None
        self.last_x = 0
        self.last_y = 0
        self.current_color = 'red'
        self.colors = ['red', 'blue', 'green', 'orange', 'pink', 'cyan']
        self.disco_mode = False

    def __del__(self):
        self.node.destroy_node()
        self.running = False

    def callback(self, msg):

        self.pc2_msg = msg
        self.points = pc2.read_points_list(msg)
        self.received = True

    def setup_widgets(self):
        self.canvas = tk.Canvas(
            self, bg='white', height=CANVAS_SIZE, width=CANVAS_SIZE)
        for i in range(0, 10):
            self.canvas.create_line(0, i * 100, 1000, i*100, fill='gray')
            self.canvas.create_line(i * 100, 0, i * 100, 1000, fill='gray')
        self.canvas.create_rectangle(475, 475, 525, 525, fill='black')
        self.canvas.create_rectangle(505, 470, 520, 475, fill='black')
        self.canvas.create_rectangle(505, 525, 520, 530, fill='black')
        self.canvas.create_oval(510-3, 500-3, 515-3,
                                505-3, fill='white', outline='white')
        self.canvas.create_oval(
            490, 495, 500, 505, fill='gray', outline='black')
        self.canvas.pack()
        self.disco_button = tk.Button(text="Disco mode",command=self.discomode)
        self.disco_button.place(x=820,y=950)


    def run(self):
        while self.running:
            if self.received:
                t = "(" + str(self.pc2_msg.header.seq) + ", " + str(
                    self.pc2_msg.header.stamp) + ", " + str(self.pc2_msg.header.frame_id) + ")" + "\nSize: (" + str(self.pc2_msg.width) + "x" + str(self.pc2_msg.height) + ")\nBig Endian: " + str(self.pc2_msg.is_bigendian) + "\nPoint step: " + str(self.pc2_msg.point_step) + "\nRow step: " + str(self.pc2_msg.row_step) + "\nDense: " + str(self.pc2_msg.is_dense)
                self.text = self.canvas.create_text(200, 100, text=t)
                for i in self.points:
                    if (self.disco_mode):
                        dist = self.distance(
                            i.x, i.y, self.last_x, self.last_y)
                        if dist > 1:
                            self.current_color = random.choice(self.colors)
                        self.draw_point(i.x * 150 + CANVAS_SIZE/2,
                                        i.y * 150 + CANVAS_SIZE/2, color=self.current_color)
                        self.last_y = i.y
                        self.last_x = i.x
                    else:
                        self.draw_point(i.x * 150 + CANVAS_SIZE/2,
                                        i.y * 150 + CANVAS_SIZE/2)

            self.master.update_idletasks()
            self.master.update()

            self.clear_screen()

    def clear_screen(self):
        if (self.received):
            # delete all previously drawn items
            self.canvas.delete(self.text)
            for i in self.canvas_points:
                self.canvas.delete(i)
            self.canvas_points.clear()

    def draw_point(self, x, y, r=5, color='red'):
        p = self.canvas.create_oval(x, y, x+r, y+r, fill=color, outline=color)
        self.canvas_points.append(p)

    def distance(self, x1: float, y1: float, x2: float, y2: float) -> float:
        return math.sqrt(((x2 - x1) * (x2 - x1)) + ((y2-y1) * (y2-y1)))

    def discomode(self):
        self.disco_mode = not self.disco_mode


if __name__ == "__main__":
    root = tk.Tk()
    root.title("turtlebot lds01 visualizer")
    app = Application(master=root)
    app.run()