expose Task::initScene()

... to allow use of local scene
This commit is contained in:
Robert Haschke 2018-02-06 09:05:00 +01:00
parent e9d0a05f75
commit 9bdfcc8c23
2 changed files with 33 additions and 25 deletions

View File

@ -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_;

View File

@ -68,34 +68,36 @@ 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;
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;
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");
if (client.call(req, res))
std::const_pointer_cast<planning_scene::PlanningScene>(scene_)->setPlanningSceneMsg(res.scene);
return scene_;
}
scene_ = std::make_shared<planning_scene::PlanningScene>(rml_->getModel());
std::const_pointer_cast<planning_scene::PlanningScene>(scene_)->setPlanningSceneMsg(res.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()) {