70 ROS_INFO_STREAM(
"Param '" << FILTER_PARAMETER_NAME_ <<
"' was not set. Using default filter values " );
73 std::vector<double> temp_coef;
75 if(temp_coef.size()%2 == 1 && temp_coef.size()>2){
77 for(
int i=0; i< (int) temp_coef.size(); i++){
82 ROS_INFO_STREAM(
"Could not read filter, using default filter coefficients");
86 ROS_ERROR(
"Initialization error on smoothing filter. Requires an odd number of coeficients");
94 virtual std::string
getDescription()
const {
return "Add Smoothing Trajectory Filter"; }
107 std::vector<std::size_t> &added_path_index)
const 114 bool result = planner(planning_scene, req, res);
121 ROS_ERROR(
"Smoothing filter of the solution path failed. Filter Not Initialized ");
robot_trajectory::RobotTrajectoryPtr trajectory_
bool init(std::vector< double > &coef)
Constructor.
bool applyFilter(robot_trajectory::RobotTrajectory &rob_trajectory) const
This class filters the trajectory using a Finite Impluse Response filter.
virtual std::string getDescription() const
Returns a short description of this plugin.
moveit_msgs::MotionPlanRequest MotionPlanRequest
boost::function< bool(const planning_scene::PlanningSceneConstPtr &planning_scene, const planning_interface::MotionPlanRequest &req, planning_interface::MotionPlanResponse &res)> PlannerFn
~AddSmoothingFilter()
Destructor.
CLASS_LOADER_REGISTER_CLASS(industrial_trajectory_filters::AddSmoothingFilter, planning_request_adapter::PlanningRequestAdapter)
#define ROS_INFO_STREAM(args)
bool getParam(const std::string &key, std::string &s) const
industrial_trajectory_filters::SmoothingTrajectoryFilter smoothing_filter_
static const std::string FILTER_PARAMETER_NAME_
AddSmoothingFilter()
Constructor AddSmoothingFilter is a planning request adapter plugin which post-processes The robot's ...
std::vector< double > filter_coef_
virtual bool adaptAndPlan(const PlannerFn &planner, const planning_scene::PlanningSceneConstPtr &planning_scene, const planning_interface::MotionPlanRequest &req, planning_interface::MotionPlanResponse &res, std::vector< std::size_t > &added_path_index) const
The work hourse of planning request adapters.