Assignment 3, Part 2

Updates

  • [Feb. 19] Deadline for Part 2 extended to Friday February 21 at 5:00 due to problems with the lab.  (Part 1 remains due at 12:30 on Thursday).
  • [Feb. 19] If there seems to be a problem including messages when running the provided code, make sure that the generate_messages line appears in CMakeLists.txt.  In other words make sure that the following appears and is not commented out:

    generate_messages(
      DEPENDENCIES
      std_msgs # Or other packages containing msgs
      geometry_msgs
    )
    In one of the links provided below (highlighted in blue below) this instruction was not provided.  Of course, you still need to follow the other instructions that describe changes to packages.xml and other changes to CMakeLists.txt.

Introduction

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.

Setting Up

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: comp4766_a3_world.zip
  • Create the comp4766_a3 package:
    catkin_create_pkg comp4766_a3 rospy geometry_msgs
  • Change into the directory for comp4766_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 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 comp4766_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 comp4766_a3 directory.

Now quit rviz and stage and execute the following:

  roslaunch comp4766_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 comp4766_a3 package:

  • Create a msg subdirectory in comp4766_a3 (execute roscd comp4766_a3; mkdir msg)
  • Create the file msg/ExtractedLine.msg and fill it with the following:
    # The r and alpha parameters define the extracted line.
    float32 r
    float32 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 firstScanPoint
    geometry_msgs/Point lastScanPoint
  • Create the file msg/ExtractedLines.msg and fill it with the following:
    Header header
    ExtractedLine[] 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.  It is not so useful now, but it will be helpful below to also visualize the range of scan points used to estimate the line.  Again click “Add” then “Marker”.  Change “Marker Topic” to /visualization_scanpoints.  You will see two new red dots appear.  These show the first and last scan points from the laser that were used to estimate this line.  Note that they are not on the line because these points are too distant to be used in the estimation of the line.  You may see the benefit of visualizing these points later on.
  • 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.

Segmentation (Your Turn!)

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.

Testing

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.

Submission

Submit extract_all_lines.py and any other files you decide to create.  You should not modify the launch files provided.  If any customization is required to run your code, provide a readme.txt file in your submission.