Merge PR #311: fix Fallbacks

This commit is contained in:
Robert Haschke 2022-01-05 19:42:49 +01:00
commit 5956e70956
14 changed files with 649 additions and 164 deletions

View File

@ -32,7 +32,7 @@ jobs:
DOCKER_RUN_OPTS: >-
-e PRELOAD=libasan.so.5
-e LSAN_OPTIONS="suppressions=$PWD/.github/workflows/lsan.suppressions"
TARGET_CMAKE_ARGS: -DCMAKE_CXX_FLAGS="-fsanitize=address -fno-omit-frame-pointer -O1"
TARGET_CMAKE_ARGS: -DCMAKE_CXX_FLAGS="-fsanitize=address -fno-omit-frame-pointer -O1 -g"
env:
CATKIN_LINT: true

View File

@ -15,7 +15,7 @@
repos:
# Standard hooks
- repo: https://github.com/pre-commit/pre-commit-hooks
rev: v3.4.0
rev: v4.0.1
hooks:
- id: check-added-large-files
- id: check-case-conflict
@ -29,7 +29,7 @@ repos:
- id: trailing-whitespace
- repo: https://github.com/psf/black
rev: 20.8b1
rev: 21.11b1
hooks:
- id: black

View File

@ -150,6 +150,7 @@ public:
void onNewSolution(const SolutionBase& s) override;
};
class FallbacksPrivate;
/** Plan for different alternatives in sequence.
*
* Try to find feasible solutions using first child. Only if this fails,
@ -158,17 +159,23 @@ public:
*/
class Fallbacks : public ParallelContainerBase
{
mutable Stage* active_child_ = nullptr;
inline void replaceImpl();
public:
Fallbacks(const std::string& name = "fallbacks") : ParallelContainerBase(name) {}
PRIVATE_CLASS(Fallbacks);
Fallbacks(const std::string& name = "fallbacks");
void reset() override;
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
bool canCompute() const override;
void compute() override;
protected:
Fallbacks(FallbacksPrivate* impl);
void onNewSolution(const SolutionBase& s) override;
private:
// not needed, we directly use corresponding virtual methods of FallbacksPrivate
bool canCompute() const final { return false; }
void compute() final {}
};
class MergerPrivate;

View File

