mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
stub: plan ad infinitum and print after each iteration
This commit is contained in:
parent
6e5308740b
commit
e3604eab38
15
src/task.cpp
15
src/task.cpp
@ -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;
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user