Allow access to `Non Const` parent (container) of a stage

[captain-yoshi]

I would like to be able to traverse the parent container of a stage and be able to modify the container before planning a task. E.g. for adding stages before/after a certain stage.

class Stage {
  ...

  /// What we have
  const ContainerBase* parent() const;

  /// What I need
  ContainerBase* parentNonConst();
}

Would there be caveats for allowing this ?

---

A follow up question: Is there a way to get the container id from a stage ?

[rhaschke]

Currently, you can access stages (and containers) via findChild(), which also works recursively with children separated by slashes.

Is there a way to get the container id from a stage ?

You mean you want to access the introspection id of the stage's parent's container? stage->parent()->introspectionId()

[captain-yoshi]

This is just a snippet to demonstrate why I need the non const version. Yes I could do it manually or add pre-post stage parameters.

The callback approach permits me to add properties as well as stages before/next to the callback stage.

using StageCallback = std::function<void(Stage&)>;

/// Fill an approach sequence
void approach(moveit::task_constructor::ContainerBase& container,             //
              moveit::task_constructor::solvers::PlannerInterfacePtr solver,  //
              const std::string& stage_name,                                  //
              ...
              const StageCallback& approach_stage_cb);                        // Can change properties and add additional stages before or after

int main(int argc, char** argv)
{
    auto stage_cb = [](Stage& stage)
    {
        auto take_photo = std::make_unique<stages::NoOp>("Mona Lisa");

        auto parent = stage.parentNonConst();
        id = getStageIndex(stage); // See next comment.
        parent.insert(std::move(take_photo), id + 1);  // insert after the stage

    }

}

[captain-yoshi]

You mean you want to access the introspection id of the stage's parent's container?

Yes. The stage->parent()->introspectionId() gave me errors. Even when the task was set with Introspection to true.

How I currently get the ID:

int getStageIndex(moveit::task_constructor::Stage& stage)
  auto parent = stage.parentNonConst();
  if (parent)
  {
    for (std::size_t i = 0; i < parent->numChildren(); ++i)
    {
      if (&stage == (*parent)[i])
        return i;
    }
    ROS_WARN_STREAM("Did not find the stage in it's parent container.");
  }
  else
    ROS_WARN_STREAM("Stage " << stage.name() << " has no parent container.");

  return -1
}

[rhaschke]

Yeah, stage->parent()->introspectionId() doesn't work, because the id is only defined when a description message is generated the first time. (Otherwise, the id is not needed.) I'm sure, you are aware of the difference between that id and the index in the parent container (I was confused, because you used the headline "how I get the ID" for getStageIndex).

Regarding non-const parent: I'm still hesitant to provide writable access to a stage's parent. Usually, you should have a handle to the stage's parent anyway - in your case via the container argument to approach? Your StageCallback could simply take another argument to that parent. If you explicitly want to ignore the constness, you can still do a const_cast. This explicitly marks the exception to the default API.

[captain-yoshi]

I'm sure, you are aware of the difference between that id and the index in the parent container (I was confused, because you used the headline "how I get the ID" for getStageIndex).

Yup that's a typo ! But at first, I though they were both the same thing.

Your StageCallback could simply take another argument to that parent.

Yes but I was trying to reduce the number of parameters. Furthermore, the inside of the function might use another container then the one passed...

Will probably use behavior trees to setup my tasks, what moveit studio uses (not tested and can't afford). The left image is an example of an mtc task. On the right with the container argument. Not really usefull from a user's perspective.

But I understand if you're hesitant about adding it. Really not a big deal, was more curious if there was a reason not being exposed.

[rhaschke]

The reason is simple: The setup of stages should not assume that they are part of a container already. If so, they should not modify the container, but only their own state. Providing a writable parent() would not make people think about their setup design.

[captain-yoshi]

Thanks for the explanation, it makes sense.