Handle updates on external states

This commit is contained in:
Robert Haschke 2021-09-16 21:46:11 +02:00
parent e5b20ac11f
commit c33b1967bc

View File

@ -195,8 +195,10 @@ template void ContainerBasePrivate::setStatus<Interface::BACKWARD>(const Interfa
template <Interface::Direction dir>
void ContainerBasePrivate::copyState(Interface::iterator external, const InterfacePtr& target, bool updated) {
if (updated) {
// update prio of all internal states linked to external
auto internals{ externalToInternalMap().equal_range(&*external) };
for (auto& i = internals.first; i != internals.second; ++i) {
// TODO: Not only update status, but full priority!
setStatus<dir>(i->second, external->priority().status());
}
return;
@ -898,9 +900,16 @@ void FallbacksPrivate::computeGenerate() {
template <typename Interface::Direction dir>
void FallbacksPrivate::onNewExternalState(Interface::iterator external, bool updated) {
// TODO(v4hn): updated is not implemented
if(updated){
ROS_DEBUG_NAMED("Fallbacks", "updating external states is not supported in Fallbacks");
if (updated) {
auto it = std::find_if(pending_states_.begin(), pending_states_.end(),
[external](const ExternalState& s) { return s.external_state == external; });
if (it == pending_states_.cend())
return; // already processed
pending_states_.update(it); // update sorting pos of this single item
// update prio of linked internal states as well
ContainerBasePrivate::copyState<dir>(external, InterfacePtr(), updated);
return;
}