Skip to content

Commit 5de1af6

Browse files
sjahrsjahr
committed
Re-order plan functions
1 parent 6805c26 commit 5de1af6

File tree

2 files changed

+86
-77
lines changed

2 files changed

+86
-77
lines changed

‎core/include/moveit/task_constructor/solvers/pipeline_planner.h‎

Lines changed: 22 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -64,39 +64,50 @@ class PipelinePlanner : public PlannerInterface
6464
std::string adapter_param{ "request_adapters" };
6565
};
6666

67-
static planning_pipeline::PlanningPipelinePtr create(const rclcpp::Node::SharedPtr& node,
68-
const moveit::core::RobotModelConstPtr& model) {
67+
static planning_pipeline::PlanningPipelinePtr
68+
createPlanningPipelines(const rclcpp::Node::SharedPtr& node, const moveit::core::RobotModelConstPtr& model) {
6969
Specification spec;
7070
spec.model = model;
7171
return create(node, spec);
7272
}
7373

74-
static planning_pipeline::PlanningPipelinePtr create(const rclcpp::Node::SharedPtr& node, const Specification& spec);
75-
7674
/**
7775
*
7876
* @param node used to load the parameters for the planning pipeline
7977
*/
80-
PipelinePlanner(const rclcpp::Node::SharedPtr& node, const std::string& pipeline = "ompl");
78+
PipelinePlanner(const rclcpp::Node::SharedPtr& node, const std::string& pipeline_namespace = "ompl");
79+
80+
PipelinePlanner(const rclcpp::Node::SharedPtr& node, const std::vector<std::string>& pipeline_namespaces);
81+
82+
PipelinePlanner(const std::unordered_map<std::string, planning_pipeline::PlanningPipelinePtr>& planning_pipelines);
8183

82-
PipelinePlanner(const planning_pipeline::PlanningPipelinePtr& planning_pipeline);
84+
PipelinePlanner(const planning_pipeline::PlanningPipelinePtr& planning_pipeline, const std::string& pipeline = "");
8385

8486
void setPlannerId(const std::string& planner) { setProperty("planner", planner); }
8587

8688
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
8789

8890
bool plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
89-
const core::JointModelGroup* joint_model_group, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
91+
const core::JointModelGroup* joint_model_group, double timeout,
92+
robot_trajectory::RobotTrajectoryPtr& result,
9093
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
9194

9295
bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
93-
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* joint_model_group,
94-
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
96+
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target,
97+
const moveit::core::JointModelGroup* joint_model_group, double timeout,
98+
robot_trajectory::RobotTrajectoryPtr& result,
9599
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
96100

97101
protected:
98-
std::string pipeline_name_;
99-
planning_pipeline::PlanningPipelinePtr planner_;
102+
bool plan(const planning_scene::PlanningSceneConstPtr& planning_scene,
103+
const moveit::core::JointModelGroup* joint_model_group,
104+
const moveit_msgs::msg::Constraints& goal_constraints, double timeout,
105+
robot_trajectory::RobotTrajectoryPtr& result,
106+
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints());
107+
108+
std::vector<std::string> pipeline_names_ = std::vector<std::string>(1);
109+
std::vector<planning_pipeline::PlanningPipelinePtr> planning_pipelines_ =
110+
std::vector<planning_pipeline::PlanningPipelinePtr>(1, nullptr);
100111
rclcpp::Node::SharedPtr node_;
101112
};
102113
} // namespace solvers

‎core/src/solvers/pipeline_planner.cpp‎

Lines changed: 64 additions & 66 deletions
Original file line numberDiff line numberDiff line change
@@ -40,6 +40,7 @@
4040
#include <moveit/task_constructor/task.h>
4141
#include <moveit/planning_scene/planning_scene.h>
4242
#include <moveit/planning_pipeline/planning_pipeline.h>
43+
#include <moveit/planning_pipeline_interfaces/planning_pipeline_interfaces.hpp>
4344
#include <moveit_msgs/msg/motion_plan_request.hpp>
4445
#include <moveit/kinematic_constraints/utils.h>
4546

