mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Distinguish STATUS and PRIORITY updates in notify() callbacks
to allow propagating status updates only if the STATUS actually changed.
This commit is contained in:
parent
fdd5ff880b
commit
a48b932dce
@ -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)
|
||||
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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
|
||||
{
|
||||
|
||||
@ -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);
|
||||
|
||||
|
||||
@ -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);
|
||||
}
|
||||
|
||||
@ -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();
|
||||
|
||||
@ -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) {
|
||||
|
||||
Loading…
Reference in New Issue
Block a user