mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
merge WrapperBase + Wrapper into WrapperBase
Wrapper is not restricted to generator-type stage anymore.
This commit is contained in:
parent
032a4e23c1
commit
449ccf55c6
@ -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;
|
||||
|
||||
@ -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)
|
||||
|
||||
} }
|
||||
|
||||
@ -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());
|
||||
|
||||
|
||||
@ -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;
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
@ -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;
|
||||
}
|
||||
};
|
||||
|
||||
@ -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 ¤t)
|
||||
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
|
||||
{
|
||||
|
||||
@ -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));
|
||||
|
||||
Loading…
Reference in New Issue
Block a user