add_smoothing_filter.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2014, Southwest Research Institute
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Southwest Research Institute nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Chris Lewis */
36 
39 
41 #include <ros/ros.h> // required for NodeHandle
42 #include <ros/console.h>
43 
45 {
46 
48 {
49 public:
50 
51  static const std::string FILTER_PARAMETER_NAME_; // base name for filter parameters
52 
58  {
59  int num_coef;
60 
61  // set of default filter coefficients in case paramter, or its associated file, or its syntax is wrong
62  filter_coef_.push_back(0.25);
63  filter_coef_.push_back(0.5);
64  filter_coef_.push_back(1.0);
65  filter_coef_.push_back(0.5);
66  filter_coef_.push_back(0.25);
67 
68  // see if a new set of filter parameters is on the parameter server, if so, install them
69  if (!nh_.getParam(FILTER_PARAMETER_NAME_, filter_name_)){
70  ROS_INFO_STREAM("Param '" << FILTER_PARAMETER_NAME_ << "' was not set. Using default filter values " );
71  }
72  else{ // base filter name exists
73  std::vector<double> temp_coef;
74  nh_.getParam(filter_name_.c_str(),temp_coef); // no need to check for success
75  if(temp_coef.size()%2 == 1 && temp_coef.size()>2){ // need to have odd number of coefficients greater than 2
76  filter_coef_.clear();
77  for(int i=0; i< (int) temp_coef.size(); i++){ // install the filter coefficients
78  filter_coef_.push_back(temp_coef[i]);
79  }
80  }
81  else{
82  ROS_INFO_STREAM("Could not read filter, using default filter coefficients");
83  }
84  }
86  ROS_ERROR("Initialization error on smoothing filter. Requires an odd number of coeficients");
87 
88  };
89 
92 
94  virtual std::string getDescription() const { return "Add Smoothing Trajectory Filter"; }
95 
103  virtual bool adaptAndPlan(const PlannerFn &planner,
104  const planning_scene::PlanningSceneConstPtr& planning_scene,
107  std::vector<std::size_t> &added_path_index) const
108  {
109  // do anything prior to calling the planner here
110  // ....
111  // IN this case nothing was done
112 
113  // call the nested planner or adapter
114  bool result = planner(planning_scene, req, res);
115 
116  // do anything after calling the nested planner or adapters here
117  if (result && res.trajectory_) // successful plan
118  {
119  ROS_DEBUG("Running '%s'", getDescription().c_str()); // inform the user
120  if (!smoothing_filter_.applyFilter(*res.trajectory_)) {// do the smoothing
121  ROS_ERROR("Smoothing filter of the solution path failed. Filter Not Initialized ");
122  }
123  }// end of if successful plan
124  return result;
125  };
126 
127 
128 private:
131  std::string filter_name_;
132  std::vector<double> filter_coef_;
133 
134 };
135 
136 const std::string AddSmoothingFilter::FILTER_PARAMETER_NAME_ = "/move_group/smoothing_filter_name";
137 
138 }
139 
robot_trajectory::RobotTrajectoryPtr trajectory_
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
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_
AddSmoothingFilter()
Constructor AddSmoothingFilter is a planning request adapter plugin which post-processes The robot&#39;s ...
#define ROS_ERROR(...)
#define ROS_DEBUG(...)
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.


open_manipulator_with_tb3_waffle_pi_moveit
Author(s): Darby Lim
autogenerated on Mon Jun 10 2019 14:13:54