Building a Basic LaserScan Subscriber Node

The Code

Copy all the code below into your lidar_subscriber.py file and then review the annotations to understand how it all works.

lidar_subscriber.py

#!/usr/bin/env python3

import rclpy
from rclpy.node import Node
from rclpy.signals import SignalHandlerOptions # (1)!

from sensor_msgs.msg import LaserScan # (2)!
import numpy as np # (3)!

class LidarSubscriber(Node): 

    def __init__(self): 
        super().__init__("lidar_subscriber")

        self.lidar_sub = self.create_subscription(
            msg_type=LaserScan,
            topic="/scan",
            callback=self.lidar_callback,
            qos_profile=10,
        ) # (4)!

        self.get_logger().info(f"The '{self.get_name()}' node is initialised.")

    def lidar_callback(self, scan_data: LaserScan): 
        left = scan_data.ranges[0:21] 
        right = scan_data.ranges[-20:] # (5)!
        front = np.array(right + left) # (6)!

        valid_data = front[front != float("inf")] # (7)!
        if np.shape(valid_data)[0] > 0: # (8)!
            single_point_average = valid_data.mean() # (9)!
        else:
            single_point_average = float("nan") # (10)!

        self.get_logger().info(
            f"LiDAR Reading (front): {single_point_average:.3f} meters.",
            throttle_duration_sec = 1,
        ) # (11)!

def main(args=None):
    rclpy.init(
        args=args,
        signal_handler_options=SignalHandlerOptions.NO
    )
    node = LidarSubscriber()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        print("Shutdown request (Ctrl+C) detected...")
    finally:
        node.destroy_node()
        rclpy.shutdown() 

if __name__ == '__main__':
    main()

1. None of this should be new to you by now. Remember from Part 2 that we're using SignalHandlerOptions to handle shutdown requests (triggered by Ctrl+C).
2. We're building a /scan subscriber here, and we know that this topic uses the sensor_msgs/msg/LaserScan interface type, so we import this here.
3. numpy is a Python library that allows us to work with numeric data, very useful for big arrays like ranges.
4. We construct a subscriber in much the same way as we have done in Parts 1 and 2, this time targetting the /scan topic though.

5. From the front of the robot, we obtain a 20° arc of scan data either side of the x-axis (see the figure below).

6. Then, we combine the left_20_deg and right_20_deg data arrays, and convert this from a Python list to a numpy array (see the figure below).

7. This illustrates one of the great features of numpy arrays: we can filter them.

   Remember that front is now a numpy array containing 40 data points.

   Remember also, that there will typically be several inf values scattered around the LaserScan array, resulting from sensor readings that are outside the sensor's measurement range (i.e. greater than range_max or less than range_min). We need to get rid of these, so we ask numpy to filter our array as follows:

   1. Of all the values in the front array, determine which ones are not equal to inf:

      front != float("inf")

   2. Use this filter to remove these inf values from the front array:

      front[front != float("inf")]

   3. Return this as a new numpy array called valid_data:

      valid_data = front[front != float("inf")]

8. In certain situations (i.e. in very sparse environments) all values could be equal toinf (imagine the "empty world" simulation). Here we're checking the size of the valid_data array to make sure that we haven't just removed all values through the above filtering process!

9. If the array is not empty, then use the mean() method to determine the average of all readings within the dataset

10. If the array is empty, then return "not a number" (aka "nan") instead

11. Print this value to the terminal, but throttle the messages so that only one is displayed every second

    Question

    If we didn't throttle this, what rate would the messages be printed at?

The data processing is illustrated in the figure below:

Package Dependencies

This node has dependencies on two external Python libraries (in addition to rclpy):

from sensor_msgs.msg import LaserScan
import numpy as np

As such, you should include these in the package.xml file (under the <exec_depend>rclpy</exec_depend> line):

package.xml

<exec_depend>sensor_msgs</exec_depend>
<exec_depend>python3-numpy</exec_depend>