From dc9f553ab0f36a4392c9583cf4e88b3bd8926310 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 13 Oct 2017 21:37:34 +0200 Subject: [PATCH] replaced IMPL macro --- include/moveit_task_constructor/subtask.h | 9 ++--- src/container.cpp | 48 ++++++++++++----------- src/subtask.cpp | 32 +++++++++------ src/subtask_p.h | 11 ++++-- src/task.cpp | 9 +++-- src/test/container.cpp | 20 ++++++---- 6 files changed, 73 insertions(+), 56 deletions(-) diff --git a/include/moveit_task_constructor/subtask.h b/include/moveit_task_constructor/subtask.h index 8bec53fa..18cbbda8 100644 --- a/include/moveit_task_constructor/subtask.h +++ b/include/moveit_task_constructor/subtask.h @@ -9,7 +9,9 @@ #include #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 { MOVEIT_CLASS_FORWARD(PlanningScene) @@ -35,12 +37,9 @@ typedef std::pair InterfaceStatePa class SubTaskPrivate; class SubTask { friend std::ostream& operator<<(std::ostream &os, const SubTask& stage); - friend class SubTaskPrivate; public: - inline const SubTaskPrivate* pimpl_func() const { return pimpl_; } - inline SubTaskPrivate* pimpl_func() { return pimpl_; } - + PRIVATE_CLASS(SubTask) typedef std::unique_ptr pointer; ~SubTask(); diff --git a/src/container.cpp b/src/container.cpp index e35f2e5b..d95f19d0 100644 --- a/src/container.cpp +++ b/src/container.cpp @@ -20,7 +20,7 @@ ContainerBasePrivate::const_iterator ContainerBasePrivate::position(int before) } 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 && impl->trajectories_.empty(); // existing trajectories would become invalid } @@ -29,7 +29,7 @@ bool ContainerBasePrivate::traverseStages(const ContainerBase::StageCallback &pr for (auto &stage : children_) { if (!processor(*stage, depth)) continue; - ContainerBasePrivate *container = dynamic_cast(stage->pimpl_func()); + ContainerBasePrivate *container = dynamic_cast(stage->pimpl()); if (container) 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::const_iterator pos) { - SubTaskPrivate *impl = subtask->pimpl_func(); + SubTaskPrivate *impl = subtask->pimpl(); impl->parent_ = this; impl->it_ = children_.insert(pos, std::move(subtask)); return impl->it_; @@ -49,34 +49,35 @@ ContainerBase::ContainerBase(ContainerBasePrivate *impl) : SubTask(impl) { } +PIMPL_FUNCTIONS(ContainerBase) void ContainerBase::clear() { - IMPL(ContainerBase); + auto impl = pimpl(); impl->clear(); } bool ContainerBase::init(const planning_scene::PlanningSceneConstPtr &scene) { - IMPL(ContainerBase); + auto impl = pimpl(); for (auto& stage : impl->children_) stage->init(scene); } bool ContainerBase::traverseStages(const ContainerBase::StageCallback &processor) const { - IMPL(const ContainerBase); + auto impl = pimpl(); return impl->traverseStages(processor, 0); } bool ContainerBase::canCompute() const { - IMPL(const ContainerBase); + auto impl = pimpl(); return impl->canCompute(); } bool ContainerBase::compute() { - IMPL(ContainerBase); + auto impl = pimpl(); return impl->compute(); } @@ -84,8 +85,8 @@ bool ContainerBase::compute() { SubTaskPrivate::InterfaceFlags SerialContainerPrivate::announcedFlags() const { InterfaceFlags f; if (children().empty()) return f; - f |= children().front()->pimpl_func()->announcedFlags() & INPUT_IF_MASK; - f |= children().back()->pimpl_func()->announcedFlags() & OUTPUT_IF_MASK; + f |= children().front()->pimpl()->announcedFlags() & INPUT_IF_MASK; + f |= children().back()->pimpl()->announcedFlags() & OUTPUT_IF_MASK; return f; } @@ -104,8 +105,8 @@ inline bool SerialContainerPrivate::canInsert(const SubTask &stage, ContainerBas // check connectedness bool at_end = (before == children().end()); - const SubTaskPrivate* next = (at_end) ? this : (*before)->pimpl_func(); - InterfaceFlags cur_flags = stage.pimpl_func()->announcedFlags(); + const SubTaskPrivate* next = (at_end) ? this : (*before)->pimpl(); + InterfaceFlags cur_flags = stage.pimpl()->announcedFlags(); InterfaceFlags next_flags = next->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_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 */ if (children().empty()) { // first child inserted setPrevEnds(cur, this->starts_); setNextStarts(cur, this->ends_); } else if (at_begin) { - SubTaskPrivate *next = (*before)->pimpl_func(); + SubTaskPrivate *next = (*before)->pimpl(); setPrevEnds(cur, this->starts_); setNextStarts(cur, next->starts_); setPrevEnds(next, cur->ends_); @@ -140,7 +141,7 @@ ContainerBasePrivate::iterator SerialContainerPrivate::insert(value_type &&stage setNextStarts(cur, this->ends_); } else { const SubTaskPrivate *prev = this->prev(before); - SubTaskPrivate *next = (*before)->pimpl_func(); + SubTaskPrivate *next = (*before)->pimpl(); setNextStarts(prev, cur->starts_); setPrevEnds(cur, prev->ends_); setNextStarts(cur, next->starts_); @@ -154,25 +155,25 @@ inline const SubTaskPrivate* SerialContainerPrivate::prev(const_iterator it) con { #ifndef NDEBUG if (it != children().end()) { - SubTaskPrivate* child = (*it)->pimpl_func(); + SubTaskPrivate* child = (*it)->pimpl(); assert(parent(child) == this); assert(this->it(child) == it); } #endif if (it == children().begin()) return this; - return (*--it)->pimpl_func(); + return (*--it)->pimpl(); } inline const SubTaskPrivate* SerialContainerPrivate::next(const_iterator it) const { #ifndef NDEBUG assert(it != children().end()); - SubTaskPrivate* child = (*it)->pimpl_func(); + SubTaskPrivate* child = (*it)->pimpl(); assert(parent(child) == this); assert(this->it(child) == it); #endif if (it == --children().end()) return this; - return (*++it)->pimpl_func(); + return (*++it)->pimpl(); } const SubTaskPrivate *SerialContainerPrivate::prev(const SubTaskPrivate *child) const @@ -192,16 +193,17 @@ SerialContainer::SerialContainer(SerialContainerPrivate *impl) SerialContainer::SerialContainer(const std::string &name) : SerialContainer(new SerialContainerPrivate(this, name)) {} +PIMPL_FUNCTIONS(SerialContainer) bool SerialContainer::canInsert(const value_type& subtask, int before) const { - IMPL(const SerialContainer); + auto impl = pimpl(); return impl->canInsert(*subtask, impl->position(before)); } bool SerialContainer::insert(value_type&& subtask, int before) { - IMPL(SerialContainer); + auto impl = pimpl(); ContainerBasePrivate::const_iterator where = impl->position(before); if (!impl->canInsert(*subtask, where)) @@ -220,10 +222,10 @@ bool SerialContainerPrivate::compute() { bool computed = false; for(const auto& stage : children()) { - if(!stage->pimpl_func()->canCompute()) + if(!stage->pimpl()->canCompute()) continue; std::cout << "Computing subtask '" << stage->getName() << "':" << std::endl; - bool success = stage->pimpl_func()->compute(); + bool success = stage->pimpl()->compute(); computed = true; std::cout << (success ? "succeeded" : "failed") << std::endl; } diff --git a/src/subtask.cpp b/src/subtask.cpp index 071dd9c2..45b408c3 100644 --- a/src/subtask.cpp +++ b/src/subtask.cpp @@ -25,7 +25,7 @@ const std::string& SubTask::getName() const { } std::ostream& operator<<(std::ostream &os, const SubTask& stage) { - os << *stage.pimpl_func(); + os << *stage.pimpl(); return os; } @@ -192,9 +192,11 @@ PropagatingEitherWay::PropagatingEitherWay(PropagatingEitherWayPrivate *impl) initInterface(); } +PIMPL_FUNCTIONS(PropagatingEitherWay) + void PropagatingEitherWay::initInterface() { - IMPL(PropagatingEitherWay) + auto impl = pimpl(); if (impl->dir & PropagatingEitherWay::FORWARD) { if (!impl->starts_) { // keep existing interface if possible 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) { - IMPL(PropagatingEitherWay); + auto impl = pimpl(); if (impl->dir == dir) return; if (impl->isConnected()) 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, - const planning_scene::PlanningSceneConstPtr& to, - const robot_trajectory::RobotTrajectoryPtr& t, - double cost){ - IMPL(PropagatingEitherWay) + const planning_scene::PlanningSceneConstPtr& to, + const robot_trajectory::RobotTrajectoryPtr& t, + double cost){ + auto impl = pimpl(); std::cout << "sending state forward" << std::endl; SubTrajectory &trajectory = impl->addTrajectory(t, cost); trajectory.setStartState(from); @@ -238,10 +240,10 @@ void PropagatingEitherWay::sendForward(const InterfaceState& from, } void PropagatingEitherWay::sendBackward(const planning_scene::PlanningSceneConstPtr& from, - const InterfaceState& to, - const robot_trajectory::RobotTrajectoryPtr& t, - double cost){ - IMPL(PropagatingEitherWay) + const InterfaceState& to, + const robot_trajectory::RobotTrajectoryPtr& t, + double cost){ + auto impl = pimpl(); std::cout << "sending state backward" << std::endl; SubTrajectory& trajectory = impl->addTrajectory(t, cost); trajectory.setEndState(to); @@ -261,6 +263,7 @@ PropagatingForwardPrivate::PropagatingForwardPrivate(PropagatingForward *me, con PropagatingForward::PropagatingForward(const std::string& name) : PropagatingEitherWay(new PropagatingForwardPrivate(this, name)) {} +PIMPL_FUNCTIONS(PropagatingForward) bool PropagatingForward::computeBackward(const InterfaceState &to) { @@ -280,6 +283,7 @@ PropagatingBackwardPrivate::PropagatingBackwardPrivate(PropagatingBackward *me, PropagatingBackward::PropagatingBackward(const std::string &name) : PropagatingEitherWay(new PropagatingBackwardPrivate(this, name)) {} +PIMPL_FUNCTIONS(PropagatingBackward) bool PropagatingBackward::computeForward(const InterfaceState &from) { @@ -307,11 +311,12 @@ bool GeneratorPrivate::compute() { Generator::Generator(const std::string &name) : SubTask(new GeneratorPrivate(this, name)) {} +PIMPL_FUNCTIONS(Generator) void Generator::spawn(const planning_scene::PlanningSceneConstPtr& ps, double cost) { std::cout << "spawning state forwards and backwards" << std::endl; - IMPL(Generator) + auto impl = pimpl(); // empty trajectory ref -> this node only produces states robot_trajectory::RobotTrajectoryPtr dummy; SubTrajectory& trajectory = impl->addTrajectory(dummy, cost); @@ -364,10 +369,11 @@ Connecting::Connecting(const std::string &name) : SubTask(new ConnectingPrivate(this, name)) { } +PIMPL_FUNCTIONS(Connecting) void Connecting::connect(const InterfaceState& from, const InterfaceState& to, const robot_trajectory::RobotTrajectoryPtr& t, double cost) { - IMPL(Connecting) + auto impl = pimpl(); SubTrajectory& trajectory = impl->addTrajectory(t, cost); trajectory.setStartState(from); trajectory.setEndState(to); diff --git a/src/subtask_p.h b/src/subtask_p.h index e8e5e4e7..72264657 100644 --- a/src/subtask_p.h +++ b/src/subtask_p.h @@ -5,6 +5,11 @@ #include #include +// define pimpl() functions accessing correctly casted pimpl_ pointer +#define PIMPL_FUNCTIONS(Class) \ + const Class##Private* Class::pimpl() const { return static_cast(pimpl_); } \ + Class##Private* Class::pimpl() { return static_cast(pimpl_); } \ + namespace moveit { namespace task_constructor { class ContainerBasePrivate; @@ -139,8 +144,6 @@ private: std::pair it_pairs_; }; +PIMPL_FUNCTIONS(SubTask) + } } - - -// get correctly casted private impl pointer -#define IMPL(Class) Class##Private * const impl = static_cast(pimpl_func()); diff --git a/src/task.cpp b/src/task.cpp index 97bc7530..4524141f 100644 --- a/src/task.cpp +++ b/src/task.cpp @@ -66,6 +66,7 @@ Task::Task(const std::string &name) : SerialContainer(new TaskPrivate(this, name)) { } +PIMPL_FUNCTIONS(Task) planning_pipeline::PlanningPipelinePtr Task::createPlanner(const robot_model::RobotModelConstPtr& model, const std::string& ns, @@ -96,7 +97,7 @@ void Task::add(pointer &&stage) { } bool Task::plan(){ - IMPL(Task); + auto impl = pimpl(); NewSolutionPublisher debug(*this); impl->initScene(); @@ -112,7 +113,7 @@ bool Task::plan(){ } const robot_state::RobotState& Task::getCurrentRobotState() const { - IMPL(const Task) + auto impl = pimpl(); return impl->scene_->getCurrentState(); } @@ -153,12 +154,12 @@ bool traverseFullTrajectories( } bool Task::processSolutions(const Task::SolutionCallback& processor) { - IMPL(Task); + auto impl = pimpl(); const TaskPrivate::container_type& children = impl->children(); const size_t nr_of_trajectories = children.size(); std::vector trace; 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) ) return false; return true; diff --git a/src/test/container.cpp b/src/test/container.cpp index f664ebb6..b41f6580 100644 --- a/src/test/container.cpp +++ b/src/test/container.cpp @@ -4,6 +4,12 @@ #include #include +namespace moveit { namespace task_constructor { +PIMPL_FUNCTIONS(Generator) +PIMPL_FUNCTIONS(PropagatingForward) +PIMPL_FUNCTIONS(SerialContainer) +}} + namespace testing { namespace internal { enum GTestColor { COLOR_DEFAULT, @@ -51,7 +57,7 @@ protected: // print order 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); // validate order @@ -60,7 +66,7 @@ protected: size_t pos = 0; auto exp_it = expected.begin(); 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->parent_, container) << "wrong parent"; EXPECT_EQ(it, container->position(pos)) << "bad forward position resolution"; @@ -78,7 +84,7 @@ protected: TEST_F(BaseTest, interfaceFlags) { std::unique_ptr g = std::make_unique(); - EXPECT_EQ(g->pimpl_func()->interfaceFlags(), + EXPECT_EQ(g->pimpl()->interfaceFlags(), SubTaskPrivate::InterfaceFlags({SubTaskPrivate::WRITES_NEXT_START, SubTaskPrivate::WRITES_PREV_END})); } @@ -89,7 +95,7 @@ TEST_F(BaseTest, interfaceFlags) { TEST_F(BaseTest, serialContainer) { SerialContainer c("serial"); - SerialContainerPrivate *cp = static_cast(c.pimpl_func()); + SerialContainerPrivate *cp = static_cast(c.pimpl()); EXPECT_TRUE(bool(cp->starts_)); EXPECT_TRUE(bool(cp->ends_)); @@ -99,7 +105,7 @@ TEST_F(BaseTest, serialContainer) { /***** inserting first stage *****/ auto g = std::make_unique(); - GeneratorPrivate *gp = static_cast(g->pimpl_func()); + GeneratorPrivate *gp = static_cast(g->pimpl()); ASSERT_TRUE(c.insert(std::move(g))); EXPECT_FALSE(g); // ownership transferred to container VALIDATE(gp); @@ -110,14 +116,14 @@ TEST_F(BaseTest, serialContainer) { /***** inserting second stage *****/ auto f = std::make_unique(); - PropagatingForwardPrivate *fp = static_cast(f->pimpl_func()); + PropagatingForwardPrivate *fp = static_cast(f->pimpl()); ASSERT_TRUE(c.insert(std::move(f))); EXPECT_FALSE(f); // ownership transferred to container VALIDATE(gp, fp); /***** inserting third stage *****/ auto f2 = std::make_unique(); - PropagatingForwardPrivate *fp2 = static_cast(f2->pimpl_func()); + PropagatingForwardPrivate *fp2 = static_cast(f2->pimpl()); EXPECT_FALSE(c.insert(std::move(f2), 0)); // should fail at first position // insert @2nd position