32 #ifndef FILTER_BASE_H_ 33 #define FILTER_BASE_H_ 131 planning_request_adapter::PlanningRequestAdapter(), nh_(
"~"), configured_(false), filter_type_(
"FilterBase"), filter_name_(
153 virtual bool update(
const T& data_in, T& data_out)=0;
182 virtual bool configure()=0;
217 std::vector<std::size_t> &added_path_index)
const 230 moveit_msgs::RobotTrajectory robot_trajectory_in, robot_trajectory_out;
234 bool result = planner(planning_scene, req, res);
240 res.
trajectory_->getRobotTrajectoryMsg(robot_trajectory_in);
244 p->
update(trajectory_in, trajectory_out);
248 res.
trajectory_->setRobotTrajectoryMsg(planning_scene->getCurrentState(), robot_trajectory_out);
264 std::stringstream ss;
265 ss <<
"Trajectory filter '" << p->
getName() <<
"' of type '" << p->
getType() <<
"'";
std::string getType()
Original FilterBase method, return filter type.
robot_trajectory::RobotTrajectoryPtr trajectory_
virtual std::string getDescription() const
Return description string.
Arm navigation message type for trajectory filter.
struct industrial_trajectory_filters::MessageAdapter::Request request
ros::NodeHandle nh_
Internal node handle (used for parameter lookup)
std::string filter_name_
filter name
const std::string & getName()
Original FitlerBase Method.
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...
void update(const std::string &key, const XmlRpc::XmlRpcValue &v)
bool configured_
Holds filter configuration state.
This version of the FilterBase<T> can be used to encapsulate the functionality from an arm navigation...
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
Message Adapter structure serves as a stand-in for the arm navigation message type with which the fil...
virtual bool configure()=0
FilterBase method for the sub class to configure the filter This function must be implemented in the ...
FilterBase()
Default constructor.
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...