Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.

Converting arm navigation trajectory filters into MoveIt Planning Request Adapters

This tutorial will guide you through the process of taking a legacy Arm Navigation trajectory filter and converting into a Planning Request Adapter that can be used within MoveIt. These filter's are usually used to check that a robot's path avoids some sort of constraint.

The following instructions are based on the process of converting the Arm Navigation Filter Base interface (the interface for an arm trajectory filters) into a Planning Request Adapter that is used by MoveIt. The arm trajectory base filter class can be located here: /industrial_trajectory_filters/include/industrial_trajectory_filters/filter_base.h Since this version of the Filter Base class only provides some degree of backwards compatibility with the original Arm Navigation interface, some parts of the original Arm Navigation filter code must be modified in order to accommodate the differences introduced by the new adapter interface class.

Every arm trajectory filter implements two pure virtual methods called update() and configure(). The update method is where the bulk of the trajectory filter computation is completed and update is where configuration changes to the filter are made. Our goal is to simply shim these two methods into our new planning adapter for MoveIt. Each filter also has a type and name getter method that simple return descriptive strings that should alos remain in place.

All the getParam() methods for parameter lookup are not supported by the new Filter Base interface and so the filter implementation must be modified so that parameters are read using the regular roscpp parameter API (either through the node handle or the bare version)

A MessageAdapter structure has been provided in order to replace the Arm Navigation message with which the original template interface was specialized.

Configuring the Package Configuration XML File

The following dependencies must be added to the package.xml file in the catkin package that contains your filter implementation. Our new planning request adapter will talk using trajectory messages so we will need both to build against and run the package with trajectory messages. To bring in the MoveIt planning request adapter templates we will need MoveIt and core and planning. We also need to bring in the old trajectory filters and finally the plugin library. Finally, at the end of the package file we need insert an export directive to tell ROS to roll our new adapter into a plugin.

   1   <build_depend>trajectory_msgs</build_depend>
   2   <build_depend>pluginlib</build_depend>
   3   <build_depend>moveit_core</build_depend>
   4   <build_depend>moveit_ros_planning</build_depend>
   5   <build_depend>industrial_trajectory_filters</build_depend>
   6 
   7   <run_depend>trajectory_msgs</run_depend>
   8   <run_depend>pluginlib</run_depend>
   9   <run_depend>moveit_core</run_depend>
  10   <run_depend>moveit_ros_planning</run_depend>
  11   <run_depend>industrial_trajectory_filters</run_depend>
  12 
  13   <export>
  14     <moveit_core 
  15       plugin="${prefix}/planning_request_adapters_plugin_description.xml"/> 
  16   </export>

Create description file for your plugin

Create a planning_request_adapters_plugin_description.xml file in your package directory and fill it with the following information:

   1 <library path="libmyplugin_package">
   2   <class name="my_filter_package/MyFilter"
   3     type="my_filter_package::MyFilterAdapter"
   4     base_class_type="planning_request_adapter::PlanningRequestAdapter">
   5     <description>
   6       This is my trajectory filter plugin
   7     </description>
   8   </class>
   9 </library>

Configure CMakeLists.txt

1. Add package dependencies

      1 find_package(catkin REQUIRED COMPONENTS moveit_ros_planning trajectory_msgs industrial_trajectory_filters)
   

2. Include location of your package's header files

      1 include_directories(include ${catkin_INCLUDE_DIRS})
   

3. Build plugin library

      1 add_library(moveit_trajectory_filter_plugins src/my_filter.cpp)
      2 target_link_libraries( moveit_trajectory_filter_plugins ${catkin_LIBRARIES})
   

4. Make plugin library available to other packages

      1 catkin_package(
      2   CATKIN_DEPENDS moveit_ros_planning trajectory_msgs
      3   INCLUDE_DIRS include
      4   LIBRARIES moveit_trajectory_filter_plugins
      5 )
   

Modifying trajectory filter into a Planning Request Adapter

These changes are to be applied to your filter code.

1. Remove all Arm Navigation header files from the source and header files of your original Trajectory Filter implementation. None of the Arm Navigation files can be included in a catkin package and so they must be removed.

2. Remove all header files that belong to a rosbuild only packages. Catkin packages can not link against dependencies that live in a rosbuild packages.

3. Include the industrial_trajectory_filters/filter_base.h header file in your own filter's header file.

4. In your filter implementation, replace the base class filters::FilterBase<T> with industrial_trajectory_filters::FilterBase<T>.

5. Remove all calls to the getParam() methods from the old FilterBase class and instead use the roscpp parameter interface for any parameter lookup tasks.

6. In your filter's header file, declare a typedef that specializes your filter template implementation to a version that uses the MessageAdapter structure. For instance, if we are converting a MyFilter class then we would add the following after the class declaration:

      1 typedef MyFilter<MessageAdapter> MyFilterAdapter;
   

   • NOTE: In case the MessageAdapter structure does not fit the expected template class, then the filter's implementation must provide its own adapter structure that resembles the necessary parts of the expected Arm Navigation message type. The specialization of such filter would then pass the custom adapter structure in the template argument list as follows:

   •    1 typedef MyFilter<CustomMessageAdapter> MyFilterAdapter;
     

7. In your filter's source file, remove the plugin declaration PLUGINLIB_DECLARE_CLASS and add the class loader macro. In the case of our MyFilter class, this would be done by placing the following line outside your class:

      1 CLASS_LOADER_REGISTER_CLASS(my_filter_package::MyFilterAdapter, planning_request_adapter::PlanningRequestAdapter);
   

   At this point you may compile your code in order to create the plugin.

   • NOTE: The adaptAndPlan(...) method in the Filter Base already maps the trajectory data between the MessageAdapter structure and the MoveIt planning interface objects passed in the argument list. The filter's implementation should override this method whenever a custom adapter structure is used or when additional functionality is needed.

8. In order to see an example look at the implementation of the industrial_trajectory_filters/src/n_point_filter.cpp planning request adapter.

Once your planning request adapter compiles you can test its functionality inside of MoveIt. This tutorial will guide you through the steps.