Add ability to preempt the task when a stage fails (preempt_on_failure)

[DaniGarciaLopez]

In our tasks, we make extensive use of the PredicateFilter stage to perform certain checks in the scene before other stages are executed. This stage is expected to always succeed for the task to proceed. However, the task currently continues planning and looks for alternative solutions in earlier stages, which unnecessarily prolongs planning time—even though we already know that if this stage fails, the task is not feasible and there is no reason to continue.

With this in mind, we propose adding a preempt_on_failure property. If set to true, the stage must always succeed; if the stage fails even once, the task is immediately preempted and planning aborts.

We can currently do something similar using addSolutionCallback and throwing an exception when any solution fails, but this does not update introspection correctly.

Thoughts, @rhaschke?

[rhaschke]

We can currently do something similar using addSolutionCallback and throwing an exception when any solution fails, but this does not update introspection correctly.

Why don't you simply call Task::preempt() in the callback?

[DaniGarciaLopez]

Why don't you simply call Task::preempt() in the callback?

Because we don't have easy access to the task pointer. Is there a way to get it from the solution?

I tried traversing each parent, but the top-level container is not a Task, so this unfortunately doesn’t work:

applicability_filter->addSolutionCallback(
  [](const mtc::SolutionBase& solution) {
    if (solution.isFailure()) {
      const mtc::ContainerBase* container = solution.creator()->parent();
      while (container && container->parent())
          container = container->parent();

      const mtc::Task* task = dynamic_cast<const mtc::Task*>(container);
      if (task)
        const_cast<mtc::Task*>(task)->preempt();
    }
  }
);

[rhaschke]

I was surprised to see this failing. Digging deeper, it turns out that the dynamic_cast fails due to protected inheritance.
I see several options:

• As you know that the top-level container pointer should be a task, you could replace dynamic_cast with reinterpret_cast.
• Alternatively, pass the task pointer via capture into your lambda function:

  applicability_filter->addSolutionCallback([&task](const mtc::SolutionBase& solution) {
    if (solution.isFailure()) {task.preempt();}
  });
  

[DaniGarciaLopez]

I wasn’t aware of this issue with dynamic_cast, but it actually makes sense. I just tried using reinterpret_cast and it works perfectly!

I agree that passing the task pointer to the lambda would be the best approach, but we don’t have easy access to it. Our stages are packed into SerialContainers and then used as drag-and-drop modules of a behavior tree. It would require too much refactor to give each module access to the task pointer. Since we only traverse them once—when the stage fails—the performance impact will be minimal, so I’m comfortable using this snippet.

Closing this PR. Thanks, Robert!