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: >-
|
DOCKER_RUN_OPTS: >-
|
||||||
-e PRELOAD=libasan.so.5
|
-e PRELOAD=libasan.so.5
|
||||||
-e LSAN_OPTIONS="suppressions=$PWD/.github/workflows/lsan.suppressions"
|
-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:
|
env:
|
||||||
CATKIN_LINT: true
|
CATKIN_LINT: true
|
||||||
|
|||||||
@ -15,7 +15,7 @@
|
|||||||
repos:
|
repos:
|
||||||
# Standard hooks
|
# Standard hooks
|
||||||
- repo: https://github.com/pre-commit/pre-commit-hooks
|
- repo: https://github.com/pre-commit/pre-commit-hooks
|
||||||
rev: v3.4.0
|
rev: v4.0.1
|
||||||
hooks:
|
hooks:
|
||||||
- id: check-added-large-files
|
- id: check-added-large-files
|
||||||
- id: check-case-conflict
|
- id: check-case-conflict
|
||||||
@ -29,7 +29,7 @@ repos:
|
|||||||
- id: trailing-whitespace
|
- id: trailing-whitespace
|
||||||
|
|
||||||
- repo: https://github.com/psf/black
|
- repo: https://github.com/psf/black
|
||||||
rev: 20.8b1
|
rev: 21.11b1
|
||||||
hooks:
|
hooks:
|
||||||
- id: black
|
- id: black
|
||||||
|
|
||||||
|
|||||||
@ -150,6 +150,7 @@ public:
|
|||||||
void onNewSolution(const SolutionBase& s) override;
|
void onNewSolution(const SolutionBase& s) override;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
class FallbacksPrivate;
|
||||||
/** Plan for different alternatives in sequence.
|
/** Plan for different alternatives in sequence.
|
||||||
*
|
*
|
||||||
* Try to find feasible solutions using first child. Only if this fails,
|
* Try to find feasible solutions using first child. Only if this fails,
|
||||||
@ -158,17 +159,23 @@ public:
|
|||||||
*/
|
*/
|
||||||
class Fallbacks : public ParallelContainerBase
|
class Fallbacks : public ParallelContainerBase
|
||||||
{
|
{
|
||||||
mutable Stage* active_child_ = nullptr;
|
inline void replaceImpl();
|
||||||
|
|
||||||
public:
|
public:
|
||||||
Fallbacks(const std::string& name = "fallbacks") : ParallelContainerBase(name) {}
|
PRIVATE_CLASS(Fallbacks);
|
||||||
|
Fallbacks(const std::string& name = "fallbacks");
|
||||||
|
|
||||||
void reset() override;
|
void reset() override;
|
||||||
void init(const moveit::core::RobotModelConstPtr& robot_model) 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;
|
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;
|
class MergerPrivate;
|
||||||
|
|||||||
@ -76,8 +76,7 @@ namespace task_constructor {
|
|||||||
class ContainerBasePrivate : public StagePrivate
|
class ContainerBasePrivate : public StagePrivate
|
||||||
{
|
{
|
||||||
friend class ContainerBase;
|
friend class ContainerBase;
|
||||||
friend class ConnectingPrivate; // needs to call protected setStatus()
|
friend class ConnectingPrivate; // needed propagate setStatus() in newState()
|
||||||
friend void swap(StagePrivate*& lhs, StagePrivate*& rhs);
|
|
||||||
|
|
||||||
public:
|
public:
|
||||||
using container_type = StagePrivate::container_type;
|
using container_type = StagePrivate::container_type;
|
||||||
@ -132,10 +131,11 @@ public:
|
|||||||
inline const auto& externalToInternalMap() const { return internal_external_.by<EXTERNAL>(); }
|
inline const auto& externalToInternalMap() const { return internal_external_.by<EXTERNAL>(); }
|
||||||
|
|
||||||
/// called by a (direct) child when a solution failed
|
/// 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:
|
protected:
|
||||||
ContainerBasePrivate(ContainerBase* me, const std::string& name);
|
ContainerBasePrivate(ContainerBase* me, const std::string& name);
|
||||||
|
ContainerBasePrivate& operator=(ContainerBasePrivate&& other);
|
||||||
|
|
||||||
// Set child's push interfaces: allow pushing if child requires it.
|
// Set child's push interfaces: allow pushing if child requires it.
|
||||||
inline void setChildsPushBackwardInterface(StagePrivate* child) {
|
inline void setChildsPushBackwardInterface(StagePrivate* child) {
|
||||||
@ -154,10 +154,14 @@ protected:
|
|||||||
void setStatus(const Stage* creator, const InterfaceState* source, const InterfaceState* target,
|
void setStatus(const Stage* creator, const InterfaceState* source, const InterfaceState* target,
|
||||||
InterfaceState::Status status);
|
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>
|
template <Interface::Direction>
|
||||||
void copyState(Interface::iterator external, const InterfacePtr& target, Interface::UpdateFlags updated);
|
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,
|
void liftSolution(const SolutionBasePtr& solution, const InterfaceState* internal_from,
|
||||||
const InterfaceState* internal_to);
|
const InterfaceState* internal_to);
|
||||||
|
|
||||||
@ -230,12 +234,91 @@ protected:
|
|||||||
void validateInterfaces(const StagePrivate& child, InterfaceFlags& external, bool first = false) const;
|
void validateInterfaces(const StagePrivate& child, InterfaceFlags& external, bool first = false) const;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
/// notify callback for new externally received states
|
/// notify callback for new externally received interface states
|
||||||
template <typename Interface::Direction>
|
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)
|
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
|
class WrapperBasePrivate : public ParallelContainerBasePrivate
|
||||||
{
|
{
|
||||||
friend class WrapperBase;
|
friend class WrapperBase;
|
||||||
|
|||||||
@ -61,7 +61,6 @@ class StagePrivate
|
|||||||
{
|
{
|
||||||
friend class Stage;
|
friend class Stage;
|
||||||
friend std::ostream& operator<<(std::ostream& os, const StagePrivate& stage);
|
friend std::ostream& operator<<(std::ostream& os, const StagePrivate& stage);
|
||||||
friend void swap(StagePrivate*& lhs, StagePrivate*& rhs);
|
|
||||||
|
|
||||||
public:
|
public:
|
||||||
/// container type used to store children
|
/// container type used to store children
|
||||||
@ -103,6 +102,8 @@ public:
|
|||||||
/// direction-based access to pull interface
|
/// direction-based access to pull interface
|
||||||
template <Interface::Direction dir>
|
template <Interface::Direction dir>
|
||||||
inline InterfacePtr pullInterface();
|
inline InterfacePtr pullInterface();
|
||||||
|
// non-template version
|
||||||
|
inline InterfacePtr pullInterface(Interface::Direction dir) { return dir == Interface::FORWARD ? starts_ : ends_; }
|
||||||
|
|
||||||
/// set parent of stage
|
/// set parent of stage
|
||||||
/// enforce only one parent exists
|
/// enforce only one parent exists
|
||||||
@ -157,6 +158,8 @@ public:
|
|||||||
void computeCost(const InterfaceState& from, const InterfaceState& to, SolutionBase& solution);
|
void computeCost(const InterfaceState& from, const InterfaceState& to, SolutionBase& solution);
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
|
StagePrivate& operator=(StagePrivate&& other);
|
||||||
|
|
||||||
// associated/owning Stage instance
|
// associated/owning Stage instance
|
||||||
Stage* me_;
|
Stage* me_;
|
||||||
|
|
||||||
@ -301,6 +304,7 @@ PIMPL_FUNCTIONS(MonitoringGenerator)
|
|||||||
class ConnectingPrivate : public ComputeBasePrivate
|
class ConnectingPrivate : public ComputeBasePrivate
|
||||||
{
|
{
|
||||||
friend class Connecting;
|
friend class Connecting;
|
||||||
|
friend struct FallbacksPrivateConnect;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
struct StatePair : std::pair<Interface::const_iterator, Interface::const_iterator>
|
struct StatePair : std::pair<Interface::const_iterator, Interface::const_iterator>
|
||||||
|
|||||||
@ -178,6 +178,7 @@ public:
|
|||||||
class iterator : public base_type::iterator
|
class iterator : public base_type::iterator
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
|
iterator() = default;
|
||||||
iterator(base_type::iterator other) : base_type::iterator(other) {}
|
iterator(base_type::iterator other) : base_type::iterator(other) {}
|
||||||
|
|
||||||
InterfaceState& operator*() const noexcept { return *base_type::iterator::operator*(); }
|
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 InterfaceState::Priority& prio);
|
||||||
std::ostream& operator<<(std::ostream& os, const Interface& interface);
|
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 CostTerm;
|
||||||
class StagePrivate;
|
class StagePrivate;
|
||||||
class ContainerBasePrivate;
|
class ContainerBasePrivate;
|
||||||
|
|||||||
@ -51,15 +51,14 @@ namespace task_constructor {
|
|||||||
class TaskPrivate : public WrapperBasePrivate
|
class TaskPrivate : public WrapperBasePrivate
|
||||||
{
|
{
|
||||||
friend class Task;
|
friend class Task;
|
||||||
|
TaskPrivate& operator=(TaskPrivate&& other);
|
||||||
|
|
||||||
public:
|
public:
|
||||||
TaskPrivate(Task* me, const std::string& ns);
|
TaskPrivate(Task* me, const std::string& ns);
|
||||||
|
|
||||||
const std::string& ns() const { return ns_; }
|
const std::string& ns() const { return ns_; }
|
||||||
const ContainerBase* stages() const;
|
const ContainerBase* stages() const;
|
||||||
|
|
||||||
protected:
|
|
||||||
static void swap(StagePrivate*& lhs, StagePrivate*& rhs);
|
|
||||||
|
|
||||||
private:
|
private:
|
||||||
std::string ns_;
|
std::string ns_;
|
||||||
robot_model_loader::RobotModelLoaderPtr robot_model_loader_;
|
robot_model_loader::RobotModelLoaderPtr robot_model_loader_;
|
||||||
|
|||||||
@ -53,8 +53,31 @@ using namespace std::placeholders;
|
|||||||
namespace moveit {
|
namespace moveit {
|
||||||
namespace task_constructor {
|
namespace task_constructor {
|
||||||
|
|
||||||
|
// for debugging of how children interfaces evolve over time
|
||||||
static void printChildrenInterfaces(const ContainerBasePrivate& container, bool success, const Stage& creator,
|
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)
|
ContainerBasePrivate::ContainerBasePrivate(ContainerBase* me, const std::string& name)
|
||||||
: StagePrivate(me, name)
|
: StagePrivate(me, name)
|
||||||
@ -62,6 +85,33 @@ ContainerBasePrivate::ContainerBasePrivate(ContainerBase* me, const std::string&
|
|||||||
, pending_backward_(new Interface)
|
, pending_backward_(new Interface)
|
||||||
, pending_forward_(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 {
|
ContainerBasePrivate::const_iterator ContainerBasePrivate::childByIndex(int index, bool for_insert) const {
|
||||||
if (!for_insert && index < 0)
|
if (!for_insert && index < 0)
|
||||||
--index;
|
--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 possible (i.e. if target has an external counterpart), escalate setStatus to external interface
|
||||||
if (parent() && trajectories<dir>(*target).empty()) {
|
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) };
|
auto external{ internalToExternalMap().find(target) };
|
||||||
if (external != internalToExternalMap().end()) { // do we have an external state?
|
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
|
// 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));
|
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,
|
void ContainerBasePrivate::liftSolution(const SolutionBasePtr& solution, const InterfaceState* internal_from,
|
||||||
const InterfaceState* internal_to) {
|
const InterfaceState* internal_to) {
|
||||||
computeCost(*internal_from, *internal_to, *solution);
|
computeCost(*internal_from, *internal_to, *solution);
|
||||||
@ -386,31 +445,6 @@ std::ostream& operator<<(std::ostream& os, const ContainerBase& container) {
|
|||||||
return os;
|
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 */
|
/** Collect all partial solution sequences originating from start into given direction */
|
||||||
template <Interface::Direction dir>
|
template <Interface::Direction dir>
|
||||||
struct SolutionCollector
|
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
|
// finally, store + announce new solutions to external interface
|
||||||
for (const auto& solution : sorted)
|
for (const auto& solution : sorted)
|
||||||
@ -557,9 +591,9 @@ void SerialContainerPrivate::resolveInterface(InterfaceFlags expected) {
|
|||||||
validateInterface<START_IF_MASK>(*first.pimpl(), expected);
|
validateInterface<START_IF_MASK>(*first.pimpl(), expected);
|
||||||
// connect first child's (start) pull interface
|
// connect first child's (start) pull interface
|
||||||
if (const InterfacePtr& target = first.pimpl()->starts())
|
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);
|
this->copyState<Interface::FORWARD>(it, target, updated);
|
||||||
}));
|
});
|
||||||
} catch (InitStageException& e) {
|
} catch (InitStageException& e) {
|
||||||
exceptions.append(e);
|
exceptions.append(e);
|
||||||
}
|
}
|
||||||
@ -570,6 +604,7 @@ void SerialContainerPrivate::resolveInterface(InterfaceFlags expected) {
|
|||||||
StagePrivate* child_impl = (**it).pimpl();
|
StagePrivate* child_impl = (**it).pimpl();
|
||||||
StagePrivate* previous_impl = (**previous_it).pimpl();
|
StagePrivate* previous_impl = (**previous_it).pimpl();
|
||||||
child_impl->resolveInterface(invert(previous_impl->requiredInterface()) & START_IF_MASK);
|
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);
|
connect(*previous_impl, *child_impl);
|
||||||
} catch (InitStageException& e) {
|
} catch (InitStageException& e) {
|
||||||
exceptions.append(e);
|
exceptions.append(e);
|
||||||
@ -582,9 +617,9 @@ void SerialContainerPrivate::resolveInterface(InterfaceFlags expected) {
|
|||||||
validateInterface<END_IF_MASK>(*last.pimpl(), expected);
|
validateInterface<END_IF_MASK>(*last.pimpl(), expected);
|
||||||
// connect last child's (end) pull interface
|
// connect last child's (end) pull interface
|
||||||
if (const InterfacePtr& target = last.pimpl()->ends())
|
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);
|
this->copyState<Interface::BACKWARD>(it, target, updated);
|
||||||
}));
|
});
|
||||||
} catch (InitStageException& e) {
|
} catch (InitStageException& e) {
|
||||||
exceptions.append(e);
|
exceptions.append(e);
|
||||||
}
|
}
|
||||||
@ -640,8 +675,7 @@ bool SerialContainer::canCompute() const {
|
|||||||
|
|
||||||
void SerialContainer::compute() {
|
void SerialContainer::compute() {
|
||||||
for (const auto& stage : pimpl()->children()) {
|
for (const auto& stage : pimpl()->children()) {
|
||||||
if (!stage->pimpl()->canCompute())
|
if (stage->pimpl()->canCompute())
|
||||||
continue;
|
|
||||||
stage->pimpl()->runCompute();
|
stage->pimpl()->runCompute();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@ -663,8 +697,8 @@ void ParallelContainerBasePrivate::resolveInterface(InterfaceFlags expected) {
|
|||||||
child_impl->resolveInterface(expected);
|
child_impl->resolveInterface(expected);
|
||||||
validateInterfaces(*child_impl, expected, first);
|
validateInterfaces(*child_impl, expected, first);
|
||||||
// initialize push connections of children according to their demands
|
// initialize push connections of children according to their demands
|
||||||
setChildsPushForwardInterface(child_impl);
|
|
||||||
setChildsPushBackwardInterface(child_impl);
|
setChildsPushBackwardInterface(child_impl);
|
||||||
|
setChildsPushForwardInterface(child_impl);
|
||||||
first = false;
|
first = false;
|
||||||
} catch (InitStageException& e) {
|
} catch (InitStageException& e) {
|
||||||
exceptions.append(e);
|
exceptions.append(e);
|
||||||
@ -675,17 +709,21 @@ void ParallelContainerBasePrivate::resolveInterface(InterfaceFlags expected) {
|
|||||||
if (exceptions)
|
if (exceptions)
|
||||||
throw 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;
|
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,
|
void ParallelContainerBasePrivate::validateInterfaces(const StagePrivate& child, InterfaceFlags& external,
|
||||||
@ -720,7 +758,7 @@ void ParallelContainerBasePrivate::validateConnectivity() const {
|
|||||||
}
|
}
|
||||||
|
|
||||||
template <Interface::Direction dir>
|
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())
|
for (const Stage::pointer& stage : children())
|
||||||
copyState<dir>(external, stage->pimpl()->pullInterface<dir>(), updated);
|
copyState<dir>(external, stage->pimpl()->pullInterface<dir>(), updated);
|
||||||
}
|
}
|
||||||
@ -793,44 +831,240 @@ void Alternatives::onNewSolution(const SolutionBase& s) {
|
|||||||
liftSolution(s);
|
liftSolution(s);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
Fallbacks::Fallbacks(const std::string& name) : Fallbacks(new FallbacksPrivate(this, name)) {}
|
||||||
|
|
||||||
|
Fallbacks::Fallbacks(FallbacksPrivate* impl) : ParallelContainerBase(impl) {}
|
||||||
|
|
||||||
void Fallbacks::reset() {
|
void Fallbacks::reset() {
|
||||||
active_child_ = nullptr;
|
|
||||||
ParallelContainerBase::reset();
|
ParallelContainerBase::reset();
|
||||||
|
pimpl()->reset();
|
||||||
}
|
}
|
||||||
|
|
||||||
void Fallbacks::init(const moveit::core::RobotModelConstPtr& robot_model) {
|
void Fallbacks::init(const moveit::core::RobotModelConstPtr& robot_model) {
|
||||||
ParallelContainerBase::init(robot_model);
|
ParallelContainerBase::init(robot_model);
|
||||||
active_child_ = pimpl()->children().front().get();
|
pimpl()->reset();
|
||||||
}
|
|
||||||
|
|
||||||
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();
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void Fallbacks::onNewSolution(const SolutionBase& s) {
|
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) {}
|
MergerPrivate::MergerPrivate(Merger* me, const std::string& name) : ParallelContainerBasePrivate(me, name) {}
|
||||||
|
|
||||||
void MergerPrivate::resolveInterface(InterfaceFlags expected) {
|
void MergerPrivate::resolveInterface(InterfaceFlags expected) {
|
||||||
|
|||||||
@ -106,6 +106,33 @@ StagePrivate::StagePrivate(Stage* me, const std::string& name)
|
|||||||
, parent_{ nullptr }
|
, parent_{ nullptr }
|
||||||
, introspection_{ 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 StagePrivate::interfaceFlags() const {
|
||||||
InterfaceFlags f;
|
InterfaceFlags f;
|
||||||
if (starts())
|
if (starts())
|
||||||
@ -483,14 +510,14 @@ void PropagatingEitherWayPrivate::initInterface(PropagatingEitherWay::Direction
|
|||||||
case PropagatingEitherWay::FORWARD:
|
case PropagatingEitherWay::FORWARD:
|
||||||
required_interface_ = PROPAGATE_FORWARDS;
|
required_interface_ = PROPAGATE_FORWARDS;
|
||||||
if (!starts_) // keep existing interface if possible
|
if (!starts_) // keep existing interface if possible
|
||||||
starts_.reset(new Interface());
|
starts_ = std::make_shared<Interface>();
|
||||||
ends_.reset();
|
ends_.reset();
|
||||||
return;
|
return;
|
||||||
case PropagatingEitherWay::BACKWARD:
|
case PropagatingEitherWay::BACKWARD:
|
||||||
required_interface_ = PROPAGATE_BACKWARDS;
|
required_interface_ = PROPAGATE_BACKWARDS;
|
||||||
starts_.reset();
|
starts_.reset();
|
||||||
if (!ends_) // keep existing interface if possible
|
if (!ends_) // keep existing interface if possible
|
||||||
ends_.reset(new Interface());
|
ends_ = std::make_shared<Interface>();
|
||||||
return;
|
return;
|
||||||
case PropagatingEitherWay::AUTO:
|
case PropagatingEitherWay::AUTO:
|
||||||
required_interface_ = UNKNOWN;
|
required_interface_ = UNKNOWN;
|
||||||
@ -688,10 +715,10 @@ void MonitoringGeneratorPrivate::solutionCB(const SolutionBase& s) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
ConnectingPrivate::ConnectingPrivate(Connecting* me, const std::string& name) : ComputeBasePrivate(me, name) {
|
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,
|
starts_ = std::make_shared<Interface>(std::bind(&ConnectingPrivate::newState<Interface::BACKWARD>, this,
|
||||||
std::placeholders::_2)));
|
std::placeholders::_1, std::placeholders::_2));
|
||||||
ends_.reset(new Interface(std::bind(&ConnectingPrivate::newState<Interface::FORWARD>, this, std::placeholders::_1,
|
ends_ = std::make_shared<Interface>(
|
||||||
std::placeholders::_2)));
|
std::bind(&ConnectingPrivate::newState<Interface::FORWARD>, this, std::placeholders::_1, std::placeholders::_2));
|
||||||
}
|
}
|
||||||
|
|
||||||
InterfaceFlags ConnectingPrivate::requiredInterface() const {
|
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
|
// pass creator=nullptr to skip hasPendingOpposites() check as we did this here already
|
||||||
parent_pimpl->setStatus<dir>(nullptr, nullptr, &*it, InterfaceState::Status::ARMED);
|
parent_pimpl->setStatus<dir>(nullptr, nullptr, &*it, InterfaceState::Status::ARMED);
|
||||||
}
|
}
|
||||||
// std::cerr << name_ << ": ";
|
#if 0
|
||||||
// printPendingPairs(std::cerr);
|
auto& os = std::cerr;
|
||||||
// std::cerr << std::endl;
|
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.
|
// 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 {
|
std::ostream& ConnectingPrivate::printPendingPairs(std::ostream& os) const {
|
||||||
const char* reset = InterfaceState::STATUS_COLOR[3];
|
const char* reset = InterfaceState::STATUS_COLOR[3];
|
||||||
for (const auto& candidate : pending) {
|
for (const auto& candidate : pending) {
|
||||||
// find indeces of InterfaceState pointers in start/end Interfaces
|
size_t first = getIndex(*starts(), candidate.first);
|
||||||
unsigned int first = 0, second = 0;
|
size_t second = getIndex(*ends(), candidate.second);
|
||||||
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;
|
|
||||||
});
|
|
||||||
os << InterfaceState::STATUS_COLOR[candidate.first->priority().status()] << first << reset << ":"
|
os << InterfaceState::STATUS_COLOR[candidate.first->priority().status()] << first << reset << ":"
|
||||||
<< InterfaceState::STATUS_COLOR[candidate.second->priority().status()] << second << reset << " ";
|
<< InterfaceState::STATUS_COLOR[candidate.second->priority().status()] << second << reset << " ";
|
||||||
}
|
}
|
||||||
|
if (pending.empty())
|
||||||
|
os << "---";
|
||||||
return os;
|
return os;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@ -74,30 +74,14 @@ namespace task_constructor {
|
|||||||
TaskPrivate::TaskPrivate(Task* me, const std::string& ns)
|
TaskPrivate::TaskPrivate(Task* me, const std::string& ns)
|
||||||
: WrapperBasePrivate(me, std::string()), ns_(rosNormalizeName(ns)), preempt_requested_(false) {}
|
: WrapperBasePrivate(me, std::string()), ns_(rosNormalizeName(ns)), preempt_requested_(false) {}
|
||||||
|
|
||||||
void swap(StagePrivate*& lhs, StagePrivate*& rhs) {
|
TaskPrivate& TaskPrivate::operator=(TaskPrivate&& other) {
|
||||||
// It only makes sense to swap pimpl instances of a Task!
|
this->WrapperBasePrivate::operator=(std::move(other));
|
||||||
// However, due to member protection rules, we can only implement it here
|
ns_ = std::move(other.ns_);
|
||||||
assert(typeid(lhs) == typeid(rhs));
|
introspection_ = std::move(other.introspection_);
|
||||||
|
robot_model_ = std::move(other.robot_model_);
|
||||||
// swap instances
|
robot_model_loader_ = std::move(other.robot_model_loader_);
|
||||||
::std::swap(lhs, rhs);
|
task_cbs_ = std::move(other.task_cbs_);
|
||||||
// as well as their me_ pointers
|
return *this;
|
||||||
::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);
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
const ContainerBase* TaskPrivate::stages() const {
|
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)
|
Task& Task::operator=(Task&& other) { // NOLINT(performance-noexcept-move-constructor)
|
||||||
clear(); // remove all stages of current task
|
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;
|
return *this;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@ -586,10 +586,6 @@ TEST(Task, move) {
|
|||||||
Task t2 = std::move(t1);
|
Task t2 = std::move(t1);
|
||||||
EXPECT_EQ(t2.stages()->numChildren(), 2u);
|
EXPECT_EQ(t2.stages()->numChildren(), 2u);
|
||||||
EXPECT_EQ(t1.stages()->numChildren(), 0u); // NOLINT(clang-analyzer-cplusplus.Move)
|
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) {
|
TEST(Task, reuse) {
|
||||||
|
|||||||
@ -17,7 +17,7 @@ using namespace moveit::task_constructor;
|
|||||||
|
|
||||||
using FallbacksFixtureGenerator = TaskTestBase;
|
using FallbacksFixtureGenerator = TaskTestBase;
|
||||||
|
|
||||||
TEST_F(FallbacksFixtureGenerator, DISABLED_stayWithFirstSuccessful) {
|
TEST_F(FallbacksFixtureGenerator, stayWithFirstSuccessful) {
|
||||||
auto fallback = std::make_unique<Fallbacks>("Fallbacks");
|
auto fallback = std::make_unique<Fallbacks>("Fallbacks");
|
||||||
fallback->add(std::make_unique<GeneratorMockup>(PredefinedCosts::single(INF)));
|
fallback->add(std::make_unique<GeneratorMockup>(PredefinedCosts::single(INF)));
|
||||||
fallback->add(std::make_unique<GeneratorMockup>(PredefinedCosts::single(1.0)));
|
fallback->add(std::make_unique<GeneratorMockup>(PredefinedCosts::single(1.0)));
|
||||||
@ -55,7 +55,7 @@ TEST_F(FallbacksFixturePropagate, failingWithFailedSolutions) {
|
|||||||
EXPECT_EQ(t.solutions().size(), 0u);
|
EXPECT_EQ(t.solutions().size(), 0u);
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(FallbacksFixturePropagate, DISABLED_ComputeFirstSuccessfulStageOnly) {
|
TEST_F(FallbacksFixturePropagate, computeFirstSuccessfulStageOnly) {
|
||||||
t.add(std::make_unique<GeneratorMockup>());
|
t.add(std::make_unique<GeneratorMockup>());
|
||||||
|
|
||||||
auto fallbacks = std::make_unique<Fallbacks>("Fallbacks");
|
auto fallbacks = std::make_unique<Fallbacks>("Fallbacks");
|
||||||
@ -67,7 +67,7 @@ TEST_F(FallbacksFixturePropagate, DISABLED_ComputeFirstSuccessfulStageOnly) {
|
|||||||
EXPECT_EQ(t.numSolutions(), 1u);
|
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 })));
|
t.add(std::make_unique<GeneratorMockup>(PredefinedCosts({ 2.0, 1.0 })));
|
||||||
// duplicate generator solutions with resulting costs: 4, 2 | 3, 1
|
// 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));
|
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));
|
t.add(std::move(fallbacks));
|
||||||
|
|
||||||
EXPECT_TRUE(t.plan());
|
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<BackwardMockup>(PredefinedCosts({ 10.0, 0.0 })));
|
||||||
t.add(std::make_unique<GeneratorMockup>(PredefinedCosts({ 1.0, 2.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
|
// 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
|
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 })));
|
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
|
// use a fallback container to delay computation: the 1st child never succeeds, but only the 2nd
|
||||||
auto inner = std::make_unique<Fallbacks>("Inner");
|
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
|
// check that first solution is not marked as pruned
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(FallbacksFixturePropagate, DISABLED_successfulWithMixedSolutions) {
|
TEST_F(FallbacksFixturePropagate, successfulWithMixedSolutions) {
|
||||||
t.add(std::make_unique<GeneratorMockup>());
|
t.add(std::make_unique<GeneratorMockup>());
|
||||||
|
|
||||||
auto fallback = std::make_unique<Fallbacks>("Fallbacks");
|
auto fallback = std::make_unique<Fallbacks>("Fallbacks");
|
||||||
@ -129,7 +131,7 @@ TEST_F(FallbacksFixturePropagate, DISABLED_successfulWithMixedSolutions) {
|
|||||||
EXPECT_COSTS(t.solutions(), testing::ElementsAre(1.0));
|
EXPECT_COSTS(t.solutions(), testing::ElementsAre(1.0));
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(FallbacksFixturePropagate, DISABLED_successfulWithMixedSolutions2) {
|
TEST_F(FallbacksFixturePropagate, successfulWithMixedSolutions2) {
|
||||||
t.add(std::make_unique<GeneratorMockup>());
|
t.add(std::make_unique<GeneratorMockup>());
|
||||||
|
|
||||||
auto fallback = std::make_unique<Fallbacks>("Fallbacks");
|
auto fallback = std::make_unique<Fallbacks>("Fallbacks");
|
||||||
@ -141,7 +143,7 @@ TEST_F(FallbacksFixturePropagate, DISABLED_successfulWithMixedSolutions2) {
|
|||||||
EXPECT_COSTS(t.solutions(), testing::ElementsAre(1.0));
|
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 })));
|
t.add(std::make_unique<GeneratorMockup>(PredefinedCosts({ 1.0, INF, 3.0 })));
|
||||||
|
|
||||||
auto fallbacks = std::make_unique<Fallbacks>("Fallbacks");
|
auto fallbacks = std::make_unique<Fallbacks>("Fallbacks");
|
||||||
@ -159,29 +161,16 @@ TEST_F(FallbacksFixturePropagate, DISABLED_ActiveChildReset) {
|
|||||||
|
|
||||||
using FallbacksFixtureConnect = TaskTestBase;
|
using FallbacksFixtureConnect = TaskTestBase;
|
||||||
|
|
||||||
TEST_F(FallbacksFixtureConnect, DISABLED_ConnectStageInsideFallbacks) {
|
TEST_F(FallbacksFixtureConnect, connectStageInsideFallbacks) {
|
||||||
t.add(std::make_unique<GeneratorMockup>(PredefinedCosts({ 1.0, 2.0 })));
|
t.add(std::make_unique<GeneratorMockup>(PredefinedCosts({ 1.0, 2.0 })));
|
||||||
|
|
||||||
auto fallbacks = std::make_unique<Fallbacks>("Fallbacks");
|
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)));
|
fallbacks->add(std::make_unique<ConnectMockup>(PredefinedCosts::constant(100.0)));
|
||||||
t.add(std::move(fallbacks));
|
t.add(std::move(fallbacks));
|
||||||
|
|
||||||
t.add(std::make_unique<GeneratorMockup>(PredefinedCosts({ 10.0, 20.0 })));
|
t.add(std::make_unique<GeneratorMockup>(PredefinedCosts({ 10.0, 20.0 })));
|
||||||
|
|
||||||
EXPECT_TRUE(t.plan());
|
EXPECT_TRUE(t.plan());
|
||||||
EXPECT_COSTS(t.solutions(), testing::ElementsAre(11, 12, 21, 22));
|
EXPECT_COSTS(t.solutions(), testing::ElementsAre(11, 12, 22, 121));
|
||||||
}
|
|
||||||
|
|
||||||
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();
|
|
||||||
}
|
}
|
||||||
|
|||||||
@ -50,6 +50,7 @@ demo(cartesian)
|
|||||||
demo(modular)
|
demo(modular)
|
||||||
demo(alternative_path_costs)
|
demo(alternative_path_costs)
|
||||||
demo(ik_clearance_cost)
|
demo(ik_clearance_cost)
|
||||||
|
demo(fallbacks_move_to)
|
||||||
|
|
||||||
demo(pick_place_demo)
|
demo(pick_place_demo)
|
||||||
target_link_libraries(${PROJECT_NAME}_pick_place_demo ${PROJECT_NAME}_pick_place_task)
|
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