mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
expose Task::initScene()
... to allow use of local scene
This commit is contained in:
parent
e9d0a05f75
commit
9bdfcc8c23
@ -91,11 +91,18 @@ public:
|
||||
/// remove function callback
|
||||
void erase(TaskCallbackList::const_iterator which);
|
||||
|
||||
/// initialize planning scene from get_planning_scene service (waiting given timeout for it)
|
||||
/// if service is not available or timeout is zero, use an empty planning scene
|
||||
planning_scene::PlanningScenePtr initScene(ros::Duration timeout = ros::Duration(-1));
|
||||
|
||||
/// reset all stages
|
||||
void reset() override;
|
||||
/// initialize all stages with given scene
|
||||
void init(const planning_scene::PlanningSceneConstPtr &scene) override;
|
||||
|
||||
/// reset, init scene (if not yet done), and init all stages, then start planning
|
||||
bool plan();
|
||||
/// print current state std::cout
|
||||
/// print current task state (number of found solutions and propagated states) to std::cout
|
||||
static void printState(const Task &t);
|
||||
|
||||
size_t numSolutions() const override;
|
||||
@ -117,7 +124,6 @@ public:
|
||||
|
||||
protected:
|
||||
void initModel();
|
||||
void initScene();
|
||||
bool canCompute() const override;
|
||||
bool compute() override;
|
||||
void onNewSolution(const SolutionBase &s) override;
|
||||
@ -125,7 +131,7 @@ protected:
|
||||
private:
|
||||
std::string id_;
|
||||
robot_model_loader::RobotModelLoaderPtr rml_;
|
||||
planning_scene::PlanningSceneConstPtr scene_; // initial scene
|
||||
planning_scene::PlanningScenePtr scene_; // initial scene
|
||||
|
||||
// introspection and monitoring
|
||||
std::unique_ptr<Introspection> introspection_;
|
||||
|
||||
@ -68,13 +68,15 @@ void Task::initModel () {
|
||||
if( !rml_->getModel() )
|
||||
throw Exception("Task failed to construct RobotModel");
|
||||
}
|
||||
void Task::initScene() {
|
||||
|
||||
planning_scene::PlanningScenePtr Task::initScene(ros::Duration timeout) {
|
||||
assert(rml_);
|
||||
|
||||
scene_ = std::make_shared<planning_scene::PlanningScene>(rml_->getModel());
|
||||
|
||||
ros::NodeHandle h;
|
||||
ros::ServiceClient client = h.serviceClient<moveit_msgs::GetPlanningScene>("get_planning_scene");
|
||||
client.waitForExistence();
|
||||
|
||||
if (timeout != ros::Duration() && client.waitForExistence(timeout)) {
|
||||
moveit_msgs::GetPlanningScene::Request req;
|
||||
moveit_msgs::GetPlanningScene::Response res;
|
||||
|
||||
@ -90,12 +92,12 @@ void Task::initScene() {
|
||||
| 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<planning_scene::PlanningScene>(rml_->getModel());
|
||||
if (client.call(req, res))
|
||||
std::const_pointer_cast<planning_scene::PlanningScene>(scene_)->setPlanningSceneMsg(res.scene);
|
||||
return scene_;
|
||||
}
|
||||
ROS_WARN("failed to acquire current PlanningScene, using empty one");
|
||||
return scene_;
|
||||
}
|
||||
|
||||
planning_pipeline::PlanningPipelinePtr
|
||||
@ -205,7 +207,7 @@ bool Task::compute()
|
||||
bool Task::plan()
|
||||
{
|
||||
reset();
|
||||
initScene();
|
||||
if (!scene_) initScene();
|
||||
init(scene_);
|
||||
|
||||
while(ros::ok() && canCompute()) {
|
||||
|
||||
Loading…
Reference in New Issue
Block a user