mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Handle updates on external states
This commit is contained in:
parent
e5b20ac11f
commit
c33b1967bc
@ -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;
|
||||
}
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user