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".
This commit is contained in:
v4hn 2017-09-04 00:52:49 +02:00
parent 491098e0c3
commit d801c4441c
8 changed files with 34 additions and 36 deletions

View File

@ -40,8 +40,7 @@ public:
Task(); Task();
~Task(); ~Task();
void addStart( SubTaskPtr ); void add( SubTaskPtr );
void addAfter( SubTaskPtr );
bool plan(); bool plan();
@ -51,9 +50,9 @@ public:
void printState(); void printState();
protected: void clear();
void addSubTask( SubTaskPtr );
protected:
std::vector<SubTaskPtr> subtasks_; std::vector<SubTaskPtr> subtasks_;
planning_scene::PlanningScenePtr scene_; planning_scene::PlanningScenePtr scene_;

View File

@ -42,13 +42,13 @@ int main(int argc, char** argv){
Task t; Task t;
t.addStart( std::make_shared<subtasks::CurrentState>("current state") ); t.add( std::make_shared<subtasks::CurrentState>("current state") );
{ {
auto move= std::make_shared<subtasks::Gripper>("open gripper"); auto move= std::make_shared<subtasks::Gripper>("open gripper");
move->setEndEffector("left_gripper"); move->setEndEffector("left_gripper");
move->setTo("open"); 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->setLink("l_gripper_tool_frame");
move->setPlannerId("RRTConnectkConfigDefault"); move->setPlannerId("RRTConnectkConfigDefault");
move->setTimeout(8.0); move->setTimeout(8.0);
t.addAfter(move); t.add(move);
} }
{ {
@ -70,7 +70,7 @@ int main(int argc, char** argv){
geometry_msgs::PointStamped target; geometry_msgs::PointStamped target;
target.header.frame_id= "object"; target.header.frame_id= "object";
move->towards(target); move->towards(target);
t.addAfter(move); t.add(move);
} }
{ {
@ -82,7 +82,7 @@ int main(int argc, char** argv){
gengrasp->setObject("object"); gengrasp->setObject("object");
gengrasp->setGraspOffset(.00); gengrasp->setGraspOffset(.00);
gengrasp->setAngleDelta(.2); gengrasp->setAngleDelta(.2);
t.addAfter(gengrasp); t.add(gengrasp);
} }
{ {
@ -90,7 +90,7 @@ int main(int argc, char** argv){
move->setEndEffector("left_gripper"); move->setEndEffector("left_gripper");
move->setTo("closed"); move->setTo("closed");
move->graspObject("object"); 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.header.frame_id= "base_link";
direction.vector.z= 1.0; direction.vector.z= 1.0;
move->along(direction); move->along(direction);
t.addAfter(move); t.add(move);
} }
t.plan(); t.plan();

View File

@ -40,13 +40,13 @@ int main(int argc, char** argv){
Task t; Task t;
t.addStart( std::make_shared<subtasks::CurrentState>("current state") ); t.add( std::make_shared<subtasks::CurrentState>("current state") );
{ {
auto move= std::make_shared<subtasks::Gripper>("open gripper"); auto move= std::make_shared<subtasks::Gripper>("open gripper");
move->setEndEffector("gripper"); move->setEndEffector("gripper");
move->setTo("open"); move->setTo("open");
t.addAfter(move); t.add(move);
} }
{ {
@ -54,7 +54,7 @@ int main(int argc, char** argv){
move->setGroup("arm"); move->setGroup("arm");
move->setPlannerId("RRTConnectkConfigDefault"); move->setPlannerId("RRTConnectkConfigDefault");
move->setTimeout(8.0); move->setTimeout(8.0);
t.addAfter(move); t.add(move);
} }
{ {
@ -67,7 +67,7 @@ int main(int argc, char** argv){
geometry_msgs::PointStamped target; geometry_msgs::PointStamped target;
target.header.frame_id= "object"; target.header.frame_id= "object";
move->towards(target); move->towards(target);
t.addAfter(move); t.add(move);
} }
{ {
@ -78,7 +78,7 @@ int main(int argc, char** argv){
gengrasp->setObject("object"); gengrasp->setObject("object");
gengrasp->setGraspOffset(.03); gengrasp->setGraspOffset(.03);
gengrasp->setAngleDelta(.2); gengrasp->setAngleDelta(.2);
t.addAfter(gengrasp); t.add(gengrasp);
} }
{ {
@ -86,7 +86,7 @@ int main(int argc, char** argv){
move->setEndEffector("gripper"); move->setEndEffector("gripper");
move->setTo("closed"); move->setTo("closed");
move->graspObject("object"); 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.header.frame_id= "world";
direction.vector.z= 1.0; direction.vector.z= 1.0;
move->along(direction); move->along(direction);
t.addAfter(move); t.add(move);
} }
t.plan(); t.plan();

View File

@ -54,15 +54,8 @@ moveit::task_constructor::Task::~Task(){
planner_.reset(); planner_.reset();
} }
void moveit::task_constructor::Task::addStart( SubTaskPtr subtask ){ void moveit::task_constructor::Task::clear(){
subtasks_.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(){ bool moveit::task_constructor::Task::plan(){
@ -85,9 +78,15 @@ bool moveit::task_constructor::Task::plan(){
return false; return false;
} }
void moveit::task_constructor::Task::addSubTask( SubTaskPtr subtask ){ void moveit::task_constructor::Task::add( SubTaskPtr subtask ){
subtask->setPlanningScene( scene_ ); subtask->setPlanningScene( scene_ );
subtask->setPlanningPipeline( planner_ ); subtask->setPlanningPipeline( planner_ );
if( !subtasks_.empty() ){
subtask->addPredecessor( subtasks_.back() );
subtasks_.back()->addSuccessor( subtask );
}
subtasks_.push_back( subtask ); subtasks_.push_back( subtask );
} }

View File

@ -12,7 +12,7 @@ int main(int argc, char** argv){
Task t; Task t;
t.addStart( std::make_shared<subtasks::CurrentState>("current state") ); t.add( std::make_shared<subtasks::CurrentState>("current state") );
{ {
auto move= std::make_shared<subtasks::CartesianPositionMotion>("lift object"); auto move= std::make_shared<subtasks::CartesianPositionMotion>("lift object");
@ -25,7 +25,7 @@ int main(int argc, char** argv){
direction.header.frame_id= "world"; direction.header.frame_id= "world";
direction.vector.x= 1.0; direction.vector.x= 1.0;
move->along(direction); 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.y= 0.7;
target.point.z= 1.5; target.point.z= 1.5;
move->towards(target); move->towards(target);
t.addAfter(move); t.add(move);
} }
t.plan(); t.plan();

View File

@ -11,7 +11,7 @@ int main(int argc, char** argv){
Task t; Task t;
t.addStart( std::make_shared<subtasks::CurrentState>("current state") ); t.add( std::make_shared<subtasks::CurrentState>("current state") );
t.plan(); t.plan();

View File

@ -45,7 +45,7 @@ int main(int argc, char** argv){
st->setAngleDelta(0.1); st->setAngleDelta(0.1);
st->setGraspOffset(0.03); st->setGraspOffset(0.03);
t.addStart(st); t.add(st);
t.plan(); t.plan();

View File

@ -14,12 +14,12 @@ int main(int argc, char** argv){
Task t; Task t;
t.addStart( std::make_shared<subtasks::CurrentState>("current state") ); t.add( std::make_shared<subtasks::CurrentState>("current state") );
{ {
auto gripper= std::make_shared<subtasks::Gripper>("close gripper"); auto gripper= std::make_shared<subtasks::Gripper>("close gripper");
gripper->setEndEffector("gripper"); gripper->setEndEffector("gripper");
gripper->setTo("closed"); gripper->setTo("closed");
t.addAfter( gripper ); t.add( gripper );
} }
t.plan(); t.plan();
@ -29,9 +29,9 @@ int main(int argc, char** argv){
auto gripper= std::make_shared<subtasks::Gripper>("close gripper"); auto gripper= std::make_shared<subtasks::Gripper>("close gripper");
gripper->setEndEffector("gripper"); gripper->setEndEffector("gripper");
gripper->setTo("closed"); gripper->setTo("closed");
t.addStart( gripper ); t.add( gripper );
} }
t.addAfter( std::make_shared<subtasks::CurrentState>("current state") ); t.add( std::make_shared<subtasks::CurrentState>("current state") );
t.plan(); t.plan();
*/ */
return 0; return 0;