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>
|
||||
|
||||
#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();
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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());
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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
|
||||
|
||||
Loading…
Reference in New Issue
Block a user