filter_base.h
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011, 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 are met:
9  *
10  * * Redistributions of source code must retain the above copyright
11  * notice, this list of conditions and the following disclaimer.
12  * * Redistributions in binary form must reproduce the above copyright
13  * notice, this list of conditions and the following disclaimer in the
14  * documentation and/or other materials provided with the distribution.
15  * * Neither the name of the Southwest Research Institute, nor the names
16  * of its contributors may be used to endorse or promote products derived
17  * from this software without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
20  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
21  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
22  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
23  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
24  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
25  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
26  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
27  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
28  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
29  * POSSIBILITY OF SUCH DAMAGE.
30  */
31 
32 #ifndef FILTER_BASE_H_
33 #define FILTER_BASE_H_
34 
35 #include <typeinfo>
36 #include "ros/assert.h"
37 #include "ros/console.h"
38 #include "ros/ros.h"
41 
43 {
44 
51 {
52 
57  struct Request
58  {
59  trajectory_msgs::JointTrajectory trajectory;
60  } request;
61 };
62 
122 template<typename T>
124  {
125  public:
126 
131  planning_request_adapter::PlanningRequestAdapter(), nh_("~"), configured_(false), filter_type_("FilterBase"), filter_name_(
132  "Unimplemented")
133  {
134 
135  }
136 
140  virtual ~FilterBase()
141  {
142  }
143  ;
144 
153  virtual bool update(const T& data_in, T& data_out)=0;
154 
159  std::string getType()
160  {
161  return filter_type_;
162  }
163  ;
164 
169  inline const std::string& getName()
170  {
171  return filter_name_;
172  }
173  ; // original FilterBase method
174 
175  protected:
176 
182  virtual bool configure()=0;
183 
187  std::string filter_name_;
188 
192  std::string filter_type_;
193 
198 
203 
204  protected:
205 
214  virtual bool adaptAndPlan(const PlannerFn &planner, const planning_scene::PlanningSceneConstPtr &planning_scene,
217  std::vector<std::size_t> &added_path_index) const
218  {
219 
220  // non const pointer to this
222 
223  // calling the configure method
224  if (!configured_ && p->configure())
225  {
226  p->configured_ = true;
227  }
228 
229  // moveit messages for saving trajectory data
230  moveit_msgs::RobotTrajectory robot_trajectory_in, robot_trajectory_out;
231  MessageAdapter trajectory_in, trajectory_out; // mapping structures
232 
233  // calling planner first
234  bool result = planner(planning_scene, req, res);
235 
236  // applying filter to planned trajectory
237  if (result && res.trajectory_)
238  {
239  // mapping arguments into message adapter struct
240  res.trajectory_->getRobotTrajectoryMsg(robot_trajectory_in);
241  trajectory_in.request.trajectory = robot_trajectory_in.joint_trajectory;
242 
243  // applying arm navigation filter to planned trajectory
244  p->update(trajectory_in, trajectory_out);
245 
246  // saving filtered trajectory into moveit message.
247  robot_trajectory_out.joint_trajectory = trajectory_out.request.trajectory;
248  res.trajectory_->setRobotTrajectoryMsg(planning_scene->getCurrentState(), robot_trajectory_out);
249 
250  }
251 
252  return result;
253  }
254 
259  virtual std::string getDescription() const
260  {
261  // non const pointer to this
263 
264  std::stringstream ss;
265  ss << "Trajectory filter '" << p->getName() << "' of type '" << p->getType() << "'";
266  return ss.str();
267  }
268  };
269 
270 }
271 
272 #endif /* FILTER_BASE_H_ */
std::string getType()
Original FilterBase method, return filter type.
Definition: filter_base.h:159
robot_trajectory::RobotTrajectoryPtr trajectory_
virtual std::string getDescription() const
Return description string.
Definition: filter_base.h:259
Arm navigation message type for trajectory filter.
Definition: filter_base.h:57
struct industrial_trajectory_filters::MessageAdapter::Request request
ros::NodeHandle nh_
Internal node handle (used for parameter lookup)
Definition: filter_base.h:202
std::string filter_name_
filter name
Definition: filter_base.h:187
const std::string & getName()
Original FitlerBase Method.
Definition: filter_base.h:169
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
Moveit Planning Request Adapter method. This basic implementation of the adaptAndPlan method calls th...
Definition: filter_base.h:214
void update(const std::string &key, const XmlRpc::XmlRpcValue &v)
bool configured_
Holds filter configuration state.
Definition: filter_base.h:197
This version of the FilterBase<T> can be used to encapsulate the functionality from an arm navigation...
Definition: filter_base.h:123
moveit_msgs::MotionPlanRequest MotionPlanRequest
boost::function< bool(const planning_scene::PlanningSceneConstPtr &planning_scene, const planning_interface::MotionPlanRequest &req, planning_interface::MotionPlanResponse &res)> PlannerFn
trajectory_msgs::JointTrajectory trajectory
Definition: filter_base.h:59
Message Adapter structure serves as a stand-in for the arm navigation message type with which the fil...
Definition: filter_base.h:50
virtual bool configure()=0
FilterBase method for the sub class to configure the filter This function must be implemented in the ...
virtual bool update(const T &data_in, T &data_out)=0
Update the filter and return the data separately. This function must be implemented in the derived cl...


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