33 #include <trajectory_msgs/JointTrajectory.h>
59 if (!this->nh_.getParam(
"n_points", n_points_))
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();
84 if (size_in > n_points_)
87 trajectory_out.request.trajectory.points.push_back(trajectory_in.request.trajectory.points.front());
89 int intermediate_points = n_points_ - 2;
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());
117 ROS_WARN_STREAM(
"Trajectory size less than n: " << n_points_ <<
", pass through");
118 trajectory_out.request.trajectory = trajectory_in.request.trajectory;