From 1c61fb3b8562d9ce5f650b83c90b696af21224c7 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 25 Feb 2018 22:07:47 +0100 Subject: [PATCH] expose solvers --- .../moveit/task_constructor/stages/pick.h | 15 ++++++++++++++- core/src/stages/pick.cpp | 16 ++++++++-------- 2 files changed, 22 insertions(+), 9 deletions(-) diff --git a/core/include/moveit/task_constructor/stages/pick.h b/core/include/moveit/task_constructor/stages/pick.h index c1ebd54e..0b828304 100644 --- a/core/include/moveit/task_constructor/stages/pick.h +++ b/core/include/moveit/task_constructor/stages/pick.h @@ -36,10 +36,18 @@ #pragma once +#include #include #include -namespace moveit { namespace task_constructor { namespace stages { +namespace moveit { namespace task_constructor { + +namespace solvers { +MOVEIT_CLASS_FORWARD(CartesianPath) +MOVEIT_CLASS_FORWARD(PipelinePlanner) +} + +namespace stages { /** Pick wraps a complete pipeline to pick up an object with a given end effector. * @@ -54,6 +62,8 @@ namespace moveit { namespace task_constructor { namespace stages { * the end effector's Cartesian pose needs to be provided by an external grasp stage. */ class Pick : public SerialContainer { + solvers::CartesianPathPtr cartesian_solver_; + solvers::PipelinePlannerPtr pipeline_solver_; Stage* grasp_stage_ = nullptr; Stage* approach_stage_ = nullptr; Stage* lift_stage_ = nullptr; @@ -70,6 +80,9 @@ public: properties().set("object", object); } + solvers::CartesianPathPtr cartesianSolver() { return cartesian_solver_; } + solvers::PipelinePlannerPtr pipelineSolver() { return pipeline_solver_; } + void setApproachMotion(const geometry_msgs::TwistStamped& motion, double min_distance, double max_distance); void setLiftMotion(const geometry_msgs::TwistStamped& motion, diff --git a/core/src/stages/pick.cpp b/core/src/stages/pick.cpp index 800b95c4..8f234046 100644 --- a/core/src/stages/pick.cpp +++ b/core/src/stages/pick.cpp @@ -23,13 +23,13 @@ Pick::Pick(Stage::pointer&& grasp_stage, const std::string& name) p.declare("eef_group", "JMG of eef"); p.declare("eef_parent_group", "JMG of eef's parent"); - auto cartesian = std::make_shared(); - auto pipeline = std::make_shared(); - pipeline->setTimeout(8.0); - pipeline->setPlannerId("RRTConnectkConfigDefault"); + cartesian_solver_ = std::make_shared(); + pipeline_solver_ = std::make_shared(); + pipeline_solver_->setTimeout(8.0); + pipeline_solver_->setPlannerId("RRTConnectkConfigDefault"); { - auto move = std::make_unique("open gripper", pipeline); + auto move = std::make_unique("open gripper", pipeline_solver_); PropertyMap& p = move->properties(); p.property("group").configureInitFrom(Stage::PARENT, "eef_group"); move->setGoal("open"); // TODO: retrieve from grasp stage @@ -37,14 +37,14 @@ Pick::Pick(Stage::pointer&& grasp_stage, const std::string& name) } { - auto move = std::make_unique("move to object", pipeline); + auto move = std::make_unique("move to object", pipeline_solver_); PropertyMap& p = move->properties(); p.property("group").configureInitFrom(Stage::PARENT, "eef_parent_group"); insert(std::move(move)); } { - auto approach = std::make_unique("approach object", cartesian); + auto approach = std::make_unique("approach object", cartesian_solver_); PropertyMap& p = approach->properties(); p.property("group").configureInitFrom(Stage::PARENT, "eef_parent_group"); p.set("marker_ns", std::string("approach")); @@ -57,7 +57,7 @@ Pick::Pick(Stage::pointer&& grasp_stage, const std::string& name) insert(std::move(grasp_stage)); { - auto lift = std::make_unique("lift object", cartesian); + auto lift = std::make_unique("lift object", cartesian_solver_); PropertyMap& p = lift->properties(); p.property("group").configureInitFrom(Stage::PARENT, "eef_parent_group"); p.set("marker_ns", std::string("lift"));