Filter Laser Scans
The aim of this example is to provide instruction on how to filter scan messages.
For robots with laser scanners, ROS provides a special Message type in the sensor_msgs package called LaserScan to hold information about a given scan. Let's take a look at the message specification itself:
# Laser scans angles are measured counter clockwise, with Stretch's LiDAR having # both angle_min and angle_max facing forward (very closely along the x-axis) Header header 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] 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]
For a Stretch robot the start angle of the scan,
angle_max, are closely located along the x-axis of Stretch's frame.
angle_max are set at -3.1416 and 3.1416, respectively. This is illustrated by the images below.
Knowing the orientation of the LiDAR allows us to filter the scan values for a desired range. In this case, we are only considering the scan ranges in front of the stretch robot.
First, open a terminal and run the stretch driver launch file.
# Terminal 1 roslaunch stretch_core stretch_driver.launch
Then in a new terminal run the
rplidar.launch file from
# Terminal 2 roslaunch stretch_core rplidar.launch
To filter the lidar scans for ranges that are directly in front of Stretch (width of 1 meter) run the scan_filter.py node by typing the following in a new terminal.
# Terminal 3 cd catkin_ws/src/stretch_tutorials/src/ python3 scan_filter.py
Then run the following command to bring up a simple RViz configuration of the Stretch robot.
# Terminal 4 rosrun rviz rviz -d `rospack find stretch_core`/rviz/stretch_simple_test.rviz
#!/usr/bin/env python3 import rospy from numpy import linspace, inf from math import sin from sensor_msgs.msg import LaserScan class ScanFilter: """ A class that implements a LaserScan filter that removes all of the points that are not in front of the robot. """ def __init__(self): self.width = 1.0 self.extent = self.width / 2.0 self.sub = rospy.Subscriber('/scan', LaserScan, self.callback) self.pub = rospy.Publisher('filtered_scan', LaserScan, queue_size=10) rospy.loginfo("Publishing the filtered_scan topic. Use RViz to visualize.") def callback(self,msg): """ Callback function to deal with incoming LaserScan messages. :param self: The self reference. :param msg: The subscribed LaserScan message. :publishes msg: updated LaserScan message. """ angles = linspace(msg.angle_min, msg.angle_max, len(msg.ranges)) points = [r * sin(theta) if (theta < -2.5 or theta > 2.5) else inf for r,theta in zip(msg.ranges, angles)] new_ranges = [r if abs(y) < self.extent else inf for r,y in zip(msg.ranges, points)] msg.ranges = new_ranges self.pub.publish(msg) if __name__ == '__main__': rospy.init_node('scan_filter') ScanFilter() rospy.spin()
The Code Explained
Now let's break the code down.
import rospy from numpy import linspace, inf from math import sin from sensor_msgs.msg import LaserScan
rospyif you are writing a ROS Node. There are functions from
maththat are required within this code, thus why linspace, inf, and sin are imported. The
sensor_msgs.msgimport is so that we can subscribe and publish
self.width = 1 self.extent = self.width / 2.0
self.sub = rospy.Subscriber('/scan', LaserScan, self.callback)
LaserScanmessages. When a message comes in, ROS is going to pass it to the function "callback" automatically.
self.pub = rospy.Publisher('filtered_scan', LaserScan, queue_size=10)
pub = rospy.Publisher("filtered_scan", LaserScan, queue_size=10)declares that your node is publishing to the filtered_scan topic using the message type
LaserScan. This lets the master tell any nodes listening on filtered_scan that we are going to publish data on that topic.
angles = linspace(msg.angle_min, msg.angle_max, len(msg.ranges))
points = [r * sin(theta) if (theta < -2.5 or theta > 2.5) else inf for r,theta in zip(msg.ranges, angles)]
new_ranges = [r if abs(y) < self.extent else inf for r,y in zip(msg.ranges, points)]
msg.ranges = new_ranges self.pub.publish(msg)
rospy.init_node(NAME, ...), is very important as it tells rospy the name of your node -- until rospy has this information, it cannot start communicating with the ROS Master. NOTE: the name must be a base name, i.e. it cannot contain any slashes "/".
Instantiate the class with