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,11 +55,17 @@ void moveit::task_constructor::Task::addAfter( SubTaskPtr subtask ){
} }
bool moveit::task_constructor::Task::plan(){ bool moveit::task_constructor::Task::plan(){
while(ros::ok()){
for( SubTaskPtr& subtask : subtasks_ ){ for( SubTaskPtr& subtask : subtasks_ ){
if( !subtask->canCompute() )
continue;
std::cout << "Computing subtask '" << subtask->getName() << "': " << std::endl; std::cout << "Computing subtask '" << subtask->getName() << "': " << std::endl;
bool success= subtask->compute(); bool success= subtask->compute();
std::cout << (success ? "succeeded" : "failed") << std::endl; std::cout << (success ? "succeeded" : "failed") << std::endl;
} }
printState();
ros::Duration(0.5).sleep();
}
return false; return false;
} }