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.

Learn how to create your own trajectory filter

This is essentially an example of Implementing a Filter with sample code.

Filter Code

Create the actual filter code at src/filter.cpp. All it does is multiply each point by -1, for example purposes.

#include <motion_planning_msgs/JointTrajectoryWithLimits.h>
#include <motion_planning_msgs/FilterJointTrajectory.h>
#include <filters/filter_base.h>
#include <pluginlib/class_list_macros.h>

template <typename T>

class MyFilter: public filters::FilterBase<T>
{
public:
  MyFilter(){};
  ~MyFilter(){};

  bool configure() { return true; }
  bool update(const T& trajectory_in, T& trajectory_out);
};

template <typename T>
inline bool MyFilter<T>::update(const T& trajectory_in, 
                                   T& trajectory_out)
{
  bool success = true;
  int size = trajectory_in.trajectory.points.size();
  int num_traj = trajectory_in.trajectory.joint_names.size();
  trajectory_out = trajectory_in;

  // for every point in time:
  for (int i=0; i<size; ++i)
  {
    // for every (joint) trajectory
    for (int j=0; j<num_traj; ++j)
    {
      double x1 = trajectory_in.trajectory.points[i].positions[j];
       trajectory_out.trajectory.points[i].positions[j] = -x1;
    }
  }

  return success;
}

PLUGINLIB_REGISTER_CLASS(MyFilter, MyFilter<motion_planning_msgs::FilterJointTrajectory::Request>, filters::FilterBase<motion_planning_msgs::FilterJointTrajectory::Request>)

Plugin Declaration

Create the plugin declaration as an xml file called default_plugins.xml

   1 <class_libraries>
   2   <library path="lib/libstyle_trajectory">
   3     <class name="MyFilter" type="MyFilter<motion_planning_msgs::FilterJointTrajectory::Request>" 
   4             base_class_type="filters::FilterBase<motion_planning_msgs::FilterJointTrajectory::Request>">
   5       <description>
   6         Generates velocities and accelerations from numerical differentiation
   7       </description>
   8     </class>
   9   </library>
  10 </class_libraries>

CMakeLists.txt

cmake_minimum_required(VERSION 2.4.6)
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)

rosbuild_init()

set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)

rosbuild_add_library(${PROJECT_NAME} src/filter.cpp)

Manifest.xml

   1 <package>
   2   <description brief="awesome_trajectory">
   3      awesome_trajectory
   4 
   5   </description>
   6   <author>David!!</author>
   7   <license>BSD</license>
   8   <review status="unreviewed" notes=""/>
   9   <url>http://ros.org/wiki/awesome_trajectory</url>
  10   <depend package="roscpp" />
  11   <depend package="filters" />
  12   <depend package="trajectory_msgs" />
  13   <depend package="motion_planning_msgs" />
  14 
  15   <export>
  16     <cpp cflags="-I${prefix}/include -I${prefix}/msg/cpp" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lawesome_trajectory"/>
  17     <filters plugin="${prefix}/default_plugins.xml" />
  18   </export>
  19 
  20 
  21 </package>