replaced IMPL macro

This commit is contained in:
Robert Haschke 2017-10-13 21:37:34 +02:00
parent 1b1a82f7c8
commit dc9f553ab0
6 changed files with 73 additions and 56 deletions

View File

@ -9,7 +9,9 @@
#include <list>
#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<const InterfaceState&, const InterfaceState&> 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<SubTask> pointer;
~SubTask();

View File

@ -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<ContainerBasePrivate*>(stage->pimpl_func());
ContainerBasePrivate *container = dynamic_cast<ContainerBasePrivate*>(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;
}

View File

@ -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);

View File

@ -5,6 +5,11 @@
#include <moveit_task_constructor/subtask.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 {
class ContainerBasePrivate;
@ -139,8 +144,6 @@ private:
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());

View File

@ -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<SubTrajectory*> 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;

View File

@ -4,6 +4,12 @@
#include <gtest/gtest.h>
#include <initializer_list>
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<Generator> g = std::make_unique<TestGenerator>();
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<SerialContainerPrivate*>(c.pimpl_func());
SerialContainerPrivate *cp = static_cast<SerialContainerPrivate*>(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<TestGenerator>();
GeneratorPrivate *gp = static_cast<GeneratorPrivate*>(g->pimpl_func());
GeneratorPrivate *gp = static_cast<GeneratorPrivate*>(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<TestPropagatingForward>();
PropagatingForwardPrivate *fp = static_cast<PropagatingForwardPrivate*>(f->pimpl_func());
PropagatingForwardPrivate *fp = static_cast<PropagatingForwardPrivate*>(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<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
// insert @2nd position