cost ordering for solutions

This commit is contained in:
Robert Haschke 2018-01-25 10:19:23 +01:00
parent c84fa4cdd6
commit 29577515a0
6 changed files with 23 additions and 9 deletions

View File

@ -39,6 +39,7 @@
#pragma once #pragma once
#include <moveit/task_constructor/container.h> #include <moveit/task_constructor/container.h>
#include "utils.h"
#include "stage_p.h" #include "stage_p.h"
#include <map> #include <map>
@ -146,7 +147,7 @@ public:
SerialContainerPrivate(SerialContainer* me, const std::string &name); SerialContainerPrivate(SerialContainer* me, const std::string &name);
void storeNewSolution(SerialContainer::solution_container &&s, double cost); void storeNewSolution(SerialContainer::solution_container &&s, double cost);
const std::list<SerialSolution>& solutions() const { return solutions_; } const ordered<SerialSolution>& solutions() const { return solutions_; }
private: private:
void connect(StagePrivate *prev, StagePrivate *next); void connect(StagePrivate *prev, StagePrivate *next);
@ -157,7 +158,7 @@ private:
InterfacePtr pending_forward_; InterfacePtr pending_forward_;
// set of all solutions // set of all solutions
std::list<SerialSolution> solutions_; ordered<SerialSolution> solutions_;
}; };
PIMPL_FUNCTIONS(SerialContainer) PIMPL_FUNCTIONS(SerialContainer)
@ -191,7 +192,7 @@ public:
WrapperPrivate(Wrapper* me, const std::string& name); WrapperPrivate(Wrapper* me, const std::string& name);
private: private:
std::list<std::unique_ptr<SolutionBase>> solutions_; ordered<std::unique_ptr<SolutionBase>, pointerLessThan<std::unique_ptr<SolutionBase>>> solutions_;
std::list<std::unique_ptr<SolutionBase>> failures_; std::list<std::unique_ptr<SolutionBase>> failures_;
std::list<InterfaceState> failure_states_; std::list<InterfaceState> failure_states_;
}; };

View File

@ -40,6 +40,7 @@
#include <moveit/task_constructor/stage.h> #include <moveit/task_constructor/stage.h>
#include <moveit/task_constructor/storage.h> #include <moveit/task_constructor/storage.h>
#include <moveit/task_constructor/cost_queue.h>
// define pimpl() functions accessing correctly casted pimpl_ pointer // define pimpl() functions accessing correctly casted pimpl_ pointer
#define PIMPL_FUNCTIONS(Class) \ #define PIMPL_FUNCTIONS(Class) \
@ -136,7 +137,7 @@ public:
} }
private: private:
std::list<SubTrajectory> solutions_; ordered<SubTrajectory> solutions_;
std::list<SubTrajectory> failures_; std::list<SubTrajectory> failures_;
size_t num_failures_ = 0; // num of failures if not stored size_t num_failures_ = 0; // num of failures if not stored
}; };

View File

@ -191,6 +191,11 @@ public:
virtual void fillMessage(moveit_task_constructor_msgs::Solution &solution, virtual void fillMessage(moveit_task_constructor_msgs::Solution &solution,
Introspection* introspection = nullptr) const = 0; Introspection* introspection = nullptr) const = 0;
/// order solutions by their cost
bool operator<(const SolutionBase& other) const {
return this->cost_ < other.cost_;
}
protected: protected:
SolutionBase(StagePrivate* creator = nullptr, double cost = 0.0) SolutionBase(StagePrivate* creator = nullptr, double cost = 0.0)
: creator_(creator), cost_(cost) : creator_(creator), cost_(cost)

View File

@ -94,3 +94,12 @@ private:
}; };
#define DECLARE_FLAGS(Flags, Enum) typedef QFlags<Enum> Flags; #define DECLARE_FLAGS(Flags, Enum) typedef QFlags<Enum> Flags;
// compare two pointers by comparing their referenced values
template <typename T>
struct pointerLessThan : std::enable_if<std::is_pointer<T>::value> {
inline bool operator()(const T& x, const T& y) const {
return *x < *y;
}
};

View File

@ -268,8 +268,7 @@ void SerialContainerPrivate::storeNewSolution(SerialContainer::solution_containe
const InterfaceState *internal_to = s.back()->end(); const InterfaceState *internal_to = s.back()->end();
// create new solution directly in solutions_ and get a reference to it // create new solution directly in solutions_ and get a reference to it
solutions_.emplace_back(SerialSolution(this, std::move(s), cost)); SerialSolution& solution = *solutions_.insert(SerialSolution(this, std::move(s), cost));
SerialSolution& solution = solutions_.back();
// add solution to existing or new start state // add solution to existing or new start state
auto it = internal_to_my_starts_.find(internal_from); auto it = internal_to_my_starts_.find(internal_from);
@ -635,7 +634,7 @@ void Wrapper::spawn(InterfaceState &&state, std::unique_ptr<SolutionBase>&& s)
s->setEndState(impl->failure_states_.back()); s->setEndState(impl->failure_states_.back());
impl->failures_.emplace_back(std::move(s)); impl->failures_.emplace_back(std::move(s));
} else { } else {
impl->solutions_.emplace_back(std::move(s)); impl->solutions_.insert(std::move(s));
impl->prevEnds()->add(InterfaceState(state), NULL, solution); impl->prevEnds()->add(InterfaceState(state), NULL, solution);
impl->nextStarts()->add(std::move(state), solution, NULL); impl->nextStarts()->add(std::move(state), solution, NULL);
} }

View File

@ -207,8 +207,7 @@ std::ostream& operator<<(std::ostream &os, const Stage& stage) {
SubTrajectory& ComputeBasePrivate::addTrajectory(SubTrajectory&& trajectory) { SubTrajectory& ComputeBasePrivate::addTrajectory(SubTrajectory&& trajectory) {
trajectory.setCreator(this); trajectory.setCreator(this);
if (!trajectory.isFailure()) { if (!trajectory.isFailure()) {
solutions_.emplace_back(std::move(trajectory)); return *solutions_.insert(std::move(trajectory));
return solutions_.back();
} else if (me()->storeFailures()) { } else if (me()->storeFailures()) {
// only store failures when introspection is enabled // only store failures when introspection is enabled
failures_.emplace_back(std::move(trajectory)); failures_.emplace_back(std::move(trajectory));