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
|
/// remove function callback
|
||||||
void erase(TaskCallbackList::const_iterator which);
|
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;
|
void reset() override;
|
||||||
|
/// initialize all stages with given scene
|
||||||
void init(const planning_scene::PlanningSceneConstPtr &scene) override;
|
void init(const planning_scene::PlanningSceneConstPtr &scene) override;
|
||||||
|
|
||||||
|
/// reset, init scene (if not yet done), and init all stages, then start planning
|
||||||
bool plan();
|
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);
|
static void printState(const Task &t);
|
||||||
|
|
||||||
size_t numSolutions() const override;
|
size_t numSolutions() const override;
|
||||||
@ -117,7 +124,6 @@ public:
|
|||||||
|
|
||||||
protected:
|
protected:
|
||||||
void initModel();
|
void initModel();
|
||||||
void initScene();
|
|
||||||
bool canCompute() const override;
|
bool canCompute() const override;
|
||||||
bool compute() override;
|
bool compute() override;
|
||||||
void onNewSolution(const SolutionBase &s) override;
|
void onNewSolution(const SolutionBase &s) override;
|
||||||
@ -125,7 +131,7 @@ protected:
|
|||||||
private:
|
private:
|
||||||
std::string id_;
|
std::string id_;
|
||||||
robot_model_loader::RobotModelLoaderPtr rml_;
|
robot_model_loader::RobotModelLoaderPtr rml_;
|
||||||
planning_scene::PlanningSceneConstPtr scene_; // initial scene
|
planning_scene::PlanningScenePtr scene_; // initial scene
|
||||||
|
|
||||||
// introspection and monitoring
|
// introspection and monitoring
|
||||||
std::unique_ptr<Introspection> introspection_;
|
std::unique_ptr<Introspection> introspection_;
|
||||||
|
|||||||
@ -68,34 +68,36 @@ void Task::initModel () {
|
|||||||
if( !rml_->getModel() )
|
if( !rml_->getModel() )
|
||||||
throw Exception("Task failed to construct RobotModel");
|
throw Exception("Task failed to construct RobotModel");
|
||||||
}
|
}
|
||||||
void Task::initScene() {
|
|
||||||
|
planning_scene::PlanningScenePtr Task::initScene(ros::Duration timeout) {
|
||||||
assert(rml_);
|
assert(rml_);
|
||||||
|
|
||||||
|
scene_ = std::make_shared<planning_scene::PlanningScene>(rml_->getModel());
|
||||||
|
|
||||||
ros::NodeHandle h;
|
ros::NodeHandle h;
|
||||||
ros::ServiceClient client = h.serviceClient<moveit_msgs::GetPlanningScene>("get_planning_scene");
|
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;
|
req.components.components =
|
||||||
moveit_msgs::GetPlanningScene::Response res;
|
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 =
|
if (client.call(req, res))
|
||||||
moveit_msgs::PlanningSceneComponents::SCENE_SETTINGS
|
std::const_pointer_cast<planning_scene::PlanningScene>(scene_)->setPlanningSceneMsg(res.scene);
|
||||||
| moveit_msgs::PlanningSceneComponents::ROBOT_STATE
|
return scene_;
|
||||||
| 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");
|
|
||||||
}
|
}
|
||||||
|
ROS_WARN("failed to acquire current PlanningScene, using empty one");
|
||||||
scene_ = std::make_shared<planning_scene::PlanningScene>(rml_->getModel());
|
return scene_;
|
||||||
std::const_pointer_cast<planning_scene::PlanningScene>(scene_)->setPlanningSceneMsg(res.scene);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
planning_pipeline::PlanningPipelinePtr
|
planning_pipeline::PlanningPipelinePtr
|
||||||
@ -205,7 +207,7 @@ bool Task::compute()
|
|||||||
bool Task::plan()
|
bool Task::plan()
|
||||||
{
|
{
|
||||||
reset();
|
reset();
|
||||||
initScene();
|
if (!scene_) initScene();
|
||||||
init(scene_);
|
init(scene_);
|
||||||
|
|
||||||
while(ros::ok() && canCompute()) {
|
while(ros::ok() && canCompute()) {
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user