mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
cost ordering for solutions
This commit is contained in:
parent
c84fa4cdd6
commit
29577515a0
@ -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_;
|
||||||
};
|
};
|
||||||
|
|||||||
@ -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
|
||||||
};
|
};
|
||||||
|
|||||||
@ -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)
|
||||||
|
|||||||
@ -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;
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|||||||
@ -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);
|
||||||
}
|
}
|
||||||
|
|||||||
@ -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));
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user