diff --git a/core/demo/plan_pick_pa10.cpp b/core/demo/plan_pick_pa10.cpp index b68c3825..c813f8fb 100644 --- a/core/demo/plan_pick_pa10.cpp +++ b/core/demo/plan_pick_pa10.cpp @@ -3,7 +3,7 @@ #include #include -#include +#include #include #include #include @@ -18,7 +18,7 @@ using namespace moveit::task_constructor; -void spawnObject(planning_scene::PlanningScenePtr scene) { +void spawnObject(const planning_scene::PlanningScenePtr& scene) { moveit_msgs::CollisionObject o; o.id= "object"; o.header.frame_id= "world"; @@ -52,7 +52,21 @@ int main(int argc, char** argv){ pipeline->setPlannerId("RRTConnectkConfigDefault"); auto cartesian = std::make_shared(); - t.add(std::make_unique()); + Stage* initial_stage = nullptr; + // create a fixed initial scene + { + auto scene = std::make_shared(t.getRobotModel()); + auto& state = scene->getCurrentStateNonConst(); + state.setToDefaultValues(state.getJointModelGroup("left_arm"), "home"); + state.setToDefaultValues(state.getJointModelGroup("right_arm"), "home"); + state.update(); + spawnObject(scene); + + auto initial = std::make_unique(); + initial_stage = initial.get(); + initial->setState(scene); + t.add(std::move(initial)); + } { auto stage = std::make_unique(); stage->restrictDirection(stages::MoveTo::FORWARD); @@ -97,6 +111,7 @@ int main(int argc, char** argv){ Eigen::AngleAxisd(-0.5*M_PI, Eigen::Vector3d::UnitY()), "lh_tool_frame"); gengrasp->setAngleDelta(M_PI / 10.); + gengrasp->setMonitoredStage(initial_stage); auto ik = std::make_unique("compute ik", std::move(gengrasp)); ik->properties().configureInitFrom(Stage::PARENT, {"group", "eef", "default_pose"}); @@ -157,12 +172,6 @@ int main(int argc, char** argv){ } try { - auto scene = t.initScene(ros::Duration(0)); - auto& state = scene->getCurrentStateNonConst(); - state.setToDefaultValues(state.getJointModelGroup("left_arm"), "home"); - state.setToDefaultValues(state.getJointModelGroup("right_arm"), "home"); - state.update(); - spawnObject(scene); t.plan(); std::cout << "waiting for \n"; char ch; diff --git a/core/demo/plan_pick_trixi.cpp b/core/demo/plan_pick_trixi.cpp index b4cacc14..e0cf26ad 100644 --- a/core/demo/plan_pick_trixi.cpp +++ b/core/demo/plan_pick_trixi.cpp @@ -43,7 +43,13 @@ int main(int argc, char** argv){ Task t; - t.add( std::make_unique("current state") ); + Stage* initial_stage = nullptr; + + { + auto initial = std::make_unique("current state"); + initial_stage = initial.get(); + t.add(std::move(initial)); + } { auto move= std::make_unique("open gripper"); @@ -81,6 +87,7 @@ int main(int argc, char** argv){ gengrasp->setObject("object"); gengrasp->setToolToGraspTF(Eigen::Affine3d::Identity(), "l_gripper_tool_frame"); gengrasp->setAngleDelta(.2); + gengrasp->setMonitoredStage(initial_stage); auto ik = std::make_unique("compute ik", std::move(gengrasp)); ik->setEndEffector("left_gripper"); diff --git a/core/demo/plan_pick_ur5.cpp b/core/demo/plan_pick_ur5.cpp index 396ba88c..92e2e1f6 100644 --- a/core/demo/plan_pick_ur5.cpp +++ b/core/demo/plan_pick_ur5.cpp @@ -46,7 +46,13 @@ int main(int argc, char** argv){ t.setProperty("planner", std::string("RRTConnectkConfigDefault")); t.setProperty("link", std::string("s_model_tool0")); - t.add(std::make_unique("current state")); + Stage* initial_stage = nullptr; + + { + auto initial = std::make_unique("current state"); + initial_stage = initial.get(); + t.add(std::move(initial)); + } { auto move = std::make_unique("open gripper"); @@ -82,6 +88,7 @@ int main(int argc, char** argv){ gengrasp->setObject("object"); gengrasp->setToolToGraspTF(Eigen::Translation3d(.03,0,0), "s_model_tool0"); gengrasp->setAngleDelta(-.2); + gengrasp->setMonitoredStage(initial_stage); auto ik = std::make_unique("compute ik", std::move(gengrasp)); ik->properties().configureInitFrom(Stage::PARENT, {"eef"}); diff --git a/core/include/moveit/task_constructor/container.h b/core/include/moveit/task_constructor/container.h index e9302ff4..f0e41bbd 100644 --- a/core/include/moveit/task_constructor/container.h +++ b/core/include/moveit/task_constructor/container.h @@ -62,7 +62,7 @@ public: virtual void clear(); void reset() override; - void init(const planning_scene::PlanningSceneConstPtr& scene) override; + void init(const moveit::core::RobotModelConstPtr& robot_model) override; /// validate connectivity of children (after init() was done) virtual void validateConnectivity() const; @@ -90,7 +90,7 @@ public: SerialContainer(const std::string& name = "serial container"); void reset() override; - void init(const planning_scene::PlanningSceneConstPtr &scene) override; + void init(const moveit::core::RobotModelConstPtr& robot_model) override; /// validate connectivity of children (after init() was done) void validateConnectivity() const override; @@ -140,7 +140,7 @@ public: ParallelContainerBase(const std::string &name); void reset() override; - void init(const planning_scene::PlanningSceneConstPtr &scene) override; + void init(const moveit::core::RobotModelConstPtr& robot_model) override; /// validate connectivity of children (after init() was done) void validateConnectivity() const override; @@ -202,7 +202,7 @@ public: Fallbacks(const std::string &name) : ParallelContainerBase(name) {} void reset() override; - void init(const planning_scene::PlanningSceneConstPtr &scene) override; + void init(const moveit::core::RobotModelConstPtr& robot_model) override; bool canCompute() const override; bool compute() override; }; diff --git a/core/include/moveit/task_constructor/stage.h b/core/include/moveit/task_constructor/stage.h index 5ab7fd7c..47b209e9 100644 --- a/core/include/moveit/task_constructor/stage.h +++ b/core/include/moveit/task_constructor/stage.h @@ -50,6 +50,10 @@ inline const Class##Private* pimpl() const; \ inline Class##Private* pimpl(); +namespace moveit { namespace core { +MOVEIT_CLASS_FORWARD(RobotModel) +} } + namespace planning_scene { MOVEIT_CLASS_FORWARD(PlanningScene) } @@ -148,7 +152,7 @@ public: * The planning scene is the initial planning scene of the task. Use it as is or * to learn about the robot model. */ - virtual void init(const planning_scene::PlanningSceneConstPtr& scene); + virtual void init(const moveit::core::RobotModelConstPtr& robot_model); const ContainerBase* parent() const; const std::string& name() const; @@ -220,7 +224,7 @@ public: void restrictDirection(Direction dir); void reset() override; - void init(const planning_scene::PlanningSceneConstPtr &scene) override; + void init(const moveit::core::RobotModelConstPtr& robot_model) override; virtual bool computeForward(const InterfaceState& from) = 0; virtual bool computeBackward(const InterfaceState& to) = 0; @@ -314,7 +318,7 @@ public: MonitoringGenerator(const std::string &name = "monitoring generator", Stage* monitored = nullptr); void setMonitoredStage(Stage* monitored); - void init(const planning_scene::PlanningSceneConstPtr &scene) override; + void init(const moveit::core::RobotModelConstPtr& robot_model) override; protected: MonitoringGenerator(MonitoringGeneratorPrivate* impl); diff --git a/core/include/moveit/task_constructor/stages/connect.h b/core/include/moveit/task_constructor/stages/connect.h index 1d7ee2b4..e030b56f 100644 --- a/core/include/moveit/task_constructor/stages/connect.h +++ b/core/include/moveit/task_constructor/stages/connect.h @@ -47,7 +47,7 @@ class Connect : public Connecting { public: Connect(std::string name, const solvers::PlannerInterfacePtr &planner); - void init(const planning_scene::PlanningSceneConstPtr &scene); + void init(const moveit::core::RobotModelConstPtr& robot_model); bool compute(const InterfaceState &from, const InterfaceState &to); protected: diff --git a/core/include/moveit/task_constructor/stages/current_state.h b/core/include/moveit/task_constructor/stages/current_state.h index 72ec9b2c..2452149d 100644 --- a/core/include/moveit/task_constructor/stages/current_state.h +++ b/core/include/moveit/task_constructor/stages/current_state.h @@ -49,7 +49,7 @@ class CurrentState : public Generator { public: CurrentState(const std::string& name = "current state"); - void init(const planning_scene::PlanningSceneConstPtr& scene) override; + void init(const moveit::core::RobotModelConstPtr& robot_model) override; bool canCompute() const override; bool compute() override; @@ -58,8 +58,8 @@ public: } protected: + moveit::core::RobotModelConstPtr robot_model_; planning_scene::PlanningScenePtr scene_; - bool ran_ = false; }; } } } diff --git a/core/include/moveit/task_constructor/stages/generate_grasp_pose.h b/core/include/moveit/task_constructor/stages/generate_grasp_pose.h index c072d38a..f81ee5d0 100644 --- a/core/include/moveit/task_constructor/stages/generate_grasp_pose.h +++ b/core/include/moveit/task_constructor/stages/generate_grasp_pose.h @@ -43,11 +43,10 @@ namespace moveit { namespace task_constructor { namespace stages { -class GenerateGraspPose : public Generator { +class GenerateGraspPose : public MonitoringGenerator { public: - GenerateGraspPose(std::string name); + GenerateGraspPose(const std::string& name); - void init(const planning_scene::PlanningSceneConstPtr& scene) override; bool canCompute() const override; bool compute() override; @@ -64,6 +63,9 @@ public: } void setAngleDelta(double delta); +protected: + void onNewSolution(const SolutionBase& s) override; + protected: planning_scene::PlanningScenePtr scene_; double current_angle_ = 0.0; diff --git a/core/include/moveit/task_constructor/stages/gripper.h b/core/include/moveit/task_constructor/stages/gripper.h index 7569155e..94ce77dc 100644 --- a/core/include/moveit/task_constructor/stages/gripper.h +++ b/core/include/moveit/task_constructor/stages/gripper.h @@ -50,7 +50,7 @@ class Gripper : public PropagatingEitherWay { public: Gripper(std::string name); - void init(const planning_scene::PlanningSceneConstPtr &scene); + void init(const moveit::core::RobotModelConstPtr& robot_model); bool computeForward(const InterfaceState& from) override; bool computeBackward(const InterfaceState& to) override; diff --git a/core/include/moveit/task_constructor/stages/move.h b/core/include/moveit/task_constructor/stages/move.h index 2a303828..e394f1bd 100644 --- a/core/include/moveit/task_constructor/stages/move.h +++ b/core/include/moveit/task_constructor/stages/move.h @@ -46,7 +46,7 @@ class Move : public Connecting { public: Move(std::string name); - void init(const planning_scene::PlanningSceneConstPtr &scene); + void init(const moveit::core::RobotModelConstPtr& robot_model); bool compute(const InterfaceState &from, const InterfaceState &to); void setGroup(const std::string &group); diff --git a/core/include/moveit/task_constructor/stages/move_relative.h b/core/include/moveit/task_constructor/stages/move_relative.h index 308cba43..ab09eef6 100644 --- a/core/include/moveit/task_constructor/stages/move_relative.h +++ b/core/include/moveit/task_constructor/stages/move_relative.h @@ -50,7 +50,7 @@ class MoveRelative : public PropagatingEitherWay { public: MoveRelative(std::string name, const solvers::PlannerInterfacePtr& planner); - void init(const planning_scene::PlanningSceneConstPtr &scene) override; + void init(const moveit::core::RobotModelConstPtr& robot_model) override; bool computeForward(const InterfaceState& from) override; bool computeBackward(const InterfaceState& to) override; diff --git a/core/include/moveit/task_constructor/stages/move_to.h b/core/include/moveit/task_constructor/stages/move_to.h index 08743580..c96a2b94 100644 --- a/core/include/moveit/task_constructor/stages/move_to.h +++ b/core/include/moveit/task_constructor/stages/move_to.h @@ -49,7 +49,7 @@ class MoveTo : public PropagatingEitherWay { public: MoveTo(std::string name, const solvers::PlannerInterfacePtr& planner); - void init(const planning_scene::PlanningSceneConstPtr &scene) override; + void init(const moveit::core::RobotModelConstPtr& robot_model) override; bool computeForward(const InterfaceState& from) override; bool computeBackward(const InterfaceState& to) override; diff --git a/core/include/moveit/task_constructor/task.h b/core/include/moveit/task_constructor/task.h index 4620ca70..e1a5d2f4 100644 --- a/core/include/moveit/task_constructor/task.h +++ b/core/include/moveit/task_constructor/task.h @@ -46,9 +46,6 @@ #include -namespace robot_model_loader { - MOVEIT_CLASS_FORWARD(RobotModelLoader) -} namespace moveit { namespace core { MOVEIT_CLASS_FORWARD(RobotModel) MOVEIT_CLASS_FORWARD(RobotState) @@ -76,7 +73,7 @@ public: ~Task(); std::string id() const; - const moveit::core::RobotModelPtr getRobotModel() const; + const moveit::core::RobotModelConstPtr getRobotModel() const { return robot_model_; } void add(Stage::pointer &&stage); void clear() override; @@ -92,14 +89,10 @@ public: /// remove function callback void erase(TaskCallbackList::const_iterator which); - /// initialize planning scene from get_planning_scene service (waiting given timeout for it) - /// if service is not available or timeout is zero, use an empty planning scene - planning_scene::PlanningScenePtr initScene(ros::Duration timeout = ros::Duration(-1)); - /// reset all stages void reset() override; /// initialize all stages with given scene - void init(const planning_scene::PlanningSceneConstPtr &scene) override; + void init(const moveit::core::RobotModelConstPtr& model) override; /// reset, init scene (if not yet done), and init all stages, then start planning bool plan(); @@ -124,15 +117,13 @@ public: void setProperty(const std::string& name, const boost::any& value); protected: - void initModel(); bool canCompute() const override; bool compute() override; void onNewSolution(const SolutionBase &s) override; private: std::string id_; - robot_model_loader::RobotModelLoaderPtr rml_; - planning_scene::PlanningScenePtr scene_; // initial scene + moveit::core::RobotModelConstPtr robot_model_; // introspection and monitoring std::unique_ptr introspection_; diff --git a/core/src/container.cpp b/core/src/container.cpp index d1d5c1a1..f6d6f559 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -198,12 +198,12 @@ void ContainerBase::reset() Stage::reset(); } -void ContainerBase::init(const planning_scene::PlanningSceneConstPtr &scene) +void ContainerBase::init(const moveit::core::RobotModelConstPtr& robot_model) { auto impl = pimpl(); auto& children = impl->children(); - Stage::init(scene); + Stage::init(robot_model); // we need to have some children to do the actual work if (children.empty()) @@ -212,7 +212,7 @@ void ContainerBase::init(const planning_scene::PlanningSceneConstPtr &scene) // recursively init all children and accumulate errors InitStageException errors; for (auto& child : children) { - try { child->init(scene); } catch (InitStageException &e) { errors.append(e); } + try { child->init(robot_model); } catch (InitStageException &e) { errors.append(e); } } if (errors) throw errors; @@ -408,14 +408,14 @@ bool SerialContainerPrivate::connect(container_type::const_iterator cur) * This context is provided by two stages pushing from both ends * into a (potentially long) sequence of propagating stages (tbd). */ -void SerialContainer::init(const planning_scene::PlanningSceneConstPtr &scene) +void SerialContainer::init(const moveit::core::RobotModelConstPtr& robot_model) { // reset pull interfaces auto impl = pimpl(); impl->starts_.reset(); impl->ends_.reset(); - ContainerBase::init(scene); // throws if there are no children + ContainerBase::init(robot_model); // throws if there are no children auto start = impl->children().begin(); auto last = --impl->children().end(); @@ -703,10 +703,10 @@ void ParallelContainerBase::reset() /* States received by the container need to be copied to all children's pull interfaces. * States generated by children can be directly forwarded into the container's push interfaces. */ -void ParallelContainerBase::init(const planning_scene::PlanningSceneConstPtr &scene) +void ParallelContainerBase::init(const moveit::core::RobotModelConstPtr& robot_model) { // recursively init children - ContainerBase::init(scene); + ContainerBase::init(robot_model); auto impl = pimpl(); // determine the union of interfaces required by children @@ -886,9 +886,9 @@ void Fallbacks::reset() ParallelContainerBase::reset(); } -void Fallbacks::init(const planning_scene::PlanningSceneConstPtr &scene) +void Fallbacks::init(const moveit::core::RobotModelConstPtr& robot_model) { - ParallelContainerBase::init(scene); + ParallelContainerBase::init(robot_model); active_child_ = pimpl()->children().front().get(); } diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 962778b6..80be4213 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -128,7 +128,7 @@ void Stage::reset() impl->properties_.reset(); } -void Stage::init(const planning_scene::PlanningSceneConstPtr &scene) +void Stage::init(const moveit::core::RobotModelConstPtr& robot_model) { // init properties once from parent auto impl = pimpl(); @@ -390,9 +390,9 @@ void PropagatingEitherWay::reset() ComputeBase::reset(); } -void PropagatingEitherWay::init(const planning_scene::PlanningSceneConstPtr& scene) +void PropagatingEitherWay::init(const moveit::core::RobotModelConstPtr& robot_model) { - Stage::init(scene); + Stage::init(robot_model); auto impl = pimpl(); // In AUTO-mode, i.e. when auto-detecting direction of propagation from context, @@ -524,7 +524,7 @@ void MonitoringGenerator::setMonitoredStage(Stage* monitored) impl->monitored_ = monitored; } -void MonitoringGenerator::init(const planning_scene::PlanningSceneConstPtr& scene) +void MonitoringGenerator::init(const moveit::core::RobotModelConstPtr& robot_model) { Generator::init(robot_model); diff --git a/core/src/stages/connect.cpp b/core/src/stages/connect.cpp index 2a11d5f0..b2805ec1 100644 --- a/core/src/stages/connect.cpp +++ b/core/src/stages/connect.cpp @@ -50,10 +50,10 @@ Connect::Connect(std::string name, const solvers::PlannerInterfacePtr& planner) p.declare("group", "name of planning group"); } -void Connect::init(const planning_scene::PlanningSceneConstPtr &scene) +void Connect::init(const core::RobotModelConstPtr& robot_model) { - Connecting::init(scene); - planner_->init(scene->getRobotModel()); + Connecting::init(robot_model); + planner_->init(robot_model); } bool Connect::compute(const InterfaceState &from, const InterfaceState &to) { diff --git a/core/src/stages/current_state.cpp b/core/src/stages/current_state.cpp index 64dae5d6..fc764cec 100644 --- a/core/src/stages/current_state.cpp +++ b/core/src/stages/current_state.cpp @@ -53,19 +53,19 @@ CurrentState::CurrentState(const std::string &name) p.declare("timeout", ros::Duration(-1), "max time to wait for get_planning_scene service"); } -void CurrentState::init(const planning_scene::PlanningSceneConstPtr &scene) +void CurrentState::init(const moveit::core::RobotModelConstPtr& robot_model) { - Generator::init(scene); - scene_ = scene->diff(); + Generator::init(robot_model); + robot_model_ = robot_model; + scene_.reset(); } bool CurrentState::canCompute() const{ - return !ran_; + return !scene_; } -bool CurrentState::compute(){ - auto rml = std::make_shared(); - scene_ = std::make_shared(rml->getModel()); +bool CurrentState::compute() { + scene_ = std::make_shared(robot_model_); ros::NodeHandle h; ros::ServiceClient client = h.serviceClient("get_planning_scene"); @@ -90,7 +90,6 @@ bool CurrentState::compute(){ if (client.call(req, res)) { scene_->setPlanningSceneMsg(res.scene); spawn(InterfaceState(scene_), 0.0); - ran_= true; return true; } } diff --git a/core/src/stages/generate_grasp_pose.cpp b/core/src/stages/generate_grasp_pose.cpp index 1fd00a56..b0124d43 100644 --- a/core/src/stages/generate_grasp_pose.cpp +++ b/core/src/stages/generate_grasp_pose.cpp @@ -46,8 +46,8 @@ namespace moveit { namespace task_constructor { namespace stages { -GenerateGraspPose::GenerateGraspPose(std::string name) -: Generator(name) +GenerateGraspPose::GenerateGraspPose(const std::string& name) + : MonitoringGenerator(name) { auto& p = properties(); p.declare("eef", "name of end-effector group"); @@ -57,12 +57,6 @@ GenerateGraspPose::GenerateGraspPose(std::string name) p.declare("angle_delta", 0.1, "angular steps (rad)"); } -void GenerateGraspPose::init(const planning_scene::PlanningSceneConstPtr &scene) -{ - Generator::init(scene); - scene_ = scene->diff(); -} - void GenerateGraspPose::setEndEffector(const std::string &eef){ setProperty("eef", eef); } @@ -91,8 +85,15 @@ void GenerateGraspPose::setAngleDelta(double delta){ setProperty("angle_delta", delta); } +void GenerateGraspPose::onNewSolution(const SolutionBase& s) +{ + if (scene_) + ROS_WARN_NAMED("GenerateGraspPose", "got additional solution from monitored stage"); + scene_ = s.end()->scene()->diff(); +} + bool GenerateGraspPose::canCompute() const{ - return current_angle_ < 2*M_PI && current_angle_ > -2*M_PI; + return scene_ && current_angle_ < 2*M_PI && current_angle_ > -2*M_PI; } bool GenerateGraspPose::compute(){ diff --git a/core/src/stages/gripper.cpp b/core/src/stages/gripper.cpp index 5b0f731e..58170982 100644 --- a/core/src/stages/gripper.cpp +++ b/core/src/stages/gripper.cpp @@ -59,10 +59,10 @@ Gripper::Gripper(std::string name) p.declare("grasp_object", "name of grasp object"); } -void Gripper::init(const planning_scene::PlanningSceneConstPtr &scene) +void Gripper::init(const moveit::core::RobotModelConstPtr& robot_model) { - PropagatingEitherWay::init(scene); - planner_ = Task::createPlanner(scene->getRobotModel()); + PropagatingEitherWay::init(robot_model); + planner_ = Task::createPlanner(robot_model); } void Gripper::setEndEffector(std::string eef){ diff --git a/core/src/stages/move.cpp b/core/src/stages/move.cpp index 78b75c82..7096d6a9 100644 --- a/core/src/stages/move.cpp +++ b/core/src/stages/move.cpp @@ -58,10 +58,10 @@ Move::Move(std::string name) p.declare("planner", "", "planner id"); } -void Move::init(const planning_scene::PlanningSceneConstPtr &scene) +void Move::init(const moveit::core::RobotModelConstPtr& robot_model) { - Connecting::init(scene); - planner_ = Task::createPlanner(scene->getRobotModel()); + Connecting::init(robot_model); + planner_ = Task::createPlanner(robot_model); } void Move::setGroup(const std::string& group){ diff --git a/core/src/stages/move_relative.cpp b/core/src/stages/move_relative.cpp index 1f9c4996..62c1e1fe 100644 --- a/core/src/stages/move_relative.cpp +++ b/core/src/stages/move_relative.cpp @@ -90,10 +90,10 @@ void MoveRelative::along(const geometry_msgs::Vector3Stamped &direction) setProperty("direction", direction); } -void MoveRelative::init(const planning_scene::PlanningSceneConstPtr &scene) +void MoveRelative::init(const moveit::core::RobotModelConstPtr& robot_model) { - PropagatingEitherWay::init(scene); - planner_->init(scene->getRobotModel()); + PropagatingEitherWay::init(robot_model); + planner_->init(robot_model); } bool MoveRelative::compute(const InterfaceState &state, planning_scene::PlanningScenePtr& scene, diff --git a/core/src/stages/move_to.cpp b/core/src/stages/move_to.cpp index 5903b089..6b547605 100644 --- a/core/src/stages/move_to.cpp +++ b/core/src/stages/move_to.cpp @@ -79,10 +79,10 @@ void MoveTo::setGoal(const std::string &joint_pose) setProperty("joint_pose", joint_pose); } -void MoveTo::init(const planning_scene::PlanningSceneConstPtr &scene) +void MoveTo::init(const moveit::core::RobotModelConstPtr& robot_model) { - PropagatingEitherWay::init(scene); - planner_->init(scene->getRobotModel()); + PropagatingEitherWay::init(robot_model); + planner_->init(robot_model); } bool MoveTo::compute(const InterfaceState &state, planning_scene::PlanningScenePtr& scene, diff --git a/core/src/task.cpp b/core/src/task.cpp index 84920d66..484ecf64 100644 --- a/core/src/task.cpp +++ b/core/src/task.cpp @@ -43,8 +43,6 @@ #include -#include - #include #include @@ -55,7 +53,10 @@ namespace moveit { namespace task_constructor { Task::Task(const std::string& id, ContainerBase::pointer &&container) : WrapperBase(std::string(), std::move(container)), id_(id) { - initModel(); + robot_model_loader::RobotModelLoader rml; + robot_model_ = rml.getModel(); + if (!robot_model_) + throw Exception("Task failed to construct RobotModel"); // monitor state on commandline //addTaskCallback(std::bind(&Task::printState, this, std::ref(std::cout))); @@ -63,47 +64,6 @@ Task::Task(const std::string& id, ContainerBase::pointer &&container) enableIntrospection(true); } -void Task::initModel () { - rml_.reset(new robot_model_loader::RobotModelLoader); - if( !rml_->getModel() ) - throw Exception("Task failed to construct RobotModel"); -} - -const moveit::core::RobotModelPtr Task::getRobotModel() const { - return rml_ ? rml_->getModel() : moveit::core::RobotModelPtr(); -} - -planning_scene::PlanningScenePtr Task::initScene(ros::Duration timeout) { - assert(rml_); - - scene_ = std::make_shared(rml_->getModel()); - - ros::NodeHandle h; - ros::ServiceClient client = h.serviceClient("get_planning_scene"); - if (timeout != ros::Duration() && client.waitForExistence(timeout)) { - moveit_msgs::GetPlanningScene::Request req; - moveit_msgs::GetPlanningScene::Response res; - - req.components.components = - moveit_msgs::PlanningSceneComponents::SCENE_SETTINGS - | moveit_msgs::PlanningSceneComponents::ROBOT_STATE - | moveit_msgs::PlanningSceneComponents::ROBOT_STATE_ATTACHED_OBJECTS - | moveit_msgs::PlanningSceneComponents::WORLD_OBJECT_NAMES - | moveit_msgs::PlanningSceneComponents::WORLD_OBJECT_GEOMETRY - | moveit_msgs::PlanningSceneComponents::OCTOMAP - | moveit_msgs::PlanningSceneComponents::TRANSFORMS - | moveit_msgs::PlanningSceneComponents::ALLOWED_COLLISION_MATRIX - | moveit_msgs::PlanningSceneComponents::LINK_PADDING_AND_SCALING - | moveit_msgs::PlanningSceneComponents::OBJECT_COLORS; - - if (client.call(req, res)) - std::const_pointer_cast(scene_)->setPlanningSceneMsg(res.scene); - return scene_; - } - ROS_WARN("failed to acquire current PlanningScene, using empty one"); - return scene_; -} - planning_pipeline::PlanningPipelinePtr Task::createPlanner(const robot_model::RobotModelConstPtr& model, const std::string& ns, const std::string& planning_plugin_param_name, @@ -184,7 +144,7 @@ void Task::reset() WrapperBase::reset(); } -void Task::init(const planning_scene::PlanningSceneConstPtr &scene) +void Task::init(const moveit::core::RobotModelConstPtr& model) { auto impl = pimpl(); // initialize push connections of wrapped child @@ -193,7 +153,7 @@ void Task::init(const planning_scene::PlanningSceneConstPtr &scene) child->setNextStarts(impl->pendingForward()); // and *afterwards* initialize all children recursively - stages()->init(scene); + stages()->init(model); // task expects its wrapped child to push to both ends, this triggers interface resolution stages()->pimpl()->pruneInterface(InterfaceFlags({GENERATE})); // and *finally* validate connectivity @@ -223,8 +183,7 @@ bool Task::compute() bool Task::plan() { reset(); - if (!scene_) initScene(); - init(scene_); + init(robot_model_); while(ros::ok() && canCompute()) { if (compute()) { diff --git a/core/test/test_container.cpp b/core/test/test_container.cpp index 4a866af4..d72d7ec0 100644 --- a/core/test/test_container.cpp +++ b/core/test/test_container.cpp @@ -68,7 +68,7 @@ void append(ContainerBase& c, const std::initializer_list& types, int class SerialTest : public ::testing::Test { protected: - planning_scene::PlanningScenePtr scene; + moveit::core::RobotModelConstPtr robot_model; SerialContainer serial; InterfacePtr dummy; @@ -112,7 +112,7 @@ protected: reset(start, end); append(serial, types); try { - serial.init(scene); + serial.init(robot_model); // prune pull interfaces based on provided external interface (start, end) InterfaceFlags accepted; if (start) accepted |= WRITES_PREV_END; @@ -141,7 +141,7 @@ TEST_F(SerialTest, insertion_order) { SerialContainerPrivate *impl = serial.pimpl(); EXPECT_EQ(impl->parent(), nullptr); - EXPECT_THROW(serial.init(scene), InitStageException); + EXPECT_THROW(serial.init(robot_model), InitStageException); VALIDATE(); /***** inserting first stage *****/