Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
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
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
00080 trajectory_out.request.trajectory = trajectory_in.request.trajectory;
00081
00082 trajectory_out.request.trajectory.points.clear();
00083
00084 if (size_in > n_points_)
00085 {
00086
00087 trajectory_out.request.trajectory.points.push_back(trajectory_in.request.trajectory.points.front());
00088
00089 int intermediate_points = n_points_ - 2;
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
00095
00096
00097
00098
00099
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
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
00126 CLASS_LOADER_REGISTER_CLASS(industrial_trajectory_filters::NPointFilterAdapter,
00127 planning_request_adapter::PlanningRequestAdapter);
00128
00129
00130
00131
00132
00133
00134
00135
00136