mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
90 lines
2.7 KiB
C++
90 lines
2.7 KiB
C++
#include <moveit_task_constructor/task.h>
|
|
#include <moveit_task_constructor/subtask.h>
|
|
|
|
#include <ros/ros.h>
|
|
|
|
#include <moveit_msgs/GetPlanningScene.h>
|
|
#include <moveit/robot_model_loader/robot_model_loader.h>
|
|
|
|
moveit::task_constructor::Task::Task(){
|
|
rml_.reset(new robot_model_loader::RobotModelLoader);
|
|
if( !rml_->getModel() )
|
|
throw Exception("Task failed to construct RobotModel");
|
|
|
|
ros::NodeHandle h;
|
|
ros::ServiceClient client = h.serviceClient<moveit_msgs::GetPlanningScene>("get_planning_scene");
|
|
client.waitForExistence();
|
|
|
|
moveit_msgs::GetPlanningScene::Request req;
|
|
moveit_msgs::GetPlanningScene::Response res;
|
|
|
|
req.components.components =
|
|
moveit_msgs::PlanningSceneComponents::SCENE_SETTINGS
|
|
| moveit_msgs::PlanningSceneComponents::ROBOT_STATE
|
|
| moveit_msgs::PlanningSceneComponents::ROBOT_STATE_ATTACHED_OBJECTS
|
|
| moveit_msgs::PlanningSceneComponents::WORLD_OBJECT_NAMES
|
|
| moveit_msgs::PlanningSceneComponents::WORLD_OBJECT_GEOMETRY
|
|
| moveit_msgs::PlanningSceneComponents::OCTOMAP
|
|
| moveit_msgs::PlanningSceneComponents::TRANSFORMS
|
|
| moveit_msgs::PlanningSceneComponents::ALLOWED_COLLISION_MATRIX
|
|
| moveit_msgs::PlanningSceneComponents::LINK_PADDING_AND_SCALING
|
|
| moveit_msgs::PlanningSceneComponents::OBJECT_COLORS;
|
|
|
|
if(!client.call(req, res)){
|
|
throw Exception("Task failed to aquire current PlanningScene");
|
|
}
|
|
|
|
scene_.reset(new planning_scene::PlanningScene(rml_->getModel()));
|
|
scene_->setPlanningSceneMsg(res.scene);
|
|
}
|
|
|
|
moveit::task_constructor::Task::~Task(){
|
|
subtasks_.clear();
|
|
scene_.reset();
|
|
}
|
|
|
|
void moveit::task_constructor::Task::addStart( SubTaskPtr subtask ){
|
|
subtasks_.clear();
|
|
addSubTask( subtask );
|
|
}
|
|
|
|
void moveit::task_constructor::Task::addAfter( SubTaskPtr subtask ){
|
|
subtask->addPredecessor( subtasks_.back() );
|
|
subtasks_.back()->addSuccessor( subtask );
|
|
addSubTask( subtask );
|
|
}
|
|
|
|
bool moveit::task_constructor::Task::plan(){
|
|
bool computed= true;
|
|
while(ros::ok() && computed){
|
|
computed= false;
|
|
for( SubTaskPtr& subtask : subtasks_ ){
|
|
if( !subtask->canCompute() )
|
|
continue;
|
|
std::cout << "Computing subtask '" << subtask->getName() << "': " << std::endl;
|
|
bool success= subtask->compute();
|
|
computed= true;
|
|
std::cout << (success ? "succeeded" : "failed") << std::endl;
|
|
}
|
|
printState();
|
|
ros::Duration(0.5).sleep();
|
|
}
|
|
return false;
|
|
}
|
|
|
|
void moveit::task_constructor::Task::addSubTask( SubTaskPtr subtask ){
|
|
subtask->setPlanningScene( scene_ );
|
|
subtasks_.push_back( subtask );
|
|
}
|
|
|
|
void moveit::task_constructor::Task::printState(){
|
|
for( auto& st : subtasks_ ){
|
|
std::cout
|
|
<< st->getBegin().size() << " -> "
|
|
<< st->getTrajectories().size()
|
|
<< " <- " << st->getEnd().size()
|
|
<< " / " << st->getName()
|
|
<< std::endl;
|
|
}
|
|
}
|