33 #include <trajectory_msgs/JointTrajectory.h> 65 ROS_WARN_STREAM(
"n_points attribute less than min(2), setting to minimum");
77 int size_in = trajectory_in.request.trajectory.points.size();
80 trajectory_out.request.trajectory = trajectory_in.request.trajectory;
82 trajectory_out.request.trajectory.points.clear();
87 trajectory_out.request.trajectory.points.push_back(trajectory_in.request.trajectory.points.front());
90 double int_point_increment = double(size_in) / double(intermediate_points + 1.0);
92 "Number of intermediate points: " << intermediate_points <<
", increment: " << int_point_increment);
100 for (
int i = 1; i <= intermediate_points; i++)
102 int int_point_index = int(
double(i) * int_point_increment);
104 trajectory_out.request.trajectory.points.push_back(trajectory_in.request.trajectory.points[int_point_index]);
108 trajectory_out.request.trajectory.points.push_back(trajectory_in.request.trajectory.points.back());
111 "Filtered trajectory from: " << trajectory_in.request.trajectory.points.size() <<
" to: " << trajectory_out.request.trajectory.points.size());
118 trajectory_out.request.trajectory = trajectory_in.request.trajectory;
~NPointFilter()
Default destructor.
ros::NodeHandle nh_
Internal node handle (used for parameter lookup)
This is a simple filter which reduces a trajectory to N points or less.
std::string filter_name_
filter name
NPointFilter()
Default constructor.
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...
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...