From d801c4441c1bdde38c7c9cdb3aa176501a78e161 Mon Sep 17 00:00:00 2001 From: v4hn Date: Mon, 4 Sep 2017 00:52:49 +0200 Subject: [PATCH] simplify interface the currently-advertised version is a linear pipeline and the graph-based version would still need some reworking, so stick with simple "add". --- include/moveit_task_constructor/task.h | 7 +++---- src/demo/plan_pick_trixi.cpp | 14 +++++++------- src/demo/plan_pick_ur5.cpp | 14 +++++++------- src/task.cpp | 17 ++++++++--------- src/test/test_plan_cartesian_forward.cpp | 6 +++--- src/test/test_plan_current_state.cpp | 2 +- src/test/test_plan_generate_grasp_pose.cpp | 2 +- src/test/test_plan_gripper.cpp | 8 ++++---- 8 files changed, 34 insertions(+), 36 deletions(-) diff --git a/include/moveit_task_constructor/task.h b/include/moveit_task_constructor/task.h index 5ca91ea5..1e0f46e7 100644 --- a/include/moveit_task_constructor/task.h +++ b/include/moveit_task_constructor/task.h @@ -40,8 +40,7 @@ public: Task(); ~Task(); - void addStart( SubTaskPtr ); - void addAfter( SubTaskPtr ); + void add( SubTaskPtr ); bool plan(); @@ -51,9 +50,9 @@ public: void printState(); -protected: - void addSubTask( SubTaskPtr ); + void clear(); +protected: std::vector subtasks_; planning_scene::PlanningScenePtr scene_; diff --git a/src/demo/plan_pick_trixi.cpp b/src/demo/plan_pick_trixi.cpp index 30f4736f..e4b987a0 100644 --- a/src/demo/plan_pick_trixi.cpp +++ b/src/demo/plan_pick_trixi.cpp @@ -42,13 +42,13 @@ int main(int argc, char** argv){ Task t; - t.addStart( std::make_shared("current state") ); + t.add( std::make_shared("current state") ); { auto move= std::make_shared("open gripper"); move->setEndEffector("left_gripper"); move->setTo("open"); - t.addAfter(move); + t.add(move); } { @@ -57,7 +57,7 @@ int main(int argc, char** argv){ move->setLink("l_gripper_tool_frame"); move->setPlannerId("RRTConnectkConfigDefault"); move->setTimeout(8.0); - t.addAfter(move); + t.add(move); } { @@ -70,7 +70,7 @@ int main(int argc, char** argv){ geometry_msgs::PointStamped target; target.header.frame_id= "object"; move->towards(target); - t.addAfter(move); + t.add(move); } { @@ -82,7 +82,7 @@ int main(int argc, char** argv){ gengrasp->setObject("object"); gengrasp->setGraspOffset(.00); gengrasp->setAngleDelta(.2); - t.addAfter(gengrasp); + t.add(gengrasp); } { @@ -90,7 +90,7 @@ int main(int argc, char** argv){ move->setEndEffector("left_gripper"); move->setTo("closed"); move->graspObject("object"); - t.addAfter(move); + t.add(move); } { @@ -104,7 +104,7 @@ int main(int argc, char** argv){ direction.header.frame_id= "base_link"; direction.vector.z= 1.0; move->along(direction); - t.addAfter(move); + t.add(move); } t.plan(); diff --git a/src/demo/plan_pick_ur5.cpp b/src/demo/plan_pick_ur5.cpp index 4f3c0750..0302a6eb 100644 --- a/src/demo/plan_pick_ur5.cpp +++ b/src/demo/plan_pick_ur5.cpp @@ -40,13 +40,13 @@ int main(int argc, char** argv){ Task t; - t.addStart( std::make_shared("current state") ); + t.add( std::make_shared("current state") ); { auto move= std::make_shared("open gripper"); move->setEndEffector("gripper"); move->setTo("open"); - t.addAfter(move); + t.add(move); } { @@ -54,7 +54,7 @@ int main(int argc, char** argv){ move->setGroup("arm"); move->setPlannerId("RRTConnectkConfigDefault"); move->setTimeout(8.0); - t.addAfter(move); + t.add(move); } { @@ -67,7 +67,7 @@ int main(int argc, char** argv){ geometry_msgs::PointStamped target; target.header.frame_id= "object"; move->towards(target); - t.addAfter(move); + t.add(move); } { @@ -78,7 +78,7 @@ int main(int argc, char** argv){ gengrasp->setObject("object"); gengrasp->setGraspOffset(.03); gengrasp->setAngleDelta(.2); - t.addAfter(gengrasp); + t.add(gengrasp); } { @@ -86,7 +86,7 @@ int main(int argc, char** argv){ move->setEndEffector("gripper"); move->setTo("closed"); move->graspObject("object"); - t.addAfter(move); + t.add(move); } { @@ -100,7 +100,7 @@ int main(int argc, char** argv){ direction.header.frame_id= "world"; direction.vector.z= 1.0; move->along(direction); - t.addAfter(move); + t.add(move); } t.plan(); diff --git a/src/task.cpp b/src/task.cpp index 6170fd74..53ad62dc 100644 --- a/src/task.cpp +++ b/src/task.cpp @@ -54,15 +54,8 @@ moveit::task_constructor::Task::~Task(){ planner_.reset(); } -void moveit::task_constructor::Task::addStart( SubTaskPtr subtask ){ +void moveit::task_constructor::Task::clear(){ subtasks_.clear(); - addSubTask( subtask ); -} - -void moveit::task_constructor::Task::addAfter( SubTaskPtr subtask ){ - subtask->addPredecessor( subtasks_.back() ); - subtasks_.back()->addSuccessor( subtask ); - addSubTask( subtask ); } bool moveit::task_constructor::Task::plan(){ @@ -85,9 +78,15 @@ bool moveit::task_constructor::Task::plan(){ return false; } -void moveit::task_constructor::Task::addSubTask( SubTaskPtr subtask ){ +void moveit::task_constructor::Task::add( SubTaskPtr subtask ){ subtask->setPlanningScene( scene_ ); subtask->setPlanningPipeline( planner_ ); + + if( !subtasks_.empty() ){ + subtask->addPredecessor( subtasks_.back() ); + subtasks_.back()->addSuccessor( subtask ); + } + subtasks_.push_back( subtask ); } diff --git a/src/test/test_plan_cartesian_forward.cpp b/src/test/test_plan_cartesian_forward.cpp index bb4d1420..29879f16 100644 --- a/src/test/test_plan_cartesian_forward.cpp +++ b/src/test/test_plan_cartesian_forward.cpp @@ -12,7 +12,7 @@ int main(int argc, char** argv){ Task t; - t.addStart( std::make_shared("current state") ); + t.add( std::make_shared("current state") ); { auto move= std::make_shared("lift object"); @@ -25,7 +25,7 @@ int main(int argc, char** argv){ direction.header.frame_id= "world"; direction.vector.x= 1.0; move->along(direction); - t.addAfter(move); + t.add(move); } { @@ -40,7 +40,7 @@ int main(int argc, char** argv){ target.point.y= 0.7; target.point.z= 1.5; move->towards(target); - t.addAfter(move); + t.add(move); } t.plan(); diff --git a/src/test/test_plan_current_state.cpp b/src/test/test_plan_current_state.cpp index 2c34820f..067b722a 100644 --- a/src/test/test_plan_current_state.cpp +++ b/src/test/test_plan_current_state.cpp @@ -11,7 +11,7 @@ int main(int argc, char** argv){ Task t; - t.addStart( std::make_shared("current state") ); + t.add( std::make_shared("current state") ); t.plan(); diff --git a/src/test/test_plan_generate_grasp_pose.cpp b/src/test/test_plan_generate_grasp_pose.cpp index f06d9a1a..77b448d6 100644 --- a/src/test/test_plan_generate_grasp_pose.cpp +++ b/src/test/test_plan_generate_grasp_pose.cpp @@ -45,7 +45,7 @@ int main(int argc, char** argv){ st->setAngleDelta(0.1); st->setGraspOffset(0.03); - t.addStart(st); + t.add(st); t.plan(); diff --git a/src/test/test_plan_gripper.cpp b/src/test/test_plan_gripper.cpp index 87a047ea..ac9b4735 100644 --- a/src/test/test_plan_gripper.cpp +++ b/src/test/test_plan_gripper.cpp @@ -14,12 +14,12 @@ int main(int argc, char** argv){ Task t; - t.addStart( std::make_shared("current state") ); + t.add( std::make_shared("current state") ); { auto gripper= std::make_shared("close gripper"); gripper->setEndEffector("gripper"); gripper->setTo("closed"); - t.addAfter( gripper ); + t.add( gripper ); } t.plan(); @@ -29,9 +29,9 @@ int main(int argc, char** argv){ auto gripper= std::make_shared("close gripper"); gripper->setEndEffector("gripper"); gripper->setTo("closed"); - t.addStart( gripper ); + t.add( gripper ); } - t.addAfter( std::make_shared("current state") ); + t.add( std::make_shared("current state") ); t.plan(); */ return 0;