Distinguish STATUS and PRIORITY updates in notify() callbacks

to allow propagating status updates only if the STATUS actually changed.
This commit is contained in:
Robert Haschke 2021-11-21 11:06:15 +01:00
parent fdd5ff880b
commit a48b932dce
7 changed files with 35 additions and 20 deletions

View File

@ -156,7 +156,7 @@ protected:
/// 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 @@ protected:
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)

View File

@ -340,9 +340,9 @@ private:
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;

View File

@ -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 @@ public:
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
{

View File

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

View File

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

View File

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

View File

@ -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,7 +140,8 @@ 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
@ -148,8 +149,14 @@ void Interface::updatePriority(InterfaceState* state, const InterfaceState::Prio
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) {