n_point_filter.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011, Southwest Research Institute
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions are met:
9  *
10  * * Redistributions of source code must retain the above copyright
11  * notice, this list of conditions and the following disclaimer.
12  * * Redistributions in binary form must reproduce the above copyright
13  * notice, this list of conditions and the following disclaimer in the
14  * documentation and/or other materials provided with the distribution.
15  * * Neither the name of the Southwest Research Institute, nor the names
16  * of its contributors may be used to endorse or promote products derived
17  * from this software without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
20  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
21  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
22  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
23  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
24  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
25  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
26  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
27  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
28  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
29  * POSSIBILITY OF SUCH DAMAGE.
30  */
31 
32 //#include <arm_navigation_msgs/FilterJointTrajectoryWithConstraints.h>
33 #include <trajectory_msgs/JointTrajectory.h>
35 
36 using namespace industrial_trajectory_filters;
37 
38 const int DEFAULT_N = 2;
39 
40 template<typename T>
42  FilterBase<T>()
43  {
44  ROS_INFO_STREAM("Constructing N point filter");
46  this->filter_name_ = "NPointFilter";
47  this->filter_type_ = "NPointFilter";
48  }
49 
50 template<typename T>
52  {
53  }
54 
55 template<typename T>
57  {
58  //if (!filters::FilterBase<T>::getParam("n_points", n_points_))
59  if (!this->nh_.getParam("n_points", n_points_))
60  {
61  ROS_WARN_STREAM("NPointFilter, params has no attribute n_points.");
62  }
63  if (n_points_ < 2)
64  {
65  ROS_WARN_STREAM( "n_points attribute less than min(2), setting to minimum");
66  n_points_ = 2;
67  }
68  ROS_INFO_STREAM("Using a n_points value of " << n_points_);
69 
70  return true;
71  }
72 
73 template<typename T>
74  bool NPointFilter<T>::update(const T& trajectory_in, T& trajectory_out)
75  {
76  bool success = false;
77  int size_in = trajectory_in.request.trajectory.points.size();
78 
79  // Copy non point related data
80  trajectory_out.request.trajectory = trajectory_in.request.trajectory;
81  // Clear out the trajectory points
82  trajectory_out.request.trajectory.points.clear();
83 
84  if (size_in > n_points_)
85  {
86  //Add first point to output trajectory
87  trajectory_out.request.trajectory.points.push_back(trajectory_in.request.trajectory.points.front());
88 
89  int intermediate_points = n_points_ - 2; //subtract the first and last elements
90  double int_point_increment = double(size_in) / double(intermediate_points + 1.0);
92  "Number of intermediate points: " << intermediate_points << ", increment: " << int_point_increment);
93 
94  // The intermediate point index is determined by the following equation:
95  // int_point_index = i * int_point_increment
96  // Ex: n_points_ = 4, size_in = 11, intermediate_points = 2 ->
97  // int_point_increment = 3.66667,
98  // i = 1: int_point_index = 3
99  // i = 2: int_point_index = 7
100  for (int i = 1; i <= intermediate_points; i++)
101  {
102  int int_point_index = int(double(i) * int_point_increment);
103  ROS_INFO_STREAM("Intermediate point index: " << int_point_index);
104  trajectory_out.request.trajectory.points.push_back(trajectory_in.request.trajectory.points[int_point_index]);
105  }
106 
107  //Add last point to output trajectory
108  trajectory_out.request.trajectory.points.push_back(trajectory_in.request.trajectory.points.back());
109 
111  "Filtered trajectory from: " << trajectory_in.request.trajectory.points.size() << " to: " << trajectory_out.request.trajectory.points.size());
112 
113  success = true;
114  }
115  else
116  {
117  ROS_WARN_STREAM( "Trajectory size less than n: " << n_points_ << ", pass through");
118  trajectory_out.request.trajectory = trajectory_in.request.trajectory;
119  success = true;
120  }
121 
122  return success;
123  }
124 
125 // registering planner adapter
128 
129 /*
130  * Old plugin declaration for arm navigation trajectory filters
131  PLUGINLIB_DECLARE_CLASS(industrial_trajectory_filters,
132  IndustrialNPointFilterJointTrajectoryWithConstraints,
133  industrial_trajectory_filters::NPointFilter<arm_navigation_msgs::FilterJointTrajectoryWithConstraints>,
134  filters::FilterBase<arm_navigation_msgs::FilterJointTrajectoryWithConstraints>);
135 
136  */
ros::NodeHandle nh_
Internal node handle (used for parameter lookup)
Definition: filter_base.h:202
This is a simple filter which reduces a trajectory to N points or less.
std::string filter_name_
filter name
Definition: filter_base.h:187
const int DEFAULT_N
virtual bool configure()
FilterBase method for the sub class to configure the filter This function must be implemented in the ...
#define ROS_WARN_STREAM(args)
This version of the FilterBase<T> can be used to encapsulate the functionality from an arm navigation...
Definition: filter_base.h:123
int n_points_
number of points to reduce trajectory to
#define ROS_INFO_STREAM(args)
bool getParam(const std::string &key, std::string &s) const
CLASS_LOADER_REGISTER_CLASS(industrial_trajectory_filters::NPointFilterAdapter, planning_request_adapter::PlanningRequestAdapter)
bool update(const T &trajectory_in, T &trajectory_out)
Reduces a trajectory to N points or less. The resulting trajectory contains only point within the ori...


open_manipulator_with_tb3_waffle_pi_moveit
Author(s): Darby Lim
autogenerated on Mon Jun 10 2019 14:13:54