Distinguish STATUS and PRIORITY updates in notify() callbacks

to allow propagating status updates only if the STATUS actually changed.

Author: rhaschke

core/include/moveit/task_constructor/container_p.h

@@ -156,7 +156,7 @@ class ContainerBasePrivate : public StagePrivate
 
 	/// copy external_state to a child's interface and remember the link in internal_external map
 	template <Interface::Direction>
-	void copyState(Interface::iterator external, const InterfacePtr& target, bool updated);
+	void copyState(Interface::iterator external, const InterfacePtr& target, Interface::UpdateFlags updated);
 	/// lift solution from internal to external level
 	void liftSolution(const SolutionBasePtr& solution, const InterfaceState* internal_from,
 	                  const InterfaceState* internal_to);
@@ -230,9 +230,9 @@ class ParallelContainerBasePrivate : public ContainerBasePrivate
 	void validateInterfaces(const StagePrivate& child, InterfaceFlags& external, bool first = false) const;
 
 private:
-	/// callback for new externally received states
+	/// notify callback for new externally received states
 	template <typename Interface::Direction>
-	void onNewExternalState(Interface::iterator external, bool updated);
+	void onNewExternalState(Interface::iterator external, Interface::UpdateFlags updated);
 };
 PIMPL_FUNCTIONS(ParallelContainerBase)
 

core/include/moveit/task_constructor/stage_p.h

@@ -340,9 +340,9 @@ class ConnectingPrivate : public ComputeBasePrivate
 	template <Interface::Direction other>
 	inline StatePair make_pair(Interface::const_iterator first, Interface::const_iterator second);
 
-	// get informed when new start or end state becomes available
+	// notify callback to get informed about newly inserted (or updated) start or end states
 	template <Interface::Direction other>
-	void newState(Interface::iterator it, bool updated);
+	void newState(Interface::iterator it, Interface::UpdateFlags updated);
 
 	// ordered list of pending state pairs
 	ordered<StatePair> pending;

core/include/moveit/task_constructor/storage.h

@@ -42,6 +42,7 @@
 #include <moveit/macros/class_forward.h>
 #include <moveit/task_constructor/properties.h>
 #include <moveit/task_constructor/cost_queue.h>
+#include <moveit/task_constructor/utils.h>
 #include <moveit_task_constructor_msgs/Solution.h>
 #include <visualization_msgs/MarkerArray.h>
 
@@ -199,7 +200,14 @@ class Interface : public ordered<InterfaceState*>
 		FORWARD,
 		BACKWARD,
 	};
-	using NotifyFunction = std::function<void(iterator, bool)>;
+	enum Update
+	{
+		STATUS = 1 << 0,
+		PRIORITY = 1 << 1,
+		ALL = STATUS | PRIORITY,
+	};
+	using UpdateFlags = utils::Flags<Update>;
+	using NotifyFunction = std::function<void(iterator, UpdateFlags)>;
 
 	class DisableNotify
 	{

core/include/moveit/task_constructor/utils.h

@@ -140,8 +140,6 @@ class Flags
 	Int i;
 };
 
-#define DECLARE_FLAGS(Flags, Enum) using Flags = QFlags<Enum>;
-
 const moveit::core::LinkModel* getRigidlyConnectedParentLinkModel(const moveit::core::RobotState& state,
                                                                   std::string frame);
 

core/src/container.cpp

@@ -189,7 +189,8 @@ void ContainerBasePrivate::onNewFailure(const Stage& child, const InterfaceState
 }
 
 template <Interface::Direction dir>
