diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index c6d68842..12ef3599 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -39,6 +39,7 @@ #pragma once #include +#include "utils.h" #include "stage_p.h" #include @@ -146,7 +147,7 @@ public: SerialContainerPrivate(SerialContainer* me, const std::string &name); void storeNewSolution(SerialContainer::solution_container &&s, double cost); - const std::list& solutions() const { return solutions_; } + const ordered& solutions() const { return solutions_; } private: void connect(StagePrivate *prev, StagePrivate *next); @@ -157,7 +158,7 @@ private: InterfacePtr pending_forward_; // set of all solutions - std::list solutions_; + ordered solutions_; }; PIMPL_FUNCTIONS(SerialContainer) @@ -191,7 +192,7 @@ public: WrapperPrivate(Wrapper* me, const std::string& name); private: - std::list> solutions_; + ordered, pointerLessThan>> solutions_; std::list> failures_; std::list failure_states_; }; diff --git a/core/include/moveit/task_constructor/stage_p.h b/core/include/moveit/task_constructor/stage_p.h index 0b86e4cc..5781d86e 100644 --- a/core/include/moveit/task_constructor/stage_p.h +++ b/core/include/moveit/task_constructor/stage_p.h @@ -40,6 +40,7 @@ #include #include +#include // define pimpl() functions accessing correctly casted pimpl_ pointer #define PIMPL_FUNCTIONS(Class) \ @@ -136,7 +137,7 @@ public: } private: - std::list solutions_; + ordered solutions_; std::list failures_; size_t num_failures_ = 0; // num of failures if not stored }; diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index dad0e641..af9a4874 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -191,6 +191,11 @@ public: virtual void fillMessage(moveit_task_constructor_msgs::Solution &solution, Introspection* introspection = nullptr) const = 0; + /// order solutions by their cost + bool operator<(const SolutionBase& other) const { + return this->cost_ < other.cost_; + } + protected: SolutionBase(StagePrivate* creator = nullptr, double cost = 0.0) : creator_(creator), cost_(cost) diff --git a/core/include/moveit/task_constructor/utils.h b/core/include/moveit/task_constructor/utils.h index 667da900..657f4e5c 100644 --- a/core/include/moveit/task_constructor/utils.h +++ b/core/include/moveit/task_constructor/utils.h @@ -94,3 +94,12 @@ private: }; #define DECLARE_FLAGS(Flags, Enum) typedef QFlags Flags; + + +// compare two pointers by comparing their referenced values +template +struct pointerLessThan : std::enable_if::value> { + inline bool operator()(const T& x, const T& y) const { + return *x < *y; + } +}; diff --git a/core/src/container.cpp b/core/src/container.cpp index 4c803c11..75e43b44 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -268,8 +268,7 @@ void SerialContainerPrivate::storeNewSolution(SerialContainer::solution_containe const InterfaceState *internal_to = s.back()->end(); // create new solution directly in solutions_ and get a reference to it - solutions_.emplace_back(SerialSolution(this, std::move(s), cost)); - SerialSolution& solution = solutions_.back(); + SerialSolution& solution = *solutions_.insert(SerialSolution(this, std::move(s), cost)); // add solution to existing or new start state auto it = internal_to_my_starts_.find(internal_from); @@ -635,7 +634,7 @@ void Wrapper::spawn(InterfaceState &&state, std::unique_ptr&& s) s->setEndState(impl->failure_states_.back()); impl->failures_.emplace_back(std::move(s)); } else { - impl->solutions_.emplace_back(std::move(s)); + impl->solutions_.insert(std::move(s)); impl->prevEnds()->add(InterfaceState(state), NULL, solution); impl->nextStarts()->add(std::move(state), solution, NULL); } diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 90ba33b0..351f4cd3 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -207,8 +207,7 @@ std::ostream& operator<<(std::ostream &os, const Stage& stage) { SubTrajectory& ComputeBasePrivate::addTrajectory(SubTrajectory&& trajectory) { trajectory.setCreator(this); if (!trajectory.isFailure()) { - solutions_.emplace_back(std::move(trajectory)); - return solutions_.back(); + return *solutions_.insert(std::move(trajectory)); } else if (me()->storeFailures()) { // only store failures when introspection is enabled failures_.emplace_back(std::move(trajectory));