Detection of Lines in Laser Data
This plugin takes a point cloud (which is assumed to have been generated from a laser sensor using the laser-pointclouds plugin?, i.e. all values lie in the x-y-plane) and uses RANSAC to find lines in this data. Up to a configurable number of lines (best fit first) are written to the blackboard.
For each line the plugin publishes a point on the line, a direction vector, and the bearing to the point on line. Additionally it publishes a point cloud with colored and optimized lines, and if compiled with visual debugging also the point on line and direction vectors, and IDs.
The lines are ordered by their bearing angle (to have stable IDs when stationary) and the point on line is optimized to be the point with the shortest distance from the reference frame origin (typically the laser sensor's center).
Configuration
The configuration supports parameterization of the processing steps and input/output data.
laser-lines: # Automatically start, i.e. set enabled to true? auto-start: true # Maximum number of iterations to perform for line segmentation line_segmentation_max_iterations: 250 # Segmentation distance threshold; m line_segmentation_distance_threshold: 0.1 # Minimum number of inliers to accept a line line_segmentation_min_inliers: 20 # The maximum number of clusters to publish on the blackboard # The interfaces will be named "/laser-lines/N" for N=1..max_num_clusters max_num_lines: 3 # If the distance of a point on line moves between two frames by this much (meters) # it is considered to be a different line and this restarts the visibility history. # This is to distinguish switch_tolerance: 0.3 # Input point cloud ID as generated by laser-pointclouds input_cloud: lase_edl-360
BlackBoard Interfaces
The blackboard interfaces depend on the particular number of lines.
- LaserLineInterface::/laser-lines/N for N=1..max_num_clusters (from config)
- SwitchInterface::laser-lines: allows to enable or disable processing
Point Clouds
- Reads the input point cloud with the ID configured as input_cloud.
- Writes a colored debugging cloud to a cloud with the ID laser-lines
Compilation and Runtime Requirements
- Laser plugin for laser input
- laser-pointclouds? plugin for converting laser data to point clouds
- Optional: ros, ros-tf?, ros-pcl?, rviz for visualization (if HAVE_VISUAL_DEBUGGING set at compile time), will publish to the marker array topic with namespace laser_lines
Usage Instructions
Run with a appropriate transforms, e.g. using static-transforms or robot-state-publisher. Setup an appropriate laser driver to gather data, and possible a laser-filter cascade to constrain the data, e.g. to look for lines only in the "forward" sector. Setup the input point cloud to match the point cloud generated by laser-pointclouds?. Then run the plugin and ros-pcl? and visualize the debugging point cloud in rviz. When sufficiently large lines are in range you should see colored and optimized (truely straight) point cloud data with a color per line (see screenshot above).