-void ContainerBasePrivate::copyState(Interface::iterator external, const InterfacePtr& target, bool updated) {
+void ContainerBasePrivate::copyState(Interface::iterator external, const InterfacePtr& target,
+                                     Interface::UpdateFlags updated) {
 	if (updated) {  // propagate external state update to internal copies
 		auto internals{ externalToInternalMap().equal_range(&*external) };
 		for (auto& i = internals.first; i != internals.second; ++i) {
@@ -550,7 +551,7 @@ void SerialContainerPrivate::resolveInterface(InterfaceFlags expected) {
 		validateInterface<START_IF_MASK>(*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) {
+			starts_.reset(new Interface([this, target](Interface::iterator it, Interface::UpdateFlags updated) {
 				this->copyState<Interface::FORWARD>(it, target, updated);
 			}));
 	} catch (InitStageException& e) {
@@ -575,7 +576,7 @@ void SerialContainerPrivate::resolveInterface(InterfaceFlags expected) {
 		validateInterface<END_IF_MASK>(*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) {
+			ends_.reset(new Interface([this, target](Interface::iterator it, Interface::UpdateFlags updated) {
 				this->copyState<Interface::BACKWARD>(it, target, updated);
 			}));
 	} catch (InitStageException& e) {
@@ -670,11 +671,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) {
+		starts().reset(new Interface([this](Interface::iterator external, Interface::UpdateFlags updated) {
 			this->onNewExternalState<Interface::FORWARD>(external, updated);
 		}));
 	if (expected & READS_END)
-		ends().reset(new Interface([this](Interface::iterator external, bool updated) {
+		ends().reset(new Interface([this](Interface::iterator external, Interface::UpdateFlags updated) {
 			this->onNewExternalState<Interface::BACKWARD>(external, updated);
 		}));
 
@@ -713,7 +714,7 @@ void ParallelContainerBasePrivate::validateConnectivity() const {
 }
 
 template <Interface::Direction dir>
-void ParallelContainerBasePrivate::onNewExternalState(Interface::iterator external, bool updated) {
+void ParallelContainerBasePrivate::onNewExternalState(Interface::iterator external, Interface::UpdateFlags updated) {
 	for (const Stage::pointer& stage : children())
 		copyState<dir>(external, stage->pimpl()->pullInterface<dir>(), updated);
 }

core/src/stage.cpp

@@ -711,11 +711,12 @@ ConnectingPrivate::StatePair ConnectingPrivate::make_pair<Interface::FORWARD>(In
 
 // TODO: bool updated -> uint_8 updated (bitfield of PRIORITY | STATUS)
 template <Interface::Direction dir>
-void ConnectingPrivate::newState(Interface::iterator it, bool updated) {
+void ConnectingPrivate::newState(Interface::iterator it, Interface::UpdateFlags updated) {
 	auto parent_pimpl = parent()->pimpl();
 	Interface::DisableNotify disable_source_interface(*pullInterface<dir>());
 	if (updated) {
-		if (pullInterface<opposite<dir>()>()->notifyEnabled())  // suppress recursive loop
+		if (updated.testFlag(Interface::STATUS) &&  // only perform these costly operations if needed
+		    pullInterface<opposite<dir>()>()->notifyEnabled())  // suppress recursive loop
 		{
 			// If status has changed, propagate the update to the opposite side
 			auto status = it->priority().status();

core/src/storage.cpp

@@ -129,7 +129,7 @@ void Interface::add(InterfaceState& state) {
 	moveFrom(it, container);
 	// and finally call notify callback
 	if (notify_)
-		notify_(it, false);
+		notify_(it, UpdateFlags());
 }
 
 Interface::container_type Interface::remove(iterator it) {
@@ -140,16 +140,23 @@ Interface::container_type Interface::remove(iterator it) {
 }
 
 void Interface::updatePriority(InterfaceState* state, const InterfaceState::Priority& priority) {
-	if (priority == state->priority())
+	const auto old_prio = state->priority();
+	if (priority == old_prio)
 		return;  // nothing to do
 
 	auto it = std::find(begin(), end(), state);  // find iterator to state
 	assert(it != end());  // state should be part of this interface
 
 	state->priority_ = priority;  // update priority
 	update(it);  // update position in ordered list
-	if (notify_)
-		notify_(it, true);  // notify callback
+
+	if (notify_) {
+		UpdateFlags updated(Update::ALL);
+		if (old_prio.status() == priority.status())
+			updated &= ~STATUS;
+
+		notify_(it, updated);  // notify callback
+	}
 }
 
 std::ostream& operator<<(std::ostream& os, const Interface& interface) {