mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
1201 lines
44 KiB
C++
1201 lines
44 KiB
C++
/*********************************************************************
|
|
* Software License Agreement (BSD License)
|
|
*
|
|
* 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: Robert Haschke */
|
|
|
|
#include <moveit/task_constructor/container_p.h>
|
|
#include <moveit/task_constructor/introspection.h>
|
|
#include <moveit/task_constructor/merge.h>
|
|
#include <moveit/planning_scene/planning_scene.h>
|
|
|
|
#include <ros/console.h>
|
|
|
|
#include <memory>
|
|
#include <iostream>
|
|
#include <algorithm>
|
|
#include <boost/range/adaptor/reversed.hpp>
|
|
#include <boost/format.hpp>
|
|
#include <functional>
|
|
|
|
using namespace std::placeholders;
|
|
|
|
namespace moveit {
|
|
namespace task_constructor {
|
|
|
|
ContainerBasePrivate::ContainerBasePrivate(ContainerBase* me, const std::string& name)
|
|
: StagePrivate(me, name)
|
|
, required_interface_(UNKNOWN)
|
|
, pending_backward_(new Interface)
|
|
, pending_forward_(new Interface) {}
|
|
|
|
ContainerBasePrivate::const_iterator ContainerBasePrivate::childByIndex(int index, bool for_insert) const {
|
|
if (!for_insert && index < 0)
|
|
--index;
|
|
const_iterator position = children_.begin();
|
|
if (index > 0) {
|
|
for (auto end = children_.end(); index > 0 && position != end; --index)
|
|
++position;
|
|
} else if (++index <= 0) {
|
|
container_type::const_reverse_iterator from_end = children_.rbegin();
|
|
for (auto end = children_.rend(); index < 0 && from_end != end; ++index)
|
|
++from_end;
|
|
position = index < 0 ? children_.end() : from_end.base();
|
|
}
|
|
return position;
|
|
}
|
|
|
|
bool ContainerBasePrivate::traverseStages(const ContainerBase::StageCallback& processor, unsigned int cur_depth,
|
|
unsigned int max_depth) const {
|
|
if (cur_depth >= max_depth)
|
|
return true;
|
|
|
|
for (auto& stage : children_) {
|
|
if (!processor(*stage, cur_depth))
|
|
return false;
|
|
const ContainerBasePrivate* container = dynamic_cast<const ContainerBasePrivate*>(stage->pimpl());
|
|
if (container)
|
|
container->traverseStages(processor, cur_depth + 1, max_depth);
|
|
}
|
|
return true;
|
|
}
|
|
|
|
void ContainerBasePrivate::validateConnectivity() const {
|
|
// recursively validate all children and accumulate errors
|
|
for (const auto& child : children())
|
|
child->pimpl()->validateConnectivity();
|
|
}
|
|
|
|
bool ContainerBasePrivate::canCompute() const {
|
|
// call the method of the public interface
|
|
return static_cast<ContainerBase*>(me_)->canCompute();
|
|
}
|
|
|
|
void ContainerBasePrivate::compute() {
|
|
// call the method of the public interface
|
|
static_cast<ContainerBase*>(me_)->compute();
|
|
}
|
|
|
|
void ContainerBasePrivate::onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) {
|
|
switch (child.pimpl()->interfaceFlags()) {
|
|
case GENERATE:
|
|
// just ignore: the pair of (new) states isn't known to us anyway
|
|
// TODO: If child is a container, from and to might have associated solutions already!
|
|
break;
|
|
|
|
case PROPAGATE_FORWARDS: // mark from as failed (backwards)
|
|
setStatus<Interface::BACKWARD>(from, InterfaceState::Status::FAILED);
|
|
break;
|
|
case PROPAGATE_BACKWARDS: // mark to as failed (forwards)
|
|
setStatus<Interface::FORWARD>(to, InterfaceState::Status::FAILED);
|
|
break;
|
|
|
|
case CONNECT:
|
|
if (const Connecting* conn = dynamic_cast<const Connecting*>(&child)) {
|
|
auto cimpl = conn->pimpl();
|
|
ROS_DEBUG_STREAM_NAMED("Connecting", "'" << child.name() << "' generated a failure");
|
|
if (!cimpl->hasPendingOpposites<Interface::FORWARD>(from)) {
|
|
ROS_DEBUG_STREAM_NAMED("Connecting", "prune backward branch");
|
|
setStatus<Interface::BACKWARD>(from, InterfaceState::Status::FAILED);
|
|
}
|
|
if (!cimpl->hasPendingOpposites<Interface::BACKWARD>(to)) {
|
|
ROS_DEBUG_STREAM_NAMED("Connecting", "prune forward branch");
|
|
setStatus<Interface::FORWARD>(to, InterfaceState::Status::FAILED);
|
|
}
|
|
}
|
|
break;
|
|
}
|
|
// printChildrenInterfaces(*this, false, child);
|
|
}
|
|
|
|
template <Interface::Direction dir>
|
|
void ContainerBasePrivate::setStatus(const InterfaceState* s, InterfaceState::Status status) {
|
|
if (s->priority().status() == status)
|
|
return; // nothing changing
|
|
|
|
// if we should disable the state, only do so when there is no enabled alternative path
|
|
if (status == InterfaceState::PRUNED) {
|
|
auto solution_is_enabled = [](auto&& solution) {
|
|
return state<opposite<dir>()>(*solution)->priority().enabled();
|
|
};
|
|
const auto& alternatives = trajectories<opposite<dir>()>(*s);
|
|
auto alternative_path = std::find_if(alternatives.cbegin(), alternatives.cend(), solution_is_enabled);
|
|
if (alternative_path != alternatives.cend())
|
|
return;
|
|
}
|
|
|
|
// actually enable/disable the state
|
|
if (s->owner()) {
|
|
s->owner()->updatePriority(const_cast<InterfaceState*>(s), InterfaceState::Priority(s->priority(), status));
|
|
} else {
|
|
const_cast<InterfaceState*>(s)->priority_ = InterfaceState::Priority(s->priority(), status);
|
|
}
|
|
|
|
// if possible (i.e. if state s has an external counterpart), escalate setStatus to external interface
|
|
if (parent() && trajectories<dir>(*s).empty()) {
|
|
auto external{ internalToExternalMap().find(s) };
|
|
if (external != internalToExternalMap().end()) { // do we have an external state?
|
|
// only escalate if there is no other *enabled* internal state connected to the same external one
|
|
// all internal states linked to external
|
|
auto internals{ externalToInternalMap().equal_range(external->get<EXTERNAL>()) };
|
|
auto is_enabled = [](const auto& ext_int_pair) { return ext_int_pair.second->priority().enabled(); };
|
|
auto other_path{ std::find_if(internals.first, internals.second, is_enabled) };
|
|
if (other_path == internals.second)
|
|
parent()->pimpl()->setStatus<dir>(external->get<EXTERNAL>(), status);
|
|
return;
|
|
}
|
|
}
|
|
|
|
// To break symmetry between both ends of a partial solution sequence that gets disabled,
|
|
// we mark the first state with FAILED and all other states down the tree only with PRUNED.
|
|
// This allows us to re-enable the FAILED side, while not (yet) consider the PRUNED states again,
|
|
// when new states arrive in a Connecting stage.
|
|
// All PRUNED states are only re-enabled if the FAILED state actually gets connected.
|
|
// For details, see: https://github.com/ros-planning/moveit_task_constructor/pull/221
|
|
if (status == InterfaceState::Status::FAILED)
|
|
status = InterfaceState::Status::PRUNED; // only the first state is marked as FAILED
|
|
|
|
// traverse solution tree
|
|
for (const SolutionBase* successor : trajectories<dir>(*s))
|
|
setStatus<dir>(state<dir>(*successor), status);
|
|
}
|
|
|
|
void ContainerBasePrivate::setStatus(Interface::Direction dir, const InterfaceState* s, InterfaceState::Status status) {
|
|
if (dir == Interface::FORWARD)
|
|
setStatus<Interface::FORWARD>(s, InterfaceState::ENABLED);
|
|
else
|
|
setStatus<Interface::BACKWARD>(s, InterfaceState::ENABLED);
|
|
}
|
|
|
|
template <Interface::Direction dir>
|
|
void ContainerBasePrivate::copyState(Interface::iterator external, const InterfacePtr& target, bool updated) {
|
|
if (updated) {
|
|
// update prio of all internal states linked to external
|
|
auto internals{ externalToInternalMap().equal_range(&*external) };
|
|
for (auto& i = internals.first; i != internals.second; ++i) {
|
|
// TODO: Not only update status, but full priority!
|
|
setStatus<dir>(i->second, external->priority().status());
|
|
}
|
|
return;
|
|
}
|
|
|
|
// create a clone of external state within target interface (child's starts() or ends())
|
|
auto internal = states_.insert(states_.end(), InterfaceState(*external));
|
|
target->add(*internal);
|
|
// and remember the mapping between them
|
|
internalToExternalMap().insert(std::make_pair(&*internal, &*external));
|
|
}
|
|
|
|
void ContainerBasePrivate::copyState(Interface::Direction dir, Interface::iterator external, const InterfacePtr& target,
|
|
bool updated) {
|
|
if (dir == Interface::FORWARD)
|
|
copyState<Interface::FORWARD>(external, target, updated);
|
|
else
|
|
copyState<Interface::BACKWARD>(external, target, updated);
|
|
}
|
|
|
|
void ContainerBasePrivate::liftSolution(const SolutionBasePtr& solution, const InterfaceState* internal_from,
|
|
const InterfaceState* internal_to) {
|
|
computeCost(*internal_from, *internal_to, *solution);
|
|
|
|
// map internal to external states
|
|
auto find_or_create_external = [this](const InterfaceState* internal, bool& created) -> InterfaceState* {
|
|
auto it = internalToExternalMap().find(internal);
|
|
if (it != internalToExternalMap().end())
|
|
return const_cast<InterfaceState*>(it->second);
|
|
|
|
InterfaceState* external = &*states_.insert(states_.end(), InterfaceState(*internal));
|
|
internalToExternalMap().insert(std::make_pair(internal, external));
|
|
created = true;
|
|
return external;
|
|
};
|
|
bool created_from = false;
|
|
bool created_to = false;
|
|
InterfaceState* external_from = find_or_create_external(internal_from, created_from);
|
|
InterfaceState* external_to = find_or_create_external(internal_to, created_to);
|
|
|
|
if (!storeSolution(solution, external_from, external_to))
|
|
return;
|
|
|
|
// connect solution to start/end state
|
|
solution->setStartState(*external_from);
|
|
solution->setEndState(*external_to);
|
|
|
|
// spawn created states in external interfaces
|
|
if (created_from)
|
|
prevEnds()->add(*external_from);
|
|
if (created_to)
|
|
nextStarts()->add(*external_to);
|
|
|
|
newSolution(solution);
|
|
}
|
|
|
|
ContainerBase::ContainerBase(ContainerBasePrivate* impl) : Stage(impl) {}
|
|
|
|
size_t ContainerBase::numChildren() const {
|
|
return pimpl()->children().size();
|
|
}
|
|
|
|
Stage* ContainerBase::findChild(const std::string& name) const {
|
|
auto pos = name.find('/');
|
|
const std::string first = name.substr(0, pos);
|
|
for (const Stage::pointer& child : pimpl()->children())
|
|
if (child->name() == first) {
|
|
if (pos == std::string::npos)
|
|
return child.get();
|
|
else if (auto* parent = dynamic_cast<const ContainerBase*>(child.get()))
|
|
return parent->findChild(name.substr(pos + 1));
|
|
}
|
|
return nullptr;
|
|
}
|
|
|
|
bool ContainerBase::traverseChildren(const ContainerBase::StageCallback& processor) const {
|
|
return pimpl()->traverseStages(processor, 0, 1);
|
|
}
|
|
bool ContainerBase::traverseRecursively(const ContainerBase::StageCallback& processor) const {
|
|
if (!processor(*this, 0))
|
|
return false;
|
|
return pimpl()->traverseStages(processor, 1, UINT_MAX);
|
|
}
|
|
|
|
void ContainerBase::add(Stage::pointer&& stage) {
|
|
insert(std::move(stage), -1);
|
|
}
|
|
|
|
void ContainerBase::insert(Stage::pointer&& stage, int before) {
|
|
if (!stage)
|
|
throw std::runtime_error(name() + ": received invalid stage pointer");
|
|
|
|
StagePrivate* impl = stage->pimpl();
|
|
impl->setParent(this);
|
|
ContainerBasePrivate::const_iterator where = pimpl()->childByIndex(before, true);
|
|
ContainerBasePrivate::iterator it = pimpl()->children_.insert(where, std::move(stage));
|
|
impl->setParentPosition(it);
|
|
}
|
|
|
|
Stage::pointer ContainerBasePrivate::remove(ContainerBasePrivate::const_iterator pos) {
|
|
if (pos == children_.end())
|
|
return Stage::pointer();
|
|
|
|
(*pos)->pimpl()->unparent();
|
|
Stage::pointer result = std::move(*children_.erase(pos, pos)); // stage from non-const iterator to pos
|
|
children_.erase(pos); // actually erase stage
|
|
return result;
|
|
}
|
|
|
|
Stage::pointer ContainerBase::remove(int pos) {
|
|
return pimpl()->remove(pimpl()->childByIndex(pos, false));
|
|
}
|
|
|
|
Stage::pointer ContainerBase::remove(Stage* child) {
|
|
auto it = pimpl()->children_.begin(), end = pimpl()->children_.end();
|
|
for (; it != end && it->get() != child; ++it)
|
|
;
|
|
return pimpl()->remove(it);
|
|
}
|
|
|
|
void ContainerBase::clear() {
|
|
pimpl()->children_.clear();
|
|
}
|
|
|
|
void ContainerBase::reset() {
|
|
auto impl = pimpl();
|
|
|
|
// recursively reset children
|
|
for (auto& child : impl->children())
|
|
child->reset();
|
|
|
|
// clear buffer interfaces
|
|
impl->pending_backward_->clear();
|
|
impl->pending_forward_->clear();
|
|
// ... and state mapping
|
|
impl->internalToExternalMap().clear();
|
|
|
|
// interfaces depend on children which might change
|
|
impl->required_interface_ = UNKNOWN;
|
|
impl->starts_.reset();
|
|
impl->ends_.reset();
|
|
|
|
Stage::reset();
|
|
}
|
|
|
|
void ContainerBase::init(const moveit::core::RobotModelConstPtr& robot_model) {
|
|
auto impl = pimpl();
|
|
auto& children = impl->children();
|
|
|
|
Stage::init(robot_model);
|
|
|
|
// we need to have some children to do the actual work
|
|
if (children.empty())
|
|
throw InitStageException(*this, "no children");
|
|
|
|
// recursively init all children and accumulate errors
|
|
InitStageException errors;
|
|
for (auto& child : children) {
|
|
try {
|
|
child->init(robot_model);
|
|
} catch (const Property::error& e) {
|
|
std::ostringstream oss;
|
|
oss << e.what();
|
|
pimpl()->composePropertyErrorMsg(e.name(), oss);
|
|
errors.push_back(*child, oss.str());
|
|
} catch (InitStageException& e) {
|
|
errors.append(e);
|
|
}
|
|
}
|
|
|
|
if (errors)
|
|
throw errors;
|
|
}
|
|
|
|
std::ostream& operator<<(std::ostream& os, const ContainerBase& container) {
|
|
ContainerBase::StageCallback processor = [&os](const Stage& stage, unsigned int depth) -> bool {
|
|
os << std::string(2 * depth, ' ') << *stage.pimpl() << std::endl;
|
|
return true;
|
|
};
|
|
container.traverseRecursively(processor);
|
|
return os;
|
|
}
|
|
|
|
// for debugging of how children interfaces evolve over time
|
|
static void printChildrenInterfaces(const ContainerBase& container, bool success, const Stage& creator,
|
|
std::ostream& os = std::cerr) {
|
|
static unsigned int id = 0;
|
|
const unsigned int width = 10; // indentation of name
|
|
os << std::endl << (success ? '+' : '-') << ' ' << creator.name() << ' ';
|
|
if (success)
|
|
os << ++id << ' ';
|
|
if (const Connecting* conn = dynamic_cast<const Connecting*>(&creator))
|
|
conn->pimpl()->printPendingPairs(os);
|
|
os << std::endl;
|
|
|
|
for (const auto& child : container.pimpl()->children()) {
|
|
auto cimpl = child->pimpl();
|
|
os << std::setw(width) << std::left << child->name();
|
|
if (!cimpl->starts() && !cimpl->ends())
|
|
os << "↕ " << std::endl;
|
|
if (cimpl->starts())
|
|
os << "↓ " << *child->pimpl()->starts() << std::endl;
|
|
if (cimpl->starts() && cimpl->ends())
|
|
os << std::setw(width) << " ";
|
|
if (cimpl->ends())
|
|
os << "↑ " << *child->pimpl()->ends() << std::endl;
|
|
}
|
|
}
|
|
/** Collect all partial solution sequences originating from start into given direction */
|
|
template <Interface::Direction dir>
|
|
struct SolutionCollector
|
|
{
|
|
SolutionCollector(size_t max_depth, const SolutionBase& start) : max_depth(max_depth) {
|
|
trace.reserve(max_depth);
|
|
traverse(start, InterfaceState::Priority(0, 0.0));
|
|
assert(trace.empty());
|
|
}
|
|
|
|
void traverse(const SolutionBase& start, const InterfaceState::Priority& prio) {
|
|
const InterfaceState::Solutions& next = trajectories<dir>(*state<dir>(start));
|
|
if (next.empty()) { // when reaching the end, add the trace to solutions
|
|
assert(prio.depth() == trace.size());
|
|
assert(prio.depth() <= max_depth);
|
|
solutions.emplace_back(std::make_pair(trace, prio));
|
|
} else {
|
|
for (SolutionBase* successor : next) {
|
|
assert(!successor->isFailure()); // We shouldn't have invalid solutions
|
|
trace.push_back(successor);
|
|
traverse(*successor, prio + InterfaceState::Priority(1, successor->cost()));
|
|
trace.pop_back();
|
|
}
|
|
}
|
|
}
|
|
|
|
using SolutionCostPairs = std::list<std::pair<SolutionSequence::container_type, InterfaceState::Priority>>;
|
|
SolutionCostPairs solutions;
|
|
const size_t max_depth;
|
|
SolutionSequence::container_type trace;
|
|
};
|
|
|
|
inline void updateStatePrio(const InterfaceState* state, const InterfaceState::Priority& prio) {
|
|
if (state->owner()) // owner becomes NULL if state is removed from (pending) Interface list
|
|
state->owner()->updatePriority(const_cast<InterfaceState*>(state),
|
|
// update depth + cost, but keep current status
|
|
InterfaceState::Priority(prio, state->priority().status()));
|
|
}
|
|
|
|
template <Interface::Direction dir>
|
|
inline void updateStatePrios(const SolutionSequence::container_type& partial_solution_path,
|
|
const InterfaceState::Priority& prio) {
|
|
for (const SolutionBase* solution : partial_solution_path)
|
|
updateStatePrio(state<dir>(*solution), prio);
|
|
}
|
|
|
|
void SerialContainer::onNewSolution(const SolutionBase& current) {
|
|
// failures should never trigger this callback
|
|
assert(!current.isFailure());
|
|
|
|
// states of solution must be active, otherwise this would not have been computed
|
|
assert(current.start()->priority().enabled());
|
|
assert(current.end()->priority().enabled());
|
|
|
|
auto impl = pimpl();
|
|
const Stage* creator = current.creator();
|
|
auto& children = impl->children();
|
|
|
|
// find number of stages before and after creator stage
|
|
size_t num_before = 0, num_after = 0;
|
|
for (auto it = children.begin(), end = children.end(); it != end; ++it, ++num_before)
|
|
if (&(**it) == creator)
|
|
break;
|
|
assert(num_before < children.size()); // creator should be one of our children
|
|
num_after = children.size() - 1 - num_before;
|
|
|
|
// find all incoming and outgoing solution paths originating from current solution
|
|
SolutionCollector<Interface::BACKWARD> incoming(num_before, current);
|
|
SolutionCollector<Interface::FORWARD> outgoing(num_after, current);
|
|
|
|
// collect (and sort) all solutions spanning from start to end of this container
|
|
ordered<SolutionSequencePtr> sorted;
|
|
for (auto& in : incoming.solutions) {
|
|
for (auto& out : outgoing.solutions) {
|
|
InterfaceState::Priority prio = in.second + InterfaceState::Priority(1u, current.cost()) + out.second;
|
|
assert(prio.enabled());
|
|
// found a complete solution path connecting start to end?
|
|
if (prio.depth() == children.size()) {
|
|
SolutionSequence::container_type solution;
|
|
solution.reserve(children.size());
|
|
// insert incoming solutions in reverse order
|
|
solution.insert(solution.end(), in.first.rbegin(), in.first.rend());
|
|
// insert current solution
|
|
solution.push_back(¤t);
|
|
// insert outgoing solutions in normal order
|
|
solution.insert(solution.end(), out.first.begin(), out.first.end());
|
|
// store solution in sorted list
|
|
sorted.insert(std::make_shared<SolutionSequence>(std::move(solution), prio.cost(), this));
|
|
}
|
|
if (prio.depth() > 1) {
|
|
// update state priorities along the whole partial solution path
|
|
updateStatePrio(current.start(), prio);
|
|
updateStatePrio(current.end(), prio);
|
|
updateStatePrios<Interface::BACKWARD>(in.first, prio);
|
|
updateStatePrios<Interface::FORWARD>(out.first, prio);
|
|
}
|
|
}
|
|
}
|
|
// printChildrenInterfaces(*this, true, *current.creator());
|
|
|
|
// finally, store + announce new solutions to external interface
|
|
for (const auto& solution : sorted)
|
|
impl->liftSolution(solution, solution->internalStart(), solution->internalEnd());
|
|
}
|
|
|
|
SerialContainer::SerialContainer(SerialContainerPrivate* impl) : ContainerBase(impl) {}
|
|
SerialContainer::SerialContainer(const std::string& name) : SerialContainer(new SerialContainerPrivate(this, name)) {}
|
|
|
|
SerialContainerPrivate::SerialContainerPrivate(SerialContainer* me, const std::string& name)
|
|
: ContainerBasePrivate(me, name) {}
|
|
|
|
void SerialContainerPrivate::connect(StagePrivate& stage1, StagePrivate& stage2) {
|
|
InterfaceFlags flags1 = stage1.requiredInterface();
|
|
InterfaceFlags flags2 = stage2.requiredInterface();
|
|
|
|
if ((flags1 & WRITES_NEXT_START) && (flags2 & READS_START))
|
|
stage1.setNextStarts(stage2.starts());
|
|
else if ((flags1 & READS_END) && (flags2 & WRITES_PREV_END))
|
|
stage2.setPrevEnds(stage1.ends());
|
|
else {
|
|
boost::format desc("cannot connect end interface of '%1%' (%2%) to start interface of '%3%' (%4%)");
|
|
desc % stage1.name() % flowSymbol<END_IF_MASK>(flags1);
|
|
desc % stage2.name() % flowSymbol<START_IF_MASK>(flags2);
|
|
throw InitStageException(*me(), desc.str());
|
|
}
|
|
}
|
|
|
|
template <unsigned int mask>
|
|
void SerialContainerPrivate::validateInterface(const StagePrivate& child, InterfaceFlags required) const {
|
|
required = required & mask;
|
|
if (required == UNKNOWN)
|
|
return; // cannot yet validate
|
|
InterfaceFlags child_interface = child.interfaceFlags() & mask;
|
|
if (required != child_interface) {
|
|
boost::format desc("%1% interface (%3%) of '%2%' does not match mine (%4%)");
|
|
desc % (mask == START_IF_MASK ? "start" : "end") % child.name();
|
|
desc % flowSymbol<mask>(child_interface) % flowSymbol<mask>(required);
|
|
throw InitStageException(*me_, desc.str());
|
|
}
|
|
}
|
|
|
|
// called by parent asking for pruning of this' interface
|
|
void SerialContainerPrivate::resolveInterface(InterfaceFlags expected) {
|
|
// we need to have some children to do the actual work
|
|
if (children().empty())
|
|
throw InitStageException(*me(), "no children");
|
|
|
|
if (!(expected & START_IF_MASK))
|
|
throw InitStageException(*me(), "unknown start interface");
|
|
|
|
Stage& first = *children().front();
|
|
Stage& last = *children().back();
|
|
|
|
InitStageException exceptions;
|
|
|
|
try { // FIRST child
|
|
first.pimpl()->resolveInterface(expected & START_IF_MASK);
|
|
// connect first child's (start) push interface
|
|
setChildsPushBackwardInterface(first.pimpl());
|
|
// validate that first child's and this container's start interfaces match
|
|
validateInterface<START_IF_MASK>(*first.pimpl(), expected);
|
|
// connect first child's (start) pull interface
|
|
if (const InterfacePtr& target = first.pimpl()->starts())
|
|
starts_.reset(new Interface([this, target](Interface::iterator it, bool updated) {
|
|
this->copyState<Interface::FORWARD>(it, target, updated);
|
|
}));
|
|
} catch (InitStageException& e) {
|
|
exceptions.append(e);
|
|
}
|
|
|
|
// process all children and connect them
|
|
for (auto it = ++children().begin(), previous_it = children().begin(); it != children().end(); ++it, ++previous_it) {
|
|
try {
|
|
StagePrivate* child_impl = (**it).pimpl();
|
|
StagePrivate* previous_impl = (**previous_it).pimpl();
|
|
child_impl->resolveInterface(invert(previous_impl->requiredInterface()) & START_IF_MASK);
|
|
connect(*previous_impl, *child_impl);
|
|
} catch (InitStageException& e) {
|
|
exceptions.append(e);
|
|
}
|
|
}
|
|
|
|
try { // connect last child's (end) push interface
|
|
setChildsPushForwardInterface(last.pimpl());
|
|
// validate that last child's and this container's end interfaces match
|
|
validateInterface<END_IF_MASK>(*last.pimpl(), expected);
|
|
// connect last child's (end) pull interface
|
|
if (const InterfacePtr& target = last.pimpl()->ends())
|
|
ends_.reset(new Interface([this, target](Interface::iterator it, bool updated) {
|
|
this->copyState<Interface::BACKWARD>(it, target, updated);
|
|
}));
|
|
} catch (InitStageException& e) {
|
|
exceptions.append(e);
|
|
}
|
|
|
|
required_interface_ = (first.pimpl()->interfaceFlags() & START_IF_MASK) | // clang-format off
|
|
(last.pimpl()->interfaceFlags() & END_IF_MASK); // clang-format off
|
|
|
|
if (exceptions)
|
|
throw exceptions;
|
|
}
|
|
|
|
void SerialContainerPrivate::validateConnectivity() const {
|
|
ContainerBasePrivate::validateConnectivity();
|
|
|
|
InterfaceFlags mine = interfaceFlags();
|
|
// check that input / output interface of first / last child matches this' resp. interface
|
|
validateInterface<START_IF_MASK>(*children().front()->pimpl(), mine);
|
|
validateInterface<END_IF_MASK>(*children().back()->pimpl(), mine);
|
|
|
|
// validate connectivity of children between each other
|
|
// ContainerBasePrivate::validateConnectivity() ensures that required push interfaces are present,
|
|
// that is, neighbouring stages have a corresponding pull interface.
|
|
// Here, it remains to check that - if a child has a pull interface - it's indeed feeded.
|
|
for (auto cur = children().begin(), end = children().end(); cur != end; ++cur) {
|
|
const StagePrivate* const cur_impl = **cur;
|
|
InterfaceFlags required = cur_impl->interfaceFlags();
|
|
|
|
// get iterators to prev / next stage in sequence
|
|
auto prev = cur;
|
|
--prev;
|
|
auto next = cur;
|
|
++next;
|
|
|
|
// start pull interface fed?
|
|
if (cur != children().begin() && // first child has not a previous one
|
|
(required & READS_START) && !(*prev)->pimpl()->nextStarts())
|
|
throw InitStageException(**cur, "start interface is not fed");
|
|
|
|
// end pull interface fed?
|
|
if (next != end && // last child has not a next one
|
|
(required & READS_END) && !(*next)->pimpl()->prevEnds())
|
|
throw InitStageException(**cur, "end interface is not fed");
|
|
}
|
|
}
|
|
|
|
bool SerialContainer::canCompute() const {
|
|
for (const auto& stage : pimpl()->children()) {
|
|
if (stage->pimpl()->canCompute())
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
void SerialContainer::compute() {
|
|
for (const auto& stage : pimpl()->children()) {
|
|
if (!stage->pimpl()->canCompute())
|
|
continue;
|
|
stage->pimpl()->runCompute();
|
|
}
|
|
}
|
|
|
|
ParallelContainerBasePrivate::ParallelContainerBasePrivate(ParallelContainerBase* me, const std::string& name)
|
|
: ContainerBasePrivate(me, name) {}
|
|
|
|
void ParallelContainerBasePrivate::resolveInterface(InterfaceFlags expected) {
|
|
// we need to have some children to do the actual work
|
|
if (children().empty())
|
|
throw InitStageException(*me(), "no children");
|
|
|
|
InitStageException exceptions;
|
|
|
|
bool first = true;
|
|
for (const Stage::pointer& child : children()) {
|
|
try {
|
|
auto child_impl = child->pimpl();
|
|
child_impl->resolveInterface(expected);
|
|
validateInterfaces(*child_impl, expected, first);
|
|
// initialize push connections of children according to their demands
|
|
setChildsPushForwardInterface(child_impl);
|
|
setChildsPushBackwardInterface(child_impl);
|
|
first = false;
|
|
} catch (InitStageException& e) {
|
|
exceptions.append(e);
|
|
continue;
|
|
}
|
|
}
|
|
|
|
if (exceptions)
|
|
throw exceptions;
|
|
|
|
required_interface_ = expected;
|
|
|
|
initializeExternalInterfaces();
|
|
}
|
|
|
|
void ParallelContainerBasePrivate::initializeExternalInterfaces() {
|
|
// States received by the container need to be copied to all children's pull interfaces.
|
|
if (requiredInterface() & READS_START)
|
|
starts().reset(new Interface([this](Interface::iterator external, bool updated) {
|
|
this->propagateStateToChildren<Interface::FORWARD>(external, updated);
|
|
}));
|
|
if (requiredInterface() & READS_END)
|
|
ends().reset(new Interface([this](Interface::iterator external, bool updated) {
|
|
this->propagateStateToChildren<Interface::BACKWARD>(external, updated);
|
|
}));
|
|
}
|
|
|
|
void ParallelContainerBasePrivate::validateInterfaces(const StagePrivate& child, InterfaceFlags& external,
|
|
bool first) const {
|
|
const InterfaceFlags child_interface = child.requiredInterface();
|
|
bool valid = true;
|
|
for (InterfaceFlags mask : { START_IF_MASK, END_IF_MASK }) {
|
|
if ((external & mask) == UNKNOWN)
|
|
external |= child_interface & mask;
|
|
|
|
valid = valid & ((external & mask) == (child_interface & mask));
|
|
}
|
|
|
|
if (!valid) {
|
|
boost::format desc("interface of '%1%' (%3% %4%) does not match %2% (%5% %6%).");
|
|
desc % child.name();
|
|
desc % (first ? "external one" : "other children's");
|
|
desc % flowSymbol<START_IF_MASK>(child_interface) % flowSymbol<END_IF_MASK>(child_interface);
|
|
desc % flowSymbol<START_IF_MASK>(external) % flowSymbol<END_IF_MASK>(external);
|
|
throw InitStageException(*me_, desc.str());
|
|
}
|
|
}
|
|
|
|
void ParallelContainerBasePrivate::validateConnectivity() const {
|
|
InterfaceFlags my_interface = interfaceFlags();
|
|
|
|
// check that input / output interfaces of all children are handled by my interface
|
|
for (const auto& child : children())
|
|
validateInterfaces(*child->pimpl(), my_interface);
|
|
|
|
ContainerBasePrivate::validateConnectivity();
|
|
}
|
|
|
|
template <Interface::Direction dir>
|
|
void ParallelContainerBasePrivate::propagateStateToChildren(Interface::iterator external, bool updated) {
|
|
for (const Stage::pointer& stage : children())
|
|
copyState<dir>(external, stage->pimpl()->pullInterface(dir), updated);
|
|
}
|
|
|
|
ParallelContainerBase::ParallelContainerBase(ParallelContainerBasePrivate* impl) : ContainerBase(impl) {}
|
|
ParallelContainerBase::ParallelContainerBase(const std::string& name)
|
|
: ParallelContainerBase(new ParallelContainerBasePrivate(this, name)) {}
|
|
|
|
void ParallelContainerBase::liftSolution(const SolutionBase& solution, double cost, std::string comment) {
|
|
pimpl()->liftSolution(std::make_shared<WrappedSolution>(this, &solution, cost, std::move(comment)),
|
|
solution.start(), solution.end());
|
|
}
|
|
|
|
void ParallelContainerBase::spawn(InterfaceState&& state, SubTrajectory&& t) {
|
|
pimpl()->StagePrivate::spawn(std::move(state), std::make_shared<SubTrajectory>(std::move(t)));
|
|
}
|
|
|
|
void ParallelContainerBase::sendForward(const InterfaceState& from, InterfaceState&& to, SubTrajectory&& t) {
|
|
pimpl()->StagePrivate::sendForward(from, std::move(to), std::make_shared<SubTrajectory>(std::move(t)));
|
|
}
|
|
|
|
void ParallelContainerBase::sendBackward(InterfaceState&& from, const InterfaceState& to, SubTrajectory&& t) {
|
|
pimpl()->StagePrivate::sendBackward(std::move(from), to, std::make_shared<SubTrajectory>(std::move(t)));
|
|
}
|
|
|
|
WrapperBasePrivate::WrapperBasePrivate(WrapperBase* me, const std::string& name)
|
|
: ParallelContainerBasePrivate(me, name) {}
|
|
|
|
WrapperBase::WrapperBase(const std::string& name, Stage::pointer&& child)
|
|
: WrapperBase(new WrapperBasePrivate(this, name), std::move(child)) {}
|
|
|
|
WrapperBase::WrapperBase(WrapperBasePrivate* impl, Stage::pointer&& child) : ParallelContainerBase(impl) {
|
|
if (child)
|
|
WrapperBase::insert(std::move(child));
|
|
}
|
|
|
|
void WrapperBase::insert(Stage::pointer&& stage, int before) {
|
|
// restrict num of children to one
|
|
if (numChildren() > 0)
|
|
throw std::runtime_error(name() + ": Wrapper only allows a single child");
|
|
return ParallelContainerBase::insert(std::move(stage), before);
|
|
}
|
|
|
|
Stage* WrapperBase::wrapped() {
|
|
return pimpl()->children().empty() ? nullptr : pimpl()->children().front().get();
|
|
}
|
|
|
|
bool WrapperBase::canCompute() const {
|
|
return wrapped()->pimpl()->canCompute();
|
|
}
|
|
|
|
void WrapperBase::compute() {
|
|
wrapped()->pimpl()->runCompute();
|
|
}
|
|
|
|
bool Alternatives::canCompute() const {
|
|
for (const auto& stage : pimpl()->children())
|
|
if (stage->pimpl()->canCompute())
|
|
return true;
|
|
return false;
|
|
}
|
|
|
|
void Alternatives::compute() {
|
|
for (const auto& stage : pimpl()->children()) {
|
|
stage->pimpl()->runCompute();
|
|
}
|
|
}
|
|
|
|
void Alternatives::onNewSolution(const SolutionBase& s) {
|
|
liftSolution(s);
|
|
}
|
|
|
|
Fallbacks::Fallbacks(const std::string& name) : Fallbacks(new FallbacksPrivate(this, name)) {}
|
|
|
|
Fallbacks::Fallbacks(FallbacksPrivate* impl) : ParallelContainerBase(impl) {}
|
|
|
|
void Fallbacks::reset() {
|
|
ParallelContainerBase::reset();
|
|
}
|
|
|
|
void Fallbacks::init(const moveit::core::RobotModelConstPtr& robot_model) {
|
|
auto& impl{ *pimpl() };
|
|
ParallelContainerBase::init(robot_model);
|
|
impl.current_generator_ = impl.children().begin();
|
|
}
|
|
|
|
bool Fallbacks::canCompute() const {
|
|
auto impl { pimpl() };
|
|
|
|
switch (impl->requiredInterface()) {
|
|
case GENERATE:
|
|
// current_generator_ is fixed if it produced solutions before
|
|
if (!solutions().empty())
|
|
return (*impl->current_generator_)->pimpl()->canCompute();
|
|
else {
|
|
// move to first generator that can run
|
|
while(impl->current_generator_ != impl->children().end() && !(*impl->current_generator_)->pimpl()->canCompute()) {
|
|
ROS_DEBUG_STREAM_NAMED("Fallbacks", "Generator '" << (*impl->current_generator_)->name() << "' can't compute, trying next one.");
|
|
++impl->current_generator_;
|
|
}
|
|
return impl->current_generator_ != impl->children().end();
|
|
}
|
|
break;
|
|
case PROPAGATE_FORWARDS:
|
|
case PROPAGATE_BACKWARDS:
|
|
case CONNECT:
|
|
return !impl->pending_states_.empty() && !impl->pending_states_.front().external_state->priority().failed();
|
|
default:
|
|
assert(false);
|
|
}
|
|
}
|
|
|
|
void Fallbacks::compute() {
|
|
auto impl { pimpl() };
|
|
|
|
switch (impl->requiredInterface()) {
|
|
case GENERATE:
|
|
impl->computeGenerate();
|
|
break;
|
|
case PROPAGATE_FORWARDS:
|
|
case PROPAGATE_BACKWARDS:
|
|
case CONNECT:
|
|
impl->computePropagate();
|
|
break;
|
|
default:
|
|
assert(false);
|
|
}
|
|
}
|
|
|
|
void Fallbacks::onNewSolution(const SolutionBase& s) {
|
|
liftSolution(s);
|
|
}
|
|
|
|
FallbacksPrivate::FallbacksPrivate(Fallbacks* me, const std::string& name)
|
|
: ParallelContainerBasePrivate(me, name) {}
|
|
|
|
void FallbacksPrivate::initializeExternalInterfaces() {
|
|
if (requiredInterface() & READS_START)
|
|
starts().reset(new Interface([this](Interface::iterator external, bool updated) {
|
|
this->onNewExternalState<Interface::FORWARD>(external, updated);
|
|
}));
|
|
if (requiredInterface() & READS_END)
|
|
ends().reset(new Interface([this](Interface::iterator external, bool updated) {
|
|
this->onNewExternalState<Interface::BACKWARD>(external, updated);
|
|
}));
|
|
}
|
|
|
|
void FallbacksPrivate::onNewFailure(const Stage& /*child*/, const InterfaceState* /*from*/, const InterfaceState* /*to*/) {
|
|
// This override is deliberately empty.
|
|
// The method prunes solution paths when a child failed to find a valid solution for it,
|
|
// but in Fallbacks the next child might still yield a successful solution
|
|
// Thus pruning must only occur once the last child is exhausted (inside computePropagate)
|
|
}
|
|
|
|
void FallbacksPrivate::computeGenerate() {
|
|
assert(current_generator_ != children().end());
|
|
(*current_generator_)->pimpl()->runCompute();
|
|
}
|
|
|
|
inline void FallbacksPrivate::printPending(const char* comment) const {
|
|
ROSCONSOLE_DEFINE_LOCATION(true, ::ros::console::levels::Debug, ROSCONSOLE_NAME_PREFIX ".Fallbacks");
|
|
if (ROS_UNLIKELY(__rosconsole_define_location__enabled)) {
|
|
std::cout << name() << ": " << comment;
|
|
std::for_each(pending_states_.begin(), pending_states_.end(), [](const auto& e) { std::cout << e.external_state->priority() << " "; });
|
|
std::cout << std::endl;
|
|
}
|
|
}
|
|
|
|
template <typename Interface::Direction dir>
|
|
void FallbacksPrivate::onNewExternalState(Interface::iterator external, bool updated) {
|
|
if (updated) {
|
|
auto it = std::find_if(pending_states_.begin(), pending_states_.end(),
|
|
[external](const ExternalState& s) { return s.external_state == external; });
|
|
if (it == pending_states_.cend())
|
|
return; // already processed
|
|
|
|
pending_states_.update(it); // update sorting pos of this single item
|
|
printPending("after update: ");
|
|
|
|
// update prio of linked internal states as well
|
|
ContainerBasePrivate::copyState<dir>(external, InterfacePtr(), updated);
|
|
return;
|
|
}
|
|
|
|
// remember external state for later processing by children.
|
|
// children().end() indicates that the states wasn't yet forwarded to any child
|
|
pending_states_.push(ExternalState(external, dir, children().cend()));
|
|
printPending("after push: ");
|
|
}
|
|
|
|
void FallbacksPrivate::computePropagate(){
|
|
while (!pending_states_.empty()) {
|
|
printPending();
|
|
|
|
auto current = pending_states_.begin();
|
|
if (!current->external_state->priority().enabled())
|
|
return;
|
|
|
|
auto pushState = [this](const ExternalState& ext) {
|
|
ROS_DEBUG_STREAM_NAMED("Fallbacks", "Push external state (" << ext.external_state->priority()
|
|
<< ") to '" << (*ext.stage)->name() << "'");
|
|
copyState(ext.dir, ext.external_state, (*ext.stage)->pimpl()->pullInterface(ext.dir), false);
|
|
};
|
|
|
|
if (current->stage == children().cend()) {
|
|
current->stage = children().begin(); // activate first child
|
|
pushState(*current);
|
|
}
|
|
|
|
StagePrivate* child = (*current->stage)->pimpl();
|
|
if (child->canCompute()) {
|
|
child->runCompute();
|
|
return; // return after first compute()
|
|
}
|
|
|
|
auto has_solutions{ [](const InterfaceState& s, Interface::Direction d){
|
|
const auto& trajectories { d == Interface::FORWARD ? s.outgoingTrajectories()
|
|
: s.incomingTrajectories() };
|
|
return std::find_if(trajectories.cbegin(), trajectories.cend(),
|
|
[](const auto& t){ return !t->isFailure();}) != trajectories.cend();
|
|
}};
|
|
|
|
if(!has_solutions(*current->external_state, current->dir)){
|
|
++current->stage;
|
|
if(current->stage != children().cend()){
|
|
ROS_DEBUG_STREAM_NAMED("Fallbacks", "Child '" << child->name() << "' failed generating solutions, trying next child: '"
|
|
<< (*current->stage)->name() << "'");
|
|
pushState(*current);
|
|
}
|
|
else {
|
|
ROS_DEBUG_STREAM_NAMED("Fallbacks", "Failed to extend state with all children, pruning path");
|
|
parent()->pimpl()->onNewFailure(*me(),
|
|
current->dir == Interface::FORWARD ? &*current->external_state : nullptr,
|
|
current->dir == Interface::BACKWARD ? nullptr : &*current->external_state);
|
|
pending_states_.erase(current);
|
|
}
|
|
}
|
|
else {
|
|
ROS_DEBUG_STREAM_NAMED("Fallbacks", "Child '" << child->name() << "' exhausted, but produced solutions before, not invoking further fallbacks");
|
|
pending_states_.erase(current);
|
|
}
|
|
// continue processing with next pending state as we didn't runCompute() yet
|
|
}
|
|
}
|
|
|
|
MergerPrivate::MergerPrivate(Merger* me, const std::string& name) : ParallelContainerBasePrivate(me, name) {}
|
|
|
|
void MergerPrivate::resolveInterface(InterfaceFlags expected) {
|
|
ParallelContainerBasePrivate::resolveInterface(expected);
|
|
switch (requiredInterface()) {
|
|
case PROPAGATE_FORWARDS:
|
|
case PROPAGATE_BACKWARDS:
|
|
case UNKNOWN:
|
|
break; // these are supported
|
|
|
|
case GENERATE:
|
|
throw InitStageException(*me_, "Generator stages not yet supported.");
|
|
case CONNECT:
|
|
throw InitStageException(*me_, "Cannot merge connecting stages. Use Connect.");
|
|
default:
|
|
throw InitStageException(*me_, "Children's interface not supported.");
|
|
}
|
|
}
|
|
|
|
Merger::Merger(const std::string& name) : Merger(new MergerPrivate(this, name)) {}
|
|
|
|
void Merger::reset() {
|
|
ParallelContainerBase::reset();
|
|
auto impl = pimpl();
|
|
impl->jmg_merged_.reset();
|
|
impl->source_state_to_solutions_.clear();
|
|
}
|
|
|
|
void Merger::init(const core::RobotModelConstPtr& robot_model) {
|
|
ParallelContainerBase::init(robot_model);
|
|
}
|
|
|
|
Merger::Merger(MergerPrivate* impl) : ParallelContainerBase(impl) {}
|
|
|
|
bool Merger::canCompute() const {
|
|
for (const auto& stage : pimpl()->children())
|
|
if (stage->pimpl()->canCompute())
|
|
return true;
|
|
return false;
|
|
}
|
|
|
|
void Merger::compute() {
|
|
for (const auto& stage : pimpl()->children()) {
|
|
stage->pimpl()->runCompute();
|
|
}
|
|
}
|
|
|
|
void Merger::onNewSolution(const SolutionBase& s) {
|
|
if (s.isFailure()) // ignore failure solutions
|
|
return;
|
|
|
|
auto impl = pimpl();
|
|
switch (impl->interfaceFlags()) {
|
|
case PROPAGATE_FORWARDS:
|
|
case PROPAGATE_BACKWARDS:
|
|
impl->onNewPropagateSolution(s);
|
|
break;
|
|
case GENERATE:
|
|
impl->onNewGeneratorSolution(s);
|
|
break;
|
|
default:
|
|
assert(false);
|
|
}
|
|
}
|
|
|
|
void MergerPrivate::onNewPropagateSolution(const SolutionBase& s) {
|
|
const SubTrajectory* trajectory = dynamic_cast<const SubTrajectory*>(&s);
|
|
if (!trajectory || !trajectory->trajectory()) {
|
|
ROS_ERROR_NAMED("Merger", "Only simple, valid trajectories are supported");
|
|
return;
|
|
}
|
|
|
|
InterfaceFlags dir = interfaceFlags();
|
|
assert(dir == PROPAGATE_FORWARDS || dir == PROPAGATE_BACKWARDS);
|
|
// internal source state
|
|
const InterfaceState* source_state = (dir == PROPAGATE_FORWARDS) ? s.start() : s.end();
|
|
|
|
// map to external source state that is shared by all children
|
|
auto source_it = internalToExternalMap().find(source_state);
|
|
// internal->external mapping for source state should have been created
|
|
assert(source_it != internalToExternalMap().end());
|
|
const InterfaceState* external_source_state = &*source_it->second;
|
|
|
|
// retrieve (or create if necessary) the ChildSolutionMap for the given external source state
|
|
ChildSolutionMap& all_solutions =
|
|
source_state_to_solutions_.insert(std::make_pair(external_source_state, ChildSolutionMap())).first->second;
|
|
|
|
// retrieve (or create if necessary) the ChildSolutionList corresponding to the child
|
|
ChildSolutionList& child_solutions =
|
|
all_solutions.insert(std::make_pair(s.creator(), ChildSolutionList())).first->second;
|
|
// insert the new child solution into the list
|
|
child_solutions.push_back(trajectory);
|
|
|
|
// do we have solutions for all children?
|
|
if (all_solutions.size() < children().size())
|
|
return;
|
|
assert(all_solutions.size() == children().size());
|
|
|
|
// combine the new solution with all solutions from other children
|
|
auto spawner = dir == PROPAGATE_FORWARDS ? &MergerPrivate::sendForward : &MergerPrivate::sendBackward;
|
|
mergeAnyCombination(all_solutions, s, external_source_state->scene(),
|
|
std::bind(spawner, this, std::placeholders::_1, external_source_state));
|
|
}
|
|
|
|
void MergerPrivate::sendForward(SubTrajectory&& t, const InterfaceState* from) {
|
|
// generate target state
|
|
planning_scene::PlanningScenePtr to = from->scene()->diff();
|
|
if (t.trajectory() && !t.trajectory()->empty())
|
|
to->setCurrentState(t.trajectory()->getLastWayPoint());
|
|
StagePrivate::sendForward(*from, InterfaceState(to), std::make_shared<SubTrajectory>(std::move(t)));
|
|
}
|
|
|
|
void MergerPrivate::sendBackward(SubTrajectory&& t, const InterfaceState* to) {
|
|
// generate target state
|
|
planning_scene::PlanningScenePtr from = to->scene()->diff();
|
|
if (t.trajectory() && !t.trajectory()->empty())
|
|
from->setCurrentState(t.trajectory()->getFirstWayPoint());
|
|
StagePrivate::sendBackward(InterfaceState(from), *to, std::make_shared<SubTrajectory>(std::move(t)));
|
|
}
|
|
|
|
void MergerPrivate::onNewGeneratorSolution(const SolutionBase& /* s */) {
|
|
// TODO: implement in similar fashion as onNewPropagateSolution(), but also merge start/end states
|
|
}
|
|
|
|
void MergerPrivate::mergeAnyCombination(const ChildSolutionMap& all_solutions, const SolutionBase& current,
|
|
const planning_scene::PlanningSceneConstPtr& start_scene,
|
|
const Spawner& spawner) {
|
|
std::vector<size_t> indices; // which solution index was considered last for i-th child?
|
|
indices.reserve(children().size());
|
|
|
|
ChildSolutionList sub_solutions;
|
|
sub_solutions.reserve(children().size());
|
|
|
|
// initialize vector of sub solutions
|
|
for (const auto& pair : all_solutions) {
|
|
// all children, except current solution's creator, start with zero index
|
|
indices.push_back(pair.first != current.creator() ? 0 : pair.second.size() - 1);
|
|
sub_solutions.push_back(pair.second[indices.back()]);
|
|
}
|
|
while (true) {
|
|
merge(sub_solutions, start_scene, spawner);
|
|
|
|
// compose next combination
|
|
size_t child = 0;
|
|
for (auto it = all_solutions.cbegin(), end = all_solutions.cend(); it != end; ++it, ++child) {
|
|
if (it->first == current.creator())
|
|
continue; // skip current solution's child
|
|
if (++indices[child] >= it->second.size()) {
|
|
indices[child] = 0; // start over with zero
|
|
sub_solutions[child] = it->second[indices[child]];
|
|
continue; // and continue with next child
|
|
}
|
|
// otherwise, a new solution combination is available
|
|
sub_solutions[child] = it->second[indices[child]];
|
|
break;
|
|
}
|
|
if (child == children().size()) // all combinations exhausted?
|
|
break;
|
|
}
|
|
}
|
|
|
|
void MergerPrivate::merge(const ChildSolutionList& sub_solutions,
|
|
const planning_scene::PlanningSceneConstPtr& start_scene, const Spawner& spawner) {
|
|
// transform vector of SubTrajectories into vector of RobotTrajectories
|
|
std::vector<robot_trajectory::RobotTrajectoryConstPtr> sub_trajectories;
|
|
sub_trajectories.reserve(sub_solutions.size());
|
|
for (const auto& sub : sub_solutions)
|
|
sub_trajectories.push_back(sub->trajectory());
|
|
|
|
moveit::core::JointModelGroup* jmg = jmg_merged_.get();
|
|
robot_trajectory::RobotTrajectoryPtr merged;
|
|
try {
|
|
merged = task_constructor::merge(sub_trajectories, start_scene->getCurrentState(), jmg);
|
|
} catch (const std::runtime_error& e) {
|
|
SubTrajectory t;
|
|
t.markAsFailure();
|
|
t.setComment(e.what());
|
|
spawner(std::move(t));
|
|
return;
|
|
}
|
|
if (jmg_merged_.get() != jmg)
|
|
jmg_merged_.reset(jmg);
|
|
|
|
assert(merged);
|
|
SubTrajectory t(merged);
|
|
|
|
// check merged trajectory for collisions
|
|
std::vector<std::size_t> invalid_index;
|
|
if (!start_scene->isPathValid(*merged, "", true, &invalid_index)) {
|
|
t.markAsFailure();
|
|
std::ostringstream oss;
|
|
oss << "Invalid waypoint(s): ";
|
|
if (invalid_index.size() == merged->getWayPointCount())
|
|
oss << "all";
|
|
else for (size_t i : invalid_index)
|
|
oss << i << ", ";
|
|
t.setComment(oss.str());
|
|
} else {
|
|
// accumulate costs and markers
|
|
double costs = 0.0;
|
|
for (const auto& sub : sub_solutions) {
|
|
costs += sub->cost();
|
|
t.markers().insert(t.markers().end(), sub->markers().begin(), sub->markers().end());
|
|
}
|
|
t.setCost(costs);
|
|
}
|
|
spawner(std::move(t));
|
|
}
|
|
} // namespace task_constructor
|
|
} // namespace moveit
|