37 #ifndef MOVEIT_PLANNING_PIPELINE_PLANNING_PIPELINE_ 38 #define MOVEIT_PLANNING_PIPELINE_PLANNING_PIPELINE_ 80 const std::string& planning_plugin_param_name =
"planning_plugin",
81 const std::string& adapter_plugins_param_name =
"request_adapters");
90 const std::string& planning_plugin_name,
const std::vector<std::string>& adapter_plugin_names);
138 bool generatePlan(
const planning_scene::PlanningSceneConstPtr& planning_scene,
140 std::vector<std::size_t>& adapter_added_state_index)
const;
188 std::unique_ptr<planning_request_adapter::PlanningRequestAdapterChain>
adapter_chain_;
const std::string & getPlannerPluginName() const
Get the name of the planning plugin used.
static const std::string MOTION_PLAN_REQUEST_TOPIC
When motion planning requests are received and they are supposed to be automatically published...
bool generatePlan(const planning_scene::PlanningSceneConstPtr &planning_scene, const planning_interface::MotionPlanRequest &req, planning_interface::MotionPlanResponse &res) const
Call the motion planner plugin and the sequence of planning request adapters (if any).
bool getDisplayComputedMotionPlans() const
Get the flag set by displayComputedMotionPlans()
std::unique_ptr< planning_request_adapter::PlanningRequestAdapterChain > adapter_chain_
void publishReceivedRequests(bool flag)
Pass a flag telling the pipeline whether or not to publish the received motion planning requests on M...
void displayComputedMotionPlans(bool flag)
Pass a flag telling the pipeline whether or not to publish the computed motion plans on DISPLAY_PATH_...
std::string planner_plugin_name_
void terminate() const
Request termination, if a generatePlan() function is currently computing plans.
std::vector< std::string > adapter_plugin_names_
This class facilitates loading planning plugins and planning request adapted plugins. and allows calling planning_interface::PlanningContext::solve() from a loaded planning plugin and the planning_request_adapter::PlanningRequestAdapter plugins, in the specified order.
robot_model::RobotModelConstPtr kmodel_
ros::Publisher display_path_publisher_
void checkSolutionPaths(bool flag)
Pass a flag telling the pipeline whether or not to re-check the solution paths reported by the planne...
const robot_model::RobotModelConstPtr & getRobotModel() const
Get the robot model that this pipeline is using.
ros::Publisher received_request_publisher_
bool check_solution_paths_
Flag indicating whether the reported plans should be checked once again, by the planning pipeline its...
moveit_msgs::MotionPlanRequest MotionPlanRequest
static const std::string DISPLAY_PATH_TOPIC
When motion plans are computed and they are supposed to be automatically displayed, they are sent to this topic (moveit_msgs::DisplauTrajectory)
std::unique_ptr< pluginlib::ClassLoader< planning_interface::PlannerManager > > planner_plugin_loader_
bool publish_received_requests_
ros::Publisher contacts_publisher_
bool display_computed_motion_plans_
Flag indicating whether motion plans should be published as a moveit_msgs::DisplayTrajectory.
planning_interface::PlannerManagerPtr planner_instance_
const std::vector< std::string > & getAdapterPluginNames() const
Get the names of the planning request adapter plugins used.
MOVEIT_CLASS_FORWARD(PlanningPipeline)
const planning_interface::PlannerManagerPtr & getPlannerManager()
Get the planner manager for the loaded planning plugin.
static const std::string MOTION_CONTACTS_TOPIC
When contacts are found in the solution path reported by a planner, they can be published as markers ...
bool getCheckSolutionPaths() const
Get the flag set by checkSolutionPaths()
std::unique_ptr< pluginlib::ClassLoader< planning_request_adapter::PlanningRequestAdapter > > adapter_plugin_loader_
PlanningPipeline(const robot_model::RobotModelConstPtr &model, const ros::NodeHandle &nh=ros::NodeHandle("~"), const std::string &planning_plugin_param_name="planning_plugin", const std::string &adapter_plugins_param_name="request_adapters")
Given a robot model (model), a node handle (nh), initialize the planning pipeline.
bool getPublishReceivedRequests() const
Get the flag set by publishReceivedRequests()