merge WrapperBase + Wrapper into WrapperBase

Wrapper is not restricted to generator-type stage anymore.
This commit is contained in:
Robert Haschke 2018-02-01 01:16:07 +01:00
parent 032a4e23c1
commit 449ccf55c6
7 changed files with 155 additions and 163 deletions

View File

@ -137,31 +137,30 @@ public:
size_t numSolutions() const override;
void processSolutions(const SolutionProcessor &processor) const override;
size_t numFailures() const override;
void processFailures(const SolutionProcessor &processor) const override;
protected:
ParallelContainerBase(ParallelContainerBasePrivate* impl);
/// method called if any child found a new (internal) solution
virtual void onNewSolution(const SolutionBase& s) override;
};
/** Wrap an existing solution - for use in containers.
*
* This essentially wraps a solution of a child and thus allows
* for new clones of start / end states, which in turn will
* have separate incoming/outgoing trajectories */
class WrappedSolution : public SolutionBase {
public:
explicit WrappedSolution(StagePrivate* creator, const SolutionBase* wrapped)
: SolutionBase(creator, wrapped->cost()), wrapped_(wrapped)
{}
void fillMessage(moveit_task_constructor_msgs::Solution &solution,
Introspection* introspection = nullptr) const override {
wrapped_->fillMessage(solution, introspection);
/// lift unmodified child solution (useful for simple filtering)
void liftSolution(const SolutionBase* solution) {
liftSolution(solution, solution->cost());
}
/// lift child solution, but allow for modifying costs
void liftSolution(const SolutionBase* solution, double cost);
private:
const SolutionBase* wrapped_;
/// spawn a new solution with given state as start and end
void spawn(InterfaceState &&state, SubTrajectory&& trajectory);
/// convience method spawning an empty SubTrajectory with given cost
void spawn(InterfaceState &&state, double cost) {
SubTrajectory trajectory;
trajectory.setCost(cost);
spawn(std::move(state), std::move(trajectory));
}
};
@ -200,10 +199,16 @@ public:
class WrapperBasePrivate;
/** Base class for Wrapper and Task
/** A wrapper wraps a single child stage, which can be accessed via wrapped().
*
* WrapperBase ensures that only a single child is wrapped in a container.
* This child can be accessed via wrapped().
* Implementations of this interface need to implement onNewSolution(), which is
* called when the child has generated a new solution.
* The wrapper may reject the solution or create one or multiple derived solutions,
* potentially adapting the cost, as well as its start and end states.
*
* Care needs to be taken to not modify pulled states, but only pushed ones!
* liftFor each new solution, liftSolution() should be called, which comes in various
* flavours to allow for handing in new states (or not).
*/
class WrapperBase : public ParallelContainerBase
{
@ -214,48 +219,17 @@ public:
/// insertion is only allowed if children() is empty
bool insert(Stage::pointer&& stage, int before = -1) override;
/// access the single wrapped child
/// access the single wrapped child, NULL if still empty
Stage* wrapped();
inline const Stage* wrapped() const {
return const_cast<WrapperBase*>(this)->wrapped();
}
protected:
WrapperBase(WrapperBasePrivate *impl, pointer &&child = Stage::pointer());
};
class WrapperPrivate;
/** A wrapper wraps a single generator-style stage (and acts itself as a generator).
*
* The wrapped stage must act as a generator, i.e. only spawn new states
* in its external interfaces. It's intended, e.g. to filter or clone
* a generated solution of its wrapped generator.
*/
class Wrapper : public WrapperBase
{
public:
PRIVATE_CLASS(Wrapper)
Wrapper(const std::string &name, pointer &&child = Stage::pointer());
void reset() override;
bool canCompute() const override;
bool compute() override;
size_t numSolutions() const override;
size_t numFailures() const override;
void processSolutions(const SolutionProcessor &processor) const override;
void processFailures(const SolutionProcessor &processor) const override;
void spawn(InterfaceState &&state, std::unique_ptr<SolutionBase>&& s);
void spawn(InterfaceState &&state, double cost) {
std::unique_ptr<SolutionBase> s(new SubTrajectory());
s->setCost(cost);
spawn(std::move(state), std::move(s));
}
protected:
Wrapper(WrapperPrivate* impl, pointer &&child = Stage::pointer());
WrapperBase(WrapperBasePrivate* impl, pointer &&child = Stage::pointer());
/// called by a (direct) child when a new solution becomes available
void onNewSolution(const SolutionBase &s) override = 0;

View File

@ -39,7 +39,6 @@
#pragma once
#include <moveit/task_constructor/container.h>
#include "utils.h"
#include "stage_p.h"
#include <map>
@ -174,6 +173,27 @@ private:
PIMPL_FUNCTIONS(SerialContainer)
/** Wrap an existing solution - for use in parallel containers and wrappers.
*
* This essentially wraps a solution of a child and thus allows
* for new clones of start / end states, which in turn will
* have separate incoming/outgoing trajectories */
class WrappedSolution : public SolutionBase {
public:
explicit WrappedSolution(StagePrivate* creator, const SolutionBase* wrapped, double cost)
: SolutionBase(creator, cost), wrapped_(wrapped)
{}
explicit WrappedSolution(StagePrivate* creator, const SolutionBase* wrapped)
: WrappedSolution(creator, wrapped, wrapped->cost())
{}
void fillMessage(moveit_task_constructor_msgs::Solution &solution,
Introspection* introspection = nullptr) const override;
private:
const SolutionBase* wrapped_;
};
class ParallelContainerBasePrivate : public ContainerBasePrivate {
friend class ParallelContainerBase;
@ -184,8 +204,17 @@ private:
/// callback for new externally received states
void onNewExternalState(Interface::Direction dir, Interface::iterator external, bool updated);
// set of all solutions
ordered<WrappedSolution> solutions_;
// buffer for wrapped solutions
std::list<WrappedSolution> wrapped_solutions_;
// buffer for newly created (not wrapped) solutions
std::list<SubTrajectory> created_solutions_;
// buffer of created states (for use in created solutions)
std::list<InterfaceState> states_;
// cost-ordered set of solutions (pointers into wrapped or created)
ordered<SolutionBase*> solutions_;
// buffer for failures (pointers into wrapped or created)
std::list<SolutionBase*> failures_;
};
PIMPL_FUNCTIONS(ParallelContainerBase)
@ -194,25 +223,8 @@ class WrapperBasePrivate : public ParallelContainerBasePrivate {
friend class WrapperBase;
public:
WrapperBasePrivate(WrapperBase* me, const std::string& name)
: ParallelContainerBasePrivate(me, name)
{}
WrapperBasePrivate(WrapperBase* me, const std::string& name);
};
PIMPL_FUNCTIONS(WrapperBase)
class WrapperPrivate : public WrapperBasePrivate {
friend class Wrapper;
public:
WrapperPrivate(Wrapper* me, const std::string& name);
private:
ordered<std::unique_ptr<SolutionBase>, pointerLessThan<std::unique_ptr<SolutionBase>>> solutions_;
std::list<std::unique_ptr<SolutionBase>> failures_;
std::list<InterfaceState> failure_states_;
};
PIMPL_FUNCTIONS(Wrapper)
} }

