mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
replaced IMPL macro
This commit is contained in:
parent
1b1a82f7c8
commit
dc9f553ab0
@ -9,7 +9,9 @@
|
|||||||
#include <list>
|
#include <list>
|
||||||
|
|
||||||
#define PRIVATE_CLASS(Class) \
|
#define PRIVATE_CLASS(Class) \
|
||||||
friend class Class##Private;
|
friend class Class##Private; \
|
||||||
|
inline const Class##Private* pimpl() const; \
|
||||||
|
inline Class##Private* pimpl();
|
||||||
|
|
||||||
namespace planning_scene {
|
namespace planning_scene {
|
||||||
MOVEIT_CLASS_FORWARD(PlanningScene)
|
MOVEIT_CLASS_FORWARD(PlanningScene)
|
||||||
@ -35,12 +37,9 @@ typedef std::pair<const InterfaceState&, const InterfaceState&> InterfaceStatePa
|
|||||||
class SubTaskPrivate;
|
class SubTaskPrivate;
|
||||||
class SubTask {
|
class SubTask {
|
||||||
friend std::ostream& operator<<(std::ostream &os, const SubTask& stage);
|
friend std::ostream& operator<<(std::ostream &os, const SubTask& stage);
|
||||||
friend class SubTaskPrivate;
|
|
||||||
|
|
||||||
public:
|
public:
|
||||||
inline const SubTaskPrivate* pimpl_func() const { return pimpl_; }
|
PRIVATE_CLASS(SubTask)
|
||||||
inline SubTaskPrivate* pimpl_func() { return pimpl_; }
|
|
||||||
|
|
||||||
typedef std::unique_ptr<SubTask> pointer;
|
typedef std::unique_ptr<SubTask> pointer;
|
||||||
|
|
||||||
~SubTask();
|
~SubTask();
|
||||||
|
|||||||
@ -20,7 +20,7 @@ ContainerBasePrivate::const_iterator ContainerBasePrivate::position(int before)
|
|||||||
}
|
}
|
||||||
|
|
||||||
inline bool ContainerBasePrivate::canInsert(const SubTask &stage) const {
|
inline bool ContainerBasePrivate::canInsert(const SubTask &stage) const {
|
||||||
const SubTaskPrivate* impl = stage.pimpl_func();
|
const SubTaskPrivate* impl = stage.pimpl();
|
||||||
return impl->parent_ == nullptr // re-parenting is not supported
|
return impl->parent_ == nullptr // re-parenting is not supported
|
||||||
&& impl->trajectories_.empty(); // existing trajectories would become invalid
|
&& impl->trajectories_.empty(); // existing trajectories would become invalid
|
||||||
}
|
}
|
||||||
@ -29,7 +29,7 @@ bool ContainerBasePrivate::traverseStages(const ContainerBase::StageCallback &pr
|
|||||||
for (auto &stage : children_) {
|
for (auto &stage : children_) {
|
||||||
if (!processor(*stage, depth))
|
if (!processor(*stage, depth))
|
||||||
continue;
|
continue;
|
||||||
ContainerBasePrivate *container = dynamic_cast<ContainerBasePrivate*>(stage->pimpl_func());
|
ContainerBasePrivate *container = dynamic_cast<ContainerBasePrivate*>(stage->pimpl());
|
||||||
if (container)
|
if (container)
|
||||||
container->traverseStages(processor, depth+1);
|
container->traverseStages(processor, depth+1);
|
||||||
}
|
}
|
||||||
@ -38,7 +38,7 @@ bool ContainerBasePrivate::traverseStages(const ContainerBase::StageCallback &pr
|
|||||||
|
|
||||||
ContainerBasePrivate::iterator ContainerBasePrivate::insert(ContainerBasePrivate::value_type &&subtask,
|
ContainerBasePrivate::iterator ContainerBasePrivate::insert(ContainerBasePrivate::value_type &&subtask,
|
||||||
ContainerBasePrivate::const_iterator pos) {
|
ContainerBasePrivate::const_iterator pos) {
|
||||||
SubTaskPrivate *impl = subtask->pimpl_func();
|
SubTaskPrivate *impl = subtask->pimpl();
|
||||||
impl->parent_ = this;
|
impl->parent_ = this;
|
||||||
impl->it_ = children_.insert(pos, std::move(subtask));
|
impl->it_ = children_.insert(pos, std::move(subtask));
|
||||||
return impl->it_;
|
return impl->it_;
|
||||||
@ -49,34 +49,35 @@ ContainerBase::ContainerBase(ContainerBasePrivate *impl)
|
|||||||
: SubTask(impl)
|
: SubTask(impl)
|
||||||
{
|
{
|
||||||
}
|
}
|
||||||
|
PIMPL_FUNCTIONS(ContainerBase)
|
||||||
|
|
||||||
void ContainerBase::clear()
|
void ContainerBase::clear()
|
||||||
{
|
{
|
||||||
IMPL(ContainerBase);
|
auto impl = pimpl();
|
||||||
impl->clear();
|
impl->clear();
|
||||||
}
|
}
|
||||||
|
|
||||||
bool ContainerBase::init(const planning_scene::PlanningSceneConstPtr &scene)
|
bool ContainerBase::init(const planning_scene::PlanningSceneConstPtr &scene)
|
||||||
{
|
{
|
||||||
IMPL(ContainerBase);
|
auto impl = pimpl();
|
||||||
for (auto& stage : impl->children_)
|
for (auto& stage : impl->children_)
|
||||||
stage->init(scene);
|
stage->init(scene);
|
||||||
}
|
}
|
||||||
|
|
||||||
bool ContainerBase::traverseStages(const ContainerBase::StageCallback &processor) const
|
bool ContainerBase::traverseStages(const ContainerBase::StageCallback &processor) const
|
||||||
{
|
{
|
||||||
IMPL(const ContainerBase);
|
auto impl = pimpl();
|
||||||
return impl->traverseStages(processor, 0);
|
return impl->traverseStages(processor, 0);
|
||||||
}
|
}
|
||||||
|
|
||||||
bool ContainerBase::canCompute() const
|
bool ContainerBase::canCompute() const
|
||||||
{
|
{
|
||||||
IMPL(const ContainerBase);
|
auto impl = pimpl();
|
||||||
return impl->canCompute();
|
return impl->canCompute();
|
||||||
}
|
}
|
||||||
|
|
||||||
bool ContainerBase::compute() {
|
bool ContainerBase::compute() {
|
||||||
IMPL(ContainerBase);
|
auto impl = pimpl();
|
||||||
return impl->compute();
|
return impl->compute();
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -84,8 +85,8 @@ bool ContainerBase::compute() {
|
|||||||
SubTaskPrivate::InterfaceFlags SerialContainerPrivate::announcedFlags() const {
|
SubTaskPrivate::InterfaceFlags SerialContainerPrivate::announcedFlags() const {
|
||||||
InterfaceFlags f;
|
InterfaceFlags f;
|
||||||
if (children().empty()) return f;
|
if (children().empty()) return f;
|
||||||
f |= children().front()->pimpl_func()->announcedFlags() & INPUT_IF_MASK;
|
f |= children().front()->pimpl()->announcedFlags() & INPUT_IF_MASK;
|
||||||
f |= children().back()->pimpl_func()->announcedFlags() & OUTPUT_IF_MASK;
|
f |= children().back()->pimpl()->announcedFlags() & OUTPUT_IF_MASK;
|
||||||
return f;
|
return f;
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -104,8 +105,8 @@ inline bool SerialContainerPrivate::canInsert(const SubTask &stage, ContainerBas
|
|||||||
|
|
||||||
// check connectedness
|
// check connectedness
|
||||||
bool at_end = (before == children().end());
|
bool at_end = (before == children().end());
|
||||||
const SubTaskPrivate* next = (at_end) ? this : (*before)->pimpl_func();
|
const SubTaskPrivate* next = (at_end) ? this : (*before)->pimpl();
|
||||||
InterfaceFlags cur_flags = stage.pimpl_func()->announcedFlags();
|
InterfaceFlags cur_flags = stage.pimpl()->announcedFlags();
|
||||||
InterfaceFlags next_flags = next->deducedFlags();
|
InterfaceFlags next_flags = next->deducedFlags();
|
||||||
InterfaceFlags prev_flags = prev(before)->deducedFlags();
|
InterfaceFlags prev_flags = prev(before)->deducedFlags();
|
||||||
|
|
||||||
@ -123,13 +124,13 @@ ContainerBasePrivate::iterator SerialContainerPrivate::insert(value_type &&stage
|
|||||||
bool at_begin = (before == children().begin());
|
bool at_begin = (before == children().begin());
|
||||||
bool at_end = (before == children().end());
|
bool at_end = (before == children().end());
|
||||||
|
|
||||||
SubTaskPrivate *cur = stage->pimpl_func();
|
SubTaskPrivate *cur = stage->pimpl();
|
||||||
/* set pointer cache (prev_ends_ and next_starts_) of prev, current, and next stage */
|
/* set pointer cache (prev_ends_ and next_starts_) of prev, current, and next stage */
|
||||||
if (children().empty()) { // first child inserted
|
if (children().empty()) { // first child inserted
|
||||||
setPrevEnds(cur, this->starts_);
|
setPrevEnds(cur, this->starts_);
|
||||||
setNextStarts(cur, this->ends_);
|
setNextStarts(cur, this->ends_);
|
||||||
} else if (at_begin) {
|
} else if (at_begin) {
|
||||||
SubTaskPrivate *next = (*before)->pimpl_func();
|
SubTaskPrivate *next = (*before)->pimpl();
|
||||||
setPrevEnds(cur, this->starts_);
|
setPrevEnds(cur, this->starts_);
|
||||||
setNextStarts(cur, next->starts_);
|
setNextStarts(cur, next->starts_);
|
||||||
setPrevEnds(next, cur->ends_);
|
setPrevEnds(next, cur->ends_);
|
||||||
@ -140,7 +141,7 @@ ContainerBasePrivate::iterator SerialContainerPrivate::insert(value_type &&stage
|
|||||||
setNextStarts(cur, this->ends_);
|
setNextStarts(cur, this->ends_);
|
||||||
} else {
|
} else {
|
||||||
const SubTaskPrivate *prev = this->prev(before);
|
const SubTaskPrivate *prev = this->prev(before);
|
||||||
SubTaskPrivate *next = (*before)->pimpl_func();
|
SubTaskPrivate *next = (*before)->pimpl();
|
||||||
setNextStarts(prev, cur->starts_);
|
setNextStarts(prev, cur->starts_);
|
||||||
setPrevEnds(cur, prev->ends_);
|
setPrevEnds(cur, prev->ends_);
|
||||||
setNextStarts(cur, next->starts_);
|
setNextStarts(cur, next->starts_);
|
||||||
@ -154,25 +155,25 @@ inline const SubTaskPrivate* SerialContainerPrivate::prev(const_iterator it) con
|
|||||||
{
|
{
|
||||||
#ifndef NDEBUG
|
#ifndef NDEBUG
|
||||||
if (it != children().end()) {
|
if (it != children().end()) {
|
||||||
SubTaskPrivate* child = (*it)->pimpl_func();
|
SubTaskPrivate* child = (*it)->pimpl();
|
||||||
assert(parent(child) == this);
|
assert(parent(child) == this);
|
||||||
assert(this->it(child) == it);
|
assert(this->it(child) == it);
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
if (it == children().begin()) return this;
|
if (it == children().begin()) return this;
|
||||||
return (*--it)->pimpl_func();
|
return (*--it)->pimpl();
|
||||||
}
|
}
|
||||||
|
|
||||||
inline const SubTaskPrivate* SerialContainerPrivate::next(const_iterator it) const
|
inline const SubTaskPrivate* SerialContainerPrivate::next(const_iterator it) const
|
||||||
{
|
{
|
||||||
#ifndef NDEBUG
|
#ifndef NDEBUG
|
||||||
assert(it != children().end());
|
assert(it != children().end());
|
||||||
SubTaskPrivate* child = (*it)->pimpl_func();
|
SubTaskPrivate* child = (*it)->pimpl();
|
||||||
assert(parent(child) == this);
|
assert(parent(child) == this);
|
||||||
assert(this->it(child) == it);
|
assert(this->it(child) == it);
|
||||||
#endif
|
#endif
|
||||||
if (it == --children().end()) return this;
|
if (it == --children().end()) return this;
|
||||||
return (*++it)->pimpl_func();
|
return (*++it)->pimpl();
|
||||||
}
|
}
|
||||||
|
|
||||||
const SubTaskPrivate *SerialContainerPrivate::prev(const SubTaskPrivate *child) const
|
const SubTaskPrivate *SerialContainerPrivate::prev(const SubTaskPrivate *child) const
|
||||||
@ -192,16 +193,17 @@ SerialContainer::SerialContainer(SerialContainerPrivate *impl)
|
|||||||
SerialContainer::SerialContainer(const std::string &name)
|
SerialContainer::SerialContainer(const std::string &name)
|
||||||
: SerialContainer(new SerialContainerPrivate(this, name))
|
: SerialContainer(new SerialContainerPrivate(this, name))
|
||||||
{}
|
{}
|
||||||
|
PIMPL_FUNCTIONS(SerialContainer)
|
||||||
|
|
||||||
bool SerialContainer::canInsert(const value_type& subtask, int before) const
|
bool SerialContainer::canInsert(const value_type& subtask, int before) const
|
||||||
{
|
{
|
||||||
IMPL(const SerialContainer);
|
auto impl = pimpl();
|
||||||
return impl->canInsert(*subtask, impl->position(before));
|
return impl->canInsert(*subtask, impl->position(before));
|
||||||
}
|
}
|
||||||
|
|
||||||
bool SerialContainer::insert(value_type&& subtask, int before)
|
bool SerialContainer::insert(value_type&& subtask, int before)
|
||||||
{
|
{
|
||||||
IMPL(SerialContainer);
|
auto impl = pimpl();
|
||||||
|
|
||||||
ContainerBasePrivate::const_iterator where = impl->position(before);
|
ContainerBasePrivate::const_iterator where = impl->position(before);
|
||||||
if (!impl->canInsert(*subtask, where))
|
if (!impl->canInsert(*subtask, where))
|
||||||
@ -220,10 +222,10 @@ bool SerialContainerPrivate::compute()
|
|||||||
{
|
{
|
||||||
bool computed = false;
|
bool computed = false;
|
||||||
for(const auto& stage : children()) {
|
for(const auto& stage : children()) {
|
||||||
if(!stage->pimpl_func()->canCompute())
|
if(!stage->pimpl()->canCompute())
|
||||||
continue;
|
continue;
|
||||||
std::cout << "Computing subtask '" << stage->getName() << "':" << std::endl;
|
std::cout << "Computing subtask '" << stage->getName() << "':" << std::endl;
|
||||||
bool success = stage->pimpl_func()->compute();
|
bool success = stage->pimpl()->compute();
|
||||||
computed = true;
|
computed = true;
|
||||||
std::cout << (success ? "succeeded" : "failed") << std::endl;
|
std::cout << (success ? "succeeded" : "failed") << std::endl;
|
||||||
}
|
}
|
||||||
|
|||||||
@ -25,7 +25,7 @@ const std::string& SubTask::getName() const {
|
|||||||
}
|
}
|
||||||
|
|
||||||
std::ostream& operator<<(std::ostream &os, const SubTask& stage) {
|
std::ostream& operator<<(std::ostream &os, const SubTask& stage) {
|
||||||
os << *stage.pimpl_func();
|
os << *stage.pimpl();
|
||||||
return os;
|
return os;
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -192,9 +192,11 @@ PropagatingEitherWay::PropagatingEitherWay(PropagatingEitherWayPrivate *impl)
|
|||||||
initInterface();
|
initInterface();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
PIMPL_FUNCTIONS(PropagatingEitherWay)
|
||||||
|
|
||||||
void PropagatingEitherWay::initInterface()
|
void PropagatingEitherWay::initInterface()
|
||||||
{
|
{
|
||||||
IMPL(PropagatingEitherWay)
|
auto impl = pimpl();
|
||||||
if (impl->dir & PropagatingEitherWay::FORWARD) {
|
if (impl->dir & PropagatingEitherWay::FORWARD) {
|
||||||
if (!impl->starts_) { // keep existing interface if possible
|
if (!impl->starts_) { // keep existing interface if possible
|
||||||
impl->starts_.reset(new Interface([impl](const Interface::iterator& it) { impl->newStartState(it); }));
|
impl->starts_.reset(new Interface([impl](const Interface::iterator& it) { impl->newStartState(it); }));
|
||||||
@ -218,7 +220,7 @@ void PropagatingEitherWay::initInterface()
|
|||||||
|
|
||||||
void PropagatingEitherWay::restrictDirection(PropagatingEitherWay::Direction dir)
|
void PropagatingEitherWay::restrictDirection(PropagatingEitherWay::Direction dir)
|
||||||
{
|
{
|
||||||
IMPL(PropagatingEitherWay);
|
auto impl = pimpl();
|
||||||
if (impl->dir == dir) return;
|
if (impl->dir == dir) return;
|
||||||
if (impl->isConnected())
|
if (impl->isConnected())
|
||||||
throw std::runtime_error("Cannot change direction after being connected");
|
throw std::runtime_error("Cannot change direction after being connected");
|
||||||
@ -227,10 +229,10 @@ void PropagatingEitherWay::restrictDirection(PropagatingEitherWay::Direction dir
|
|||||||
}
|
}
|
||||||
|
|
||||||
void PropagatingEitherWay::sendForward(const InterfaceState& from,
|
void PropagatingEitherWay::sendForward(const InterfaceState& from,
|
||||||
const planning_scene::PlanningSceneConstPtr& to,
|
const planning_scene::PlanningSceneConstPtr& to,
|
||||||
const robot_trajectory::RobotTrajectoryPtr& t,
|
const robot_trajectory::RobotTrajectoryPtr& t,
|
||||||
double cost){
|
double cost){
|
||||||
IMPL(PropagatingEitherWay)
|
auto impl = pimpl();
|
||||||
std::cout << "sending state forward" << std::endl;
|
std::cout << "sending state forward" << std::endl;
|
||||||
SubTrajectory &trajectory = impl->addTrajectory(t, cost);
|
SubTrajectory &trajectory = impl->addTrajectory(t, cost);
|
||||||
trajectory.setStartState(from);
|
trajectory.setStartState(from);
|
||||||
@ -238,10 +240,10 @@ void PropagatingEitherWay::sendForward(const InterfaceState& from,
|
|||||||
}
|
}
|
||||||
|
|
||||||
void PropagatingEitherWay::sendBackward(const planning_scene::PlanningSceneConstPtr& from,
|
void PropagatingEitherWay::sendBackward(const planning_scene::PlanningSceneConstPtr& from,
|
||||||
const InterfaceState& to,
|
const InterfaceState& to,
|
||||||
const robot_trajectory::RobotTrajectoryPtr& t,
|
const robot_trajectory::RobotTrajectoryPtr& t,
|
||||||
double cost){
|
double cost){
|
||||||
IMPL(PropagatingEitherWay)
|
auto impl = pimpl();
|
||||||
std::cout << "sending state backward" << std::endl;
|
std::cout << "sending state backward" << std::endl;
|
||||||
SubTrajectory& trajectory = impl->addTrajectory(t, cost);
|
SubTrajectory& trajectory = impl->addTrajectory(t, cost);
|
||||||
trajectory.setEndState(to);
|
trajectory.setEndState(to);
|
||||||
@ -261,6 +263,7 @@ PropagatingForwardPrivate::PropagatingForwardPrivate(PropagatingForward *me, con
|
|||||||
PropagatingForward::PropagatingForward(const std::string& name)
|
PropagatingForward::PropagatingForward(const std::string& name)
|
||||||
: PropagatingEitherWay(new PropagatingForwardPrivate(this, name))
|
: PropagatingEitherWay(new PropagatingForwardPrivate(this, name))
|
||||||
{}
|
{}
|
||||||
|
PIMPL_FUNCTIONS(PropagatingForward)
|
||||||
|
|
||||||
bool PropagatingForward::computeBackward(const InterfaceState &to)
|
bool PropagatingForward::computeBackward(const InterfaceState &to)
|
||||||
{
|
{
|
||||||
@ -280,6 +283,7 @@ PropagatingBackwardPrivate::PropagatingBackwardPrivate(PropagatingBackward *me,
|
|||||||
PropagatingBackward::PropagatingBackward(const std::string &name)
|
PropagatingBackward::PropagatingBackward(const std::string &name)
|
||||||
: PropagatingEitherWay(new PropagatingBackwardPrivate(this, name))
|
: PropagatingEitherWay(new PropagatingBackwardPrivate(this, name))
|
||||||
{}
|
{}
|
||||||
|
PIMPL_FUNCTIONS(PropagatingBackward)
|
||||||
|
|
||||||
bool PropagatingBackward::computeForward(const InterfaceState &from)
|
bool PropagatingBackward::computeForward(const InterfaceState &from)
|
||||||
{
|
{
|
||||||
@ -307,11 +311,12 @@ bool GeneratorPrivate::compute() {
|
|||||||
Generator::Generator(const std::string &name)
|
Generator::Generator(const std::string &name)
|
||||||
: SubTask(new GeneratorPrivate(this, name))
|
: SubTask(new GeneratorPrivate(this, name))
|
||||||
{}
|
{}
|
||||||
|
PIMPL_FUNCTIONS(Generator)
|
||||||
|
|
||||||
void Generator::spawn(const planning_scene::PlanningSceneConstPtr& ps, double cost)
|
void Generator::spawn(const planning_scene::PlanningSceneConstPtr& ps, double cost)
|
||||||
{
|
{
|
||||||
std::cout << "spawning state forwards and backwards" << std::endl;
|
std::cout << "spawning state forwards and backwards" << std::endl;
|
||||||
IMPL(Generator)
|
auto impl = pimpl();
|
||||||
// empty trajectory ref -> this node only produces states
|
// empty trajectory ref -> this node only produces states
|
||||||
robot_trajectory::RobotTrajectoryPtr dummy;
|
robot_trajectory::RobotTrajectoryPtr dummy;
|
||||||
SubTrajectory& trajectory = impl->addTrajectory(dummy, cost);
|
SubTrajectory& trajectory = impl->addTrajectory(dummy, cost);
|
||||||
@ -364,10 +369,11 @@ Connecting::Connecting(const std::string &name)
|
|||||||
: SubTask(new ConnectingPrivate(this, name))
|
: SubTask(new ConnectingPrivate(this, name))
|
||||||
{
|
{
|
||||||
}
|
}
|
||||||
|
PIMPL_FUNCTIONS(Connecting)
|
||||||
|
|
||||||
void Connecting::connect(const InterfaceState& from, const InterfaceState& to,
|
void Connecting::connect(const InterfaceState& from, const InterfaceState& to,
|
||||||
const robot_trajectory::RobotTrajectoryPtr& t, double cost) {
|
const robot_trajectory::RobotTrajectoryPtr& t, double cost) {
|
||||||
IMPL(Connecting)
|
auto impl = pimpl();
|
||||||
SubTrajectory& trajectory = impl->addTrajectory(t, cost);
|
SubTrajectory& trajectory = impl->addTrajectory(t, cost);
|
||||||
trajectory.setStartState(from);
|
trajectory.setStartState(from);
|
||||||
trajectory.setEndState(to);
|
trajectory.setEndState(to);
|
||||||
|
|||||||
@ -5,6 +5,11 @@
|
|||||||
#include <moveit_task_constructor/subtask.h>
|
#include <moveit_task_constructor/subtask.h>
|
||||||
#include <moveit_task_constructor/storage.h>
|
#include <moveit_task_constructor/storage.h>
|
||||||
|
|
||||||
|
// define pimpl() functions accessing correctly casted pimpl_ pointer
|
||||||
|
#define PIMPL_FUNCTIONS(Class) \
|
||||||
|
const Class##Private* Class::pimpl() const { return static_cast<const Class##Private*>(pimpl_); } \
|
||||||
|
Class##Private* Class::pimpl() { return static_cast<Class##Private*>(pimpl_); } \
|
||||||
|
|
||||||
namespace moveit { namespace task_constructor {
|
namespace moveit { namespace task_constructor {
|
||||||
|
|
||||||
class ContainerBasePrivate;
|
class ContainerBasePrivate;
|
||||||
@ -139,8 +144,6 @@ private:
|
|||||||
std::pair<Interface::const_iterator, Interface::const_iterator> it_pairs_;
|
std::pair<Interface::const_iterator, Interface::const_iterator> it_pairs_;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
PIMPL_FUNCTIONS(SubTask)
|
||||||
|
|
||||||
} }
|
} }
|
||||||
|
|
||||||
|
|
||||||
// get correctly casted private impl pointer
|
|
||||||
#define IMPL(Class) Class##Private * const impl = static_cast<Class##Private*>(pimpl_func());
|
|
||||||
|
|||||||
@ -66,6 +66,7 @@ Task::Task(const std::string &name)
|
|||||||
: SerialContainer(new TaskPrivate(this, name))
|
: SerialContainer(new TaskPrivate(this, name))
|
||||||
{
|
{
|
||||||
}
|
}
|
||||||
|
PIMPL_FUNCTIONS(Task)
|
||||||
|
|
||||||
planning_pipeline::PlanningPipelinePtr
|
planning_pipeline::PlanningPipelinePtr
|
||||||
Task::createPlanner(const robot_model::RobotModelConstPtr& model, const std::string& ns,
|
Task::createPlanner(const robot_model::RobotModelConstPtr& model, const std::string& ns,
|
||||||
@ -96,7 +97,7 @@ void Task::add(pointer &&stage) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
bool Task::plan(){
|
bool Task::plan(){
|
||||||
IMPL(Task);
|
auto impl = pimpl();
|
||||||
NewSolutionPublisher debug(*this);
|
NewSolutionPublisher debug(*this);
|
||||||
|
|
||||||
impl->initScene();
|
impl->initScene();
|
||||||
@ -112,7 +113,7 @@ bool Task::plan(){
|
|||||||
}
|
}
|
||||||
|
|
||||||
const robot_state::RobotState& Task::getCurrentRobotState() const {
|
const robot_state::RobotState& Task::getCurrentRobotState() const {
|
||||||
IMPL(const Task)
|
auto impl = pimpl();
|
||||||
return impl->scene_->getCurrentState();
|
return impl->scene_->getCurrentState();
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -153,12 +154,12 @@ bool traverseFullTrajectories(
|
|||||||
}
|
}
|
||||||
|
|
||||||
bool Task::processSolutions(const Task::SolutionCallback& processor) {
|
bool Task::processSolutions(const Task::SolutionCallback& processor) {
|
||||||
IMPL(Task);
|
auto impl = pimpl();
|
||||||
const TaskPrivate::container_type& children = impl->children();
|
const TaskPrivate::container_type& children = impl->children();
|
||||||
const size_t nr_of_trajectories = children.size();
|
const size_t nr_of_trajectories = children.size();
|
||||||
std::vector<SubTrajectory*> trace;
|
std::vector<SubTrajectory*> trace;
|
||||||
trace.reserve(nr_of_trajectories);
|
trace.reserve(nr_of_trajectories);
|
||||||
for(SubTrajectory& st : children.front()->pimpl_func()->trajectories())
|
for(SubTrajectory& st : children.front()->pimpl()->trajectories())
|
||||||
if( !traverseFullTrajectories(st, nr_of_trajectories, processor, trace) )
|
if( !traverseFullTrajectories(st, nr_of_trajectories, processor, trace) )
|
||||||
return false;
|
return false;
|
||||||
return true;
|
return true;
|
||||||
|
|||||||
@ -4,6 +4,12 @@
|
|||||||
#include <gtest/gtest.h>
|
#include <gtest/gtest.h>
|
||||||
#include <initializer_list>
|
#include <initializer_list>
|
||||||
|
|
||||||
|
namespace moveit { namespace task_constructor {
|
||||||
|
PIMPL_FUNCTIONS(Generator)
|
||||||
|
PIMPL_FUNCTIONS(PropagatingForward)
|
||||||
|
PIMPL_FUNCTIONS(SerialContainer)
|
||||||
|
}}
|
||||||
|
|
||||||
namespace testing { namespace internal {
|
namespace testing { namespace internal {
|
||||||
enum GTestColor {
|
enum GTestColor {
|
||||||
COLOR_DEFAULT,
|
COLOR_DEFAULT,
|
||||||
@ -51,7 +57,7 @@ protected:
|
|||||||
|
|
||||||
// print order
|
// print order
|
||||||
for (auto it = container->children().begin(), end = container->children().end(); it != end; ++it)
|
for (auto it = container->children().begin(), end = container->children().end(); it != end; ++it)
|
||||||
PRINTF(" %p", (*it)->pimpl_func());
|
PRINTF(" %p", (*it)->pimpl());
|
||||||
PRINTF(" *** parent: %p ***\n", container);
|
PRINTF(" *** parent: %p ***\n", container);
|
||||||
|
|
||||||
// validate order
|
// validate order
|
||||||
@ -60,7 +66,7 @@ protected:
|
|||||||
size_t pos = 0;
|
size_t pos = 0;
|
||||||
auto exp_it = expected.begin();
|
auto exp_it = expected.begin();
|
||||||
for (auto it = container->children().begin(), end = container->children().end(); it != end; ++it, ++exp_it, ++pos) {
|
for (auto it = container->children().begin(), end = container->children().end(); it != end; ++it, ++exp_it, ++pos) {
|
||||||
SubTaskPrivate *child = (*it)->pimpl_func();
|
SubTaskPrivate *child = (*it)->pimpl();
|
||||||
EXPECT_EQ(child, *exp_it) << "wrong order";
|
EXPECT_EQ(child, *exp_it) << "wrong order";
|
||||||
EXPECT_EQ(child->parent_, container) << "wrong parent";
|
EXPECT_EQ(child->parent_, container) << "wrong parent";
|
||||||
EXPECT_EQ(it, container->position(pos)) << "bad forward position resolution";
|
EXPECT_EQ(it, container->position(pos)) << "bad forward position resolution";
|
||||||
@ -78,7 +84,7 @@ protected:
|
|||||||
|
|
||||||
TEST_F(BaseTest, interfaceFlags) {
|
TEST_F(BaseTest, interfaceFlags) {
|
||||||
std::unique_ptr<Generator> g = std::make_unique<TestGenerator>();
|
std::unique_ptr<Generator> g = std::make_unique<TestGenerator>();
|
||||||
EXPECT_EQ(g->pimpl_func()->interfaceFlags(),
|
EXPECT_EQ(g->pimpl()->interfaceFlags(),
|
||||||
SubTaskPrivate::InterfaceFlags({SubTaskPrivate::WRITES_NEXT_START,
|
SubTaskPrivate::InterfaceFlags({SubTaskPrivate::WRITES_NEXT_START,
|
||||||
SubTaskPrivate::WRITES_PREV_END}));
|
SubTaskPrivate::WRITES_PREV_END}));
|
||||||
}
|
}
|
||||||
@ -89,7 +95,7 @@ TEST_F(BaseTest, interfaceFlags) {
|
|||||||
|
|
||||||
TEST_F(BaseTest, serialContainer) {
|
TEST_F(BaseTest, serialContainer) {
|
||||||
SerialContainer c("serial");
|
SerialContainer c("serial");
|
||||||
SerialContainerPrivate *cp = static_cast<SerialContainerPrivate*>(c.pimpl_func());
|
SerialContainerPrivate *cp = static_cast<SerialContainerPrivate*>(c.pimpl());
|
||||||
|
|
||||||
EXPECT_TRUE(bool(cp->starts_));
|
EXPECT_TRUE(bool(cp->starts_));
|
||||||
EXPECT_TRUE(bool(cp->ends_));
|
EXPECT_TRUE(bool(cp->ends_));
|
||||||
@ -99,7 +105,7 @@ TEST_F(BaseTest, serialContainer) {
|
|||||||
|
|
||||||
/***** inserting first stage *****/
|
/***** inserting first stage *****/
|
||||||
auto g = std::make_unique<TestGenerator>();
|
auto g = std::make_unique<TestGenerator>();
|
||||||
GeneratorPrivate *gp = static_cast<GeneratorPrivate*>(g->pimpl_func());
|
GeneratorPrivate *gp = static_cast<GeneratorPrivate*>(g->pimpl());
|
||||||
ASSERT_TRUE(c.insert(std::move(g)));
|
ASSERT_TRUE(c.insert(std::move(g)));
|
||||||
EXPECT_FALSE(g); // ownership transferred to container
|
EXPECT_FALSE(g); // ownership transferred to container
|
||||||
VALIDATE(gp);
|
VALIDATE(gp);
|
||||||
@ -110,14 +116,14 @@ TEST_F(BaseTest, serialContainer) {
|
|||||||
|
|
||||||
/***** inserting second stage *****/
|
/***** inserting second stage *****/
|
||||||
auto f = std::make_unique<TestPropagatingForward>();
|
auto f = std::make_unique<TestPropagatingForward>();
|
||||||
PropagatingForwardPrivate *fp = static_cast<PropagatingForwardPrivate*>(f->pimpl_func());
|
PropagatingForwardPrivate *fp = static_cast<PropagatingForwardPrivate*>(f->pimpl());
|
||||||
ASSERT_TRUE(c.insert(std::move(f)));
|
ASSERT_TRUE(c.insert(std::move(f)));
|
||||||
EXPECT_FALSE(f); // ownership transferred to container
|
EXPECT_FALSE(f); // ownership transferred to container
|
||||||
VALIDATE(gp, fp);
|
VALIDATE(gp, fp);
|
||||||
|
|
||||||
/***** inserting third stage *****/
|
/***** inserting third stage *****/
|
||||||
auto f2 = std::make_unique<TestPropagatingForward>();
|
auto f2 = std::make_unique<TestPropagatingForward>();
|
||||||
PropagatingForwardPrivate *fp2 = static_cast<PropagatingForwardPrivate*>(f2->pimpl_func());
|
PropagatingForwardPrivate *fp2 = static_cast<PropagatingForwardPrivate*>(f2->pimpl());
|
||||||
EXPECT_FALSE(c.insert(std::move(f2), 0)); // should fail at first position
|
EXPECT_FALSE(c.insert(std::move(f2), 0)); // should fail at first position
|
||||||
|
|
||||||
// insert @2nd position
|
// insert @2nd position
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user