moveit_task_constructor/core/src/task.cpp
Robert Haschke 381edf2d22 ModifyPlanningScene stage
- attach / detach objects to robot
- enable / disable collision pairs
- works in either direction (FORWARD +  BACKWARD)
2018-02-12 23:40:33 +01:00

288 lines
8.2 KiB
C++

/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2017, Hamburg University
* Copyright (c) 2017, Bielefeld University
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Bielefeld University nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Authors: Michael Goerner, Robert Haschke */
#include <moveit/task_constructor/container_p.h>
#include <moveit/task_constructor/task.h>
#include <moveit/task_constructor/container.h>
#include <moveit/task_constructor/introspection.h>
#include <ros/ros.h>
#include <moveit_msgs/GetPlanningScene.h>
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/planning_pipeline/planning_pipeline.h>
#include <functional>
namespace moveit { namespace task_constructor {
Task::Task(const std::string& id, ContainerBase::pointer &&container)
: WrapperBase(std::string(), std::move(container)), id_(id)
{
initModel();
// monitor state on commandline
addTaskCallback(&printState);
// enable introspection by default
enableIntrospection(true);
}
void Task::initModel () {
rml_.reset(new robot_model_loader::RobotModelLoader);
if( !rml_->getModel() )
throw Exception("Task failed to construct RobotModel");
}
const moveit::core::RobotModelPtr Task::getRobotModel() const {
return rml_ ? rml_->getModel() : moveit::core::RobotModelPtr();
}
void Task::initScene() {
assert(rml_);
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 acquire current PlanningScene");
}
scene_ = std::make_shared<planning_scene::PlanningScene>(rml_->getModel());
std::const_pointer_cast<planning_scene::PlanningScene>(scene_)->setPlanningSceneMsg(res.scene);
}
planning_pipeline::PlanningPipelinePtr
Task::createPlanner(const robot_model::RobotModelConstPtr& model, const std::string& ns,
const std::string& planning_plugin_param_name,
const std::string& adapter_plugins_param_name) {
typedef std::tuple<std::string, std::string, std::string, std::string> PlannerID;
static std::map<PlannerID, std::weak_ptr<planning_pipeline::PlanningPipeline> > planner_cache;
PlannerID id (model->getName(), ns, planning_plugin_param_name, adapter_plugins_param_name);
auto it = planner_cache.find(id);
planning_pipeline::PlanningPipelinePtr planner;
if (it != planner_cache.cend())
planner = it->second.lock();
if (!planner) {
planner = std::make_shared<planning_pipeline::PlanningPipeline>
(model, ros::NodeHandle(ns), planning_plugin_param_name, adapter_plugins_param_name);
planner_cache[id] = planner;
}
return planner;
}
Task::~Task()
{
reset();
}
void Task::add(pointer &&stage) {
if (!stage)
throw std::runtime_error("stage insertion failed: invalid stage pointer");
if (!stages()->insert(std::move(stage)))
throw std::runtime_error(std::string("insertion failed for stage: ") + stage->name());
}
void Task::clear()
{
stages()->clear();
}
void Task::enableIntrospection(bool enable)
{
if (enable && !introspection_)
introspection_.reset(new Introspection(*this));
else if (!enable && introspection_) {
// reset introspection instance of all stages
pimpl()->setIntrospection(nullptr);
pimpl()->traverseStages([this](Stage& stage, int) {
stage.pimpl()->setIntrospection(nullptr);
return true;
}, 1, UINT_MAX);
introspection_.reset();
}
}
Introspection &Task::introspection()
{
enableIntrospection(true);
return *introspection_;
}
Task::TaskCallbackList::const_iterator Task::addTaskCallback(TaskCallback &&cb)
{
task_cbs_.emplace_back(std::move(cb));
return --task_cbs_.cend();
}
void Task::erase(TaskCallbackList::const_iterator which)
{
task_cbs_.erase(which);
}
void Task::reset()
{
// signal introspection, that this task was reset
if (introspection_)
introspection_->reset();
WrapperBase::reset();
}
void Task::init(const planning_scene::PlanningSceneConstPtr &scene)
{
WrapperBase::init(scene);
// provide introspection instance to all stages
pimpl()->setIntrospection(introspection_.get());
pimpl()->traverseStages([this](Stage& stage, int) {
stage.pimpl()->setIntrospection(introspection_.get());
return true;
}, 1, UINT_MAX);
// first time publish task
introspection_->publishTaskDescription();
}
bool Task::canCompute() const
{
return stages()->canCompute();
}
bool Task::compute()
{
return stages()->compute();
}
bool Task::plan()
{
reset();
initScene();
init(scene_);
while(ros::ok() && canCompute()) {
if (compute()) {
for (const auto& cb : task_cbs_)
cb(*this);
if (introspection_)
introspection_->publishTaskState();
} else
break;
}
return numSolutions() > 0;
}
size_t Task::numSolutions() const
{
auto w = stages();
// during initial insert() we call numSolutions(), but wrapped() is not yet defined
return w ? w->numSolutions() : 0;
}
void Task::processSolutions(const ContainerBase::SolutionProcessor &processor) const
{
stages()->processSolutions(processor);
}
void Task::publishAllSolutions(bool wait)
{
enableIntrospection(true);
introspection_->publishAllSolutions(wait);
}
void Task::onNewSolution(const SolutionBase &s)
{
WrapperBase::onNewSolution(s);
if (introspection_)
introspection_->publishSolution(s);
}
ContainerBase* Task::stages()
{
return static_cast<ContainerBase*>(WrapperBase::wrapped());
}
const ContainerBase* Task::stages() const
{
return const_cast<Task*>(this)->stages();
}
PropertyMap &Task::properties()
{
// forward to wrapped() stage
return wrapped()->properties();
}
void Task::setProperty(const std::string &name, const boost::any &value)
{
// forward to wrapped() stage
wrapped()->setProperty(name, value);
}
std::string Task::id() const
{
return id_;
}
void Task::printState(const Task &t){
ContainerBase::StageCallback processor = [](const Stage& stage, int depth) -> bool {
std::cout << std::string(2*depth, ' ') << stage << std::endl;
return true;
};
t.stages()->traverseRecursively(processor);
}
} }