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 {
00055 class PlanningPipeline
00056 {
00057 public:
00060 static const std::string DISPLAY_PATH_TOPIC;
00061
00064 static const std::string MOTION_PLAN_REQUEST_TOPIC;
00065
00068 static const std::string MOTION_CONTACTS_TOPIC;
00069
00078 PlanningPipeline(const robot_model::RobotModelConstPtr& model, const ros::NodeHandle& nh = ros::NodeHandle("~"),
00079 const std::string& planning_plugin_param_name = "planning_plugin",
00080 const std::string& adapter_plugins_param_name = "request_adapters");
00081
00088 PlanningPipeline(const robot_model::RobotModelConstPtr& model, const ros::NodeHandle& nh,
00089 const std::string& planning_plugin_name, const std::vector<std::string>& adapter_plugin_names);
00090
00093 void displayComputedMotionPlans(bool flag);
00094
00097 void publishReceivedRequests(bool flag);
00098
00101 void checkSolutionPaths(bool flag);
00102
00104 bool getDisplayComputedMotionPlans() const
00105 {
00106 return display_computed_motion_plans_;
00107 }
00108
00110 bool getPublishReceivedRequests() const
00111 {
00112 return publish_received_requests_;
00113 }
00114
00116 bool getCheckSolutionPaths() const
00117 {
00118 return check_solution_paths_;
00119 }
00120
00125 bool generatePlan(const planning_scene::PlanningSceneConstPtr& planning_scene,
00126 const planning_interface::MotionPlanRequest& req,
00127 planning_interface::MotionPlanResponse& res) const;
00128
00137 bool generatePlan(const planning_scene::PlanningSceneConstPtr& planning_scene,
00138 const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res,
00139 std::vector<std::size_t>& adapter_added_state_index) const;
00140
00142 void terminate() const;
00143
00145 const std::string& getPlannerPluginName() const
00146 {
00147 return planner_plugin_name_;
00148 }
00149
00151 const std::vector<std::string>& getAdapterPluginNames() const
00152 {
00153 return adapter_plugin_names_;
00154 }
00155
00157 const planning_interface::PlannerManagerPtr& getPlannerManager()
00158 {
00159 return planner_instance_;
00160 }
00161
00163 const robot_model::RobotModelConstPtr& getRobotModel() const
00164 {
00165 return kmodel_;
00166 }
00167
00168 private:
00169 void configure();
00170
00171 ros::NodeHandle nh_;
00172
00174 bool display_computed_motion_plans_;
00175 ros::Publisher display_path_publisher_;
00176
00179 bool publish_received_requests_;
00180 ros::Publisher received_request_publisher_;
00181
00182 boost::scoped_ptr<pluginlib::ClassLoader<planning_interface::PlannerManager> > planner_plugin_loader_;
00183 planning_interface::PlannerManagerPtr planner_instance_;
00184 std::string planner_plugin_name_;
00185
00186 boost::scoped_ptr<pluginlib::ClassLoader<planning_request_adapter::PlanningRequestAdapter> > adapter_plugin_loader_;
00187 boost::scoped_ptr<planning_request_adapter::PlanningRequestAdapterChain> adapter_chain_;
00188 std::vector<std::string> adapter_plugin_names_;
00189
00190 robot_model::RobotModelConstPtr kmodel_;
00191
00193 bool check_solution_paths_;
00194 ros::Publisher contacts_publisher_;
00195 };
00196
00197 MOVEIT_CLASS_FORWARD(PlanningPipeline);
00198 }
00199
00200 #endif