38 #include <console_bridge/console.h> 56 if(coef.size()%2 == 1) {
61 coef_.push_back(coef[i]);
83 if(num_points <=2)
return(
false);
84 const int num_states = rob_trajectory.
getWayPoint(0).getVariableCount();
85 std::vector<double> xv;
88 for(
int i=0; i<num_states; i++){
89 double start_value = rob_trajectory.
getWayPoint(0).getVariablePosition(i);
90 double start_slope = rob_trajectory.
getWayPoint(1).getVariablePosition(i) - start_value;
91 double end_value = rob_trajectory.
getWayPoint(num_points-1).getVariablePosition(i);
92 double end_slope = end_value - rob_trajectory.
getWayPoint(num_points-2).getVariablePosition(i);
103 for(
int j=1; j<num_points-1; j++){
105 for(
int k=0; k<num_coef_-1; k++){
110 if(j+num_coef_/2 < num_points){
111 xv[num_coef_ - 1] = rob_trajectory.
getWayPoint(j+num_coef_/2).getVariablePosition(i);
114 end_value += end_slope;
115 xv[num_coef_-1] = end_value;
120 sum += xv[k]*
coef_[k];
robot_state::RobotStatePtr & getWayPointPtr(std::size_t index)
std::vector< double > coef_
bool init(std::vector< double > &coef)
Constructor.
bool applyFilter(robot_trajectory::RobotTrajectory &rob_trajectory) const
~SmoothingTrajectoryFilter()
Destructor.
const robot_state::RobotState & getWayPoint(std::size_t index) const
std::size_t getWayPointCount() const
SmoothingTrajectoryFilter()
Constructor.