Skip to content

Commit

Permalink
Revert "Enable parallel planning with PipelinePlanner (#450)"
Browse files Browse the repository at this point in the history 
This reverts commit 0e02fca.
  • Loading branch information
@rhaschke
rhaschke committed May 25, 2024
1 parent cd741ba commit 078af42
Show file tree
Hide file tree
Showing 8 changed files with 173 additions and 295 deletions.
127 changes: 33 additions & 94 deletions core/include/moveit/task_constructor/solvers/pipeline_planner.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,16 +32,14 @@
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/

/* Authors: Robert Haschke, Sebastian Jahr
Description: Solver that uses a set of MoveIt PlanningPipelines to solve a given planning problem.
/* Authors: Robert Haschke
Desc: plan using MoveIt's PlanningPipeline
*/

#pragma once

#include <moveit/task_constructor/solvers/planner_interface.h>
#include <moveit/planning_pipeline_interfaces/planning_pipeline_interfaces.hpp>
#include <moveit/planning_pipeline_interfaces/solution_selection_functions.hpp>
#include <moveit/planning_pipeline_interfaces/stopping_criterion_functions.hpp>
#include <moveit_msgs/msg/motion_plan_request.hpp>
#include <rclcpp/node.hpp>
#include <moveit/macros/class_forward.h>

