Install Lidar and Laser Filter

YD Lidar Package Installation

The Lidar comes with an open-source ROS package of Lidar nodes. It is an user-defined, independent package which is not included in the set of released ROS packages we installed previously. Thus, it needs to be installed separately, in a workspace other than the workspace used for ROS.

The instructions below largely reference the YD Lidar documentation page [2], which contains the source files and a user manual.

(Referenced from the Linux ROS operation -> ROS Drive Installation section of the manual)

First, download the ROS package source file (ROS.zip) from the website. Going through the YDLIDAR F4PRO User Manual is also helpful.

Click here to see the ydlidar download page

Then, make a workspace for the Lidar and create a folder called “src” within the workspace:

$ mkdir -p ydlidar_ws/src   // could also have your own name for the workspace

Copy over the source files into the /src folder.

Then, in /ydlidar_ws, issue the command:

$ catkin_make

(catkin_make is used to compile the ROS packages.)

After the compilation, add the environment variable:

$ source ./devel/setup.bash

And add a device alias /dev/ydlidar to F4PRO’s serial port.

$ cd ydlidar_ws/src/ydlidar/startup

$ sudo chmod +x initenv.sh

$ sudo sh initenv.sh

Because we have already installed the full set of ROS packages on Raspbian, which includes Rviz (a visualization tool used for the Lidar data), there is no need to install it separately as suggested in the manual.

To verify that the package is successfully installed, you can run the Lidar by issuing the following command:

$ roslaunch ydlidar lidar.launch

And the Lidar will start spinning (and taking in scanned data).

YD Lidar Nodes, LaserScan, and Launch Files  

After the YD Lidar package was installed, we could take a closer look at what was inside the package.

Nodes/Publisher/Subscriber

In the directory ydlidar_ws/src/ydlidar_master/src (assuming the original directory name for the downloaded Lidar packaged was kept), there are two files:

ydlidar_node.cpp

ydlidar_client.cpp

These two C++ files define ROS nodes. ROS nodes are blocks of code that define an object which exists in the system with certain features. One effective way to communicate between the ROS nodes is to make them Publishers and Subscribers, as what the Lidar source code does. In our case, ydlidar_node.cpp defines a Publisher node and ydlidar_client.cpp defines a Subscriber node.

A Publisher node, as the name suggests, publishes the data on the ROS platform with an unique identifier (called “topic” in ROS), and every Subscriber node that “listens” to that topic could receive the data. It is a one-to-many system: multiple Subscribers could subscribe to the same Publisher, as long as the specified topics match.

For the syntax, the ROS wiki has a beginner tutorial on how to write a simple Publisher and Subscriber system. [3]

Writing a Simple Publisher and Subscriber (C++):

http://wiki.ros.org/ROS/Tutorials/WritingPublisherSubscriber%28c%2B%2B%29

Writing a Simple Publisher and Subscriber (Python):

http://wiki.ros.org/ROS/Tutorials/WritingPublisherSubscriber%28python%29

(There are a lot of other tutorials to help understand Publisher/Subscriber system on the ROS wiki. Other helpful ones include writing the makefile and the .xml file for different nodes, etc. )

Once launched, a node will keep spinning and continuously publishing/receiving data. To run a node on ROS, simply issue the following command:

$ rosrun

E.g.

$ rosrun ydlidar ydlidar_node

Message Type/LaserScan

The thing sent by the Publisher and caught by the Subscriber is of the type “message” (msg in short). It is like an object that has different “fields” that can be assigned by the Publisher and accessed by the Subscriber. The message fields are in the form of:

variable_type variable_name

