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
00033
00034
00035
00036
00037 #ifndef MOVEIT_PLANNING_PIPELINE_PLANNING_PIPELINE_
00038 #define MOVEIT_PLANNING_PIPELINE_PLANNING_PIPELINE_
00039
00040 #include <moveit/planning_interface/planning_interface.h>
00041 #include <moveit/planning_request_adapter/planning_request_adapter.h>
00042 #include <pluginlib/class_loader.h>
00043 #include <boost/scoped_ptr.hpp>
00044 #include <ros/ros.h>
00045
00047 namespace planning_pipeline
00048 {
00049
00056 class PlanningPipeline
00057 {
00058
00059 public:
00060
00062 static const std::string DISPLAY_PATH_TOPIC;
00063
00065 static const std::string MOTION_PLAN_REQUEST_TOPIC;
00066
00068 static const std::string MOTION_CONTACTS_TOPIC;
00069
00076 PlanningPipeline(const robot_model::RobotModelConstPtr& model,
00077 const ros::NodeHandle &nh = ros::NodeHandle("~"),
00078 const std::string &planning_plugin_param_name = "planning_plugin",
00079 const std::string &adapter_plugins_param_name = "request_adapters");
00080
00087 PlanningPipeline(const robot_model::RobotModelConstPtr& model,
00088 const ros::NodeHandle &nh,
00089 const std::string &planning_plugin_name,
00090 const std::vector<std::string> &adapter_plugin_names);
00091
00093 void displayComputedMotionPlans(bool flag);
00094
00096 void publishReceivedRequests(bool flag);
00097
00099 void checkSolutionPaths(bool flag);
00100
00102 bool getDisplayComputedMotionPlans() const
00103 {
00104 return display_computed_motion_plans_;
00105 }
00106
00108 bool getPublishReceivedRequests() const
00109 {
00110 return publish_received_requests_;
00111 }
00112
00114 bool getCheckSolutionPaths() const
00115 {
00116 return check_solution_paths_;
00117 }
00118
00123 bool generatePlan(const planning_scene::PlanningSceneConstPtr& planning_scene,
00124 const planning_interface::MotionPlanRequest& req,
00125 planning_interface::MotionPlanResponse& res) const;
00126
00132 bool generatePlan(const planning_scene::PlanningSceneConstPtr& planning_scene,
00133 const planning_interface::MotionPlanRequest& req,
00134 planning_interface::MotionPlanResponse& res,
00135 std::vector<std::size_t> &adapter_added_state_index) const;
00136
00138 void terminate() const;
00139
00141 const std::string& getPlannerPluginName() const
00142 {
00143 return planner_plugin_name_;
00144 }
00145
00147 const std::vector<std::string>& getAdapterPluginNames() const
00148 {
00149 return adapter_plugin_names_;
00150 }
00151
00153 const planning_interface::PlannerManagerPtr& getPlannerManager()
00154 {
00155 return planner_instance_;
00156 }
00157
00159 const robot_model::RobotModelConstPtr& getRobotModel() const
00160 {
00161 return kmodel_;
00162 }
00163
00164 private:
00165
00166 void configure();
00167
00168 ros::NodeHandle nh_;
00169
00171 bool display_computed_motion_plans_;
00172 ros::Publisher display_path_publisher_;
00173
00175 bool publish_received_requests_;
00176 ros::Publisher received_request_publisher_;
00177
00178 boost::scoped_ptr<pluginlib::ClassLoader<planning_interface::PlannerManager> > planner_plugin_loader_;
00179 planning_interface::PlannerManagerPtr planner_instance_;
00180 std::string planner_plugin_name_;
00181
00182 boost::scoped_ptr<pluginlib::ClassLoader<planning_request_adapter::PlanningRequestAdapter> > adapter_plugin_loader_;
00183 boost::scoped_ptr<planning_request_adapter::PlanningRequestAdapterChain> adapter_chain_;
00184 std::vector<std::string> adapter_plugin_names_;
00185
00186 robot_model::RobotModelConstPtr kmodel_;
00187
00189 bool check_solution_paths_;
00190 ros::Publisher contacts_publisher_;
00191
00192 };
00193
00194 MOVEIT_CLASS_FORWARD(PlanningPipeline);
00195
00196 }
00197
00198 #endif