@@ -53,11 +54,13 @@ namespace moveit {
5354
namespace task_constructor {
5455
namespace solvers {
5556

57+
static constexpr char const* PLUGIN_PARAMETER_NAME = "planning_plugin";
58+
5659
struct PlannerCache
5760
{
5861
using PlannerID = std::tuple<std::string, std::string>;
59-
using PlannerMap = std::map<PlannerID, std::weak_ptr<planning_pipeline::PlanningPipeline> >;
60-
using ModelList = std::list<std::pair<std::weak_ptr<const moveit::core::RobotModel>, PlannerMap> >;
62+
using PlannerMap = std::map<PlannerID, std::weak_ptr<planning_pipeline::PlanningPipeline>>;
63+
using ModelList = std::list<std::pair<std::weak_ptr<const moveit::core::RobotModel>, PlannerMap>>;
6164
ModelList cache_;
6265

6366
PlannerMap::mapped_type& retrieve(const moveit::core::RobotModelConstPtr& model, const PlannerID& planner_id) {
@@ -83,8 +86,6 @@ planning_pipeline::PlanningPipelinePtr PipelinePlanner::create(const rclcpp::Nod
8386
const PipelinePlanner::Specification& specification) {
8487
static PlannerCache cache;
8588

86-
static constexpr char const* PLUGIN_PARAMETER_NAME = "planning_plugin";
87-
8889
std::string pipeline_ns = specification.ns;
8990
const std::string parameter_name = pipeline_ns + "." + PLUGIN_PARAMETER_NAME;
9091
// fallback to old structure for pipeline parameters in MoveIt
@@ -111,104 +112,101 @@ planning_pipeline::PlanningPipelinePtr PipelinePlanner::create(const rclcpp::Nod
111112
return planner;
112113
}
113114

114-
PipelinePlanner::PipelinePlanner(const rclcpp::Node::SharedPtr& node, const std::string& pipeline_name)
115-
: pipeline_name_{ pipeline_name }, node_(node) {
116-
auto& planner_properties = properties();
117-
planner_properties.declare<std::string>("planner", "", "planner id");
115+
PipelinePlanner::PipelinePlanner(const rclcpp::Node::SharedPtr& node, const std::string& pipeline_name) : node_(node) {
116+
pipeline_names_.at(0) = pipeline_name;
117+
properties().declare<std::string>("planner", "", "planner id");
118118

119-
planner_properties.declare<uint>("num_planning_attempts", 1u, "number of planning attempts");
120-
planner_properties.declare<moveit_msgs::msg::WorkspaceParameters>(
119+
properties().declare<uint>("num_planning_attempts", 1u, "number of planning attempts");
120+
properties().declare<moveit_msgs::msg::WorkspaceParameters>(
121121
"workspace_parameters", moveit_msgs::msg::WorkspaceParameters(), "allowed workspace of mobile base?");
122122

123-
planner_properties.declare<double>("goal_joint_tolerance", 1e-4, "tolerance for reaching joint goals");
124-
planner_properties.declare<double>("goal_position_tolerance", 1e-4, "tolerance for reaching position goals");
125-
planner_properties.declare<double>("goal_orientation_tolerance", 1e-4, "tolerance for reaching orientation goals");
123+
properties().declare<double>("goal_joint_tolerance", 1e-4, "tolerance for reaching joint goals");
124+
properties().declare<double>("goal_position_tolerance", 1e-4, "tolerance for reaching position goals");
125+
properties().declare<double>("goal_orientation_tolerance", 1e-4, "tolerance for reaching orientation goals");
126126

127-
planner_properties.declare<bool>("display_motion_plans", false,
128-
"publish generated solutions on topic " +
129-
planning_pipeline::PlanningPipeline::DISPLAY_PATH_TOPIC);
130-
planner_properties.declare<bool>("publish_planning_requests", false,
131-
"publish motion planning requests on topic " +
132-
planning_pipeline::PlanningPipeline::MOTION_PLAN_REQUEST_TOPIC);
127+
properties().declare<bool>("display_motion_plans", false,
128+
"publish generated solutions on topic " +
129+
planning_pipeline::PlanningPipeline::DISPLAY_PATH_TOPIC);
130+
properties().declare<bool>("publish_planning_requests", false,
131+
"publish motion planning requests on topic " +
132+
planning_pipeline::PlanningPipeline::MOTION_PLAN_REQUEST_TOPIC);
133133
}
134134

135135
PipelinePlanner::PipelinePlanner(const planning_pipeline::PlanningPipelinePtr& planning_pipeline)
136136
: PipelinePlanner(rclcpp::Node::SharedPtr()) {
137-
planner_ = planning_pipeline;
137+
planning_pipelines_.at(0) = planning_pipeline;
138138
}
139139

140140
void PipelinePlanner::init(const core::RobotModelConstPtr& robot_model) {
141-
if (!planner_) {
141+
if (!planning_pipelines_.at(0)) {
142142
Specification specification;
143143
specification.model = robot_model;
144-
specification.pipeline = pipeline_name_;
145-
specification.ns = pipeline_name_;
146-
planner_ = create(node_, specification);
147-
} else if (robot_model != planner_->getRobotModel()) {
144+
specification.pipeline = pipeline_names_.at(0);
145+
specification.ns = pipeline_names_.at(0);
146+
planning_pipelines_.at(0) = create(node_, specification);
147+
} else if (robot_model != planning_pipelines_.at(0)->getRobotModel()) {
148148
throw std::runtime_error(
149149
"The robot model of the planning pipeline isn't the same as the task's robot model -- "
150150
"use Task::setRobotModel for setting the robot model when using custom planning pipeline");
151151
}
152-
planner_->displayComputedMotionPlans(properties().get<bool>("display_motion_plans"));
153-
planner_->publishReceivedRequests(properties().get<bool>("publish_planning_requests"));
154-
}
155-
156-
void initMotionPlanRequest(moveit_msgs::msg::MotionPlanRequest& request, const PropertyMap& p,
157-
const moveit::core::JointModelGroup* joint_model_group, double timeout) {
158-
request.group_name = joint_model_group->getName();
159-
request.planner_id = p.get<std::string>("planner");
160-
request.allowed_planning_time = timeout;
161-
request.start_state.is_diff = true; // we don't specify an extra start state
162-
163-
request.num_planning_attempts = p.get<uint>("num_planning_attempts");
164-
request.max_velocity_scaling_factor = p.get<double>("max_velocity_scaling_factor");
165-
request.max_acceleration_scaling_factor = p.get<double>("max_acceleration_scaling_factor");
166-
request.workspace_parameters = p.get<moveit_msgs::msg::WorkspaceParameters>("workspace_parameters");
152+
planning_pipelines_.at(0)->displayComputedMotionPlans(properties().get<bool>("display_motion_plans"));
153+
planning_pipelines_.at(0)->publishReceivedRequests(properties().get<bool>("publish_planning_requests"));
167154
}
168155

169156
bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
170157
const planning_scene::PlanningSceneConstPtr& to,
171158
const moveit::core::JointModelGroup* joint_model_group, double timeout,
172159
robot_trajectory::RobotTrajectoryPtr& result,
173160
const moveit_msgs::msg::Constraints& path_constraints) {
174-
const auto& planner_properties = properties();
175-
moveit_msgs::msg::MotionPlanRequest request;
176-
initMotionPlanRequest(request, planner_properties, joint_model_group, timeout);
177-
178-
request.goal_constraints.resize(1);
179-
request.goal_constraints[0] = kinematic_constraints::constructGoalConstraints(
180-
to->getCurrentState(), joint_model_group, planner_properties.get<double>("goal_joint_tolerance"));
181-
request.path_constraints = path_constraints;
182-
183-
::planning_interface::MotionPlanResponse response;
184-
bool success = planner_->generatePlan(from, request, response);
185-
result = response.trajectory;
186-
return success;
161+
const auto goal_constraints = kinematic_constraints::constructGoalConstraints(
162+
to->getCurrentState(), joint_model_group, properties().get<double>("goal_joint_tolerance"));
163+
return plan(from, joint_model_group, goal_constraints, timeout, result, path_constraints);
187164
}
188165

189166
bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
190167
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target_eigen,
191168
const moveit::core::JointModelGroup* joint_model_group, double timeout,
192169
robot_trajectory::RobotTrajectoryPtr& result,
193170
const moveit_msgs::msg::Constraints& path_constraints) {
194-
const auto& planner_properties = properties();
195-
moveit_msgs::msg::MotionPlanRequest request;
196-
initMotionPlanRequest(request, planner_properties, joint_model_group, timeout);
197-
198171
geometry_msgs::msg::PoseStamped target;
199172
target.header.frame_id = from->getPlanningFrame();
200173
target.pose = tf2::toMsg(target_eigen * offset.inverse());
201174

202-
request.goal_constraints.resize(1);
203-
request.goal_constraints[0] = kinematic_constraints::constructGoalConstraints(
204-
link.getName(), target, planner_properties.get<double>("goal_position_tolerance"),
205-
planner_properties.get<double>("goal_orientation_tolerance"));
206-
request.path_constraints = path_constraints;
175+
const auto goal_constraints = kinematic_constraints::constructGoalConstraints(
176+
link.getName(), target, properties().get<double>("goal_position_tolerance"),
177+
properties().get<double>("goal_orientation_tolerance"));
178+
179+
return plan(from, joint_model_group, goal_constraints, timeout, result, path_constraints);
180+
}
181+
182+
bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& planning_scene,
183+
const moveit::core::JointModelGroup* joint_model_group,
184+
const moveit_msgs::msg::Constraints& goal_constraints, double timeout,
185+
robot_trajectory::RobotTrajectoryPtr& result,
186+
const moveit_msgs::msg::Constraints& path_constraints) {
187+
std::vector<moveit_msgs::msg::MotionPlanRequest> requests;
188+
requests.reserve(pipeline_names_.size());
189+
190+
for (auto const& pipeline_name : pipeline_names_) {
191+
moveit_msgs::msg::MotionPlanRequest request;
192+
request.group_name = joint_model_group->getName();
193+
request.planner_id = properties().get<std::string>("planner");
194+
request.allowed_planning_time = timeout;
195+
request.start_state.is_diff = true; // we don't specify an extra start state
196+
request.num_planning_attempts = properties().get<uint>("num_planning_attempts");
197+
request.max_velocity_scaling_factor = properties().get<double>("max_velocity_scaling_factor");
198+
request.max_acceleration_scaling_factor = properties().get<double>("max_acceleration_scaling_factor");
199+
request.workspace_parameters = properties().get<moveit_msgs::msg::WorkspaceParameters>("workspace_parameters");
200+
request.goal_constraints.resize(1);
201+
request.goal_constraints.at(0) = goal_constraints;
202+
request.path_constraints = path_constraints;
203+
requests.push_back(request);
204+
}
207205

208-
::planning_interface::MotionPlanResponse response;
209-
bool success = planner_->generatePlan(from, request, response);
210-
result = response.trajectory;
211-
return success;
206+
std::vector<::planning_interface::MotionPlanResponse> responses =
207+
moveit::planning_pipeline_interfaces::planWithParallelPipelines(requests, planning_scene, planning_pipelines_);
208+
result = responses.at(0).trajectory;
209+
return bool(responses);
212210
}
213211
} // namespace solvers
214212
} // namespace task_constructor

0 commit comments

Comments
 (0)