mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
284 lines
7.7 KiB
C++
284 lines
7.7 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/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)
|
|
{
|
|
// monitor state on commandline
|
|
//addTaskCallback(std::bind(&Task::printState, this, std::ref(std::cout)));
|
|
// enable introspection by default, but only if ros::init() was called
|
|
if (ros::isInitialized())
|
|
enableIntrospection(true);
|
|
}
|
|
|
|
Task::Task(Task&& other)
|
|
: WrapperBase(std::string(), std::make_unique<SerialContainer>())
|
|
{
|
|
*this = std::move(other);
|
|
}
|
|
|
|
Task& Task::operator=(Task&& other)
|
|
{
|
|
id_ = std::move(other.id_);
|
|
robot_model_ = std::move(other.robot_model_);
|
|
introspection_ = std::move(other.introspection_);
|
|
task_cbs_ = std::move(other.task_cbs_);
|
|
std::swap(pimpl_, other.pimpl_);
|
|
return *this;
|
|
}
|
|
|
|
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::setRobotModel(const core::RobotModelConstPtr& robot_model)
|
|
{
|
|
reset();
|
|
robot_model_ = robot_model;
|
|
}
|
|
|
|
void Task::loadRobotModel(const std::string& robot_description) {
|
|
robot_model_loader::RobotModelLoader rml(robot_description);
|
|
setRobotModel(rml.getModel());
|
|
if (!robot_model_)
|
|
throw Exception("Task failed to construct RobotModel");
|
|
}
|
|
|
|
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()
|
|
{
|
|
if (!robot_model_)
|
|
loadRobotModel();
|
|
|
|
auto impl = pimpl();
|
|
// initialize push connections of wrapped child
|
|
StagePrivate *child = wrapped()->pimpl();
|
|
child->setPrevEnds(impl->pendingBackward());
|
|
child->setNextStarts(impl->pendingForward());
|
|
|
|
// and *afterwards* initialize all children recursively
|
|
stages()->init(robot_model_);
|
|
// task expects its wrapped child to push to both ends, this triggers interface resolution
|
|
stages()->pimpl()->pruneInterface(InterfaceFlags({GENERATE}));
|
|
// and *finally* validate connectivity
|
|
stages()->validateConnectivity();
|
|
|
|
// provide introspection instance to all stages
|
|
impl->setIntrospection(introspection_.get());
|
|
impl->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();
|
|
init();
|
|
|
|
while(ros::ok() && canCompute()) {
|
|
if (compute()) {
|
|
for (const auto& cb : task_cbs_)
|
|
cb(*this);
|
|
if (introspection_)
|
|
introspection_->publishTaskState();
|
|
} else
|
|
break;
|
|
}
|
|
printState();
|
|
return numSolutions() > 0;
|
|
}
|
|
|
|
size_t Task::numSolutions() const
|
|
{
|
|
return stages()->numSolutions();
|
|
}
|
|
|
|
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)
|
|
{
|
|
// no need to call WrapperBase::onNewSolution!
|
|
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(std::ostream& os) const {
|
|
os << *stages();
|
|
}
|
|
|
|
} }
|