stub: plan ad infinitum and print after each iteration

This commit is contained in:
v4hn 2017-03-02 13:37:28 +01:00
parent 6e5308740b
commit e3604eab38

View File

@ -43,7 +43,6 @@ moveit::task_constructor::Task::~Task(){
scene_.reset(); scene_.reset();
} }
void moveit::task_constructor::Task::addStart( SubTaskPtr subtask ){ void moveit::task_constructor::Task::addStart( SubTaskPtr subtask ){
subtasks_.clear(); subtasks_.clear();
addSubTask( subtask ); addSubTask( subtask );
@ -56,10 +55,16 @@ void moveit::task_constructor::Task::addAfter( SubTaskPtr subtask ){
} }
bool moveit::task_constructor::Task::plan(){ bool moveit::task_constructor::Task::plan(){
for( SubTaskPtr& subtask : subtasks_ ){ while(ros::ok()){
std::cout << "Computing subtask '" << subtask->getName() << "': " << std::endl; for( SubTaskPtr& subtask : subtasks_ ){
bool success= subtask->compute(); if( !subtask->canCompute() )
std::cout << (success ? "succeeded" : "failed") << std::endl; continue;
std::cout << "Computing subtask '" << subtask->getName() << "': " << std::endl;
bool success= subtask->compute();
std::cout << (success ? "succeeded" : "failed") << std::endl;
}
printState();
ros::Duration(0.5).sleep();
} }
return false; return false;
} }