n_point_filter.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  * Copyright (c) 2011, Southwest Research Institute
00005  * All rights reserved.
00006  *
00007  * Redistribution and use in source and binary forms, with or without
00008  * modification, are permitted provided that the following conditions are met:
00009  *
00010  *      * Redistributions of source code must retain the above copyright
00011  *      notice, this list of conditions and the following disclaimer.
00012  *      * Redistributions in binary form must reproduce the above copyright
00013  *      notice, this list of conditions and the following disclaimer in the
00014  *      documentation and/or other materials provided with the distribution.
00015  *      * Neither the name of the Southwest Research Institute, nor the names
00016  *      of its contributors may be used to endorse or promote products derived
00017  *      from this software without specific prior written permission.
00018  *
00019  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00020  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00021  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00022  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00023  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00024  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00025  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00026  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00027  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00028  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00029  * POSSIBILITY OF SUCH DAMAGE.
00030  */
00031 
00032 //#include <arm_navigation_msgs/FilterJointTrajectoryWithConstraints.h>
00033 #include <trajectory_msgs/JointTrajectory.h>
00034 #include <industrial_trajectory_filters/n_point_filter.h>
00035 
00036 using namespace industrial_trajectory_filters;
00037 
00038 const int DEFAULT_N = 2;
00039 
00040 template<typename T>
00041   NPointFilter<T>::NPointFilter() :
00042       FilterBase<T>()
00043   {
00044     ROS_INFO_STREAM("Constructing N point filter");
00045     n_points_ = DEFAULT_N;
00046     this->filter_name_ = "NPointFilter";
00047     this->filter_type_ = "NPointFilter";
00048   }
00049 
00050 template<typename T>
00051   NPointFilter<T>::~NPointFilter()
00052   {
00053   }
00054 
00055 template<typename T>
00056   bool NPointFilter<T>::configure()
00057   {
00058     //if (!filters::FilterBase<T>::getParam("n_points", n_points_))
00059     if (!this->nh_.getParam("n_points", n_points_))
00060     {
00061       ROS_WARN_STREAM("NPointFilter, params has no attribute n_points.");
00062     }
00063     if (n_points_ < 2)
00064     {
00065       ROS_WARN_STREAM( "n_points attribute less than min(2), setting to minimum");
00066       n_points_ = 2;
00067     }
00068     ROS_INFO_STREAM("Using a n_points value of " << n_points_);
00069 
00070     return true;
00071   }
00072 
00073 template<typename T>
00074   bool NPointFilter<T>::update(const T& trajectory_in, T& trajectory_out)
00075   {
00076     bool success = false;
00077     int size_in = trajectory_in.request.trajectory.points.size();
00078 
00079     // Copy non point related data
00080     trajectory_out.request.trajectory = trajectory_in.request.trajectory;
00081     // Clear out the trajectory points
00082     trajectory_out.request.trajectory.points.clear();
00083 
00084     if (size_in > n_points_)
00085     {
00086       //Add first point to output trajectory
00087       trajectory_out.request.trajectory.points.push_back(trajectory_in.request.trajectory.points.front());
00088 
00089       int intermediate_points = n_points_ - 2; //subtract the first and last elements
00090       double int_point_increment = double(size_in) / double(intermediate_points + 1.0);
00091       ROS_INFO_STREAM(
00092           "Number of intermediate points: " << intermediate_points << ", increment: " << int_point_increment);
00093 
00094       // The intermediate point index is determined by the following equation:
00095       //     int_point_index = i * int_point_increment
00096       // Ex: n_points_ = 4, size_in = 11, intermediate_points = 2 ->
00097       //     int_point_increment = 3.66667,
00098       //     i = 1: int_point_index = 3
00099       //                 i = 2: int_point_index = 7
00100       for (int i = 1; i <= intermediate_points; i++)
00101       {
00102         int int_point_index = int(double(i) * int_point_increment);
00103         ROS_INFO_STREAM("Intermediate point index: " << int_point_index);
00104         trajectory_out.request.trajectory.points.push_back(trajectory_in.request.trajectory.points[int_point_index]);
00105       }
00106 
00107       //Add last point to output trajectory
00108       trajectory_out.request.trajectory.points.push_back(trajectory_in.request.trajectory.points.back());
00109 
00110       ROS_INFO_STREAM(
00111           "Filtered trajectory from: " << trajectory_in.request.trajectory.points.size() << " to: " << trajectory_out.request.trajectory.points.size());
00112 
00113       success = true;
00114     }
00115     else
00116     {
00117       ROS_WARN_STREAM( "Trajectory size less than n: " << n_points_ << ", pass through");
00118       trajectory_out.request.trajectory = trajectory_in.request.trajectory;
00119       success = true;
00120     }
00121 
00122     return success;
00123   }
00124 
00125 // registering planner adapter
00126 CLASS_LOADER_REGISTER_CLASS(industrial_trajectory_filters::NPointFilterAdapter,
00127                             planning_request_adapter::PlanningRequestAdapter);
00128 
00129 /*
00130  * Old plugin declaration for arm navigation trajectory filters
00131  PLUGINLIB_DECLARE_CLASS(industrial_trajectory_filters,
00132  IndustrialNPointFilterJointTrajectoryWithConstraints,
00133  industrial_trajectory_filters::NPointFilter<arm_navigation_msgs::FilterJointTrajectoryWithConstraints>,
00134  filters::FilterBase<arm_navigation_msgs::FilterJointTrajectoryWithConstraints>);
00135 
00136  */


industrial_trajectory_filters
Author(s): Shaun Edwards , Jorge Nicho
autogenerated on Sat Jun 8 2019 20:43:16