@ -76,8 +76,7 @@ namespace task_constructor {
class ContainerBasePrivate : public StagePrivate
{
friend class ContainerBase;
friend class ConnectingPrivate; // needs to call protected setStatus()
friend void swap(StagePrivate*& lhs, StagePrivate*& rhs);
friend class ConnectingPrivate; // needed propagate setStatus() in newState()
public:
using container_type = StagePrivate::container_type;
@ -132,10 +131,11 @@ public:
inline const auto& externalToInternalMap() const { return internal_external_.by<EXTERNAL>(); }
/// called by a (direct) child when a solution failed
void onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to);
virtual void onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to);
protected:
ContainerBasePrivate(ContainerBase* me, const std::string& name);
ContainerBasePrivate& operator=(ContainerBasePrivate&& other);
// Set child's push interfaces: allow pushing if child requires it.
inline void setChildsPushBackwardInterface(StagePrivate* child) {
@ -154,10 +154,14 @@ protected:
void setStatus(const Stage* creator, const InterfaceState* source, const InterfaceState* target,
InterfaceState::Status status);
/// copy external_state to a child's interface and remember the link in internal_external map
/// Copy external_state to a child's interface and remember the link in internal_external map
template <Interface::Direction>
void copyState(Interface::iterator external, const InterfacePtr& target, Interface::UpdateFlags updated);
/// lift solution from internal to external level
// non-template version
void copyState(Interface::Direction dir, Interface::iterator external, const InterfacePtr& target,
Interface::UpdateFlags updated);
/// Lift solution from internal to external level
void liftSolution(const SolutionBasePtr& solution, const InterfaceState* internal_from,
const InterfaceState* internal_to);
@ -230,12 +234,91 @@ protected:
void validateInterfaces(const StagePrivate& child, InterfaceFlags& external, bool first = false) const;
private:
/// notify callback for new externally received states
/// notify callback for new externally received interface states
template <typename Interface::Direction>
void onNewExternalState(Interface::iterator external, Interface::UpdateFlags updated);
void propagateStateToAllChildren(Interface::iterator external, Interface::UpdateFlags updated);
// override to customize behavior on received interface states (default: propagateStateToAllChildren())
virtual void initializeExternalInterfaces();
};
PIMPL_FUNCTIONS(ParallelContainerBase)
/* The Fallbacks container needs to implement different behaviour based on its interface.
* Thus, we implement 3 different classes: for Generator, Propagator, and Connect-like interfaces.
* FallbacksPrivate is the common base class for all of them, defining the common API
* to be used by the Fallbacks container.
* The actual interface-specific class is instantiated in initializeExternalInterfaces()
* resp. Fallbacks::replaceImpl() when the actual interface is known.
* The key difference between the 3 variants is how they advance to the next job. */
class FallbacksPrivate : public ParallelContainerBasePrivate
{
public:
FallbacksPrivate(Fallbacks* me, const std::string& name);
FallbacksPrivate(FallbacksPrivate&& other);
void initializeExternalInterfaces() final;
void onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) override;
// virtual methods specific to each variant
virtual void onNewSolution(const SolutionBase& s);
virtual void reset() {}
};
PIMPL_FUNCTIONS(Fallbacks)
/* Class shared between FallbacksPrivateGenerator and FallbacksPrivatePropagator,
which both have the notion of a currently active child stage */
class FallbacksPrivateCommon : public FallbacksPrivate
{
public:
FallbacksPrivateCommon(FallbacksPrivate&& other) : FallbacksPrivate(std::move(other)) {}
/// Advance to next child
inline void nextChild();
/// Advance to the next job, assuming that the current child is exhausted on the current job.
virtual bool nextJob() = 0;
void reset() override;
bool canCompute() const override;
void compute() override;
container_type::const_iterator current_; // currently active child
};
/// Fallbacks implementation for GENERATOR interface
struct FallbacksPrivateGenerator : FallbacksPrivateCommon
{
FallbacksPrivateGenerator(FallbacksPrivate&& old);
bool nextJob() override;
};
/// Fallbacks implementation for FORWARD or BACKWARD interface
struct FallbacksPrivatePropagator : FallbacksPrivateCommon
{
FallbacksPrivatePropagator(FallbacksPrivate&& old);
void reset() override;
void onNewSolution(const SolutionBase& s) override;
bool nextJob() override;
Interface::Direction dir_; // propagation direction
Interface::iterator job_; // pointer to currently processed external state
bool job_has_solutions_; // flag indicating whether the current job generated solutions
};
/// Fallbacks implementation for CONNECT interface
struct FallbacksPrivateConnect : FallbacksPrivate
{
FallbacksPrivateConnect(FallbacksPrivate&& old);
void reset() override;
bool canCompute() const override;
void compute() override;
void onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) override;
template <Interface::Direction dir>
void propagateStateUpdate(Interface::iterator external, Interface::UpdateFlags updated);
mutable container_type::const_iterator active_; // child picked for compute()
};
class WrapperBasePrivate : public ParallelContainerBasePrivate
{
friend class WrapperBase;

View File

@ -61,7 +61,6 @@ class StagePrivate
{
friend class Stage;
friend std::ostream& operator<<(std::ostream& os, const StagePrivate& stage);
friend void swap(StagePrivate*& lhs, StagePrivate*& rhs);
public:
/// container type used to store children
@ -103,6 +102,8 @@ public:
/// direction-based access to pull interface
template <Interface::Direction dir>
inline InterfacePtr pullInterface();
// non-template version
inline InterfacePtr pullInterface(Interface::Direction dir) { return dir == Interface::FORWARD ? starts_ : ends_; }
/// set parent of stage
/// enforce only one parent exists
@ -157,6 +158,8 @@ public:
void computeCost(const InterfaceState& from, const InterfaceState& to, SolutionBase& solution);
protected:
StagePrivate& operator=(StagePrivate&& other);
// associated/owning Stage instance
Stage* me_;
@ -301,6 +304,7 @@ PIMPL_FUNCTIONS(MonitoringGenerator)
class ConnectingPrivate : public ComputeBasePrivate
{
friend class Connecting;
friend struct FallbacksPrivateConnect;
public:
struct StatePair : std::pair<Interface::const_iterator, Interface::const_iterator>

View File

@ -178,6 +178,7 @@ public:
class iterator : public base_type::iterator
{
public:
iterator() = default;
iterator(base_type::iterator other) : base_type::iterator(other) {}
InterfaceState& operator*() const noexcept { return *base_type::iterator::operator*(); }
@ -247,6 +248,16 @@ private:
std::ostream& operator<<(std::ostream& os, const InterfaceState::Priority& prio);
std::ostream& operator<<(std::ostream& os, const Interface& interface);
/// Find index of the iterator in the container. Counting starts at 1. Zero corresponds to not found.
template <typename T>
size_t getIndex(const T& container, typename T::const_iterator search) {
size_t index = 1;
for (typename T::const_iterator it = container.begin(), end = container.end(); it != end; ++it, ++index)
if (it == search)
return index;
return 0;
}
class CostTerm;
class StagePrivate;
class ContainerBasePrivate;

View File

@ -51,15 +51,14 @@ namespace task_constructor {
class TaskPrivate : public WrapperBasePrivate
{
friend class Task;
TaskPrivate& operator=(TaskPrivate&& other);
public:
TaskPrivate(Task* me, const std::string& ns);
const std::string& ns() const { return ns_; }
const ContainerBase* stages() const;
protected:
static void swap(StagePrivate*& lhs, StagePrivate*& rhs);
private:
std::string ns_;
robot_model_loader::RobotModelLoaderPtr robot_model_loader_;

View File

@ -53,8 +53,31 @@ using namespace std::placeholders;
namespace moveit {
namespace task_constructor {
// for debugging of how children interfaces evolve over time
static void printChildrenInterfaces(const ContainerBasePrivate& container, bool success, const Stage& creator,
std::ostream& os = std::cerr);
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 auto conn = dynamic_cast<const ConnectingPrivate*>(creator.pimpl()))
conn->printPendingPairs(os);
os << std::endl;
for (const auto& child : container.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;
}
}
ContainerBasePrivate::ContainerBasePrivate(ContainerBase* me, const std::string& name)
: StagePrivate(me, name)
@ -62,6 +85,33 @@ ContainerBasePrivate::ContainerBasePrivate(ContainerBase* me, const std::string&
, pending_backward_(new Interface)
, pending_forward_(new Interface) {}
ContainerBasePrivate& ContainerBasePrivate::operator=(ContainerBasePrivate&& other) {
assert(internal_external_.empty() && other.internal_external_.empty());
// move StagePrivate members
this->StagePrivate::operator=(std::move(other));
// swapping of container members needed to maintain valid pending_* interfaces
// and children (e.g. for TaskPrivate)
required_interface_ = other.required_interface_;
std::swap(pending_backward_, other.pending_backward_);
std::swap(pending_forward_, other.pending_forward_);
std::swap(children_, other.children_);
// redirect all children's parent pointers to the new parent
auto reparent_children = [](ContainerBasePrivate& self) {
for (auto it = self.children_.begin(), end = self.children_.end(); it != end; ++it) {
auto cimpl = (*it)->pimpl();
cimpl->unparent();
cimpl->setParent(static_cast<ContainerBase*>(self.me_));
cimpl->setParentPosition(it);
}
};
reparent_children(*this);
reparent_children(other);
return *this;
}
ContainerBasePrivate::const_iterator ContainerBasePrivate::childByIndex(int index, bool for_insert) const {
if (!for_insert && index < 0)
--index;
@ -139,6 +189,7 @@ void ContainerBasePrivate::setStatus(const Stage* creator, const InterfaceState*
// if possible (i.e. if target has an external counterpart), escalate setStatus to external interface
if (parent() && trajectories<dir>(*target).empty()) {
// TODO: This was coded with SerialContainer in mind. Not sure, it works for ParallelContainers
auto external{ internalToExternalMap().find(target) };
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
@ -223,6 +274,14 @@ void ContainerBasePrivate::copyState(Interface::iterator external, const Interfa
internalToExternalMap().insert(std::make_pair(&*internal, &*external));
}
void ContainerBasePrivate::copyState(Interface::Direction dir, Interface::iterator external, const InterfacePtr& target,
Interface::UpdateFlags 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);
@ -386,31 +445,6 @@ std::ostream& operator<<(std::ostream& os, const ContainerBase& container) {
return os;
}
// for debugging of how children interfaces evolve over time
static void printChildrenInterfaces(const ContainerBasePrivate& container, bool success, const Stage& creator,
std::ostream& os) {
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.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
@ -492,7 +526,7 @@ void SerialContainer::onNewSolution(const SolutionBase& current) {
}
}
}
// printChildrenInterfaces(*this, true, *current.creator());
// printChildrenInterfaces(*this->pimpl(), true, *current.creator());
// finally, store + announce new solutions to external interface
for (const auto& solution : sorted)
@ -557,9 +591,9 @@ void SerialContainerPrivate::resolveInterface(InterfaceFlags expected) {
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, Interface::UpdateFlags updated) {
starts_ = std::make_shared<Interface>([this, target](Interface::iterator it, Interface::UpdateFlags updated) {
this->copyState<Interface::FORWARD>(it, target, updated);
}));
});
} catch (InitStageException& e) {
exceptions.append(e);
}
@ -570,6 +604,7 @@ void SerialContainerPrivate::resolveInterface(InterfaceFlags expected) {
StagePrivate* child_impl = (**it).pimpl();
StagePrivate* previous_impl = (**previous_it).pimpl();
child_impl->resolveInterface(invert(previous_impl->requiredInterface()) & START_IF_MASK);
child_impl = (**it).pimpl(); // re-assign as pimpl_ pointer of a Fallback container will change!
connect(*previous_impl, *child_impl);
} catch (InitStageException& e) {
exceptions.append(e);
@ -582,9 +617,9 @@ void SerialContainerPrivate::resolveInterface(InterfaceFlags expected) {
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, Interface::UpdateFlags updated) {
ends_ = std::make_shared<Interface>([this, target](Interface::iterator it, Interface::UpdateFlags updated) {
this->copyState<Interface::BACKWARD>(it, target, updated);
}));
});
} catch (InitStageException& e) {
exceptions.append(e);
}
@ -640,9 +675,8 @@ bool SerialContainer::canCompute() const {
void SerialContainer::compute() {
for (const auto& stage : pimpl()->children()) {
if (!stage->pimpl()->canCompute())
continue;
stage->pimpl()->runCompute();
if (stage->pimpl()->canCompute())
stage->pimpl()->runCompute();
}
}
@ -663,8 +697,8 @@ void ParallelContainerBasePrivate::resolveInterface(InterfaceFlags expected) {
child_impl->resolveInterface(expected);
validateInterfaces(*child_impl, expected, first);
// initialize push connections of children according to their demands
setChildsPushForwardInterface(child_impl);
setChildsPushBackwardInterface(child_impl);
setChildsPushForwardInterface(child_impl);
first = false;
} catch (InitStageException& e) {
exceptions.append(e);
@ -675,17 +709,21 @@ void ParallelContainerBasePrivate::resolveInterface(InterfaceFlags expected) {
if (exceptions)
throw exceptions;
// States received by the container need to be copied to all children's pull interfaces.
if (expected & READS_START)
starts().reset(new Interface([this](Interface::iterator external, Interface::UpdateFlags updated) {
this->onNewExternalState<Interface::FORWARD>(external, updated);
}));
if (expected & READS_END)
ends().reset(new Interface([this](Interface::iterator external, Interface::UpdateFlags updated) {
this->onNewExternalState<Interface::BACKWARD>(external, updated);
}));
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() = std::make_shared<Interface>([this](Interface::iterator external, Interface::UpdateFlags updated) {
this->propagateStateToAllChildren<Interface::FORWARD>(external, updated);
});
if (requiredInterface() & READS_END)
ends() = std::make_shared<Interface>([this](Interface::iterator external, Interface::UpdateFlags updated) {
this->propagateStateToAllChildren<Interface::BACKWARD>(external, updated);
});
}
void ParallelContainerBasePrivate::validateInterfaces(const StagePrivate& child, InterfaceFlags& external,
@ -720,7 +758,7 @@ void ParallelContainerBasePrivate::validateConnectivity() const {
}
template <Interface::Direction dir>
void ParallelContainerBasePrivate::onNewExternalState(Interface::iterator external, Interface::UpdateFlags updated) {
void ParallelContainerBasePrivate::propagateStateToAllChildren(Interface::iterator external, Interface::UpdateFlags updated) {
for (const Stage::pointer& stage : children())
copyState<dir>(external, stage->pimpl()->pullInterface<dir>(), updated);
}
@ -793,44 +831,240 @@ 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() {
active_child_ = nullptr;
ParallelContainerBase::reset();
pimpl()->reset();
}
void Fallbacks::init(const moveit::core::RobotModelConstPtr& robot_model) {
ParallelContainerBase::init(robot_model);
active_child_ = pimpl()->children().front().get();
}
bool Fallbacks::canCompute() const {
while (active_child_) {
StagePrivate* child = active_child_->pimpl();
if (child->canCompute())
return true;
// active child failed, continue with next
auto next = child->it();
++next;
if (next == pimpl()->children().end())
active_child_ = nullptr;
else
active_child_ = next->get();
}
return false;
}
void Fallbacks::compute() {
if (!active_child_)
return;
active_child_->pimpl()->runCompute();
pimpl()->reset();
}
void Fallbacks::onNewSolution(const SolutionBase& s) {
liftSolution(s);
pimpl()->onNewSolution(s);
}
inline void Fallbacks::replaceImpl() {
FallbacksPrivate *impl = pimpl();
switch (pimpl()->requiredInterface()) {
case GENERATE:
impl = new FallbacksPrivateGenerator(std::move(*impl));
break;
case PROPAGATE_FORWARDS:
case PROPAGATE_BACKWARDS:
impl = new FallbacksPrivatePropagator(std::move(*impl));
break;
case CONNECT:
// For now, we only support Connecting children
for (const auto& child : impl->children())
if (!dynamic_cast<Connecting*>(child.get()))
throw std::runtime_error("CONNECT-like interface is only supported for Connecting children");
impl = new FallbacksPrivateConnect(std::move(*impl));
break;
}
delete pimpl_;
pimpl_ = impl;
}
FallbacksPrivate::FallbacksPrivate(Fallbacks* me, const std::string& name)
: ParallelContainerBasePrivate(me, name) {}
FallbacksPrivate::FallbacksPrivate(FallbacksPrivate&& other)
: ParallelContainerBasePrivate(static_cast<Fallbacks*>(other.me()), "") {
// move contents of other
this->ParallelContainerBasePrivate::operator=(std::move(other));
}
void FallbacksPrivate::initializeExternalInterfaces() {
// Here we know the final interface of the container (and all its children)
// Thus replace, this pimpl() with a new interface-specific one:
static_cast<Fallbacks*>(me())->replaceImpl();
}
void FallbacksPrivate::onNewSolution(const SolutionBase& s) {
// printChildrenInterfaces(*this, true, *s.creator());
static_cast<Fallbacks*>(me())->liftSolution(s);
}
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)
// printChildrenInterfaces(*this, false, child);
(void)child;
}
void FallbacksPrivateCommon::reset() {
current_ = children().begin();
}
bool FallbacksPrivateCommon::canCompute() const {
while(current_ != children().end() && // not completely exhausted
!(*current_)->pimpl()->canCompute()) // but current child cannot compute
return const_cast<FallbacksPrivateCommon*>(this)->nextJob(); // advance to next job
// return value: current child is well defined and thus can compute?
return current_ != children().end();
}
void FallbacksPrivateCommon::compute() {
(*current_)->pimpl()->runCompute();
}
inline void FallbacksPrivateCommon::nextChild() {
if (std::next(current_) != children().end())
ROS_DEBUG_STREAM_NAMED("Fallbacks", "Child '" << (*current_)->name() << "' failed, trying next one.");
++current_; // advance to next child
}
FallbacksPrivateGenerator::FallbacksPrivateGenerator(FallbacksPrivate&& old)
: FallbacksPrivateCommon(std::move(old)) { FallbacksPrivateCommon::reset(); }
bool FallbacksPrivateGenerator::nextJob() {
assert(current_ != children().end() && !(*current_)->pimpl()->canCompute());
// don't advance to next child when we already produced solutions
if (!solutions_.empty()) {
current_ = children().end(); // indicate that we are exhausted
return false;
}
do { nextChild(); }
while (current_ != children().end() && !(*current_)->pimpl()->canCompute());
// return value shall indicate current_->canCompute()
return current_ != children().end();
}
FallbacksPrivatePropagator::FallbacksPrivatePropagator(FallbacksPrivate&& old)
: FallbacksPrivateCommon(std::move(old)) {
switch (requiredInterface()) {
case PROPAGATE_FORWARDS:
dir_ = Interface::FORWARD;
starts() = std::make_shared<Interface>();
break;
case PROPAGATE_BACKWARDS:
dir_ = Interface::BACKWARD;
ends() = std::make_shared<Interface>();
break;
default:
assert(false);
}
FallbacksPrivatePropagator::reset();
}
void FallbacksPrivatePropagator::reset() {
FallbacksPrivateCommon::reset();
job_ = pullInterface(dir_)->end(); // indicate fresh start
job_has_solutions_ = false;
}
void FallbacksPrivatePropagator::onNewSolution(const SolutionBase& s) {
job_has_solutions_ = true;
FallbacksPrivateCommon::onNewSolution(s);
}
bool FallbacksPrivatePropagator::nextJob() {
assert(current_ != children().end() && !(*current_)->pimpl()->canCompute());
const auto jobs = pullInterface(dir_);
if (job_ != jobs->end()) { // current job exists, but is exhausted on current child
if (!job_has_solutions_) // job didn't produce solutions -> feed to next child
nextChild();
else
current_ = children().end(); // indicate that this job is exhausted on all children
}
job_has_solutions_ = false;
if (current_ == children().end()) { // all children processed the job_
if (job_ != jobs->end()) {
jobs->remove(job_); // we don't need the job in our interface list anymore
job_ = jobs->end(); // indicate that we need to fetch a new job
}
current_ = children().begin(); // start next job with first child again
}
// pick next job if needed and possible
if (job_ == jobs->end()) { // need to pick next job
if (!jobs->empty() && jobs->front()->priority().enabled())
job_ = jobs->begin();
else
return false; // no more jobs available
}
// When arriving here, we have a valid job_ and a current_ child to feed it. Let's do that.
copyState(dir_, job_, (*current_)->pimpl()->pullInterface(dir_), Interface::UpdateFlags());
return true;
}
FallbacksPrivateConnect::FallbacksPrivateConnect(FallbacksPrivate&& old)
: FallbacksPrivate(std::move(old)) {
starts_ = std::make_shared<Interface>(
std::bind(&FallbacksPrivateConnect::propagateStateUpdate<Interface::FORWARD>, this, std::placeholders::_1, std::placeholders::_2));
ends_ = std::make_shared<Interface>(
std::bind(&FallbacksPrivateConnect::propagateStateUpdate<Interface::BACKWARD>, this, std::placeholders::_1, std::placeholders::_2));
FallbacksPrivateConnect::reset();
}
void FallbacksPrivateConnect::reset() {
active_ = children().end();
}
template <Interface::Direction dir>
void FallbacksPrivateConnect::propagateStateUpdate(Interface::iterator external, Interface::UpdateFlags updated) {
copyState<dir>(external, children().front()->pimpl()->pullInterface(dir), updated);
// As we use the Interface* from the first child for all children (we just populate their pending lists)
// there is no need to explicitly propagate state updates to other children.
}
bool FallbacksPrivateConnect::canCompute() const {
for (auto it=children().begin(), end=children().end(); it!=end; ++it)
if ((*it)->pimpl()->canCompute()) {
active_ = it;
return true;
}
active_ = children().end();
return false;
}
void FallbacksPrivateConnect::compute() {
// Alternatively, we could also compute() all children that canCompute()
assert(active_ != children().end());
(*active_)->pimpl()->runCompute();
}
void FallbacksPrivateConnect::onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) {
// expect failure to be reported from active child
assert(active_ != children().end() && active_->get() == &child);
// ... thus we can use std::next(active_) to find the next child
auto next = std::next(active_);
auto findIteratorFor = [](const InterfaceState* state, const Interface& interface) {
auto it = std::find(interface.begin(), interface.end(), state);
assert(it != interface.end());
return it;
};
if (next != children().end()) { // pass job to next child
auto next_con = static_cast<ConnectingPrivate*>(const_cast<StagePrivate*>((*next)->pimpl()));
auto first_con = static_cast<const ConnectingPrivate*>(children().front()->pimpl());
auto fromIt = findIteratorFor(from, *first_con->starts());
auto toIt = findIteratorFor(to, *first_con->ends());
next_con->pending.insert(std::make_pair(fromIt, toIt));
} else // or report failure to parent
parent()->pimpl()->onNewFailure(*me(), from, to);
}
MergerPrivate::MergerPrivate(Merger* me, const std::string& name) : ParallelContainerBasePrivate(me, name) {}
void MergerPrivate::resolveInterface(InterfaceFlags expected) {

View File

@ -106,6 +106,33 @@ StagePrivate::StagePrivate(Stage* me, const std::string& name)
, parent_{ nullptr }
, introspection_{ nullptr } {}
StagePrivate& StagePrivate::operator=(StagePrivate&& other) {
assert(typeid(*this) == typeid(other));
assert(states_.empty() && other.states_.empty());
assert((!starts_ || starts_->empty()) && (!other.starts_ || other.starts_->empty()));
assert((!ends_ || ends_->empty()) && (!other.ends_ || other.ends_->empty()));
assert(solutions_.empty() && other.solutions_.empty());
assert(failures_.empty() && other.failures_.empty());
// me_ must not be changed!
name_ = std::move(other.name_);
properties_ = std::move(other.properties_);
cost_term_ = std::move(other.cost_term_);
solution_cbs_ = std::move(other.solution_cbs_);
starts_ = std::move(other.starts_);
ends_ = std::move(other.ends_);
prev_ends_ = std::move(other.prev_ends_);
next_starts_ = std::move(other.next_starts_);
parent_ = std::move(other.parent_);
it_ = std::move(other.it_);
other.unparent();
return *this;
}
InterfaceFlags StagePrivate::interfaceFlags() const {
InterfaceFlags f;
if (starts())
@ -483,14 +510,14 @@ void PropagatingEitherWayPrivate::initInterface(PropagatingEitherWay::Direction
case PropagatingEitherWay::FORWARD:
required_interface_ = PROPAGATE_FORWARDS;
if (!starts_) // keep existing interface if possible
starts_.reset(new Interface());
starts_ = std::make_shared<Interface>();
ends_.reset();
return;
case PropagatingEitherWay::BACKWARD:
required_interface_ = PROPAGATE_BACKWARDS;
starts_.reset();
if (!ends_) // keep existing interface if possible
ends_.reset(new Interface());
ends_ = std::make_shared<Interface>();
return;
case PropagatingEitherWay::AUTO:
required_interface_ = UNKNOWN;
@ -688,10 +715,10 @@ void MonitoringGeneratorPrivate::solutionCB(const SolutionBase& s) {
}
ConnectingPrivate::ConnectingPrivate(Connecting* me, const std::string& name) : ComputeBasePrivate(me, name) {
starts_.reset(new Interface(std::bind(&ConnectingPrivate::newState<Interface::BACKWARD>, this, std::placeholders::_1,
std::placeholders::_2)));
ends_.reset(new Interface(std::bind(&ConnectingPrivate::newState<Interface::FORWARD>, this, std::placeholders::_1,
std::placeholders::_2)));
starts_ = std::make_shared<Interface>(std::bind(&ConnectingPrivate::newState<Interface::BACKWARD>, this,
std::placeholders::_1, std::placeholders::_2));
ends_ = std::make_shared<Interface>(
std::bind(&ConnectingPrivate::newState<Interface::FORWARD>, this, std::placeholders::_1, std::placeholders::_2));
}
InterfaceFlags ConnectingPrivate::requiredInterface() const {
@ -768,9 +795,23 @@ void ConnectingPrivate::newState(Interface::iterator it, Interface::UpdateFlags
// pass creator=nullptr to skip hasPendingOpposites() check as we did this here already
parent_pimpl->setStatus<dir>(nullptr, nullptr, &*it, InterfaceState::Status::ARMED);
}
// std::cerr << name_ << ": ";
// printPendingPairs(std::cerr);
// std::cerr << std::endl;
#if 0
auto& os = std::cerr;
for (auto d : { Interface::FORWARD, Interface::BACKWARD }) {
bool fw = (d == Interface::FORWARD);
if (fw)
os << " " << std::setw(10) << std::left << this->name();
else
os << std::setw(12) << std::right << "";
if (dir != d)
os << (updated ? " !" : " +");
else
os << " ";
os << (fw ? "" : "") << this->pullInterface(d) << ": " << *this->pullInterface(d) << std::endl;
}
os << std::setw(15) << " ";
printPendingPairs(os) << std::endl;
#endif
}
// Check whether there are pending feasible states (other than source) that could connect to target.
@ -815,19 +856,13 @@ void ConnectingPrivate::compute() {
std::ostream& ConnectingPrivate::printPendingPairs(std::ostream& os) const {
const char* reset = InterfaceState::STATUS_COLOR[3];
for (const auto& candidate : pending) {
// find indeces of InterfaceState pointers in start/end Interfaces
unsigned int first = 0, second = 0;
std::find_if(starts()->begin(), starts()->end(), [&](const InterfaceState* s) {
++first;
return &*candidate.first == s;
});
std::find_if(ends()->begin(), ends()->end(), [&](const InterfaceState* s) {
++second;
return &*candidate.second == s;
});
size_t first = getIndex(*starts(), candidate.first);
size_t second = getIndex(*ends(), candidate.second);
os << InterfaceState::STATUS_COLOR[candidate.first->priority().status()] << first << reset << ":"
<< InterfaceState::STATUS_COLOR[candidate.second->priority().status()] << second << reset << " ";
}
if (pending.empty())
os << "---";
return os;
}

View File

@ -74,30 +74,14 @@ namespace task_constructor {
TaskPrivate::TaskPrivate(Task* me, const std::string& ns)
: WrapperBasePrivate(me, std::string()), ns_(rosNormalizeName(ns)), preempt_requested_(false) {}
void swap(StagePrivate*& lhs, StagePrivate*& rhs) {
// It only makes sense to swap pimpl instances of a Task!
// However, due to member protection rules, we can only implement it here
assert(typeid(lhs) == typeid(rhs));
// swap instances
::std::swap(lhs, rhs);
// as well as their me_ pointers
::std::swap(lhs->me_, rhs->me_);
// and redirect the parent pointers of children to new parents
auto& lhs_children = static_cast<ContainerBasePrivate*>(lhs)->children_;
for (auto it = lhs_children.begin(), end = lhs_children.end(); it != end; ++it) {
(*it)->pimpl()->unparent();
(*it)->pimpl()->setParent(static_cast<ContainerBase*>(lhs->me_));
(*it)->pimpl()->setParentPosition(it);
}
auto& rhs_children = static_cast<ContainerBasePrivate*>(rhs)->children_;
for (auto it = rhs_children.begin(), end = rhs_children.end(); it != end; ++it) {
(*it)->pimpl()->unparent();
(*it)->pimpl()->setParent(static_cast<ContainerBase*>(rhs->me_));
(*it)->pimpl()->setParentPosition(it);
}
TaskPrivate& TaskPrivate::operator=(TaskPrivate&& other) {
this->WrapperBasePrivate::operator=(std::move(other));
ns_ = std::move(other.ns_);
introspection_ = std::move(other.introspection_);
robot_model_ = std::move(other.robot_model_);
robot_model_loader_ = std::move(other.robot_model_loader_);
task_cbs_ = std::move(other.task_cbs_);
return *this;
}
const ContainerBase* TaskPrivate::stages() const {
@ -122,7 +106,7 @@ Task::Task(Task&& other) // NOLINT(performance-noexcept-move-constructor)
Task& Task::operator=(Task&& other) { // NOLINT(performance-noexcept-move-constructor)
clear(); // remove all stages of current task
swap(this->pimpl_, other.pimpl_);
*static_cast<TaskPrivate*>(pimpl_) = std::move(*static_cast<TaskPrivate*>(other.pimpl_));
return *this;
}

View File

@ -586,10 +586,6 @@ TEST(Task, move) {
Task t2 = std::move(t1);
EXPECT_EQ(t2.stages()->numChildren(), 2u);
EXPECT_EQ(t1.stages()->numChildren(), 0u); // NOLINT(clang-analyzer-cplusplus.Move)
t1 = std::move(t2);
EXPECT_EQ(t1.stages()->numChildren(), 2u);
EXPECT_EQ(t2.stages()->numChildren(), 0u); // NOLINT(clang-analyzer-cplusplus.Move)
}
TEST(Task, reuse) {

View File

@ -17,7 +17,7 @@ using namespace moveit::task_constructor;
using FallbacksFixtureGenerator = TaskTestBase;
TEST_F(FallbacksFixtureGenerator, DISABLED_stayWithFirstSuccessful) {
TEST_F(FallbacksFixtureGenerator, stayWithFirstSuccessful) {
auto fallback = std::make_unique<Fallbacks>("Fallbacks");
fallback->add(std::make_unique<GeneratorMockup>(PredefinedCosts::single(INF)));
fallback->add(std::make_unique<GeneratorMockup>(PredefinedCosts::single(1.0)));
@ -55,7 +55,7 @@ TEST_F(FallbacksFixturePropagate, failingWithFailedSolutions) {
EXPECT_EQ(t.solutions().size(), 0u);
}
TEST_F(FallbacksFixturePropagate, DISABLED_ComputeFirstSuccessfulStageOnly) {
TEST_F(FallbacksFixturePropagate, computeFirstSuccessfulStageOnly) {
t.add(std::make_unique<GeneratorMockup>());
auto fallbacks = std::make_unique<Fallbacks>("Fallbacks");
@ -67,7 +67,7 @@ TEST_F(FallbacksFixturePropagate, DISABLED_ComputeFirstSuccessfulStageOnly) {
EXPECT_EQ(t.numSolutions(), 1u);
}
TEST_F(FallbacksFixturePropagate, DISABLED_ComputeFirstSuccessfulStagePerSolutionOnly) {
TEST_F(FallbacksFixturePropagate, computeFirstSuccessfulStagePerSolutionOnly) {
t.add(std::make_unique<GeneratorMockup>(PredefinedCosts({ 2.0, 1.0 })));
// duplicate generator solutions with resulting costs: 4, 2 | 3, 1
t.add(std::make_unique<ForwardMockup>(PredefinedCosts({ 2.0, 0.0, 2.0, 0.0 }), 2));
@ -78,10 +78,11 @@ TEST_F(FallbacksFixturePropagate, DISABLED_ComputeFirstSuccessfulStagePerSolutio
t.add(std::move(fallbacks));
EXPECT_TRUE(t.plan());
EXPECT_COSTS(t.solutions(), testing::ElementsAre(113, 124, 211, 222));
EXPECT_COSTS(t.solutions(), testing::ElementsAre(113, 124, 212, 221));
}
TEST_F(FallbacksFixturePropagate, DISABLED_UpdateSolutionOrder) {
// requires individual job control in Fallbacks's children
TEST_F(FallbacksFixturePropagate, DISABLED_updateSolutionOrder) {
t.add(std::make_unique<BackwardMockup>(PredefinedCosts({ 10.0, 0.0 })));
t.add(std::make_unique<GeneratorMockup>(PredefinedCosts({ 1.0, 2.0 })));
// available solutions (sorted) in individual runs of fallbacks: 1 | 11, 2 | 2, 11
@ -100,7 +101,8 @@ TEST_F(FallbacksFixturePropagate, DISABLED_UpdateSolutionOrder) {
EXPECT_COSTS(t.solutions(), testing::ElementsAre(2)); // expecting less costly solution as result
}
TEST_F(FallbacksFixturePropagate, DISABLED_MultipleActivePendingStates) {
// requires individual job control in Fallbacks's children
TEST_F(FallbacksFixturePropagate, DISABLED_multipleActivePendingStates) {
t.add(std::make_unique<GeneratorMockup>(PredefinedCosts({ 2.0, 1.0, 3.0 })));
// use a fallback container to delay computation: the 1st child never succeeds, but only the 2nd
auto inner = std::make_unique<Fallbacks>("Inner");
@ -117,7 +119,7 @@ TEST_F(FallbacksFixturePropagate, DISABLED_MultipleActivePendingStates) {
// check that first solution is not marked as pruned
}
TEST_F(FallbacksFixturePropagate, DISABLED_successfulWithMixedSolutions) {
TEST_F(FallbacksFixturePropagate, successfulWithMixedSolutions) {
t.add(std::make_unique<GeneratorMockup>());
auto fallback = std::make_unique<Fallbacks>("Fallbacks");
@ -129,7 +131,7 @@ TEST_F(FallbacksFixturePropagate, DISABLED_successfulWithMixedSolutions) {
EXPECT_COSTS(t.solutions(), testing::ElementsAre(1.0));
}
TEST_F(FallbacksFixturePropagate, DISABLED_successfulWithMixedSolutions2) {
TEST_F(FallbacksFixturePropagate, successfulWithMixedSolutions2) {
t.add(std::make_unique<GeneratorMockup>());
auto fallback = std::make_unique<Fallbacks>("Fallbacks");
@ -141,7 +143,7 @@ TEST_F(FallbacksFixturePropagate, DISABLED_successfulWithMixedSolutions2) {
EXPECT_COSTS(t.solutions(), testing::ElementsAre(1.0));
}
TEST_F(FallbacksFixturePropagate, DISABLED_ActiveChildReset) {
TEST_F(FallbacksFixturePropagate, activeChildReset) {
t.add(std::make_unique<GeneratorMockup>(PredefinedCosts({ 1.0, INF, 3.0 })));
auto fallbacks = std::make_unique<Fallbacks>("Fallbacks");
@ -159,29 +161,16 @@ TEST_F(FallbacksFixturePropagate, DISABLED_ActiveChildReset) {
using FallbacksFixtureConnect = TaskTestBase;
TEST_F(FallbacksFixtureConnect, DISABLED_ConnectStageInsideFallbacks) {
TEST_F(FallbacksFixtureConnect, connectStageInsideFallbacks) {
t.add(std::make_unique<GeneratorMockup>(PredefinedCosts({ 1.0, 2.0 })));
auto fallbacks = std::make_unique<Fallbacks>("Fallbacks");
fallbacks->add(std::make_unique<ConnectMockup>(PredefinedCosts::constant(0.0)));
fallbacks->add(std::make_unique<ConnectMockup>(PredefinedCosts({ 0.0, 0.0, INF, 0.0 })));
fallbacks->add(std::make_unique<ConnectMockup>(PredefinedCosts::constant(100.0)));
t.add(std::move(fallbacks));
t.add(std::make_unique<GeneratorMockup>(PredefinedCosts({ 10.0, 20.0 })));
EXPECT_TRUE(t.plan());
EXPECT_COSTS(t.solutions(), testing::ElementsAre(11, 12, 21, 22));
}
int main(int argc, char** argv) {
for (int i = 1; i < argc; ++i) {
if (strcmp(argv[i], "--debug") == 0) {
if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug))
ros::console::notifyLoggerLevelsChanged();
break;
}
}
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
EXPECT_COSTS(t.solutions(), testing::ElementsAre(11, 12, 22, 121));
}

View File

@ -50,6 +50,7 @@ demo(cartesian)
demo(modular)
demo(alternative_path_costs)
demo(ik_clearance_cost)
demo(fallbacks_move_to)
demo(pick_place_demo)
target_link_libraries(${PROJECT_NAME}_pick_place_demo ${PROJECT_NAME}_pick_place_task)

View File

@ -0,0 +1,142 @@
#include <ros/ros.h>
#include <moveit/robot_model/robot_model.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/task_constructor/moveit_compat.h>
#include <moveit/task_constructor/task.h>
#include <moveit/task_constructor/container.h>
#include <moveit/task_constructor/solvers/cartesian_path.h>
#include <moveit/task_constructor/solvers/pipeline_planner.h>
#include <moveit/task_constructor/stages.h>
constexpr double TAU = 2 * M_PI;
using namespace moveit::task_constructor;
/** CurrentState -> Fallbacks( MoveTo<CartesianPath>, MoveTo<PTP>, MoveTo<OMPL> )*/
int main(int argc, char** argv) {
ros::init(argc, argv, "mtc_tutorial");
ros::AsyncSpinner spinner{ 1 };
spinner.start();
// setup Task
Task t;
t.loadRobotModel();
const moveit::core::RobotModelConstPtr robot{ t.getRobotModel() };
assert(robot->getName() == "panda");
// setup solvers
auto cartesian = std::make_shared<solvers::CartesianPath>();
cartesian->setJumpThreshold(2.0);
const auto ptp = []() {
auto pp{ std::make_shared<solvers::PipelinePlanner>("pilz_industrial_motion_planner") };
pp->setPlannerId("PTP");
return pp;
}();
const auto rrtconnect = []() {
auto pp{ std::make_shared<solvers::PipelinePlanner>("ompl") };
pp->setPlannerId("RRTConnectkConfigDefault");
return pp;
}();
// target state for Task
std::map<std::string, double> target_state;
robot->getJointModelGroup("panda_arm")->getVariableDefaultPositions("ready", target_state);
target_state["panda_joint1"] = +TAU / 8;
// define initial scenes
auto initial_scene{ std::make_shared<planning_scene::PlanningScene>(robot) };
initial_scene->getCurrentStateNonConst().setToDefaultValues(robot->getJointModelGroup("panda_arm"), "ready");
auto initial_alternatives = std::make_unique<Alternatives>("initial states");
{
// can reach target with Cartesian motion
auto fixed{ std::make_unique<stages::FixedState>("current state") };
auto scene{ initial_scene->diff() };
scene->getCurrentStateNonConst().setVariablePositions({ { "panda_joint1", -TAU / 8 } });
fixed->setState(scene);
initial_alternatives->add(std::move(fixed));
}
{
// Cartesian motion to target is impossible, but PTP is collision-free
auto fixed{ std::make_unique<stages::FixedState>("current state") };
auto scene{ initial_scene->diff() };
scene->getCurrentStateNonConst().setVariablePositions({
{ "panda_joint1", +TAU / 8 },
{ "panda_joint4", 0 },
});
fixed->setState(scene);
initial_alternatives->add(std::move(fixed));
}
{
// Cartesian and PTP motion to target would be in collision
auto fixed = std::make_unique<stages::FixedState>("current state");
auto scene{ initial_scene->diff() };
scene->getCurrentStateNonConst().setVariablePositions({ { "panda_joint1", -TAU / 8 } });
scene->processCollisionObjectMsg([]() {
moveit_msgs::CollisionObject co;
co.id = "box";
co.header.frame_id = "panda_link0";
co.operation = co.ADD;
#if MOVEIT_HAS_OBJECT_POSE
auto& pose{ co.pose };
#else
co.primitive_poses.emplace_back();
auto& pose{ co.primitive_poses[0] };
#endif
pose = []() {
geometry_msgs::Pose p;
p.position.x = 0.3;
p.position.y = 0.0;
p.position.z = 0.64 / 2;
p.orientation.w = 1.0;
return p;
}();
co.primitives.push_back([]() {
shape_msgs::SolidPrimitive sp;
sp.type = sp.BOX;
sp.dimensions = { 0.2, 0.05, 0.64 };
return sp;
}());
return co;
}());
fixed->setState(scene);
initial_alternatives->add(std::move(fixed));
}
t.add(std::move(initial_alternatives));
// fallbacks to reach target_state
auto fallbacks = std::make_unique<Fallbacks>("move to other side");
auto add_to_fallbacks{ [&](auto& solver, auto& name) {
auto move_to = std::make_unique<stages::MoveTo>(name, solver);
move_to->setGroup("panda_arm");
move_to->setGoal(target_state);
fallbacks->add(std::move(move_to));
} };
add_to_fallbacks(cartesian, "Cartesian path");
add_to_fallbacks(ptp, "PTP path");
add_to_fallbacks(rrtconnect, "RRT path");
t.add(std::move(fallbacks));
try {
t.init();
std::cout << t << std::endl;
t.plan();
} catch (const InitStageException& e) {
std::cout << e << std::endl;
}
ros::waitForShutdown();
return 0;
}