View File

@ -15,7 +15,7 @@ namespace core { MOVEIT_CLASS_FORWARD(RobotState) }
namespace moveit { namespace task_constructor { namespace stages {
class ComputeIK : public Wrapper {
class ComputeIK : public WrapperBase {
public:
ComputeIK(const std::string &name, pointer &&child = Stage::pointer());

View File

@ -258,3 +258,13 @@ const InterfaceState::Solutions& SolutionBase::trajectories<Interface::BACKWARD>
}
} }
namespace std {
// comparison for pointers to SolutionBase: compare based on value
template<> struct less<moveit::task_constructor::SolutionBase*> {
bool operator()(const moveit::task_constructor::SolutionBase* x,
const moveit::task_constructor::SolutionBase* y) {
return *x < *y;
}
};
}

View File

@ -94,12 +94,3 @@ private:
};
#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

@ -130,9 +130,6 @@ void ContainerBasePrivate::liftSolution(SolutionBase& solution,
Interface::iterator external = nextStarts()->add(InterfaceState(*internal_to), &solution, NULL);
internal_to_external_.insert(std::make_pair(internal_to, external));
}
// perform default stage action on new solution
StagePrivate::newSolution(solution);
}
@ -301,6 +298,7 @@ void SerialContainer::onNewSolution(const SolutionBase &current)
for (auto it = sorted.begin(), end = sorted.end(); it != end; ++it) {
auto inserted = impl->solutions_.insert(std::move(*it));
impl->liftSolution(*inserted, inserted->internalStart(), inserted->internalEnd());
impl->newSolution(*inserted);
}
}
@ -436,7 +434,7 @@ void SerialContainer::traverse(const SolutionBase &start, const SolutionProcesso
}
void SerialSolution::fillMessage(moveit_task_constructor_msgs::Solution &msg,
Introspection* introspection = nullptr) const
Introspection* introspection) const
{
moveit_task_constructor_msgs::SubSolution sub_msg;
sub_msg.id = introspection ? introspection->solutionId(*this) : 0;
@ -458,6 +456,12 @@ void SerialSolution::fillMessage(moveit_task_constructor_msgs::Solution &msg,
}
void WrappedSolution::fillMessage(moveit_task_constructor_msgs::Solution &solution,
Introspection *introspection) const
{
wrapped_->fillMessage(solution, introspection);
}
ParallelContainerBasePrivate::ParallelContainerBasePrivate(ParallelContainerBase *me, const std::string &name)
: ContainerBasePrivate(me, name)
{
@ -480,6 +484,13 @@ void ParallelContainerBase::reset()
{
// recursively reset children
ContainerBase::reset();
// clear buffers
auto impl = pimpl();
impl->solutions_.clear();
impl->failures_.clear();
impl->wrapped_solutions_.clear();
impl->created_solutions_.clear();
impl->states_.clear();
}
/* States received by the container need to be copied to all children's pull interfaces.
@ -532,18 +543,73 @@ size_t ParallelContainerBase::numSolutions() const
void ParallelContainerBase::processSolutions(const Stage::SolutionProcessor &processor) const
{
for(const SolutionBase& s : pimpl()->solutions_)
if (!processor(s))
for(const SolutionBase* s : pimpl()->solutions_)
if (!processor(*s))
break;
}
void ParallelContainerBase::onNewSolution(const SolutionBase &s)
size_t ParallelContainerBase::numFailures() const
{
return pimpl()->failures_.size();
}
void ParallelContainerBase::processFailures(const Stage::SolutionProcessor &processor) const
{
for(const SolutionBase* f : pimpl()->failures_)
if (!processor(*f))
break;
}
void ParallelContainerBase::onNewSolution(const SolutionBase& s)
{
liftSolution(&s);
}
void ParallelContainerBase::liftSolution(const SolutionBase* solution, double cost)
{
auto impl = pimpl();
auto it = impl->solutions_.insert(WrappedSolution(impl, &s));
impl->liftSolution(*it, s.start(), s.end());
// create new WrappedSolution instance
auto wit = impl->wrapped_solutions_.insert(impl->wrapped_solutions_.end(), WrappedSolution(impl, solution, cost));
if (wit->isFailure()) {
wit->setStartState(*solution->start());
wit->setEndState(*solution->end());
impl->failures_.push_back(&*wit);
} else {
impl->solutions_.insert(&*wit);
impl->liftSolution(*wit, solution->start(), solution->end());
}
impl->newSolution(*wit);
}
void ParallelContainerBase::spawn(InterfaceState &&state, SubTrajectory&& t)
{
auto impl = pimpl();
assert(impl->prevEnds() && impl->nextStarts());
t.setCreator(impl);
// store newly created solution (otherwise it's lost)
auto it = impl->created_solutions_.insert(impl->created_solutions_.end(), std::move(t));
if (it->isFailure()) {
auto state_it = impl->states_.insert(impl->states_.end(), std::move(state));
it->setStartState(*state_it);
it->setEndState(*state_it);
impl->failures_.push_back(&*it);
} else {
// directly spawn states in push interfaces
impl->prevEnds()->add(InterfaceState(state), NULL, &*it);
impl->nextStarts()->add(std::move(state), &*it, NULL);
impl->solutions_.insert(&*it);
}
impl->newSolution(*it);
}
WrapperBasePrivate::WrapperBasePrivate(WrapperBase *me, const std::string &name)
: ParallelContainerBasePrivate(me, name)
{}
WrapperBase::WrapperBase(const std::string &name, Stage::pointer &&child)
: WrapperBase(new WrapperBasePrivate(this, name), std::move(child))
@ -568,79 +634,18 @@ Stage* WrapperBase::wrapped()
return pimpl()->children().empty() ? nullptr : pimpl()->children().front().get();
}
WrapperPrivate::WrapperPrivate(Wrapper *me, const std::string &name)
: WrapperBasePrivate(me, name)
{}
Wrapper::Wrapper(WrapperPrivate *impl, Stage::pointer &&child)
: WrapperBase(impl, std::move(child))
{}
Wrapper::Wrapper(const std::string &name, Stage::pointer &&child)
: Wrapper(new WrapperPrivate(this, name), std::move(child))
{}
void Wrapper::reset()
{
WrapperBase::reset();
pimpl()->solutions_.clear();
}
bool Wrapper::canCompute() const
bool WrapperBase::canCompute() const
{
return wrapped()->pimpl()->canCompute();
}
bool Wrapper::compute()
bool WrapperBase::compute()
{
size_t num_before = numSolutions();
wrapped()->pimpl()->compute();
return numSolutions() > num_before;
}
size_t Wrapper::numSolutions() const
{
return pimpl()->solutions_.size();
}
size_t Wrapper::numFailures() const
{
return pimpl()->failures_.size();
}
void Wrapper::processSolutions(const Stage::SolutionProcessor &processor) const
{
for(const auto& s : pimpl()->solutions_)
if (!processor(*s))
break;
}
void Wrapper::processFailures(const Stage::SolutionProcessor &processor) const
{
for(const auto& s : pimpl()->failures_)
if (!processor(*s))
break;
}
void Wrapper::spawn(InterfaceState &&state, std::unique_ptr<SolutionBase>&& s)
{
auto impl = pimpl();
s->setCreator(impl);
SolutionBase* solution = s.get();
if (s->isFailure()) {
impl->failure_states_.emplace_back(std::move(state));
s->setStartState(impl->failure_states_.back());
s->setEndState(impl->failure_states_.back());
impl->failures_.emplace_back(std::move(s));
} else {
impl->solutions_.insert(std::move(s));
impl->prevEnds()->add(InterfaceState(state), NULL, solution);
impl->nextStarts()->add(std::move(state), solution, NULL);
}
impl->newSolution(*solution);
}
bool Alternatives::canCompute() const
{

View File

@ -20,7 +20,7 @@
namespace moveit { namespace task_constructor { namespace stages {
ComputeIK::ComputeIK(const std::string &name, Stage::pointer &&child)
: Wrapper(name, std::move(child))
: WrapperBase(name, std::move(child))
{
auto& p = properties();
p.declare<double>("timeout", 1.0);
@ -218,17 +218,17 @@ void ComputeIK::onNewSolution(const SolutionBase &s)
start_time = now;
planning_scene::PlanningSceneConstPtr scene = s.start()->scene();
std::unique_ptr<SolutionBase> solution(new SubTrajectory());
SubTrajectory solution;
// include markers from original solution
std::copy(s.markers().begin(), s.markers().end(), std::back_inserter(solution->markers()));
std::copy(s.markers().begin(), s.markers().end(), std::back_inserter(solution.markers()));
// frame at target pose
target_pose_msg.header.frame_id = scene->getPlanningFrame();
rviz_marker_tools::appendFrame(solution->markers(), target_pose_msg, 0.1, "ik frame");
rviz_marker_tools::appendFrame(solution.markers(), target_pose_msg, 0.1, "ik frame");
if (succeeded) {
solution->setCost(s.cost() + jmg->distance(ik_solutions.back().data(), compare_pose.data()));
solution.setCost(s.cost() + jmg->distance(ik_solutions.back().data(), compare_pose.data()));
// create a new scene for each solution as they will have different robot states
planning_scene::PlanningScenePtr new_scene = s.start()->scene()->diff();
robot_state::RobotState& robot_state = new_scene->getCurrentStateNonConst();
@ -240,11 +240,11 @@ void ComputeIK::onNewSolution(const SolutionBase &s)
auto appender = [&solution](visualization_msgs::Marker& marker, const std::string& name) {
marker.ns = "ik solution";
marker.color.a *= 0.5;
solution->markers().push_back(marker);
solution.markers().push_back(marker);
};
generateVisualMarkers(robot_state, appender, jmg->getLinkModelNames());
} else {
solution->setCost(std::numeric_limits<double>::infinity());
solution.setCost(std::numeric_limits<double>::infinity());
}
spawn(InterfaceState(scene), std::move(solution));