| Navigation |
|---|
| Home |
| Programming |
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:
rqt_graph yielded the following:
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:
The code can be found in scan.py. When running rqt_graph
again, we now get this:
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:
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:
You can also see that the visualisation is working in This video (mute the audio, my mic driver was goofin'). I added some other info about the message and a disco mode.
#! /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()
#! /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()