The documentation on the ROS wiki page for messages [4] (http://wiki.ros.org/msg) covers more details.

In YD Lidar, the message type that the Publisher and the Subscriber used to communicate is LaserScan, which could be found under sensor_msgs/LaserScan.msg [5] (http://docs.ros.org/melodic/api/sensor_msgs/html/msg/LaserScan.html). It is a common message type that applies to many devices that are scanning in data, and is ideal for Lidar data transfers.

To view the fields inside LaserScan, use the following command in any ROS package:

$ rosmsg show sensor_msgs/LaserScan

Launch Files

Finally, we will examine the ROS launch files [6] (http://wiki.ros.org/roslaunch). A launch file is not absolutely necessary for running the node, but it is used to supply the parameters that need to be passed into the node.

A launch file specifies the package, the node, the Publisher/Subscriber communication topic, and the different parameter types and values. Whenever a launch file is present, the node needs to be launched via the launch file:

$ roslaunch

E.g.

$ roslaunch ydlidar lidar.launch

In YD Lidar package, the launch files can be found in the directory ydlidar_master/launch.There are two files: lidar.launch is the plain launch file that starts the Lidar which publishes the data scanned in. lidar_view.launch uses Rviz, the visualization tool, to visualize the data scanned by the Lidar and draw the “obstacles” on the screen.

Rewriting Lidar Client in Python  

Now that we understand all the important parts of the Lidar package, we can start to modify the package to make something we desire.

One thing we noticed first was that the Lidar Publisher and Subscriber nodes are both written in C++. However, we would like our code to be in Python, to align with the other works already done on the PiCar, and to better reuse the previously developed code on Pi/Arduino communication, which were all written in Python.

ROS supports both Python and C++ equivalently, and the Publisher/Subscriber pairs are not restricted to the same programming language. Therefore, we decided to rewrite only the Subscriber node in Python and left the Publisher node unchanged. This required minimum effort but also met what we desired.

The Python code could be found on Github [7].

https://github.com/ameliama/PiCar_self/blob/master/ydlidar_client.py

After the code was written, we needed to build the node using catkin_make:

$ cd ydlidar_ws  // the catkin workspace folder

$ catkin_make

There is no need to change the makefile in order to build a Python ROS node (whereas for C++ nodes the file names will need to be added to the CMakeList.txt file).

After compilation, run the new Python node with the command:
$ roslaunch ydlidar ydlidar_client.py  // the name of the Python Subscriber

LaserScan Filters

As discussed before, YD Lidar uses LaserScan message type and outputs data in [angle, distance] pairs.

The LaserScan data originally taken in is the raw data, which is really noisy. Thus, we need to apply filters to remove the noise, and make our object detection algorithm more accurate.

There are several built-in LaserScan filters in ROS that specifically apply to LaserScan message type data. Their source code could be found in the laser_filters ROS package [8] that builds upon the filters package.

http://wiki.ros.org/laser_filters

Besides using a single filter, we can also connect several LaserScan filters to build a filter chain. The data will be passed into the first filter, and then from the first to the second, and finally come out from the last one.

One way to implement the filters is to use a scan-to-scan filter node. The node should be put in the middle of the Publisher (which publishes the raw scanned data) and the Subscriber (which receives the data). That way the Subscriber will receive the filtered data. The Subscriber should subscribe to the new topic of the filter node, instead of the Publisher topic.

Below are two simple diagrams that illustrate how the scan-to-scan filter node works:

Before adding the node:

After adding the node:

We picked the Median filter [9] (http://wiki.ros.org/filters/MedianFilter) provided in the package to filter our LaserScan data. The Median filter is a common type of filter which works by taking in the current measurement, finding the median of the current and an user defined number of previous values, and using the median as the current value. We implemented the filter within the scan-to-scan filter node, and we might add more filters to the filter chain as we see fit.

Below are the instructions on implementing the filter node and inserting a Median filter.

Download and install laser_filters package

The laser_filters package is not included in the set of ROS packages we installed before, and we need to install the package separately.

First, create a workspace to hold the package: (we named it laser_filters)

$ mkdir -p laser_filters/src

Then, in the src folder, download the source code of the laser_filters package from github.

E.g. by using wget:

$ cd laser_filters/src

$ wget https://github.com/ros-perception/laser_filters

Go back to the top of the workspace and issue catkin_make to compile the package:

$ cd ..

$ catkin_make

Set environment variable:

$ source ./devel/setup.bash

Now, issue the rospack command to check that the package is successfully installed:

$ rospack find laser_filters

Explore and reconfigure files

Go to the directory that contains the source files:

$ cd laser_filters/src/laser_filters/src

All C++ source files for the laser_filters package are under laser_filters/src (header files under laser_filters/include/laser_filters). It is very helpful to go through the source code to understand the code structure before making any changes. Be careful that some implementations might have been deprecated.

The most up-to-date, and commonly used nodes are:

  1. scan_to_scan_filter_chain (takes in and sends out data in LaserScan format)
  2. scan_to_cloud_filter_chain (takes in data LaserScan format and sends out data in PointCloud format (coordinate system))

Both of the nodes are really useful. We will focus on scan_to_scan_filter_chain here and will cover scan_to_cloud_filter_chain in a later section.

Under the same directory we can find median_filter.cpp, which is a definition file of the Median filter. It extends on the Filter base class and defines the functions configure() and update(), the required interface for every filter. The filter chain node calls these functions when the Median filter is used within the node.

Now, go to the directory that contains the configuration files:
$ cd ../examples

The directory contains both launch files and yaml files. As discussed before, the launch files are used to “wrap” the nodes and feed in parameters. In package ydlidar, the parameters are written in the launch file itself. However, here the parameters are written in a yaml file which is loaded into the launch file. Separating the parameters from the launch file makes the file structure more organized.

For the Median filter, open median_filter_5_example.yaml and set the configurations.

The fields in the yaml file are self-explanatory. For more explanations please see the documentation on Median filter on the ROS wiki.

The most important parameters here is the number_of_observations, which indicates the number of values that the filter will choose the median value from. The default value is set to be 5. Bigger number_of_observations will generally lead to less noise but longer “reaction” time (the older values will “mask” the new values coming in, and the change in data will not be reflected until many cycles later). There is a trade off between the noise level and the reaction time and you need to find the optimal number_of_observations to balance the two.

Here, we set the number of observations for our Median filter to be 13.

Then we will take a look at the launch file (median_filter_5_example.launch).

The launch file specifies the package (laser_filters), the node to call upon launch (scan_to_scan_filter_chain), the output (screen) and the name (laser_filter). It also specifies the yaml file to load upon launch.

In the middle, there is a line:

 

Which makes the Subscriber node search for the topic “base_scan” instead of “scan”. However, since our YD Lidar Publisher publishes data on the topic “scan”, there is no need for the remap. We can remove this line from the launch file.

Launch the filter chain with median filter

First, make sure that the YD Lidar is launched:

(In YD Lidar workspace)

$ roslaunch ydlidar lidar.launch

Then, simply use the launch file to launch the filter chain:

$ roslaunch laser_filters median_filter_5_example.launch

To check that the filter chain was successfully launched, use the rostopic command:

$ rostopic info

E.g.

$ rostopic info scan

shows the YD Lidar as the Publisher and the laser_filter (the name of the scan_to_scan_filter node specified in the launch file) as the Subscriber.

$ rostopic info scan_filtered

shows the laser_filter as the Publisher.

To receive the filtered data, change the ydlidar_client Subscriber node to listen to topic “scan_filtered” in the source code, and launch the node in its directory:

$ rosrun ydlidar ydlidar_client.py  // or ydlidar_client if using C++

Now the filtered data can be received by the Subscriber (use rostopic to double check).