diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index ef03b6f6..c35d17b2 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -136,6 +136,7 @@ protected: } /// copy external_state to a child's interface and remember the link in internal_to map + template void copyState(Interface::iterator external, const InterfacePtr& target, bool updated); /// lift solution from internal to external level void liftSolution(const SolutionBasePtr& solution, const InterfaceState* internal_from, @@ -207,7 +208,8 @@ protected: private: /// callback for new externally received states - void onNewExternalState(Interface::Direction dir, Interface::iterator external, bool updated); + template + void onNewExternalState(Interface::iterator external, bool updated); }; PIMPL_FUNCTIONS(ParallelContainerBase) diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index 24e0cb45..b948baea 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -146,7 +146,9 @@ private: private: planning_scene::PlanningSceneConstPtr scene_; PropertyMap properties_; + /// trajectories which are *timewise before* this state Solutions incoming_trajectories_; + /// trajectories which are *timewise after* this state Solutions outgoing_trajectories_; // members needed for priority scheduling in Interface list diff --git a/core/src/container.cpp b/core/src/container.cpp index ececf0dc..cf6d8932 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -106,10 +106,17 @@ void ContainerBasePrivate::compute() { static_cast(me_)->compute(); } +template void ContainerBasePrivate::copyState(Interface::iterator external, const InterfacePtr& target, bool updated) { - // TODO: update internal's prio from external's new priority - if (updated) + if (updated) { + // TODO(v4hn): This is inefficient, consider storing inverse mapping for each start/end pair in children + // for parallel containers there will be multiple internal states + std::for_each(internal_to_external_.begin(), internal_to_external_.end(), [&external](auto& p) { + if (p.second == &*external) + setStatus(p.first, external->priority().status()); + }); return; + } // create a clone of external state within target interface (child's starts() or ends()) auto internal = states_.insert(states_.end(), InterfaceState(*external)); @@ -497,8 +504,9 @@ void SerialContainerPrivate::resolveInterface(InterfaceFlags expected) { validateInterface(*first.pimpl(), expected); // connect first child's (start) pull interface if (const InterfacePtr& target = first.pimpl()->starts()) - starts_.reset(new Interface( - [this, target](Interface::iterator it, bool updated) { this->copyState(it, target, updated); })); + starts_.reset(new Interface([this, target](Interface::iterator it, bool updated) { + this->copyState(it, target, updated); + })); } catch (InitStageException& e) { exceptions.append(e); } @@ -521,8 +529,9 @@ void SerialContainerPrivate::resolveInterface(InterfaceFlags expected) { validateInterface(*last.pimpl(), expected); // connect last child's (end) pull interface if (const InterfacePtr& target = last.pimpl()->ends()) - ends_.reset(new Interface( - [this, target](Interface::iterator it, bool updated) { this->copyState(it, target, updated); })); + ends_.reset(new Interface([this, target](Interface::iterator it, bool updated) { + this->copyState(it, target, updated); + })); } catch (InitStageException& e) { exceptions.append(e); } @@ -622,11 +631,11 @@ void ParallelContainerBasePrivate::resolveInterface(InterfaceFlags expected) { // States received by the container need to be copied to all children's pull interfaces. if (expected & READS_START) starts().reset(new Interface([this](Interface::iterator external, bool updated) { - this->onNewExternalState(Interface::FORWARD, external, updated); + this->onNewExternalState(external, updated); })); if (expected & READS_END) ends().reset(new Interface([this](Interface::iterator external, bool updated) { - this->onNewExternalState(Interface::BACKWARD, external, updated); + this->onNewExternalState(external, updated); })); required_interface_ = expected; @@ -663,10 +672,10 @@ void ParallelContainerBasePrivate::validateConnectivity() const { ContainerBasePrivate::validateConnectivity(); } -void ParallelContainerBasePrivate::onNewExternalState(Interface::Direction dir, Interface::iterator external, - bool updated) { +template +void ParallelContainerBasePrivate::onNewExternalState(Interface::iterator external, bool updated) { for (const Stage::pointer& stage : children()) - copyState(external, stage->pimpl()->pullInterface(dir), updated); + copyState(external, stage->pimpl()->pullInterface(dir), updated); } ParallelContainerBase::ParallelContainerBase(ParallelContainerBasePrivate* impl) : ContainerBase(impl) {} diff --git a/core/test/test_serial.cpp b/core/test/test_serial.cpp index 5eeb96f4..1e6ca584 100644 --- a/core/test/test_serial.cpp +++ b/core/test/test_serial.cpp @@ -145,6 +145,13 @@ void resetIds() { Connect::id_ = 0; } +template +auto add(C& container, S* stage) -> S* { + Stage::pointer ptr{ stage }; + container.add(std::move(ptr)); + return stage; +} + // https://github.com/ros-planning/moveit_task_constructor/issues/182 TEST(ConnectConnect, SuccSucc) { resetIds(); @@ -208,7 +215,7 @@ TEST(Pruning, PruningMultiForward) { t.add(Stage::pointer(new GeneratorMockup())); // spawn two solutions for the only incoming state t.add(Stage::pointer(new ForwardMockup({ 0, 0 }, 2))); - // fail to exten + // fail to extend the second solution t.add(Stage::pointer(new ForwardMockup({ 0, inf }))); t.plan(); @@ -268,3 +275,39 @@ TEST(Pruning, ConnectConnectBackward) { EXPECT_EQ(c2->calls_, 3u); EXPECT_EQ(c1->calls_, 6u); } + +TEST(Pruning, PropagateInsideContainerBoundaries) { + resetIds(); + Task t; + t.setRobotModel(getModel()); + add(t, new BackwardMockup({ inf })); + add(t, new GeneratorMockup({ 0 })); + auto c{ std::make_unique() }; + auto con = add(*c, new Connect()); + add(*c, new GeneratorMockup({ 0 })); + t.add(std::move(c)); + + t.plan(); + + // the failure in the backward stage (outside the container) + // should prune the expected computation of con + EXPECT_EQ(con->calls_, 0); +} + +TEST(Pruning, DISABLED_PropagateOutsideContainerBoundaries) { + resetIds(); + Task t; + t.setRobotModel(getModel()); + auto back = add(t, new BackwardMockup()); + add(t, new BackwardMockup()); + add(t, new GeneratorMockup({ 0 })); + auto c{ std::make_unique() }; + add(*c, new ForwardMockup({ inf })); + add(*c, new ForwardMockup()); + t.add(std::move(c)); + + t.plan(); + + // the failure inside the container should prune computing of back + EXPECT_EQ(back->calls_, 0); +}