add a simple printState function to task

This commit is contained in:
v4hn 2017-03-01 16:12:16 +01:00
parent 63e1d4fd78
commit a1ef95300c
3 changed files with 17 additions and 2 deletions

View File

@ -30,6 +30,8 @@ public:
bool plan();
void printState();
protected:
void addSubTask( SubTaskPtr );

View File

@ -61,5 +61,8 @@ int main(int argc, char** argv){
*/
t.plan();
t.printState();
return 0;
}

View File

@ -52,15 +52,25 @@ void moveit::task_constructor::Task::addAfter( SubTaskPtr subtask ){
bool moveit::task_constructor::Task::plan(){
for( SubTaskPtr& subtask : subtasks_ ){
std::cout << "Computing subtask '" << subtask->getName() << "': ";
std::cout << "Computing subtask '" << subtask->getName() << "': " << std::endl;
bool success= subtask->compute();
std::cout << (success ? "succeeded" : "failed") << std::endl;
}
return false;
}
void moveit::task_constructor::Task::addSubTask( SubTaskPtr subtask ){
subtask->setPlanningScene( scene_ );
subtasks_.push_back( subtask );
}
void moveit::task_constructor::Task::printState(){
for( auto& st : subtasks_ ){
std::cout
<< st->getBegin().size() << " -> "
<< st->getTrajectories().size()
<< " <- " << st->getEnd().size()
<< " / " << st->getName()
<< std::endl;
}
}