Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032 #ifndef FILTER_BASE_H_
00033 #define FILTER_BASE_H_
00034
00035 #include <typeinfo>
00036 #include "ros/assert.h"
00037 #include "ros/console.h"
00038 #include "ros/ros.h"
00039 #include <moveit/planning_request_adapter/planning_request_adapter.h>
00040 #include <class_loader/class_loader.h>
00041
00042 namespace industrial_trajectory_filters
00043 {
00044
00050 struct MessageAdapter
00051 {
00052
00057 struct Request
00058 {
00059 trajectory_msgs::JointTrajectory trajectory;
00060 } request;
00061 };
00062
00122 template<typename T>
00123 class FilterBase : public planning_request_adapter::PlanningRequestAdapter
00124 {
00125 public:
00126
00130 FilterBase() :
00131 planning_request_adapter::PlanningRequestAdapter(), nh_("~"), configured_(false), filter_type_("FilterBase"), filter_name_(
00132 "Unimplemented")
00133 {
00134
00135 }
00136
00140 virtual ~FilterBase()
00141 {
00142 }
00143 ;
00144
00153 virtual bool update(const T& data_in, T& data_out)=0;
00154
00159 std::string getType()
00160 {
00161 return filter_type_;
00162 }
00163 ;
00164
00169 inline const std::string& getName()
00170 {
00171 return filter_name_;
00172 }
00173 ;
00174
00175 protected:
00176
00182 virtual bool configure()=0;
00183
00187 std::string filter_name_;
00188
00192 std::string filter_type_;
00193
00197 bool configured_;
00198
00202 ros::NodeHandle nh_;
00203
00204 protected:
00205
00214 virtual bool adaptAndPlan(const PlannerFn &planner, const planning_scene::PlanningSceneConstPtr &planning_scene,
00215 const planning_interface::MotionPlanRequest &req,
00216 planning_interface::MotionPlanResponse &res,
00217 std::vector<std::size_t> &added_path_index) const
00218 {
00219
00220
00221 FilterBase<MessageAdapter> *p = const_cast<FilterBase<MessageAdapter>*>(this);
00222
00223
00224 if (!configured_ && p->configure())
00225 {
00226 p->configured_ = true;
00227 }
00228
00229
00230 moveit_msgs::RobotTrajectory robot_trajectory_in, robot_trajectory_out;
00231 MessageAdapter trajectory_in, trajectory_out;
00232
00233
00234 bool result = planner(planning_scene, req, res);
00235
00236
00237 if (result && res.trajectory_)
00238 {
00239
00240 res.trajectory_->getRobotTrajectoryMsg(robot_trajectory_in);
00241 trajectory_in.request.trajectory = robot_trajectory_in.joint_trajectory;
00242
00243
00244 p->update(trajectory_in, trajectory_out);
00245
00246
00247 robot_trajectory_out.joint_trajectory = trajectory_out.request.trajectory;
00248 res.trajectory_->setRobotTrajectoryMsg(planning_scene->getCurrentState(), robot_trajectory_out);
00249
00250 }
00251
00252 return result;
00253 }
00254
00259 virtual std::string getDescription() const
00260 {
00261
00262 FilterBase<MessageAdapter> *p = const_cast<FilterBase<MessageAdapter>*>(this);
00263
00264 std::stringstream ss;
00265 ss << "Trajectory filter '" << p->getName() << "' of type '" << p->getType() << "'";
00266 return ss.str();
00267 }
00268 };
00269
00270 }
00271
00272 #endif