#include "container_p.h" #include #include #include #include #include #include #include namespace moveit { namespace task_constructor { class TaskPrivate : public SerialContainerPrivate { friend class Task; robot_model_loader::RobotModelLoaderPtr rml_; public: TaskPrivate(Task* me, const std::string &name) : SerialContainerPrivate(me, name) { initModel(); initScene(); initPlanner(); } void initModel () { rml_.reset(new robot_model_loader::RobotModelLoader); if( !rml_->getModel() ) throw Exception("Task failed to construct RobotModel"); } void initScene() { assert(rml_); ros::NodeHandle h; ros::ServiceClient client = h.serviceClient("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 acquire current PlanningScene"); } scene_ = std::make_shared(rml_->getModel()); std::const_pointer_cast(scene_)->setPlanningSceneMsg(res.scene); } void initPlanner() { assert(rml_); assert(scene_); planner_ = std::make_shared(rml_->getModel(), ros::NodeHandle("move_group")); } }; Task::Task(const std::string &name) : SerialContainer(new TaskPrivate(this, name)) { } void Task::add(pointer &&stage) { if (!stage) throw std::runtime_error("Task::add() failed: invalid stage pointer"); if (!insert(std::move(stage))) throw std::runtime_error(std::string("Task::add() failed for stage: ") + stage->getName()); } bool Task::plan(){ NewSolutionPublisher debug(*this); while(ros::ok() && canCompute()) { if (compute()) { debug.publish(); printState(); } else break; } return false; } const robot_state::RobotState& Task::getCurrentRobotState() const { return pimpl_func()->scene_->getCurrentState(); } void Task::printState(){ ContainerBase::StageCallback processor = [](const SubTask& stage, int depth) -> bool { std::cout << std::string(2*depth, ' ') << stage << std::endl; }; traverseStages(processor); } namespace { bool traverseFullTrajectories( SubTrajectory& start, int nr_of_trajectories, const Task::SolutionCallback& cb, std::vector& trace) { bool ret= true; trace.push_back(&start); if(nr_of_trajectories == 1){ ret= cb(trace); } else if( start.end ){ for( SubTrajectory* successor : start.end->outgoingTrajectories() ){ if( !traverseFullTrajectories(*successor, nr_of_trajectories-1, cb, trace) ){ ret= false; break; } } } trace.pop_back(); return ret; } } bool Task::processSolutions(const Task::SolutionCallback& processor) { IMPL(Task); const TaskPrivate::array_type& children = impl->children(); const size_t nr_of_trajectories = children.size(); std::vector trace; trace.reserve(nr_of_trajectories); for(SubTrajectory& st : children.front()->pimpl_func()->trajectories_) if( !traverseFullTrajectories(st, nr_of_trajectories, processor, trace) ) return false; return true; } bool Task::processSolutions(const Task::SolutionCallback& processor) const { return const_cast(this)->processSolutions(processor); } } }