mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Merge PR #311: fix Fallbacks
This commit is contained in:
commit
5956e70956
2
.github/workflows/ci.yaml
vendored
2
.github/workflows/ci.yaml
vendored
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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>
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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_;
|
||||
|
||||
@ -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) {
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
|
||||
@ -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) {
|
||||
|
||||
@ -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));
|
||||
}
|
||||
|
||||
@ -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)
|
||||
|
||||
142
demo/src/fallbacks_move_to.cpp
Normal file
142
demo/src/fallbacks_move_to.cpp
Normal 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;
|
||||
}
|
||||
Loading…
Reference in New Issue
Block a user