diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index c37d6caf..04dea516 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -39,7 +39,7 @@ jobs: -e PRELOAD=libasan.so.5 -e LSAN_OPTIONS="suppressions=$PWD/.github/workflows/lsan.suppressions,fast_unwind_on_malloc=0" -e ASAN_OPTIONS="new_delete_type_mismatch=0,alloc_dealloc_mismatch=0" - 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: CLANG_TIDY_ARGS: --fix --fix-errors --format-style=file diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 60e63f35..b610362f 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -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 diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt index a7bcdfdd..ba2a608b 100644 --- a/core/CMakeLists.txt +++ b/core/CMakeLists.txt @@ -14,9 +14,9 @@ find_package(rviz_marker_tools REQUIRED) find_package(tf2_eigen REQUIRED) find_package(visualization_msgs REQUIRED) -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) add_compile_options(-fvisibility-inlines-hidden) diff --git a/core/include/moveit/task_constructor/container.h b/core/include/moveit/task_constructor/container.h index fab732a7..cea4a418 100644 --- a/core/include/moveit/task_constructor/container.h +++ b/core/include/moveit/task_constructor/container.h @@ -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; diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index 3b58d0da..c9fb61f2 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -76,7 +76,7 @@ namespace task_constructor { class ContainerBasePrivate : public StagePrivate { friend class ContainerBase; - friend void swap(StagePrivate*& lhs, StagePrivate*& rhs); + friend class ConnectingPrivate; // needed propagate setStatus() in newState() public: using container_type = StagePrivate::container_type; @@ -131,10 +131,11 @@ public: inline const auto& externalToInternalMap() const { return internal_external_.by(); } /// 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) { @@ -148,14 +149,19 @@ protected: child->setNextStarts(allowed ? pending_forward_ : InterfacePtr()); } - /// Set ENABLED / PRUNED status of the solution tree starting from s into given direction + /// Set ENABLED/PRUNED status of a solution branch starting from target into the given direction template - void setStatus(const InterfaceState* s, InterfaceState::Status status); + 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 - void copyState(Interface::iterator external, const InterfacePtr& target, bool updated); - /// lift solution from internal to external level + void copyState(Interface::iterator external, const InterfacePtr& target, Interface::UpdateFlags updated); + // 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); @@ -228,12 +234,91 @@ protected: void validateInterfaces(const StagePrivate& child, InterfaceFlags& external, bool first = false) const; private: - /// callback for new externally received states + /// notify callback for new externally received interface states template - void onNewExternalState(Interface::iterator external, bool 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 + 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; diff --git a/core/include/moveit/task_constructor/merge.h b/core/include/moveit/task_constructor/merge.h index 7ac7eef5..734418f1 100644 --- a/core/include/moveit/task_constructor/merge.h +++ b/core/include/moveit/task_constructor/merge.h @@ -39,6 +39,7 @@ #include #include #include +#include namespace moveit { namespace task_constructor { @@ -57,6 +58,7 @@ moveit::core::JointModelGroup* merge(const std::vector& sub_trajectories, - const moveit::core::RobotState& base_state, moveit::core::JointModelGroup*& merged_group); + const moveit::core::RobotState& base_state, moveit::core::JointModelGroup*& merged_group, + const trajectory_processing::TimeParameterization& time_parameterization); } // namespace task_constructor } // namespace moveit diff --git a/core/include/moveit/task_constructor/moveit_compat.h b/core/include/moveit/task_constructor/moveit_compat.h index 1f41353b..7ce3162e 100644 --- a/core/include/moveit/task_constructor/moveit_compat.h +++ b/core/include/moveit/task_constructor/moveit_compat.h @@ -39,6 +39,7 @@ #pragma once #include +#include #define MOVEIT_VERSION_GE(major, minor, patch) \ (MOVEIT_VERSION_MAJOR * 1'000'000 + MOVEIT_VERSION_MINOR * 1'000 + MOVEIT_VERSION_PATCH >= \ @@ -47,4 +48,11 @@ // use object shape poses relative to a single object pose #define MOVEIT_HAS_OBJECT_POSE 1 -#define MOVEIT_HAS_STATE_RIGID_PARENT_LINK 0 +#define MOVEIT_HAS_STATE_RIGID_PARENT_LINK 1 + +#if !MOVEIT_VERSION_GE(3, 0, 0) +// the pointers are not yet available +namespace trajectory_processing { +MOVEIT_CLASS_FORWARD(TimeParameterization); +} +#endif diff --git a/core/include/moveit/task_constructor/stage_p.h b/core/include/moveit/task_constructor/stage_p.h index 0dbd9f69..55ae265f 100644 --- a/core/include/moveit/task_constructor/stage_p.h +++ b/core/include/moveit/task_constructor/stage_p.h @@ -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 @@ -100,17 +99,11 @@ public: inline InterfaceConstPtr prevEnds() const { return prev_ends_.lock(); } inline InterfaceConstPtr nextStarts() const { return next_starts_.lock(); } - /// direction-based access to pull/push interfaces - inline InterfacePtr& pullInterface(Interface::Direction dir) { return dir == Interface::FORWARD ? starts_ : ends_; } - inline InterfacePtr pushInterface(Interface::Direction dir) { - return dir == Interface::FORWARD ? next_starts_.lock() : prev_ends_.lock(); - } - inline InterfaceConstPtr pullInterface(Interface::Direction dir) const { - return dir == Interface::FORWARD ? starts_ : ends_; - } - inline InterfaceConstPtr pushInterface(Interface::Direction dir) const { - return dir == Interface::FORWARD ? next_starts_.lock() : prev_ends_.lock(); - } + /// direction-based access to pull interface + template + 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 @@ -165,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_; @@ -205,6 +200,15 @@ private: PIMPL_FUNCTIONS(Stage) std::ostream& operator<<(std::ostream& os, const StagePrivate& stage); +template <> +inline InterfacePtr StagePrivate::pullInterface() { + return starts_; +} +template <> +inline InterfacePtr StagePrivate::pullInterface() { + return ends_; +} + template <> inline void StagePrivate::send(const InterfaceState& start, InterfaceState&& end, const SolutionBasePtr& solution) { @@ -301,6 +305,7 @@ PIMPL_FUNCTIONS(MonitoringGenerator) class ConnectingPrivate : public ComputeBasePrivate { friend class Connecting; + friend struct FallbacksPrivateConnect; public: struct StatePair : std::pair @@ -311,18 +316,15 @@ public: } static inline bool less(const InterfaceState::Priority& lhsA, const InterfaceState::Priority& lhsB, const InterfaceState::Priority& rhsA, const InterfaceState::Priority& rhsB) { - unsigned char lhs = (lhsA.enabled() << 1) | lhsB.enabled(); // combine bits into two-digit binary number - unsigned char rhs = (rhsA.enabled() << 1) | rhsB.enabled(); + bool lhs = lhsA.enabled() && lhsB.enabled(); + bool rhs = rhsA.enabled() && rhsB.enabled(); + if (lhs == rhs) // if enabled status is identical return lhsA + lhsB < rhsA + rhsB; // compare the sums of both contributions - // one of the states in each pair should be enabled - assert(lhs != 0b00 && rhs != 0b00); - // both states valid (b11) - if (lhs == 0b11) - return true; - if (rhs == 0b11) - return false; - return lhs < rhs; // disabled states in 1st component go before disabled states in 2nd component + + // sort both-enabled pairs first + static_assert(true > false, "Comparing enabled states requires true > false"); + return lhs > rhs; } }; @@ -334,7 +336,7 @@ public: // Check whether there are pending feasible states that could connect to source template - bool hasPendingOpposites(const InterfaceState* source) const; + bool hasPendingOpposites(const InterfaceState* source, const InterfaceState* target) const; std::ostream& printPendingPairs(std::ostream& os = std::cerr) const; @@ -343,9 +345,9 @@ private: template inline StatePair make_pair(Interface::const_iterator first, Interface::const_iterator second); - // get informed when new start or end state becomes available + // notify callback to get informed about newly inserted (or updated) start or end states template - void newState(Interface::iterator it, bool updated); + void newState(Interface::iterator it, Interface::UpdateFlags updated); // ordered list of pending state pairs ordered pending; diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index dc278dd9..992cdc10 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -44,6 +44,7 @@ #include #include #include +#include #include #include @@ -83,9 +84,11 @@ public: enum Status { ENABLED, // state is actively considered during planning - PRUNED, // state is disabled because a required connected state failed - FAILED, // state that failed, causing the whole partial solution to be disabled + ARMED, // disabled state in a Connecting interface that will become re-enabled with a new opposite state + PRUNED, // disabled state on a pruned solution branch }; + static const char* STATUS_COLOR[]; + /** InterfaceStates are ordered according to two values: * Depth of interlinked trajectory parts and accumulated trajectory costs along that path. * Preference ordering considers high-depth first and within same depth, minimal cost paths. @@ -101,8 +104,6 @@ public: inline Status status() const { return std::get<0>(*this); } inline bool enabled() const { return std::get<0>(*this) == ENABLED; } - inline bool failed() const { return std::get<0>(*this) == FAILED; } - inline bool pruned() const { return std::get<0>(*this) == PRUNED; } inline unsigned int depth() const { return std::get<1>(*this); } inline double cost() const { return std::get<2>(*this); } @@ -129,6 +130,7 @@ public: /// copy an existing InterfaceState, but not including incoming/outgoing trajectories InterfaceState(const InterfaceState& other); InterfaceState(InterfaceState&& other) = default; + InterfaceState& operator=(const InterfaceState& other) = default; inline const planning_scene::PlanningSceneConstPtr& scene() const { return scene_; } inline const Solutions& incomingTrajectories() const { return incoming_trajectories_; } @@ -139,13 +141,21 @@ public: /// states are ordered by priority inline bool operator<(const InterfaceState& other) const { return this->priority_ < other.priority_; } + inline const Priority& priority() const { return priority_; } + /// Update priority and call owner's notify() if possible + void updatePriority(const InterfaceState::Priority& priority); + /// Update status, but keep current priority + void updateStatus(Status status); + Interface* owner() const { return owner_; } private: // these methods should be only called by SolutionBase::set[Start|End]State() inline void addIncoming(SolutionBase* t) { incoming_trajectories_.push_back(t); } inline void addOutgoing(SolutionBase* t) { outgoing_trajectories_.push_back(t); } + // Set new priority without updating the owning interface (USE WITH CARE) + inline void setPriority(const Priority& prio) { priority_ = prio; } private: planning_scene::PlanningSceneConstPtr scene_; @@ -170,6 +180,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*(); } @@ -191,10 +202,27 @@ public: { FORWARD, BACKWARD, - START = FORWARD, - END = BACKWARD }; - using NotifyFunction = std::function; + enum Update + { + STATUS = 1 << 0, + PRIORITY = 1 << 1, + ALL = STATUS | PRIORITY, + }; + using UpdateFlags = utils::Flags; + using NotifyFunction = std::function; + + class DisableNotify + { + Interface& if_; + Interface::NotifyFunction old_; + + public: + DisableNotify(Interface& i) : if_(i) { old_.swap(if_.notify_); } + ~DisableNotify() { old_.swap(if_.notify_); } + }; + friend class DisableNotify; + Interface(const NotifyFunction& notify = NotifyFunction()); /// add a new InterfaceState @@ -205,9 +233,10 @@ public: /// update state's priority (and call notify_ if it really has changed) void updatePriority(InterfaceState* state, const InterfaceState::Priority& priority); + inline bool notifyEnabled() const { return static_cast(notify_); } private: - const NotifyFunction notify_; + NotifyFunction notify_; // restrict access to some functions to ensure consistency // (we need to set/unset InterfaceState::owner_) @@ -220,6 +249,17 @@ private: 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, Interface::Direction); + +/// Find index of the iterator in the container. Counting starts at 1. Zero corresponds to not found. +template +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; diff --git a/core/include/moveit/task_constructor/task.h b/core/include/moveit/task_constructor/task.h index 04ca2ceb..a4c600f3 100644 --- a/core/include/moveit/task_constructor/task.h +++ b/core/include/moveit/task_constructor/task.h @@ -47,6 +47,7 @@ #include #include +#include #include @@ -119,11 +120,11 @@ public: void init(); /// reset, init scene (if not yet done), and init all stages, then start planning - bool plan(size_t max_solutions = 0); + moveit::core::MoveItErrorCode plan(size_t max_solutions = 0); /// interrupt current planning (or execution) void preempt(); /// execute solution, return the result - moveit_msgs::msg::MoveItErrorCodes execute(const SolutionBase& s); + moveit::core::MoveItErrorCode execute(const SolutionBase& s); /// print current task state (number of found solutions and propagated states) to std::cout void printState(std::ostream& os = std::cout) const; diff --git a/core/include/moveit/task_constructor/task_p.h b/core/include/moveit/task_constructor/task_p.h index 327f1631..f0e6dc0a 100644 --- a/core/include/moveit/task_constructor/task_p.h +++ b/core/include/moveit/task_constructor/task_p.h @@ -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_; diff --git a/core/include/moveit/task_constructor/utils.h b/core/include/moveit/task_constructor/utils.h index 302d4780..70b89482 100644 --- a/core/include/moveit/task_constructor/utils.h +++ b/core/include/moveit/task_constructor/utils.h @@ -140,8 +140,6 @@ private: Int i; }; -#define DECLARE_FLAGS(Flags, Enum) using Flags = QFlags; - const moveit::core::LinkModel* getRigidlyConnectedParentLinkModel(const moveit::core::RobotState& state, std::string frame); diff --git a/core/src/container.cpp b/core/src/container.cpp index af7daf0e..f5fb0e78 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -37,8 +37,11 @@ #include #include #include +#include #include +#include +#include #include #include @@ -49,16 +52,70 @@ #include using namespace std::placeholders; +using namespace trajectory_processing; 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) { + 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(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) , required_interface_(UNKNOWN) , 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(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; @@ -106,6 +163,74 @@ void ContainerBasePrivate::compute() { static_cast(me_)->compute(); } +template +void ContainerBasePrivate::setStatus(const Stage* creator, const InterfaceState* source, const InterfaceState* target, + InterfaceState::Status status) { + if (status != InterfaceState::Status::ENABLED && creator) { + if (const auto* conn = dynamic_cast(creator)) { + auto cimpl = conn->pimpl(); + // if creator is a Connecting stage and target has enabled opposite states (other than source) + if (cimpl->hasPendingOpposites(source, target)) + return; // don't prune + } + } + if (target->priority().status() == status) + return; // nothing changing + + // Skip disabling the state, if there are alternative enabled solutions + if (status != InterfaceState::ENABLED) { + auto solution_is_enabled = [](auto&& solution) { + return state()>(*solution)->priority().enabled(); + }; + const auto& alternatives = trajectories()>(*target); + auto alternative_path = std::find_if(alternatives.cbegin(), alternatives.cend(), solution_is_enabled); + if (alternative_path != alternatives.cend()) + return; + } + + // actually enable/disable the state + const_cast(target)->updateStatus(status); + + // if possible (i.e. if target has an external counterpart), escalate setStatus to external interface + if (parent() && trajectories(*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 + // all internal states linked to external + auto internals{ externalToInternalMap().equal_range(external->get()) }; + auto is_enabled = [](const auto& ext_int_pair) { return ext_int_pair.second->priority().enabled(); }; + auto other_path{ std::find_if(internals.first, internals.second, is_enabled) }; + if (other_path == internals.second) + parent()->pimpl()->setStatus(nullptr, nullptr, external->get(), status); + return; + } + } + + // To break symmetry between both ends of a partial solution sequence that gets disabled, + // we mark the first state with ARMED and all other states down the tree with PRUNED. + // This allows us to re-enable the ARMED state, but not the PRUNED states, + // when new states arrive in a Connecting stage. + // For details, https://github.com/ros-planning/moveit_task_constructor/pull/309#issuecomment-974636202 + if (status == InterfaceState::Status::ARMED) + status = InterfaceState::Status::PRUNED; // only the first state is marked as ARMED + + // traverse solution tree + for (const SolutionBase* successor : trajectories(*target)) + setStatus(successor->creator(), target, state(*successor), status); +} + +// recursively update state priorities along solution path +template +inline void updateStatePrios(const InterfaceState& s, const InterfaceState::Priority& prio) { + InterfaceState::Priority priority(prio, s.priority().status()); + if (s.priority() == priority) + return; + const_cast(s).updatePriority(priority); + for (const SolutionBase* successor : trajectories(s)) + updateStatePrios(*state(*successor), prio); +} + void ContainerBasePrivate::onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) { RCLCPP_DEBUG_STREAM(rclcpp::get_logger("Pruning"), "'" << child.name() << "' generated a failure"); switch (child.pimpl()->interfaceFlags()) { @@ -115,95 +240,37 @@ void ContainerBasePrivate::onNewFailure(const Stage& child, const InterfaceState break; case PROPAGATE_FORWARDS: // mark from as failed (backwards) - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("Pruning"), "prune backward branch"); - setStatus(from, InterfaceState::Status::FAILED); + setStatus(nullptr, nullptr, from, InterfaceState::Status::PRUNED); break; case PROPAGATE_BACKWARDS: // mark to as failed (forwards) - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("Pruning"), "prune backward branch"); - setStatus(to, InterfaceState::Status::FAILED); + setStatus(nullptr, nullptr, to, InterfaceState::Status::PRUNED); break; case CONNECT: - if (const Connecting* conn = dynamic_cast(&child)) { - auto cimpl = conn->pimpl(); - if (!cimpl->hasPendingOpposites(from)) { - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("Pruning"), "prune backward branch"); - setStatus(from, InterfaceState::Status::FAILED); - } - if (!cimpl->hasPendingOpposites(to)) { - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("Pruning"), "prune forward branch"); - setStatus(to, InterfaceState::Status::FAILED); - } - } + setStatus(&child, to, from, InterfaceState::Status::ARMED); + setStatus(&child, from, to, InterfaceState::Status::ARMED); break; } // printChildrenInterfaces(*this, false, child); } template -void ContainerBasePrivate::setStatus(const InterfaceState* s, InterfaceState::Status status) { - if (s->priority().status() == status) - return; // nothing changing - - // if we should disable the state, only do so when there is no enabled alternative path - if (status == InterfaceState::PRUNED) { - auto solution_is_enabled = [](auto&& solution) { - return state()>(*solution)->priority().enabled(); - }; - const auto& alternatives = trajectories()>(*s); - auto alternative_path = std::find_if(alternatives.cbegin(), alternatives.cend(), solution_is_enabled); - if (alternative_path != alternatives.cend()) - return; - } - - // actually enable/disable the state - if (s->owner()) { - s->owner()->updatePriority(const_cast(s), InterfaceState::Priority(s->priority(), status)); - } else { - const_cast(s)->priority_ = InterfaceState::Priority(s->priority(), status); - } - - // if possible (i.e. if state s has an external counterpart), escalate setStatus to external interface - if (parent() && trajectories(*s).empty()) { - auto external{ internalToExternalMap().find(s) }; - if (external != internalToExternalMap().end()) { // do we have an external state? - // only escalate if there is no other *enabled* internal state connected to the same external one - // all internal states linked to external - auto internals{ externalToInternalMap().equal_range(external->get()) }; - auto is_enabled = [](const auto& ext_int_pair) { return ext_int_pair.second->priority().enabled(); }; - auto other_path{ std::find_if(internals.first, internals.second, is_enabled) }; - if (other_path == internals.second) - parent()->pimpl()->setStatus(external->get(), status); - return; - } - } - - // To break symmetry between both ends of a partial solution sequence that gets disabled, - // we mark the first state with FAILED and all other states down the tree only with PRUNED. - // This allows us to re-enable the FAILED side, while not (yet) consider the PRUNED states again, - // when new states arrive in a Connecting stage. - // All PRUNED states are only re-enabled if the FAILED state actually gets connected. - // For details, see: https://github.com/ros-planning/moveit_task_constructor/pull/221 - if (status == InterfaceState::Status::FAILED) - status = InterfaceState::Status::PRUNED; // only the first state is marked as FAILED - - // traverse solution tree - for (const SolutionBase* successor : trajectories(*s)) - setStatus(state(*successor), status); -} -template void ContainerBasePrivate::setStatus(const InterfaceState*, InterfaceState::Status); -template void ContainerBasePrivate::setStatus(const InterfaceState*, InterfaceState::Status); - -template -void ContainerBasePrivate::copyState(Interface::iterator external, const InterfacePtr& target, bool updated) { +void ContainerBasePrivate::copyState(Interface::iterator external, const InterfacePtr& target, + Interface::UpdateFlags updated) { if (updated) { - auto internals{ externalToInternalMap().equal_range(&*external) }; - for (auto& i = internals.first; i != internals.second; ++i) { - setStatus(i->second, external->priority().status()); - } + auto prio = external->priority(); + auto internals = externalToInternalMap().equal_range(&*external); + + if (updated.testFlag(Interface::Update::STATUS)) { // propagate external status updates to internal copies + for (auto& i = internals.first; i != internals.second; ++i) + setStatus(nullptr, nullptr, i->second, prio.status()); + } else if (updated.testFlag(Interface::Update::PRIORITY)) { + for (auto& i = internals.first; i != internals.second; ++i) + updateStatePrios()>(*i->second, prio); + } else + assert(false); // Expecting either STATUS or PRIORITY updates, not both! return; } - // create a clone of external state within target interface (child's starts() or ends()) auto internal = states_.insert(states_.end(), InterfaceState(*external)); target->add(*internal); @@ -211,6 +278,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(external, target, updated); + else + copyState(external, target, updated); +} + void ContainerBasePrivate::liftSolution(const SolutionBasePtr& solution, const InterfaceState* internal_from, const InterfaceState* internal_to) { computeCost(*internal_from, *internal_to, *solution); @@ -374,31 +449,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 ContainerBase& container, bool success, const Stage& creator, - std::ostream& os = std::cerr) { - static unsigned int id = 0; - const unsigned int width = 10; // indentation of name - os << std::endl << (success ? '+' : '-') << ' ' << creator.name() << ' '; - if (success) - os << ++id << ' '; - if (const Connecting* conn = dynamic_cast(&creator)) - conn->pimpl()->printPendingPairs(os); - os << std::endl; - - for (const auto& child : container.pimpl()->children()) { - auto cimpl = child->pimpl(); - os << std::setw(width) << std::left << child->name(); - if (!cimpl->starts() && !cimpl->ends()) - os << "↕ " << std::endl; - if (cimpl->starts()) - os << "↓ " << *child->pimpl()->starts() << std::endl; - if (cimpl->starts() && cimpl->ends()) - os << std::setw(width) << " "; - if (cimpl->ends()) - os << "↑ " << *child->pimpl()->ends() << std::endl; - } -} /** Collect all partial solution sequences originating from start into given direction */ template struct SolutionCollector @@ -431,20 +481,6 @@ struct SolutionCollector SolutionSequence::container_type trace; }; -inline void updateStatePrio(const InterfaceState* state, const InterfaceState::Priority& prio) { - if (state->owner()) // owner becomes NULL if state is removed from (pending) Interface list - state->owner()->updatePriority(const_cast(state), - // update depth + cost, but keep current status - InterfaceState::Priority(prio, state->priority().status())); -} - -template -inline void updateStatePrios(const SolutionSequence::container_type& partial_solution_path, - const InterfaceState::Priority& prio) { - for (const SolutionBase* solution : partial_solution_path) - updateStatePrio(state(*solution), prio); -} - void SerialContainer::onNewSolution(const SolutionBase& current) { RCLCPP_DEBUG_STREAM(rclcpp::get_logger("SerialContainer"), "'" << this->name() << "' received solution of child stage '" @@ -453,10 +489,6 @@ void SerialContainer::onNewSolution(const SolutionBase& current) { // failures should never trigger this callback assert(!current.isFailure()); - // states of solution must be active, otherwise this would not have been computed - assert(current.start()->priority().enabled()); - assert(current.end()->priority().enabled()); - auto impl = pimpl(); const Stage* creator = current.creator(); auto& children = impl->children(); @@ -494,14 +526,12 @@ void SerialContainer::onNewSolution(const SolutionBase& current) { } if (prio.depth() > 1) { // update state priorities along the whole partial solution path - updateStatePrio(current.start(), prio); - updateStatePrio(current.end(), prio); - updateStatePrios(in.first, prio); - updateStatePrios(out.first, prio); + updateStatePrios(*current.start(), prio); + updateStatePrios(*current.end(), prio); } } } - // printChildrenInterfaces(*this, true, *current.creator()); + // printChildrenInterfaces(*this->pimpl(), true, *current.creator()); // finally, store + announce new solutions to external interface for (const auto& solution : sorted) @@ -566,9 +596,9 @@ void SerialContainerPrivate::resolveInterface(InterfaceFlags expected) { validateInterface(*first.pimpl(), expected); // connect first child's (start) pull interface if (const InterfacePtr& target = first.pimpl()->starts()) - starts_.reset(new Interface([this, target](Interface::iterator it, bool updated) { + starts_ = std::make_shared([this, target](Interface::iterator it, Interface::UpdateFlags updated) { this->copyState(it, target, updated); - })); + }); } catch (InitStageException& e) { exceptions.append(e); } @@ -579,6 +609,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); @@ -591,9 +622,9 @@ void SerialContainerPrivate::resolveInterface(InterfaceFlags expected) { validateInterface(*last.pimpl(), expected); // connect last child's (end) pull interface if (const InterfacePtr& target = last.pimpl()->ends()) - ends_.reset(new Interface([this, target](Interface::iterator it, bool updated) { + ends_ = std::make_shared([this, target](Interface::iterator it, Interface::UpdateFlags updated) { this->copyState(it, target, updated); - })); + }); } catch (InitStageException& e) { exceptions.append(e); } @@ -609,7 +640,7 @@ void SerialContainerPrivate::validateConnectivity() const { ContainerBasePrivate::validateConnectivity(); InterfaceFlags mine = interfaceFlags(); - // check that input / output interface of first / last child matches this' resp. interface + // check that input/output interface of first/last child matches this' resp. interface validateInterface(*children().front()->pimpl(), mine); validateInterface(*children().back()->pimpl(), mine); @@ -621,7 +652,7 @@ void SerialContainerPrivate::validateConnectivity() const { const StagePrivate* const cur_impl = **cur; InterfaceFlags required = cur_impl->interfaceFlags(); - // get iterators to prev / next stage in sequence + // get iterators to prev/next stage in sequence auto prev = cur; --prev; auto next = cur; @@ -649,9 +680,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(); } } @@ -672,8 +702,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); @@ -684,17 +714,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, bool updated) { - this->onNewExternalState(external, updated); - })); - if (expected & READS_END) - ends().reset(new Interface([this](Interface::iterator external, bool updated) { - this->onNewExternalState(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([this](Interface::iterator external, Interface::UpdateFlags updated) { + this->propagateStateToAllChildren(external, updated); + }); + if (requiredInterface() & READS_END) + ends() = std::make_shared([this](Interface::iterator external, Interface::UpdateFlags updated) { + this->propagateStateToAllChildren(external, updated); + }); } void ParallelContainerBasePrivate::validateInterfaces(const StagePrivate& child, InterfaceFlags& external, @@ -721,7 +755,7 @@ void ParallelContainerBasePrivate::validateInterfaces(const StagePrivate& child, void ParallelContainerBasePrivate::validateConnectivity() const { InterfaceFlags my_interface = interfaceFlags(); - // check that input / output interfaces of all children are handled by my interface + // check that input/output interfaces of all children are handled by my interface for (const auto& child : children()) validateInterfaces(*child->pimpl(), my_interface); @@ -729,9 +763,9 @@ void ParallelContainerBasePrivate::validateConnectivity() const { } template -void ParallelContainerBasePrivate::onNewExternalState(Interface::iterator external, bool updated) { +void ParallelContainerBasePrivate::propagateStateToAllChildren(Interface::iterator external, Interface::UpdateFlags updated) { for (const Stage::pointer& stage : children()) - copyState(external, stage->pimpl()->pullInterface(dir), updated); + copyState(external, stage->pimpl()->pullInterface(), updated); } ParallelContainerBase::ParallelContainerBase(ParallelContainerBasePrivate* impl) : ContainerBase(impl) {} @@ -802,44 +836,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(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(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(me())->replaceImpl(); +} + +void FallbacksPrivate::onNewSolution(const SolutionBase& s) { + // printChildrenInterfaces(*this, true, *s.creator()); + static_cast(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(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()) + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("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(); + break; + case PROPAGATE_BACKWARDS: + dir_ = Interface::BACKWARD; + ends() = std::make_shared(); + 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( + std::bind(&FallbacksPrivateConnect::propagateStateUpdate, this, std::placeholders::_1, std::placeholders::_2)); + ends_ = std::make_shared( + std::bind(&FallbacksPrivateConnect::propagateStateUpdate, this, std::placeholders::_1, std::placeholders::_2)); + + FallbacksPrivateConnect::reset(); +} + +void FallbacksPrivateConnect::reset() { + active_ = children().end(); +} + +template +void FallbacksPrivateConnect::propagateStateUpdate(Interface::iterator external, Interface::UpdateFlags updated) { + copyState(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(const_cast((*next)->pimpl())); + auto first_con = static_cast(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) { @@ -859,7 +1089,10 @@ void MergerPrivate::resolveInterface(InterfaceFlags expected) { } } -Merger::Merger(const std::string& name) : Merger(new MergerPrivate(this, name)) {} +Merger::Merger(const std::string& name) : Merger(new MergerPrivate(this, name)) { + properties().declare("time_parameterization", + std::make_shared()); +} void Merger::reset() { ParallelContainerBase::reset(); @@ -1012,7 +1245,8 @@ void MergerPrivate::merge(const ChildSolutionList& sub_solutions, moveit::core::JointModelGroup* jmg = jmg_merged_.get(); robot_trajectory::RobotTrajectoryPtr merged; try { - merged = task_constructor::merge(sub_trajectories, start_scene->getCurrentState(), jmg); + auto timing = me_->properties().get("time_parameterization"); + merged = task_constructor::merge(sub_trajectories, start_scene->getCurrentState(), jmg, *timing); } catch (const std::runtime_error& e) { SubTrajectory t; t.markAsFailure(); diff --git a/core/src/merge.cpp b/core/src/merge.cpp index f43a00aa..e02acb56 100644 --- a/core/src/merge.cpp +++ b/core/src/merge.cpp @@ -36,7 +36,6 @@ /* Authors: Luca Lach, Robert Haschke */ #include -#include #include #include @@ -106,7 +105,8 @@ moveit::core::JointModelGroup* merge(const std::vector& sub_trajectories, - const moveit::core::RobotState& base_state, moveit::core::JointModelGroup*& merged_group) { + const moveit::core::RobotState& base_state, moveit::core::JointModelGroup*& merged_group, + const trajectory_processing::TimeParameterization& time_parameterization) { if (sub_trajectories.size() <= 1) throw std::runtime_error("Expected multiple sub solutions"); @@ -166,8 +166,7 @@ merge(const std::vector& sub_trajecto } // add timing - trajectory_processing::IterativeParabolicTimeParameterization timing; - timing.computeTimeStamps(*merged_traj, 1.0, 1.0); + time_parameterization.computeTimeStamps(*merged_traj, 1.0, 1.0); return merged_traj; } } // namespace task_constructor diff --git a/core/src/solvers/cartesian_path.cpp b/core/src/solvers/cartesian_path.cpp index 059e8348..d247831a 100644 --- a/core/src/solvers/cartesian_path.cpp +++ b/core/src/solvers/cartesian_path.cpp @@ -40,9 +40,11 @@ #include #include -#include +#include #include +using namespace trajectory_processing; + namespace moveit { namespace task_constructor { namespace solvers { @@ -101,9 +103,9 @@ bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, cons for (const auto& waypoint : trajectory) result->addSuffixWayPoint(waypoint, 0.0); - trajectory_processing::IterativeParabolicTimeParameterization timing; - timing.computeTimeStamps(*result, props.get("max_velocity_scaling_factor"), - props.get("max_acceleration_scaling_factor")); + auto timing = props.get("time_parameterization"); + timing->computeTimeStamps(*result, props.get("max_velocity_scaling_factor"), + props.get("max_acceleration_scaling_factor")); return achieved_fraction >= props.get("min_fraction"); } diff --git a/core/src/solvers/joint_interpolation.cpp b/core/src/solvers/joint_interpolation.cpp index 0729b6f3..fb017431 100644 --- a/core/src/solvers/joint_interpolation.cpp +++ b/core/src/solvers/joint_interpolation.cpp @@ -37,8 +37,9 @@ */ #include +#include #include -#include +#include #include @@ -48,6 +49,8 @@ namespace solvers { static const auto LOGGER = rclcpp::get_logger("JointInterpolationPlanner"); +using namespace trajectory_processing; + JointInterpolationPlanner::JointInterpolationPlanner() { auto& p = properties(); p.declare("max_step", 0.1, "max joint step"); @@ -91,10 +94,9 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr if (from->isStateColliding(to_state, jmg->getName())) return false; - // add timing, TODO: use a generic method to add timing via plugins - trajectory_processing::IterativeParabolicTimeParameterization timing; - timing.computeTimeStamps(*result, props.get("max_velocity_scaling_factor"), - props.get("max_acceleration_scaling_factor")); + auto timing = props.get("time_parameterization"); + timing->computeTimeStamps(*result, props.get("max_velocity_scaling_factor"), + props.get("max_acceleration_scaling_factor")); return true; } diff --git a/core/src/solvers/pipeline_planner.cpp b/core/src/solvers/pipeline_planner.cpp index 4575a6f0..36125410 100644 --- a/core/src/solvers/pipeline_planner.cpp +++ b/core/src/solvers/pipeline_planner.cpp @@ -141,6 +141,7 @@ void PipelinePlanner::init(const core::RobotModelConstPtr& robot_model) { Specification spec; spec.model = robot_model; spec.pipeline = pipeline_name_; + spec.ns = pipeline_name_; planner_ = create(node_, spec); } else if (robot_model != planner_->getRobotModel()) { throw std::runtime_error( diff --git a/core/src/solvers/planner_interface.cpp b/core/src/solvers/planner_interface.cpp index 23e31473..bb2a49e7 100644 --- a/core/src/solvers/planner_interface.cpp +++ b/core/src/solvers/planner_interface.cpp @@ -37,6 +37,10 @@ */ #include +#include +#include + +using namespace trajectory_processing; namespace moveit { namespace task_constructor { @@ -46,6 +50,7 @@ PlannerInterface::PlannerInterface() { auto& p = properties(); p.declare("max_velocity_scaling_factor", 1.0, "scale down max velocity by this factor"); p.declare("max_acceleration_scaling_factor", 1.0, "scale down max acceleration by this factor"); + p.declare("time_parameterization", std::make_shared()); } } // namespace solvers } // namespace task_constructor diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 946125ba..19c7db09 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -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()) @@ -316,6 +343,7 @@ void Stage::reset() { impl->next_starts_.reset(); // reset inherited properties impl->properties_.reset(); + impl->total_compute_time_ = std::chrono::duration::zero(); } void Stage::init(const moveit::core::RobotModelConstPtr& /* robot_model */) { @@ -483,14 +511,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(); 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(); return; case PropagatingEitherWay::AUTO: required_interface_ = UNKNOWN; @@ -688,10 +716,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, this, std::placeholders::_1, - std::placeholders::_2))); - ends_.reset(new Interface(std::bind(&ConnectingPrivate::newState, this, std::placeholders::_1, - std::placeholders::_2))); + starts_ = std::make_shared(std::bind(&ConnectingPrivate::newState, this, + std::placeholders::_1, std::placeholders::_2)); + ends_ = std::make_shared( + std::bind(&ConnectingPrivate::newState, this, std::placeholders::_1, std::placeholders::_2)); } InterfaceFlags ConnectingPrivate::requiredInterface() const { @@ -709,56 +737,108 @@ ConnectingPrivate::StatePair ConnectingPrivate::make_pair(In return StatePair(second, first); } -template -void ConnectingPrivate::newState(Interface::iterator it, bool updated) { - if (updated) { // many pairs might be affected: resort - if (it->priority().pruned()) - // remove all pending pairs involving this state - pending.remove_if([it](const StatePair& p) { return std::get()>(p) == it; }); - else - // TODO(v4hn): If a state becomes reenabled, this skips all previously removed pairs, right? - pending.sort(); - } else { // new state: insert all pairs with other interface - assert(it->priority().enabled()); // new solutions are feasible, aren't they? - InterfacePtr other_interface = pullInterface(other); - for (Interface::iterator oit = other_interface->begin(), oend = other_interface->end(); oit != oend; ++oit) { - // Don't re-enable states that are marked DISABLED - if (static_cast(me_)->compatible(*it, *oit)) { - // re-enable the opposing state oit if its status is FAILED - if (oit->priority().status() == InterfaceState::Status::FAILED) - oit->owner()->updatePriority(&*oit, - InterfaceState::Priority(oit->priority(), InterfaceState::Status::ENABLED)); - pending.insert(make_pair(it, oit)); +template +void ConnectingPrivate::newState(Interface::iterator it, Interface::UpdateFlags updated) { + auto parent_pimpl = parent()->pimpl(); + // disable current interface to break loop (jumping back and forth between both interfaces) + // this will be checked by notifyEnabled() below + Interface::DisableNotify disable_source_interface(*pullInterface()); + if (updated) { + if (updated.testFlag(Interface::STATUS) && // only perform these costly operations if needed + pullInterface()>()->notifyEnabled()) // suppressing recursive loop? + { + // If status has changed, propagate the update to the opposite side + auto status = it->priority().status(); + if (status == InterfaceState::Status::PRUNED) // PRUNED becomes ARMED on opposite side + status = InterfaceState::Status::ARMED; // (only for pending state pairs) + + for (const auto& candidate : this->pending) { + if (std::get()>(candidate) != it) // only consider pairs with source state == state + continue; + auto oit = std::get(candidate); // opposite target state + auto ostatus = oit->priority().status(); + if (ostatus != status) { + if (status != InterfaceState::Status::ENABLED) { + // quicker check for hasPendingOpposites(): search in it->owner() for an enabled alternative + bool cancel = false; // if found, cancel propagation of new status + for (const auto alternative : *it->owner()) + if ((cancel = alternative->priority().enabled())) + break; + if (cancel) + continue; + } + // pass creator=nullptr to skip hasPendingOpposites() check + parent_pimpl->setStatus()>(nullptr, nullptr, &*oit, status); + } } } + + // many pairs will have changed priorities: resort pending list + pending.sort(); + } else { // new state: insert all pairs with other interface + assert(it->priority().enabled()); // new solutions are feasible, aren't they? + InterfacePtr other_interface = pullInterface(); + bool have_enabled_opposites = false; + for (Interface::iterator oit = other_interface->begin(), oend = other_interface->end(); oit != oend; ++oit) { + if (!static_cast(me_)->compatible(*it, *oit)) + continue; + + // re-enable the opposing state oit (and its associated solution branch) if its status is ARMED + // https://github.com/ros-planning/moveit_task_constructor/pull/309#issuecomment-974636202 + if (oit->priority().status() == InterfaceState::Status::ARMED) + parent_pimpl->setStatus()>(me(), &*it, &*oit, InterfaceState::Status::ENABLED); + if (oit->priority().enabled()) + have_enabled_opposites = true; + + // Remember all pending states, regardless of their status! + pending.insert(make_pair(it, oit)); + } + if (!have_enabled_opposites) // prune new state and associated branch if necessary + // pass creator=nullptr to skip hasPendingOpposites() check as we did this here already + parent_pimpl->setStatus(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 }) { + if (d == Interface::FORWARD) + os << " " << std::setw(10) << std::left << this->name(); + else + os << std::setw(12) << std::right << ""; + if (dir != d) + os << (updated ? " !" : " +"); + else + os << " "; + os << d << " " << this->pullInterface(d) << ": " << *this->pullInterface(d) << std::endl; + } + os << std::setw(15) << " "; + printPendingPairs(os) << std::endl; +#endif } -// Check whether there are pending feasible states that could connect to source. -// If not, we exhausted all solution candidates for source and thus should mark it as failure. +// Check whether there are pending feasible states (other than source) that could connect to target. +// If not, we exhausted all solution candidates for target and thus should mark it as failure. template -inline bool ConnectingPrivate::hasPendingOpposites(const InterfaceState* source) const { +inline bool ConnectingPrivate::hasPendingOpposites(const InterfaceState* source, const InterfaceState* target) const { for (const auto& candidate : this->pending) { - static_assert(Interface::FORWARD == 0, "This code assumes FORWARD=0, BACKWARD=1. Don't change their order!"); - const auto src = std::get(candidate); - static_assert(Interface::BACKWARD == 1, "This code assumes FORWARD=0, BACKWARD=1. Don't change their order!"); - const auto tgt = std::get()>(candidate); + static_assert(Interface::FORWARD == 0 && Interface::BACKWARD == 1, + "This code assumes FORWARD=0, BACKWARD=1. Don't change their order!"); + const InterfaceState* src = &*std::get(candidate); + const InterfaceState* tgt = &*std::get()>(candidate); - if (&*src == source && tgt->priority().enabled()) + if (tgt == target && src != source && src->priority().enabled()) return true; // early stopping when only infeasible pairs are to come - if (!std::get<0>(candidate)->priority().enabled()) + if (!std::get<0>(candidate)->priority().enabled() || !std::get<1>(candidate)->priority().enabled()) break; } return false; } // explicitly instantiate templates for both directions -template bool ConnectingPrivate::hasPendingOpposites(const InterfaceState* source) const; -template bool ConnectingPrivate::hasPendingOpposites(const InterfaceState* source) const; +template bool ConnectingPrivate::hasPendingOpposites(const InterfaceState* start, + const InterfaceState* end) const; +template bool ConnectingPrivate::hasPendingOpposites(const InterfaceState* end, + const InterfaceState* start) const; bool ConnectingPrivate::canCompute() const { // Do we still have feasible pending state pairs? @@ -775,24 +855,15 @@ void ConnectingPrivate::compute() { } std::ostream& ConnectingPrivate::printPendingPairs(std::ostream& os) const { - static const char* red = "\033[31m"; - static const char* reset = "\033[m"; + const char* reset = InterfaceState::STATUS_COLOR[3]; for (const auto& candidate : pending) { - if (!candidate.first->priority().enabled() || !candidate.second->priority().enabled()) - os << " " << red; - // 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; - }); - os << first << ":" << second << " "; + 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 << " "; } - os << reset; + if (pending.empty()) + os << "---"; return os; } static const rclcpp::Logger LOGGER = rclcpp::get_logger("Connecting"); diff --git a/core/src/stages/compute_ik.cpp b/core/src/stages/compute_ik.cpp index 70233068..89389ebc 100644 --- a/core/src/stages/compute_ik.cpp +++ b/core/src/stages/compute_ik.cpp @@ -311,24 +311,26 @@ void ComputeIK::compute() { bool colliding = !ignore_collisions && isTargetPoseCollidingInEEF(scene, sandbox_state, target_pose, link, &collisions); - // markers used for failures - std::deque failure_markers; // frames at target pose and ik frame - rviz_marker_tools::appendFrame(failure_markers, target_pose_msg, 0.1, "target frame"); - rviz_marker_tools::appendFrame(failure_markers, ik_pose_msg, 0.1, "ik frame"); + std::deque frame_markers; + rviz_marker_tools::appendFrame(frame_markers, target_pose_msg, 0.1, "target frame"); + rviz_marker_tools::appendFrame(frame_markers, ik_pose_msg, 0.1, "ik frame"); + // end-effector markers + std::deque eef_markers; // visualize placed end-effector - auto appender = [&failure_markers](visualization_msgs::msg::Marker& marker, const std::string& /*name*/) { + auto appender = [&eef_markers](visualization_msgs::msg::Marker& marker, const std::string& /*name*/) { marker.ns = "ik target"; marker.color.a *= 0.5; - failure_markers.push_back(marker); + eef_markers.push_back(marker); }; const auto& links_to_visualize = moveit::core::RobotModel::getRigidlyConnectedParentLinkModel(link) ->getParentJointModel() ->getDescendantLinkModels(); if (colliding) { SubTrajectory solution; + std::copy(frame_markers.begin(), frame_markers.end(), std::back_inserter(solution.markers())); generateCollisionMarkers(sandbox_state, appender, links_to_visualize); - std::copy(failure_markers.begin(), failure_markers.end(), std::back_inserter(solution.markers())); + std::copy(eef_markers.begin(), eef_markers.end(), std::back_inserter(solution.markers())); solution.markAsFailure(); // TODO: visualize collisions solution.setComment(s.comment() + " eef in collision: " + listCollisionPairs(collisions.contacts, ", ")); @@ -389,10 +391,7 @@ void ComputeIK::compute() { planning_scene::PlanningScenePtr solution_scene = scene->diff(); SubTrajectory solution; solution.setComment(s.comment()); - - // frames at target pose and ik frame - rviz_marker_tools::appendFrame(solution.markers(), target_pose_msg, 0.1, "ik frame"); - rviz_marker_tools::appendFrame(solution.markers(), ik_pose_msg, 0.1, "ik frame"); + std::copy(frame_markers.begin(), frame_markers.end(), std::back_inserter(solution.markers())); if (succeeded && i + 1 == ik_solutions.size()) // compute cost as distance to compare_pose @@ -407,6 +406,10 @@ void ComputeIK::compute() { InterfaceState state(solution_scene); forwardProperties(*s.start(), state); + + // ik target link placement + std::copy(eef_markers.begin(), eef_markers.end(), std::back_inserter(solution.markers())); + spawn(std::move(state), std::move(solution)); } @@ -423,9 +426,17 @@ void ComputeIK::compute() { solution.markAsFailure(); solution.setComment(s.comment() + " no IK found"); + std::copy(frame_markers.begin(), frame_markers.end(), std::back_inserter(solution.markers())); // ik target link placement - std::copy(failure_markers.begin(), failure_markers.end(), std::back_inserter(solution.markers())); + std_msgs::msg::ColorRGBA tint_color; + tint_color.r = 1.0; + tint_color.g = 0.0; + tint_color.b = 0.0; + tint_color.a = 0.5; + for (auto& marker : eef_markers) + marker.color = tint_color; + std::copy(eef_markers.begin(), eef_markers.end(), std::back_inserter(solution.markers())); spawn(InterfaceState(scene), std::move(solution)); } diff --git a/core/src/stages/connect.cpp b/core/src/stages/connect.cpp index 0fac2feb..8019dc55 100644 --- a/core/src/stages/connect.cpp +++ b/core/src/stages/connect.cpp @@ -38,10 +38,14 @@ #include #include -#include - +#include #include +#include +#include + +using namespace trajectory_processing; + namespace moveit { namespace task_constructor { namespace stages { @@ -56,6 +60,8 @@ Connect::Connect(const std::string& name, const GroupPlannerVector& planners) : p.declare("merge_mode", WAYPOINTS, "merge mode"); p.declare("path_constraints", moveit_msgs::msg::Constraints(), "constraints to maintain during trajectory"); + properties().declare("merge_time_parameterization", + std::make_shared()); } void Connect::reset() { @@ -221,7 +227,8 @@ SubTrajectoryPtr Connect::merge(const std::vector("merge_time_parameterization"); + robot_trajectory::RobotTrajectoryPtr trajectory = task_constructor::merge(sub_trajectories, state, jmg, *timing); if (!trajectory) return SubTrajectoryPtr(); diff --git a/core/src/stages/fixed_state.cpp b/core/src/stages/fixed_state.cpp index 71ad60ad..9c4d99b0 100644 --- a/core/src/stages/fixed_state.cpp +++ b/core/src/stages/fixed_state.cpp @@ -45,6 +45,7 @@ namespace stages { FixedState::FixedState(const std::string& name, planning_scene::PlanningScenePtr scene) : Generator(name), scene_(scene) { + properties().declare("ignore_collisions", false); setCostTerm(std::make_unique(0.0)); } @@ -62,7 +63,12 @@ bool FixedState::canCompute() const { } void FixedState::compute() { - spawn(InterfaceState(scene_), 0.0); + SubTrajectory trajectory; + if (!properties().get("ignore_collisions") && scene_->isStateColliding()) { + trajectory.markAsFailure("in collision"); + } + + spawn(InterfaceState(scene_), std::move(trajectory)); ran_ = true; } } // namespace stages diff --git a/core/src/stages/generate_place_pose.cpp b/core/src/stages/generate_place_pose.cpp index 56dd2bde..7fe2ff6a 100644 --- a/core/src/stages/generate_place_pose.cpp +++ b/core/src/stages/generate_place_pose.cpp @@ -61,6 +61,7 @@ GeneratePlacePose::GeneratePlacePose(const std::string& name) : GeneratePose(nam auto& p = properties(); p.declare("object"); p.declare("ik_frame"); + p.declare("allow_z_flip", false, "allow placing objects upside down"); } void GeneratePlacePose::onNewSolution(const SolutionBase& s) { @@ -116,7 +117,7 @@ void GeneratePlacePose::compute() { // spawn the nominal target object pose, considering flip about z and rotations about z-axis auto spawner = [&s, &scene, &object_to_ik, this](const Eigen::Isometry3d& nominal, uint z_flips, uint z_rotations = 10) { - for (uint flip = 0; flip < z_flips; ++flip) { + for (uint flip = 0; flip <= z_flips; ++flip) { // flip about object's x-axis Eigen::Isometry3d object = nominal * Eigen::AngleAxisd(flip * M_PI, Eigen::Vector3d::UnitX()); for (uint i = 0; i < z_rotations; ++i) { @@ -144,20 +145,21 @@ void GeneratePlacePose::compute() { } }; + uint z_flips = props.get("allow_z_flip") ? 1 : 0; if (object->getShapes().size() == 1) { switch (object->getShapes()[0]->type) { case shapes::CYLINDER: - spawner(target_pose, 2); + spawner(target_pose, z_flips); return; case shapes::BOX: { // consider 180/90 degree rotations about z axis const double* dims = static_cast(*object->getShapes()[0]).size; - spawner(target_pose, 2, (std::abs(dims[0] - dims[1]) < 1e-5) ? 4 : 2); + spawner(target_pose, z_flips, (std::abs(dims[0] - dims[1]) < 1e-5) ? 4 : 2); return; } case shapes::SPHERE: // keep original orientation and rotate about world's z target_pose.linear() = orig_object_pose.linear(); - spawner(target_pose, 1); + spawner(target_pose, z_flips); return; default: break; diff --git a/core/src/stages/move_relative.cpp b/core/src/stages/move_relative.cpp index 23b7a9dc..8dd66edd 100644 --- a/core/src/stages/move_relative.cpp +++ b/core/src/stages/move_relative.cpp @@ -249,9 +249,9 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning // compute absolute transform for link linear = frame_pose.linear() * linear; angular = frame_pose.linear() * angular; - target_eigen = link_pose; + target_eigen = ik_pose_world; target_eigen.linear() = - target_eigen.linear() * Eigen::AngleAxisd(angular_norm, link_pose.linear().transpose() * angular); + target_eigen.linear() * Eigen::AngleAxisd(angular_norm, ik_pose_world.linear().transpose() * angular); target_eigen.translation() += linear; goto COMPUTE; } catch (const boost::bad_any_cast&) { /* continue with Vector */ @@ -276,7 +276,7 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning // compute absolute transform for link linear = frame_pose.linear() * linear; - target_eigen = link_pose; + target_eigen = ik_pose_world; target_eigen.translation() += linear; } catch (const boost::bad_any_cast&) { solution.markAsFailure(std::string("invalid direction type: ") + direction.type().name()); @@ -285,7 +285,7 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning COMPUTE: // transform target pose such that ik frame will reach there if link does - target_eigen = target_eigen * scene->getCurrentState().getGlobalLinkTransform(link).inverse() * ik_pose_world; + target_eigen = target_eigen * ik_pose_world.inverse() * scene->getCurrentState().getGlobalLinkTransform(link); success = planner_->plan(state.scene(), *link, target_eigen, jmg, timeout, robot_trajectory, path_constraints); diff --git a/core/src/storage.cpp b/core/src/storage.cpp index f08eb259..69671079 100644 --- a/core/src/storage.cpp +++ b/core/src/storage.cpp @@ -84,6 +84,21 @@ bool InterfaceState::Priority::operator<(const InterfaceState::Priority& other) return cost() < other.cost(); } +void InterfaceState::updatePriority(const InterfaceState::Priority& priority) { + // Never overwrite ARMED with PRUNED + if (priority.status() == InterfaceState::Status::PRUNED && priority_.status() == InterfaceState::Status::ARMED) + return; + + if (owner()) { + owner()->updatePriority(this, priority); + } else { + setPriority(priority); + } +} +void InterfaceState::updateStatus(Status status) { + updatePriority(InterfaceState::Priority(priority_, status)); +} + Interface::Interface(const Interface::NotifyFunction& notify) : notify_(notify) {} // Announce a new InterfaceState @@ -117,7 +132,7 @@ void Interface::add(InterfaceState& state) { moveFrom(it, container); // and finally call notify callback if (notify_) - notify_(it, false); + notify_(it, UpdateFlags()); } Interface::container_type Interface::remove(iterator it) { @@ -128,15 +143,23 @@ Interface::container_type Interface::remove(iterator it) { } void Interface::updatePriority(InterfaceState* state, const InterfaceState::Priority& priority) { - if (priority == state->priority()) + const auto old_prio = state->priority(); + if (priority == old_prio) return; // nothing to do auto it = std::find(begin(), end(), state); // find iterator to state assert(it != end()); // state should be part of this interface + state->priority_ = priority; // update priority update(it); // update position in ordered list - if (notify_) - notify_(it, true); // notify callback + + if (notify_) { + UpdateFlags updated(Update::ALL); + if (old_prio.status() == priority.status()) + updated &= ~STATUS; + + notify_(it, updated); // notify callback + } } std::ostream& operator<<(std::ostream& os, const Interface& interface) { @@ -146,15 +169,20 @@ std::ostream& operator<<(std::ostream& os, const Interface& interface) { os << istate->priority() << " "; return os; } +const char* InterfaceState::STATUS_COLOR[] = { + "\033[32m", // ENABLED - green + "\033[33m", // ARMED - yellow + "\033[31m", // PRUNED - red + "\033[m" // reset +}; std::ostream& operator<<(std::ostream& os, const InterfaceState::Priority& prio) { // maps InterfaceState::Status values to output (color-changing) prefix - static const char* prefix[] = { - "\033[32me:", // ENABLED - green - "\033[33md:", // PRUNED - yellow - "\033[31mf:", // FAILED - red - }; - static const char* color_reset = "\033[m"; - os << prefix[prio.status()] << prio.depth() << ":" << prio.cost() << color_reset; + os << InterfaceState::STATUS_COLOR[prio.status()] << prio.depth() << ":" << prio.cost() + << InterfaceState::STATUS_COLOR[3]; + return os; +} +std::ostream& operator<<(std::ostream& os, Interface::Direction dir) { + os << (dir == Interface::FORWARD ? "↓" : "↑"); return os; } @@ -169,8 +197,13 @@ void SolutionBase::setCost(double cost) { void SolutionBase::markAsFailure(const std::string& msg) { setCost(std::numeric_limits::infinity()); - if (!msg.empty()) - setComment(msg + "\n" + comment()); + if (!msg.empty()) { + std::stringstream ss; + ss << msg; + if (!comment().empty()) + ss << "\n" << comment(); + setComment(ss.str()); + } } void SolutionBase::fillInfo(moveit_task_constructor_msgs::msg::SolutionInfo& info, Introspection* introspection) const { diff --git a/core/src/task.cpp b/core/src/task.cpp index 9d1694fc..81aca0f3 100644 --- a/core/src/task.cpp +++ b/core/src/task.cpp @@ -76,30 +76,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(lhs)->children_; - for (auto it = lhs_children.begin(), end = lhs_children.end(); it != end; ++it) { - (*it)->pimpl()->unparent(); - (*it)->pimpl()->setParent(static_cast(lhs->me_)); - (*it)->pimpl()->setParentPosition(it); - } - - auto& rhs_children = static_cast(rhs)->children_; - for (auto it = rhs_children.begin(), end = rhs_children.end(); it != end; ++it) { - (*it)->pimpl()->unparent(); - (*it)->pimpl()->setParent(static_cast(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 { @@ -124,7 +108,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(pimpl_) = std::move(*static_cast(other.pimpl_)); return *this; } @@ -248,30 +232,37 @@ void Task::compute() { stages()->pimpl()->runCompute(); } -bool Task::plan(size_t max_solutions) { +moveit::core::MoveItErrorCode Task::plan(size_t max_solutions) { auto impl = pimpl(); init(); + // Print state and return success if there are solutions otherwise the input error_code + const auto success_or = [this](const int32_t error_code) { + printState(); + return numSolutions() > 0 ? moveit::core::MoveItErrorCode::SUCCESS : error_code; + }; impl->preempt_requested_ = false; const double available_time = timeout(); const auto start_time = std::chrono::steady_clock::now(); - while (!impl->preempt_requested_ && canCompute() && (max_solutions == 0 || numSolutions() < max_solutions) && - std::chrono::duration(std::chrono::steady_clock::now() - start_time).count() < available_time) { + while (canCompute() && (max_solutions == 0 || numSolutions() < max_solutions)) { + if (impl->preempt_requested_) + return success_or(moveit::core::MoveItErrorCode::PREEMPTED); + if (std::chrono::duration(std::chrono::steady_clock::now() - start_time).count() > available_time) + return success_or(moveit::core::MoveItErrorCode::TIMED_OUT); compute(); for (const auto& cb : impl->task_cbs_) cb(*this); if (impl->introspection_) impl->introspection_->publishTaskState(); - } - printState(); - return numSolutions() > 0; + }; + return success_or(moveit::core::MoveItErrorCode::PLANNING_FAILED); } void Task::preempt() { pimpl()->preempt_requested_ = true; } -moveit_msgs::msg::MoveItErrorCodes Task::execute(const SolutionBase& s) { +moveit::core::MoveItErrorCode Task::execute(const SolutionBase& s) { // Add random ID to prevent warnings about multiple publishers within the same node rclcpp::NodeOptions options; options.arguments( diff --git a/core/test/CMakeLists.txt b/core/test/CMakeLists.txt index 99eb6097..18ad864c 100644 --- a/core/test/CMakeLists.txt +++ b/core/test/CMakeLists.txt @@ -26,6 +26,7 @@ if (BUILD_TESTING) if(TEST_FILE) # Add launch test if .launch.py file was specified ament_add_gtest_executable(${PROJECT_NAME}-${TEST_NAME} ${SOURCES}) add_launch_test(${TEST_FILE} + TARGET ${TEST_NAME} ARGS "test_binary:=$") else() if("${TYPE}" STREQUAL "gtest") @@ -55,6 +56,7 @@ if (BUILD_TESTING) mtc_add_gmock(test_interface_state.cpp) mtc_add_gtest(test_move_to.cpp move_to.launch.py) + mtc_add_gtest(test_move_relative.cpp move_to.launch.py) # building these integration tests works without moveit config packages ament_add_gtest_executable(pick_ur5 pick_ur5.cpp) diff --git a/core/test/test_container.cpp b/core/test/test_container.cpp index 4eaa143b..7d736bd0 100644 --- a/core/test/test_container.cpp +++ b/core/test/test_container.cpp @@ -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) { @@ -655,7 +651,7 @@ TEST(Task, timeout) { // zero timeout fails t.reset(); t.setTimeout(0.0); - EXPECT_FALSE(t.plan()); + EXPECT_EQ(t.plan(), moveit::core::MoveItErrorCode::TIMED_OUT); // time for 1 solution t.reset(); diff --git a/core/test/test_cost_queue.cpp b/core/test/test_cost_queue.cpp index 0f4a0433..1d665704 100644 --- a/core/test/test_cost_queue.cpp +++ b/core/test/test_cost_queue.cpp @@ -5,6 +5,10 @@ #include #include +#ifndef TYPED_TEST_SUITE +#define TYPED_TEST_SUITE(SUITE, TYPES) TYPED_TEST_CASE(SUITE, TYPES) +#endif + namespace mtc = moveit::task_constructor; // type-trait functions for OrderedTest @@ -62,11 +66,7 @@ protected: }; // set of template types to test for using TypeInstances = ::testing::Types; -#ifdef TYPED_TEST_SUITE TYPED_TEST_SUITE(ValueOrPointeeLessTest, TypeInstances); -#else -TYPED_TEST_CASE(ValueOrPointeeLessTest, TypeInstances); -#endif TYPED_TEST(ValueOrPointeeLessTest, less) { EXPECT_TRUE(this->less(2, 3)); EXPECT_FALSE(this->less(1, 1)); @@ -105,11 +105,7 @@ protected: SCOPED_TRACE("pushAndValidate(" #cost ", " #__VA_ARGS__ ")"); \ this->pushAndValidate(cost, __VA_ARGS__); \ } -#ifdef TYPED_TEST_SUITE TYPED_TEST_SUITE(OrderedTest, TypeInstances); -#else -TYPED_TEST_CASE(OrderedTest, TypeInstances); -#endif TYPED_TEST(OrderedTest, sorting) { pushAndValidate(2, { 2 }); pushAndValidate(1, { 1, 2 }); diff --git a/core/test/test_fallback.cpp b/core/test/test_fallback.cpp index d3f7e906..cf2d7050 100644 --- a/core/test/test_fallback.cpp +++ b/core/test/test_fallback.cpp @@ -18,7 +18,7 @@ using namespace moveit::task_constructor; using FallbacksFixtureGenerator = TaskTestBase; -TEST_F(FallbacksFixtureGenerator, DISABLED_stayWithFirstSuccessful) { +TEST_F(FallbacksFixtureGenerator, stayWithFirstSuccessful) { auto fallback = std::make_unique("Fallbacks"); fallback->add(std::make_unique(PredefinedCosts::single(INF))); fallback->add(std::make_unique(PredefinedCosts::single(1.0))); @@ -56,7 +56,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()); auto fallbacks = std::make_unique("Fallbacks"); @@ -68,7 +68,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(PredefinedCosts({ 2.0, 1.0 }))); // duplicate generator solutions with resulting costs: 4, 2 | 3, 1 t.add(std::make_unique(PredefinedCosts({ 2.0, 0.0, 2.0, 0.0 }), 2)); @@ -79,10 +79,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(PredefinedCosts({ 10.0, 0.0 }))); t.add(std::make_unique(PredefinedCosts({ 1.0, 2.0 }))); // available solutions (sorted) in individual runs of fallbacks: 1 | 11, 2 | 2, 11 @@ -101,7 +102,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(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("Inner"); @@ -118,7 +120,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()); auto fallback = std::make_unique("Fallbacks"); @@ -130,7 +132,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()); auto fallback = std::make_unique("Fallbacks"); @@ -142,7 +144,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(PredefinedCosts({ 1.0, INF, 3.0 }))); auto fallbacks = std::make_unique("Fallbacks"); @@ -160,18 +162,18 @@ TEST_F(FallbacksFixturePropagate, DISABLED_ActiveChildReset) { using FallbacksFixtureConnect = TaskTestBase; -TEST_F(FallbacksFixtureConnect, DISABLED_ConnectStageInsideFallbacks) { +TEST_F(FallbacksFixtureConnect, connectStageInsideFallbacks) { t.add(std::make_unique(PredefinedCosts({ 1.0, 2.0 }))); auto fallbacks = std::make_unique("Fallbacks"); - fallbacks->add(std::make_unique(PredefinedCosts::constant(0.0))); + fallbacks->add(std::make_unique(PredefinedCosts({ 0.0, 0.0, INF, 0.0 }))); fallbacks->add(std::make_unique(PredefinedCosts::constant(100.0))); t.add(std::move(fallbacks)); t.add(std::make_unique(PredefinedCosts({ 10.0, 20.0 }))); 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) { diff --git a/core/test/test_interface_state.cpp b/core/test/test_interface_state.cpp index 9ca6af53..65aeee36 100644 --- a/core/test/test_interface_state.cpp +++ b/core/test/test_interface_state.cpp @@ -19,7 +19,7 @@ TEST(InterfaceStatePriority, compare) { EXPECT_TRUE(Prio(1, 42) < Prio(0, 0)); EXPECT_TRUE(Prio(0, 0) < Prio(0, 42)); // at same depth, higher cost is larger - auto dstart = InterfaceState::Status::FAILED; + auto dstart = InterfaceState::Status::ARMED; EXPECT_TRUE(Prio(0, 0, dstart) == Prio(0, 0, dstart)); EXPECT_TRUE(Prio(1, 0, dstart) < Prio(0, 0, dstart)); EXPECT_TRUE(Prio(1, 42, dstart) < Prio(0, 0, dstart)); @@ -68,7 +68,7 @@ TEST(Interface, update) { i.updatePriority(*i.rbegin(), Prio(5, 0.0)); EXPECT_THAT(i.depths(), ::testing::ElementsAreArray({ 5, 3 })); - i.updatePriority(*i.begin(), Prio(6, 0, InterfaceState::Status::FAILED)); + i.updatePriority(*i.begin(), Prio(6, 0, InterfaceState::Status::ARMED)); EXPECT_THAT(i.depths(), ::testing::ElementsAreArray({ 3, 6 })); } @@ -87,8 +87,11 @@ TEST(StatePairs, compare) { EXPECT_TRUE(pair(Prio(1, 1), Prio(1, 1)) < pair(Prio(1, 0), Prio(0, 0))); auto good = InterfaceState::Status::ENABLED; - auto bad = InterfaceState::Status::FAILED; - EXPECT_TRUE(pair(good, good) < pair(good, bad)); - EXPECT_TRUE(pair(good, good) < pair(bad, good)); - EXPECT_TRUE(pair(bad, good) < pair(good, bad)); + auto good_good = pair(Prio(0, 10, good), Prio(0, 0, good)); + ASSERT_TRUE(good_good > pair(good, good)); // a bad status should reverse this relation + for (auto bad : { InterfaceState::Status::ARMED, InterfaceState::Status::PRUNED }) { + EXPECT_TRUE(good_good < pair(bad, good)); + EXPECT_TRUE(good_good < pair(good, bad)); + EXPECT_TRUE(good_good < pair(bad, bad)); + } } diff --git a/core/test/test_move_relative.cpp b/core/test/test_move_relative.cpp new file mode 100644 index 00000000..d7720428 --- /dev/null +++ b/core/test/test_move_relative.cpp @@ -0,0 +1,128 @@ +#include "models.h" + +#include +#include +#include +#include +#include + +#include + +#include +#include + +#include + +using namespace moveit::task_constructor; +using namespace planning_scene; +using namespace moveit::core; + +constexpr double TAU{ 2 * M_PI }; +constexpr double EPS{ 1e-6 }; + +// provide a basic test fixture that prepares a Task +struct PandaMoveRelative : public testing::Test +{ + Task t; + stages::MoveRelative* move; + PlanningScenePtr scene; + rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("panda_move_relative"); + + const JointModelGroup* group; + + PandaMoveRelative() { + t.setRobotModel(loadModel(node)); + + group = t.getRobotModel()->getJointModelGroup("panda_arm"); + + scene = std::make_shared(t.getRobotModel()); + scene->getCurrentStateNonConst().setToDefaultValues(); + scene->getCurrentStateNonConst().setToDefaultValues(t.getRobotModel()->getJointModelGroup("panda_arm"), "ready"); + t.add(std::make_unique("start", scene)); + + auto move_relative = std::make_unique("move", std::make_shared()); + move_relative->setGroup(group->getName()); + move = move_relative.get(); + t.add(std::move(move_relative)); + } +}; + +moveit_msgs::msg::AttachedCollisionObject createAttachedObject(const std::string& id) { + moveit_msgs::msg::AttachedCollisionObject aco; + aco.link_name = "panda_hand"; + aco.object.header.frame_id = aco.link_name; + aco.object.operation = aco.object.ADD; + aco.object.id = id; + aco.object.primitives.resize(1, [] { + shape_msgs::msg::SolidPrimitive p; + p.type = p.SPHERE; + p.dimensions.resize(1); + p.dimensions[p.SPHERE_RADIUS] = 0.01; + return p; + }()); + + geometry_msgs::msg::Pose p; + p.position.x = 0.1; + p.orientation.w = 1.0; +#if MOVEIT_HAS_OBJECT_POSE + aco.object.pose = p; +#else + aco.object.primitive_poses.resize(1, p); + aco.object.primitive_poses[0] = p; +#endif + return aco; +} + +inline auto position(const PlanningSceneConstPtr& scene, const std::string& frame) { + return scene->getFrameTransform(frame).translation(); +} + +TEST_F(PandaMoveRelative, cartesianRotateEEF) { + move->setDirection([] { + geometry_msgs::msg::TwistStamped twist; + twist.header.frame_id = "world"; + twist.twist.angular.z = TAU / 8.0; + return twist; + }()); + + ASSERT_TRUE(t.plan()) << "Failed to plan"; + + const auto& tip_name{ group->getOnlyOneEndEffectorTip()->getName() }; + const auto start_eef_position{ position(scene, tip_name) }; + const auto end_eef_position{ position(move->solutions().front()->end()->scene(), tip_name) }; + + EXPECT_TRUE(start_eef_position.isApprox(end_eef_position, EPS)) + << "Cartesian rotation unexpectedly changed position of '" << tip_name << "' (must only change orientation)\n" + << start_eef_position << "\nvs\n" + << end_eef_position; +} + +TEST_F(PandaMoveRelative, cartesianRotateAttachedIKFrame) { + const std::string ATTACHED_OBJECT{ "attached_object" }; + scene->processAttachedCollisionObjectMsg(createAttachedObject(ATTACHED_OBJECT)); + move->setIKFrame(ATTACHED_OBJECT); + + move->setDirection([] { + geometry_msgs::msg::TwistStamped twist; + twist.header.frame_id = "world"; + twist.twist.angular.z = TAU / 8.0; + return twist; + }()); + + ASSERT_TRUE(t.plan()); + + const auto start_eef_position{ position(scene, ATTACHED_OBJECT) }; + const auto end_eef_position{ position(move->solutions().front()->end()->scene(), ATTACHED_OBJECT) }; + + EXPECT_TRUE(start_eef_position.isApprox(end_eef_position, EPS)) + << "Cartesian rotation unexpectedly changed position of ik frame (must only change orientation)\n" + << start_eef_position << "\nvs\n" + << end_eef_position; +} + +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + + return RUN_ALL_TESTS(); +} diff --git a/core/test/test_pruning.cpp b/core/test/test_pruning.cpp index 5da29cdd..b26ac527 100644 --- a/core/test/test_pruning.cpp +++ b/core/test/test_pruning.cpp @@ -8,6 +8,10 @@ #include +#ifndef TYPED_TEST_SUITE +#define TYPED_TEST_SUITE(SUITE, TYPES) TYPED_TEST_CASE(SUITE, TYPES) +#endif + using namespace moveit::task_constructor; using Pruning = TaskTestBase; @@ -40,7 +44,54 @@ TEST_F(Pruning, PruningMultiForward) { EXPECT_EQ((*t.solutions().begin())->cost(), 0u); } +// The 2nd failing FW attempt would prune the path through CON, +// but shouldn't because there exist two more GEN2 solutions +TEST_F(Pruning, NoPruningIfAlternativesExist) { + add(t, new GeneratorMockup(PredefinedCosts({ 0.0 }))); + add(t, new ConnectMockup()); + add(t, new GeneratorMockup(std::list{ 0, 10, 20, 30 }, 2)); + add(t, new ForwardMockup({ INF, INF, 0.0, INF })); + + t.plan(); + + EXPECT_EQ(t.solutions().size(), 1u); +} + +TEST_F(Pruning, ConnectReactivatesPrunedPaths) { + add(t, new BackwardMockup); + add(t, new GeneratorMockup({ 0 })); + add(t, new ConnectMockup()); + // the solution here should re-activate the initially pruned backward path + add(t, new GeneratorMockup({ 0 })); + + EXPECT_TRUE(t.plan()); + EXPECT_EQ(t.solutions().size(), 1u); +} + +// same as before, but wrapping Connect into a container +template +struct PruningContainerTests : public Pruning +{ + void test() { + add(t, new BackwardMockup); + add(t, new GeneratorMockup({ 0 })); + auto c = new T(); + add(*c, new ConnectMockup()); + add(t, c); + add(t, new GeneratorMockup({ 0 })); + + EXPECT_TRUE(t.plan()); + EXPECT_EQ(t.solutions().size(), 1u); + } +}; +using ContainerTypes = ::testing::Types; // TODO: fails for Fallbacks! +TYPED_TEST_SUITE(PruningContainerTests, ContainerTypes); +TYPED_TEST(PruningContainerTests, ConnectReactivatesPrunedPaths) { + this->test(); +} + TEST_F(Pruning, ConnectConnectForward) { + add(t, new BackwardMockup()); add(t, new GeneratorMockup()); auto c1 = add(t, new ConnectMockup({ INF, 0, 0 })); // 1st attempt is a failue add(t, new GeneratorMockup({ 0, 10, 20 })); @@ -62,6 +113,7 @@ TEST_F(Pruning, ConnectConnectForward) { } TEST_F(Pruning, ConnectConnectBackward) { + add(t, new BackwardMockup()); add(t, new GeneratorMockup({ 1, 2, 3 })); auto c1 = add(t, new ConnectMockup()); add(t, new BackwardMockup()); @@ -141,3 +193,17 @@ TEST_F(Pruning, PropagateFromParallelContainerMultiplePaths) { // the failure in one branch of Alternatives must not prune computing back EXPECT_EQ(back->runs_, 1u); } + +TEST_F(Pruning, TwoConnects) { + add(t, new GeneratorMockup({ 0 })); + add(t, new ForwardMockup({ INF })); + add(t, new ConnectMockup()); + + add(t, new GeneratorMockup()); + add(t, new ConnectMockup()); + + add(t, new GeneratorMockup()); + add(t, new ForwardMockup()); + + EXPECT_FALSE(t.plan()); +} diff --git a/demo/CMakeLists.txt b/demo/CMakeLists.txt index a3b9d8bc..fc144f12 100644 --- a/demo/CMakeLists.txt +++ b/demo/CMakeLists.txt @@ -1,9 +1,7 @@ cmake_minimum_required(VERSION 3.5) project(moveit_task_constructor_demo) -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() +set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) @@ -54,6 +52,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) diff --git a/demo/launch/fallbacks_move_to.launch.py b/demo/launch/fallbacks_move_to.launch.py new file mode 100644 index 00000000..9bd20936 --- /dev/null +++ b/demo/launch/fallbacks_move_to.launch.py @@ -0,0 +1,29 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from moveit_configs_utils import MoveItConfigsBuilder + + +def generate_launch_description(): + moveit_config = ( + MoveItConfigsBuilder("moveit_resources_panda") + .robot_description(file_path="config/panda.urdf.xacro") + .planning_pipelines(pipelines=["ompl", "pilz_industrial_motion_planner"]) + .to_moveit_configs() + ) + + print(moveit_config.planning_pipelines) + fallbacks_move_to_task = Node( + package="moveit_task_constructor_demo", + executable="fallbacks_move_to", + output="screen", + parameters=[ + moveit_config.cartesian_limits, + moveit_config.joint_limits, + moveit_config.planning_pipelines, + moveit_config.robot_description, + moveit_config.robot_description_kinematics, + moveit_config.robot_description_semantic, + ], + ) + + return LaunchDescription([fallbacks_move_to_task]) diff --git a/demo/src/fallbacks_move_to.cpp b/demo/src/fallbacks_move_to.cpp new file mode 100644 index 00000000..2ccecc09 --- /dev/null +++ b/demo/src/fallbacks_move_to.cpp @@ -0,0 +1,141 @@ +#include + +#include +#include + +#include + +#include +#include +#include +#include +#include + +constexpr double TAU = 2 * M_PI; + +using namespace moveit::task_constructor; + +/** CurrentState -> Fallbacks( MoveTo, MoveTo, MoveTo )*/ +int main(int argc, char** argv) { + rclcpp::init(argc, argv); + auto node = rclcpp::Node::make_shared("mtc_tutorial"); + std::thread spinning_thread([node] { rclcpp::spin(node); }); + + // setup Task + Task t; + t.loadRobotModel(node); + const moveit::core::RobotModelConstPtr robot{ t.getRobotModel() }; + + assert(robot->getName() == "panda"); + + // setup solvers + auto cartesian = std::make_shared(); + cartesian->setJumpThreshold(2.0); + + const auto ptp = [&node]() { + auto pp{ std::make_shared(node, "pilz_industrial_motion_planner") }; + pp->setPlannerId("PTP"); + return pp; + }(); + + const auto rrtconnect = [&node]() { + auto pp{ std::make_shared(node, "ompl") }; + pp->setPlannerId("RRTConnect"); + return pp; + }(); + + // target state for Task + std::map 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(robot) }; + initial_scene->getCurrentStateNonConst().setToDefaultValues(robot->getJointModelGroup("panda_arm"), "ready"); + + auto initial_alternatives = std::make_unique("initial states"); + + { + // can reach target with Cartesian motion + auto fixed{ std::make_unique("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("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("current state"); + auto scene{ initial_scene->diff() }; + scene->getCurrentStateNonConst().setVariablePositions({ { "panda_joint1", -TAU / 8 } }); + scene->processCollisionObjectMsg([]() { + moveit_msgs::msg::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::msg::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::msg::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("move to other side"); + + auto add_to_fallbacks{ [&](auto& solver, auto& name) { + auto move_to = std::make_unique(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 { + std::cout << t << std::endl; + t.plan(); + } catch (const InitStageException& e) { + std::cout << e << std::endl; + } + + // keep alive for interactive inspection in rviz + spinning_thread.join(); + + return 0; +} diff --git a/demo/src/ik_clearance_cost.cpp b/demo/src/ik_clearance_cost.cpp index 33cb9eb8..71c6adb0 100644 --- a/demo/src/ik_clearance_cost.cpp +++ b/demo/src/ik_clearance_cost.cpp @@ -46,6 +46,7 @@ int main(int argc, char** argv) { auto initial = std::make_unique(); initial->setState(scene); + initial->properties().set("ignore_collisions", true); auto ik = std::make_unique(); ik->insert(std::move(initial)); diff --git a/demo/src/pick_place_task.cpp b/demo/src/pick_place_task.cpp index 43eee00c..cd589ce4 100644 --- a/demo/src/pick_place_task.cpp +++ b/demo/src/pick_place_task.cpp @@ -491,7 +491,7 @@ bool PickPlaceTask::plan() { int max_solutions = 10; rosparam_shortcuts::get(node_, "max_solutions", max_solutions); - return task_->plan(max_solutions); + return static_cast(task_->plan(max_solutions)); } bool PickPlaceTask::execute() {