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();
}
void moveit::task_constructor::Task::addStart( SubTaskPtr subtask ){
subtasks_.clear();
addSubTask( subtask );
@ -56,10 +55,16 @@ 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::endl;
bool success= subtask->compute();
std::cout << (success ? "succeeded" : "failed") << std::endl;
while(ros::ok()){
for( SubTaskPtr& subtask : subtasks_ ){
if( !subtask->canCompute() )
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;
}