About | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange

Developing a Planning Request Adapter

Description: This tutorial is a step by step development of a planning request adapter using a simple smoothing filter as an example

Tutorial Level: ADVANCED

Next Tutorial: Using a planning adapter inside of MoveIt

Overview

The industrial_trajectory_filters package in the industrial_core repository generally contains filters which were adapted from the arm navigation package which is a precursor to MoveIt!. Prior tutorials explained how to convert these older packages into planning adapter plugins. However this tutorial will explain the details of creating a planning adapter plugin. The tutorial uses a simple smoothing filter as an example to explain the concepts.

Required Matrials

A checked out copy of the industrial_core repository within a catkin work space such that it compiles. All the code is contained in the "industrial_trajectory_filters" package. Two other files are necessary to install the filter. First, the ompl_planning_pipeline.launch file needs to be modified to include the plugin in the pipeline. Next, a yaml file defining the filter coefficients needs to be created and loaded as part of the launch.

Files of Interest

• industrial_trajectory_filters
  • CMakeLists.txt
  • include/industrial_trjectory_filters/smoothing_trajectory_filter.h
  • src/smoothing_trajectory_filter.cpp
  • src/add_smoothing_filter.cpp
• planning_request_adapters_plugin_description.xml
• Launch file usually ompl_planning_pipeline.launch
• Yaml file defining the filter coefficients

The Plugin

MoveIt! uses 6 different kinds of plugins to perform a variety of functions. The specific plugin is determined by the launch file. Typically a ROS parameter is set telling the ROS node which of the available plugins to use. The 6 types are:

1. planning_interface::PlannerManager

2. kinematics::KinematicsBase

3. move_group::MoveGroupCapability

4. occupancy_map_monitor::OccupancyMapUpdater

5. rviz::Display

6. planning_request_adapter::PlanningRequestAdapter

Implementation of any one of these plugins requires the following steps

1. Create a class which inherits from one of the above classes
2. Implement all of the necessary functions of that class
3. Load and Register your class at the bottom of the class.cpp file
   1. CLASS_LOADER_REGISTER_CLASS(your_class_name,base_class_name)
4. Modify the xml file which tells the plugin loader about your plugin
5. Modify the CMakeLists.txt file so that your plugin is added to the library.

Creation of the Plugin Class

In this example we created the file "add_smoothing_filter.cpp" which inherits from "PlanningRequestAdapter." It therefore has to implement the following functions:

1. A constructor with no parameters
2. std::string getDescription() which returns a string describing the plugin
3. bool adaptAndPlan() which has very unique expectations
4. bool init() An Optional method

Our parameter-less constructor retrieves filter values from the ROS parameter server. The smoothing filter simply replaces every point/state in a robot trajectory with a weighted average of the surrounding points. This type of smoothing filter has advantages over finite impulse or infinite impulse response filters because it does not introduce phase lag. Don't worry if you are not familiar with filter design. It is enough to understand that it smooths out the trajectory while not causing significant deviation from the path. OMPL planners are sample based. They tend to produce trajectories which are not smooth, especially as they attempt to negotiate obstacles. Typically a smoothing filter for our application will have symmetric coefficients which are large in the middle and decrease toward the ends. It is necessary to have an odd number of coefficients. The default filter has 5 coefficients [ 0.25 0.5 1.0 0.5 0.25] which is both odd and symmetric.

Defining an arbitrary smoothing filter using ROS parameters

The constructor looks first for the following parameter: "/move_group/smoothing_filter_name" It then uses this base name to load the filter coefficients.

A valid smoothing_filter.yaml file looks like this

smoothing_filter_name: /move_group/smoothing_5_coef
smoothing_5_coef:
       - 0.25
       - 0.50
       - 1.00
       - 0.50
       - 0.25

These coefficients are added to a vector of doubles and define the filter. Changing the values changes the degree of smoothing. Generally more coefficients with thicker tails increases the degree to which the robot cuts corners of the original trajectory. One must load the yaml file somewhere in your launch scripts. For example, one might add the following lines to their ompl_planning_pipeline.launch file.

<rosparam command="load" file="$(find moveit_config)/config/smoothing_filter_params.yaml"/>

The constructor then initializes the filter with these coefficients.

 if(!smoothing_filter_->Init(filter_coef_))

getDescription()

This member function has an obvious purpose and simple implementation.

adaptAndPlan()

The adapt and plan function has very special requirements because of the way in which it is used by the planning pipeline. Adpaters are chained together so that calling the top of the chain calls all of the adapters in the chain. In order for the calling chain to work, each adapter must call the PlannerFn (planner function) provided in the signature. Some planning adapter plugins perform work prior to executing the planner, some perform work after, and some do both. Regardless, they must call the planner function passing along all the necessary parameters. For example, our plugin post-processes the trajectory, so it calls the planner prior to doing anything as follows:

bool result = planner(planning_scene,req,res);

If the results are good, it then post processes the resulting trajectory using the filter object as follows:

smoothing_filter_->applyFilter(*res.trajectory_);

Any planning request adapter that adds new waypoints to the trajectory must also keep track of the indices of the additional points. These are returned as the last parameter as a vector of indices. Our filter does not add any new points.

The smoothing filter: the plugin's workhorse

The smoothing filter is quite simple, it's applyfilter() method goes through the way points in the robot trajectory one by one, replacing each one with the weighted average of the adjacent points. However, it must do some special math at the beginning and end where no adjacent points exists. To handle the end cases we artificially generate the required adjacent points in such a way as to continue the slope.

One should also note that the trajectory of each joint is processed independently. Therefore, the body of the code has two nested for loops:

 1.  for(int i=0; i<num_states; i++){
 2.  for(int j=1; j<num_points-1; j++){

Within these loops, a new trajectory is computed using the coefficients as weights. Neither the first nor the last point is adjusted since these were the start and goal. One should also note how the original trajectory points are read into the vector "xv" so that they may be modified in place. With each iteration, the vector "xv" is shifted using the following lines of code:

for(int k=0; k<num_coef_-1; k++){
      xv[k] = xv[k+1];
}

The modified value computed as the weighted sum of adjacent values is stored using the command.

 rob_trajectory.getWayPointPtr(j)->setVariablePosition(i,sum/gain_); 

At the bottom of this file is call to a macro which must be called by all plugins.

CLASS_LOADER_REGISTER_CLASS(industrial_trajectory_filters::AddSmoothingFilter,
                            planning_request_adapter::PlanningRequestAdapter);

Adding the filter to the xml file which defines the available plugins

The file "planning_request_adapters_plugin_description.xml" should contain an entry for your newly created plugin similar to the following:

    <class   name="default_planner_request_adapters/AddFIRFilter“type="default_planner_request_adapters::AddFIRFilter" base_class_type="planning_request_adapter::PlanningRequestAdapter"> <description> </description> </class>