n_point_filter.cpp

Go to the documentation of this file.

/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2011, Southwest Research Institute
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
*       * Redistributions of source code must retain the above copyright
*       notice, this list of conditions and the following disclaimer.
*       * Redistributions in binary form must reproduce the above copyright
*       notice, this list of conditions and the following disclaimer in the
*       documentation and/or other materials provided with the distribution.
*       * Neither the name of the Southwest Research Institute, nor the names
*       of its contributors may be used to endorse or promote products derived
*       from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/

#include <arm_navigation_msgs/FilterJointTrajectoryWithConstraints.h>
#include <industrial_trajectory_filters/n_point_filter.h>

using namespace industrial_trajectory_filters;

const int DEFAULT_N=2;


template <typename T>
NPointFilter<T>::NPointFilter()
{
        ROS_INFO_STREAM("Constructing N point filter");
        n_points_ = DEFAULT_N;
}

template <typename T>
NPointFilter<T>::~NPointFilter()
{}

template <typename T>
bool NPointFilter<T>::configure()
{
  if (!filters::FilterBase<T>::getParam("n_points", n_points_))
  {
    ROS_WARN_STREAM("NPointFilter, params has no attribute n_points.");
  }
  if (n_points_ < 2)
  {
        ROS_WARN_STREAM("n_points attribute less than min(2), setting to minimum");
        n_points_ = 2;
  }
  ROS_INFO_STREAM("Using a n_points value of " << n_points_);

 return true;
}


template <typename T>
bool NPointFilter<T>::update(const T& trajectory_in,
                                   T& trajectory_out)
{
  bool success = false;
  int size_in = trajectory_in.request.trajectory.points.size();

  // Copy non point related data
  trajectory_out = trajectory_in;
  // Clear out the trajectory points
  trajectory_out.request.trajectory.points.clear();

  if (size_in > n_points_)
  {
                //Add first point to output trajectory
                trajectory_out.request.trajectory.points.push_back(
                                trajectory_in.request.trajectory.points.front());

                int intermediate_points = n_points_ - 2;  //subtract the first and last elements
                double int_point_increment = double(size_in)/double(intermediate_points + 1.0);
                ROS_INFO_STREAM("Number of intermediate points: " << intermediate_points <<
                                ", increment: " << int_point_increment);

                // The intermediate point index is determined by the following equation:
                //     int_point_index = i * int_point_increment
                // Ex: n_points_ = 4, size_in = 11, intermediate_points = 2 ->
                //     int_point_increment = 3.66667,
                //     i = 1: int_point_index = 3
                //               i = 2: int_point_index = 7
                for (int i = 1; i <= intermediate_points; i++)
                {
                        int int_point_index = int(double(i) * int_point_increment);
                        ROS_INFO_STREAM("Intermediate point index: " << int_point_index);
                        trajectory_out.request.trajectory.points.push_back(
                                        trajectory_in.request.trajectory.points[int_point_index]);
                }

                //Add last point to output trajectory
                trajectory_out.request.trajectory.points.push_back(
                                trajectory_in.request.trajectory.points.back());

                ROS_INFO_STREAM("Filtered trajectory from: " << trajectory_in.request.trajectory.points.size()
                                << " to: " << trajectory_out.request.trajectory.points.size());

                success = true;
  }
  else
  {
        ROS_WARN_STREAM("Trajectory size less than n: " << n_points_ << ", pass through");
        trajectory_out = trajectory_in;
        success = true;
  }

  return success;
}



PLUGINLIB_DECLARE_CLASS(industrial_trajectory_filters,
                IndustrialNPointFilterJointTrajectoryWithConstraints,
                industrial_trajectory_filters::NPointFilter<arm_navigation_msgs::FilterJointTrajectoryWithConstraints>,
                filters::FilterBase<arm_navigation_msgs::FilterJointTrajectoryWithConstraints>);