From bd60e6f65d39c0c7b0cec5a3cc16d3bd0573c08f Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 6 Apr 2018 10:55:48 +0200 Subject: [PATCH] Task::setRobotModel() / Task::loadRobotModel() --- core/demo/plan_pick_pa10.cpp | 1 + core/include/moveit/task_constructor/task.h | 6 ++++- core/src/task.cpp | 27 +++++++++++++++------ 3 files changed, 25 insertions(+), 9 deletions(-) diff --git a/core/demo/plan_pick_pa10.cpp b/core/demo/plan_pick_pa10.cpp index 3a570cc3..dea25768 100644 --- a/core/demo/plan_pick_pa10.cpp +++ b/core/demo/plan_pick_pa10.cpp @@ -41,6 +41,7 @@ int main(int argc, char** argv){ spinner.start(); Task t; + t.loadRobotModel(); // define global properties used by most stages t.setProperty("group", std::string("left_arm")); t.setProperty("eef", std::string("la_tool_mount")); diff --git a/core/include/moveit/task_constructor/task.h b/core/include/moveit/task_constructor/task.h index 653e7c71..9642230e 100644 --- a/core/include/moveit/task_constructor/task.h +++ b/core/include/moveit/task_constructor/task.h @@ -74,6 +74,10 @@ public: std::string id() const; const moveit::core::RobotModelConstPtr getRobotModel() const { return robot_model_; } + /// setting the robot model also resets the task + void setRobotModel(const moveit::core::RobotModelConstPtr& robot_model); + /// load robot model from given parameter + void loadRobotModel(const std::string& robot_description = "robot_description"); void add(Stage::pointer &&stage); void clear() override; @@ -92,7 +96,7 @@ public: /// reset all stages void reset() override; /// initialize all stages with given scene - void init(const moveit::core::RobotModelConstPtr& model) override; + void init(); /// reset, init scene (if not yet done), and init all stages, then start planning bool plan(); diff --git a/core/src/task.cpp b/core/src/task.cpp index 484ecf64..a8ada821 100644 --- a/core/src/task.cpp +++ b/core/src/task.cpp @@ -53,11 +53,6 @@ namespace moveit { namespace task_constructor { Task::Task(const std::string& id, ContainerBase::pointer &&container) : WrapperBase(std::string(), std::move(container)), id_(id) { - 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))); // enable introspection by default @@ -90,6 +85,19 @@ Task::~Task() reset(); } +void Task::setRobotModel(const core::RobotModelConstPtr& robot_model) +{ + reset(); + robot_model_ = robot_model; +} + +void Task::loadRobotModel(const std::string& robot_description) { + robot_model_loader::RobotModelLoader rml(robot_description); + setRobotModel(rml.getModel()); + if (!robot_model_) + throw Exception("Task failed to construct RobotModel"); +} + void Task::add(pointer &&stage) { if (!stage) throw std::runtime_error("stage insertion failed: invalid stage pointer"); @@ -144,8 +152,11 @@ void Task::reset() WrapperBase::reset(); } -void Task::init(const moveit::core::RobotModelConstPtr& model) +void Task::init() { + if (!robot_model_) + loadRobotModel(); + auto impl = pimpl(); // initialize push connections of wrapped child StagePrivate *child = wrapped()->pimpl(); @@ -153,7 +164,7 @@ void Task::init(const moveit::core::RobotModelConstPtr& model) child->setNextStarts(impl->pendingForward()); // and *afterwards* initialize all children recursively - stages()->init(model); + stages()->init(robot_model_); // task expects its wrapped child to push to both ends, this triggers interface resolution stages()->pimpl()->pruneInterface(InterfaceFlags({GENERATE})); // and *finally* validate connectivity @@ -183,7 +194,7 @@ bool Task::compute() bool Task::plan() { reset(); - init(robot_model_); + init(); while(ros::ok() && canCompute()) { if (compute()) {