The task here is to extract lines as features from laser scan data. There is a rather long setup procedure required before getting to your task, which is to implement the segmentation algorithm discussed in class. Code for the line fitting algorithm is already provided.
First, you will need to download some files and setup your package for this assignment.
- Unzip the following into your catkin workspace’s src directory: a3_world.zip
- Create the a3 package:
catkin_create_pkg a3 rospy geometry_msgs
- Change into the directory for a3 and create scripts and launch directories.
- Unzip the following in your scripts directory: a3_scripts.zip
- Unzip the following in your launch directory: a3_launch.zip
We will use rviz (ROS’s visualization tool) to display the raw scan data from the laser range finder, as well as our own estimated lines which should ideally fit the raw data. In order to understand this process, you should go through the following steps. However, this would be tedious to do each time so at the end you will save the rviz configuration file and load it whenever rviz starts.
First, we bring up our simulation using one of the launch files provided above. Execute the following:
roslaunch a3 setting_up.launch
Both stage and rviz should open. If not, do some investigation then ask for help.
In rviz an error message will appear in the left panel saying “.Global Status: Error”. Don’t worry. We will take care of this. Click on “Add” and select “TF” under “rviz”. Now under the “Global Options” item in the left tab select “Fixed Frame” and choose /odom.
Click the “Add” button in the left tab to incorporate a new display. In this case select “LaserScan”. Now look for “topic” under the “LaserScan” item just added. Set the topic to “base_scan” and you should see a live update of the data points from the current laser scan. You can play with the “Style” and “Size” attributes to improve the visibility of the laser return points in rviz. Its helpful at this stage to save rviz’s current configuration by selecting “Save Config As” from the file menu. Save the configuration file as “rviz.rviz” in the a3 directory.
Now quit rviz and stage and execute the following:
roslaunch a3 stage_rviz.launch
You should immediately see the TF frames and the live laser scan data being continuously updated. Note that laser scan data is noise-free! The launch file stage_rviz_noisy.launch is provided so that you can test your segmentation algorithm against more realistic data.
Setting Up the ExtractedLine and ExtractedLines Message
Follow these steps to create the ExtractedLine and ExtractedLines message types within your comp6912_a3 package:
- Create a msg subdirectory in a3 (execute roscd a3; mkdir msg)
- Create the file msg/ExtractedLine.msg and fill it with the following:
Python123456789101112# The r and alpha parameters define the extracted line.float32 rfloat32 alpha## The following points are used for display / debugging purposes only.# These points come from the position of the first and last laser scan points# used to estimate the line. They do not define the line and may not have even# been used in estimating the line (e.g. they may have exceeded the# maximum_range parameter). However, they are useful in visualizing what range# of scan points were used to estimate the line.geometry_msgs/Point firstScanPointgeometry_msgs/Point lastScanPoint
- Create the file msg/ExtractedLines.msg and fill it with the following:
Python12Header headerExtractedLine lines
- Follow the instructions in section 3 on this page. These are essentially the same instructions as mentioned in ROS tutorial 10. Be mindful that both sets of instructions are really examples that need to be adapted. For example, you have to add two message files, not just one.
- Go to the root of the workspace (cd ~/catkin_ws) and execute catkin_make.
Testing the Setup
To test that everything is setup correctly, do the following:
- Launch either stage_rviz.launch (for noise-free laser data) or stage_rviz_noisy.launch (for noisy data) in one terminal
- Launch extract_single.launch in another terminal
- Go to rviz, click the “Add” button and select “Marker”. Now change “Marker Topic” to /visualization_lines. In rviz you should see a single red line being fitted to the data.
- Once again, save rviz’s configuration so that the steps above do not have to be repeated.
- Finally, read through extract_single_line.py to see what is happening here.
Create a new script called extract_all_lines.py based on extract_single_line.py. The fundamental difference is that extract_all_lines should apply the segmentation algorithm discussed in class to fit multiple lines to the data. You should feel free to create additional functions and/or classes to make this job easier.
The segmentation algorithm discussed in the notes has two phases: split and merge. You should implement only the split phase and be careful to prevent extraneous splits. Ideally, the algorithm should run in real-time, meaning that it can keep up with the 10 Hz update cycle of the Stage simulator. If not, you may see lagging in the lines displayed by rviz.
Utilize the following parameters which are provided in the extract_all launch file:
- orthog_distance_threshold: The distance threshold used when deciding to split a line.
- min_points_per_line: The minimum number of data points required to fit a line.
- maximum_range: The maximum range of data points. You should ignore points in the laser scan which are further than this range.
These parameters can be obtained from the ROS parameter server. You can find an example in extract_single_line.py. You should feel free to change these values to suit your implementation.
Run your code by launching extract_all.launch.
The three coloured squares in the stage simulation indicate testing positions. You should find that your algorithm consistently yields the following line configurations when tested at each square:
- Blue square: A single vertical line
- Green square: Two horizontal lines above and below
- Red square: Two horizontal and one vertical line
When running using the noisy laser data there will be some variation (e.g. extra lines will suddenly appear, then disappear). This is acceptable.
Submit extract_all_lines.py and extract_all.launch (since you may customize the parameters it contains).