Expand All @@ -59,111 +57,52 @@ MOVEIT_CLASS_FORWARD(PipelinePlanner);
class PipelinePlanner : public PlannerInterface
{
public:
/** Simple Constructor to use only a single pipeline
* \param [in] node ROS 2 node
* \param [in] pipeline_name Name of the planning pipeline to be used.
* This is also the assumed namespace where the parameters of this pipeline can be found
* \param [in] planner_id Planner id to be used for planning. Empty string means default.
struct Specification
{
moveit::core::RobotModelConstPtr model;
std::string ns{ "ompl" };
std::string pipeline{ "ompl" };
std::string adapter_param{ "request_adapters" };
};

static planning_pipeline::PlanningPipelinePtr create(const rclcpp::Node::SharedPtr& node,
const moveit::core::RobotModelConstPtr& model) {
Specification spec;
spec.model = model;
return create(node, spec);
}

static planning_pipeline::PlanningPipelinePtr create(const rclcpp::Node::SharedPtr& node, const Specification& spec);

/**
*
* @param node used to load the parameters for the planning pipeline
*/
PipelinePlanner(const rclcpp::Node::SharedPtr& node, const std::string& pipeline_name = "ompl",
const std::string& planner_id = "")
: PipelinePlanner(node, { { pipeline_name, planner_id } }) {}

/** \brief Constructor
* \param [in] node ROS 2 node
* \param [in] pipeline_id_planner_id_map map containing pairs of pipeline and plugin names to be used for planning
* \param [in] stopping_criterion_callback callback to decide when to stop parallel planning
* \param [in] solution_selection_function callback to choose the best solution from multiple ran pipelines
*/
PipelinePlanner(
const rclcpp::Node::SharedPtr& node,
const std::unordered_map<std::string, std::string>& pipeline_id_planner_id_map,
const moveit::planning_pipeline_interfaces::StoppingCriterionFunction& stopping_criterion_callback = nullptr,
const moveit::planning_pipeline_interfaces::SolutionSelectionFunction& solution_selection_function =
&moveit::planning_pipeline_interfaces::getShortestSolution);

/** \brief Set the planner id for a specific planning pipeline
* \param [in] pipeline_name Name of the to-be-used planning pipeline
* \param [in] planner_id Name of the to-be-used planner ID
* \return true if the pipeline exists and the corresponding ID is set successfully
*/
bool setPlannerId(const std::string& pipeline_name, const std::string& planner_id);
PipelinePlanner(const rclcpp::Node::SharedPtr& node, const std::string& pipeline = "ompl");

/** \brief Set stopping criterion function for parallel planning
* \param [in] stopping_criterion_callback New stopping criterion function that will be used
*/
void setStoppingCriterionFunction(
const moveit::planning_pipeline_interfaces::StoppingCriterionFunction& stopping_criterion_callback);
PipelinePlanner(const planning_pipeline::PlanningPipelinePtr& planning_pipeline);

/** \brief Set solution selection function for parallel planning
* \param [in] solution_selection_function New solution selection that will be used
*/
void setSolutionSelectionFunction(
const moveit::planning_pipeline_interfaces::SolutionSelectionFunction& solution_selection_function);
void setPlannerId(const std::string& planner) { setProperty("planner", planner); }
std::string getPlannerId() const override { return properties().get<std::string>("planner"); }

/** \brief If not yet done, initialize pipelines from pipeline_id_planner_id_map
* \param [in] robot_model robot model used to initialize the planning pipelines of this solver
*/
void init(const moveit::core::RobotModelConstPtr& robot_model) override;

/** \brief Plan a trajectory from a planning scene 'from' to scene 'to'
* \param [in] from Start planning scene
* \param [in] to Goal planning scene (used to create goal constraints)
* \param [in] joint_model_group Group of joints for which this trajectory is created
* \param [in] timeout Maximum planning timeout for an individual stage that is using the pipeline planner in seconds
* \param [in] result Reference to the location where the created trajectory is stored if planning is successful
* \param [in] path_constraints Path contraints for the planning problem
* \return true If the solver found a successful solution for the given planning problem
*/
Result plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
const core::JointModelGroup* joint_model_group, double timeout,
robot_trajectory::RobotTrajectoryPtr& result,
const core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;

/** \brief Plan a trajectory from a planning scene 'from' to a Cartesian target pose with an offset
* \param [in] from Start planning scene
* \param [in] link Link for which a target pose is given
* \param [in] offset Offset to be applied to a given target pose
* \param [in] target Target pose
* \param [in] joint_model_group Group of joints for which this trajectory is created
* \param [in] timeout Maximum planning timeout for an individual stage that is using the pipeline planner in seconds
* \param [in] result Reference to the location where the created trajectory is stored if planning is successful
* \param [in] path_constraints Path contraints for the planning problem
* \return true If the solver found a successful solution for the given planning problem
*/
Result plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target,
const moveit::core::JointModelGroup* joint_model_group, double timeout,
robot_trajectory::RobotTrajectoryPtr& result,
const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;

std::string getPlannerId() const override { return last_successful_planner_; }

protected:
/** \brief Actual plan() implementation, targeting the given goal_constraints.
* \param [in] planning_scene Scene for which the planning should be solved
* \param [in] joint_model_group Group of joints for which this trajectory is created
* \param [in] goal_constraints Set of constraints that need to be satisfied by the solution
* \param [in] timeout Maximum planning timeout for an individual stage that is using the pipeline planner in seconds
* \param [in] result Reference to the location where the created trajectory is stored if planning is successful
* \param [in] path_constraints Path contraints for the planning problem
* \return true if the solver found a successful solution for the given planning problem
*/
Result plan(const planning_scene::PlanningSceneConstPtr& planning_scene,
const moveit::core::JointModelGroup* joint_model_group,
const moveit_msgs::msg::Constraints& goal_constraints, double timeout,
robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints());
Result plan(const planning_scene::PlanningSceneConstPtr& from, const moveit_msgs::msg::MotionPlanRequest& req,
robot_trajectory::RobotTrajectoryPtr& result);

std::string pipeline_name_;
planning_pipeline::PlanningPipelinePtr planner_;
rclcpp::Node::SharedPtr node_;

std::string last_successful_planner_;

/** \brief Map of instantiated (and named) planning pipelines. */
std::unordered_map<std::string, planning_pipeline::PlanningPipelinePtr> planning_pipelines_;

moveit::planning_pipeline_interfaces::StoppingCriterionFunction stopping_criterion_callback_;
moveit::planning_pipeline_interfaces::SolutionSelectionFunction solution_selection_function_;
};
} // namespace solvers
} // namespace task_constructor
Expand Down
Loading

0 comments on commit 078af42

Please sign in to comment.