mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
static TaskPrivate::swap() -> ContainerBasePrivate::operator=()
- Enable moving/swapping of other container impls (e.g. Fallbacks) - Clarify (via move semantics) that content of source impl will be lost - Get rid of friend declarations
This commit is contained in:
parent
b84bb87102
commit
e67b3252fc
@ -76,7 +76,6 @@ namespace task_constructor {
|
||||
class ContainerBasePrivate : public StagePrivate
|
||||
{
|
||||
friend class ContainerBase;
|
||||
friend void swap(StagePrivate*& lhs, StagePrivate*& rhs);
|
||||
|
||||
public:
|
||||
using container_type = StagePrivate::container_type;
|
||||
@ -135,6 +134,7 @@ public:
|
||||
|
||||
protected:
|
||||
ContainerBasePrivate(ContainerBase* me, const std::string& name);
|
||||
ContainerBasePrivate& operator=(ContainerBasePrivate&& other);
|
||||
|
||||
// Set child's push interfaces: allow pushing if child requires it.
|
||||
inline void setChildsPushBackwardInterface(StagePrivate* child) {
|
||||
|
||||
@ -61,7 +61,6 @@ class StagePrivate
|
||||
{
|
||||
friend class Stage;
|
||||
friend std::ostream& operator<<(std::ostream& os, const StagePrivate& stage);
|
||||
friend void swap(StagePrivate*& lhs, StagePrivate*& rhs);
|
||||
|
||||
public:
|
||||
/// container type used to store children
|
||||
@ -165,6 +164,8 @@ public:
|
||||
void computeCost(const InterfaceState& from, const InterfaceState& to, SolutionBase& solution);
|
||||
|
||||
protected:
|
||||
StagePrivate& operator=(StagePrivate&& other);
|
||||
|
||||
// associated/owning Stage instance
|
||||
Stage* me_;
|
||||
|
||||
|
||||
@ -51,15 +51,14 @@ namespace task_constructor {
|
||||
class TaskPrivate : public WrapperBasePrivate
|
||||
{
|
||||
friend class Task;
|
||||
TaskPrivate& operator=(TaskPrivate&& other);
|
||||
|
||||
public:
|
||||
TaskPrivate(Task* me, const std::string& ns);
|
||||
|
||||
const std::string& ns() const { return ns_; }
|
||||
const ContainerBase* stages() const;
|
||||
|
||||
protected:
|
||||
static void swap(StagePrivate*& lhs, StagePrivate*& rhs);
|
||||
|
||||
private:
|
||||
std::string ns_;
|
||||
robot_model_loader::RobotModelLoaderPtr robot_model_loader_;
|
||||
|
||||
@ -59,6 +59,33 @@ ContainerBasePrivate::ContainerBasePrivate(ContainerBase* me, const std::string&
|
||||
, pending_backward_(new Interface)
|
||||
, pending_forward_(new Interface) {}
|
||||
|
||||
ContainerBasePrivate& ContainerBasePrivate::operator=(ContainerBasePrivate&& other) {
|
||||
assert(internal_external_.empty() && other.internal_external_.empty());
|
||||
|
||||
// move StagePrivate members
|
||||
this->StagePrivate::operator=(std::move(other));
|
||||
|
||||
// swapping of container members needed to maintain valid pending_* interfaces
|
||||
// and children (e.g. for TaskPrivate)
|
||||
required_interface_ = other.required_interface_;
|
||||
std::swap(pending_backward_, other.pending_backward_);
|
||||
std::swap(pending_forward_, other.pending_forward_);
|
||||
std::swap(children_, other.children_);
|
||||
|
||||
// redirect all children's parent pointers to the new parent
|
||||
auto reparent_children = [](ContainerBasePrivate& self) {
|
||||
for (auto it = self.children_.begin(), end = self.children_.end(); it != end; ++it) {
|
||||
auto cimpl = (*it)->pimpl();
|
||||
cimpl->unparent();
|
||||
cimpl->setParent(static_cast<ContainerBase*>(self.me_));
|
||||
cimpl->setParentPosition(it);
|
||||
}
|
||||
};
|
||||
reparent_children(*this);
|
||||
reparent_children(other);
|
||||
return *this;
|
||||
}
|
||||
|
||||
ContainerBasePrivate::const_iterator ContainerBasePrivate::childByIndex(int index, bool for_insert) const {
|
||||
if (!for_insert && index < 0)
|
||||
--index;
|
||||
@ -681,8 +708,8 @@ void ParallelContainerBasePrivate::resolveInterface(InterfaceFlags expected) {
|
||||
child_impl->resolveInterface(expected);
|
||||
validateInterfaces(*child_impl, expected, first);
|
||||
// initialize push connections of children according to their demands
|
||||
setChildsPushForwardInterface(child_impl);
|
||||
setChildsPushBackwardInterface(child_impl);
|
||||
setChildsPushForwardInterface(child_impl);
|
||||
first = false;
|
||||
} catch (InitStageException& e) {
|
||||
exceptions.append(e);
|
||||
|
||||
@ -106,6 +106,33 @@ StagePrivate::StagePrivate(Stage* me, const std::string& name)
|
||||
, parent_{ nullptr }
|
||||
, introspection_{ nullptr } {}
|
||||
|
||||
StagePrivate& StagePrivate::operator=(StagePrivate&& other) {
|
||||
assert(typeid(*this) == typeid(other));
|
||||
|
||||
assert(states_.empty() && other.states_.empty());
|
||||
assert((!starts_ || starts_->empty()) && (!other.starts_ || other.starts_->empty()));
|
||||
assert((!ends_ || ends_->empty()) && (!other.ends_ || other.ends_->empty()));
|
||||
assert(solutions_.empty() && other.solutions_.empty());
|
||||
assert(failures_.empty() && other.failures_.empty());
|
||||
|
||||
// me_ must not be changed!
|
||||
name_ = std::move(other.name_);
|
||||
properties_ = std::move(other.properties_);
|
||||
cost_term_ = std::move(other.cost_term_);
|
||||
solution_cbs_ = std::move(other.solution_cbs_);
|
||||
|
||||
starts_ = std::move(other.starts_);
|
||||
ends_ = std::move(other.ends_);
|
||||
prev_ends_ = std::move(other.prev_ends_);
|
||||
next_starts_ = std::move(other.next_starts_);
|
||||
|
||||
parent_ = std::move(other.parent_);
|
||||
it_ = std::move(other.it_);
|
||||
other.unparent();
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
InterfaceFlags StagePrivate::interfaceFlags() const {
|
||||
InterfaceFlags f;
|
||||
if (starts())
|
||||
|
||||
@ -74,30 +74,14 @@ namespace task_constructor {
|
||||
TaskPrivate::TaskPrivate(Task* me, const std::string& ns)
|
||||
: WrapperBasePrivate(me, std::string()), ns_(rosNormalizeName(ns)), preempt_requested_(false) {}
|
||||
|
||||
void swap(StagePrivate*& lhs, StagePrivate*& rhs) {
|
||||
// It only makes sense to swap pimpl instances of a Task!
|
||||
// However, due to member protection rules, we can only implement it here
|
||||
assert(typeid(lhs) == typeid(rhs));
|
||||
|
||||
// swap instances
|
||||
::std::swap(lhs, rhs);
|
||||
// as well as their me_ pointers
|
||||
::std::swap(lhs->me_, rhs->me_);
|
||||
|
||||
// and redirect the parent pointers of children to new parents
|
||||
auto& lhs_children = static_cast<ContainerBasePrivate*>(lhs)->children_;
|
||||
for (auto it = lhs_children.begin(), end = lhs_children.end(); it != end; ++it) {
|
||||
(*it)->pimpl()->unparent();
|
||||
(*it)->pimpl()->setParent(static_cast<ContainerBase*>(lhs->me_));
|
||||
(*it)->pimpl()->setParentPosition(it);
|
||||
}
|
||||
|
||||
auto& rhs_children = static_cast<ContainerBasePrivate*>(rhs)->children_;
|
||||
for (auto it = rhs_children.begin(), end = rhs_children.end(); it != end; ++it) {
|
||||
(*it)->pimpl()->unparent();
|
||||
(*it)->pimpl()->setParent(static_cast<ContainerBase*>(rhs->me_));
|
||||
(*it)->pimpl()->setParentPosition(it);
|
||||
}
|
||||
TaskPrivate& TaskPrivate::operator=(TaskPrivate&& other) {
|
||||
this->WrapperBasePrivate::operator=(std::move(other));
|
||||
ns_ = std::move(other.ns_);
|
||||
introspection_ = std::move(other.introspection_);
|
||||
robot_model_ = std::move(other.robot_model_);
|
||||
robot_model_loader_ = std::move(other.robot_model_loader_);
|
||||
task_cbs_ = std::move(other.task_cbs_);
|
||||
return *this;
|
||||
}
|
||||
|
||||
const ContainerBase* TaskPrivate::stages() const {
|
||||
@ -122,7 +106,7 @@ Task::Task(Task&& other) // NOLINT(performance-noexcept-move-constructor)
|
||||
|
||||
Task& Task::operator=(Task&& other) { // NOLINT(performance-noexcept-move-constructor)
|
||||
clear(); // remove all stages of current task
|
||||
swap(this->pimpl_, other.pimpl_);
|
||||
*static_cast<TaskPrivate*>(pimpl_) = std::move(*static_cast<TaskPrivate*>(other.pimpl_));
|
||||
return *this;
|
||||
}
|
||||
|
||||
|
||||
@ -586,10 +586,6 @@ TEST(Task, move) {
|
||||
Task t2 = std::move(t1);
|
||||
EXPECT_EQ(t2.stages()->numChildren(), 2u);
|
||||
EXPECT_EQ(t1.stages()->numChildren(), 0u); // NOLINT(clang-analyzer-cplusplus.Move)
|
||||
|
||||
t1 = std::move(t2);
|
||||
EXPECT_EQ(t1.stages()->numChildren(), 2u);
|
||||
EXPECT_EQ(t2.stages()->numChildren(), 0u); // NOLINT(clang-analyzer-cplusplus.Move)
|
||||
}
|
||||
|
||||
TEST(Task, reuse) {
|
||||
|
||||
Loading…
Reference in New Issue
Block a user