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();
|
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;
|
||||||
}
|
}
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user