Add creator stage name to SolutionInfo

[felixvd]

In #162 , @v4hn suggested the option of executing the solution sequentially to execute custom functions. We tried using this in an application, but when stepping through the solution's subtrajectories, we are somewhat blind as the solution does not seem to contain any semantic cues by itself*. If it does, a student of mine and I could not find them. This is a complex piece of software.

This PR extends the subtrajectory message by the name of the stage that created it so that the user can tell which part of the task they are at and execute arbitrary code (e.g. gripper open/close, set I/O, change display, call service...) when desired. Naming stages appropriately seems to be the easiest way to implement what @v4hn called "the trivial way".

Adding code hooks may be the proper way, but as mentioned in #162 there is no time frame on this feature, and this workaround is simple, lightweight and works in Python.

This does not need to be merged, it can just be a reference for whoever wants to execute solutions this way.

*edit: Yes, you could get the stage_id from the SolutionInfo of each trajectory and search through the TaskDescription to retrieve the name, but it is awkward.

[codecov[bot]]

Codecov Report

Merging #192 into master will decrease coverage by 0.00%. The diff coverage is 0.00%.

@@            Coverage Diff             @@
##           master     #192      +/-   ##
==========================================
- Coverage   26.99%   26.98%   -0.01%     
==========================================
  Files          90       90              
  Lines        5746     5747       +1     
==========================================
  Hits         1551     1551              
- Misses       4195     4196       +1     

Impacted Files	Coverage Δ
core/src/storage.cpp	42.52% <0.00%> (-0.50%)	:arrow_down:

---

Continue to review full report at Codecov.

Legend - Click here to learn more Δ = absolute <relative> (impact), ø = not affected, ? = missing data Powered by Codecov. Last update d031f1b...a7fb545. Read the comment docs.

[v4hn]

*edit: Yes, you could get the stage_id from the SolutionInfo of each trajectory and search through the TaskDescription to retrieve the name, but it is awkward.

Indeed, but what's wrong with extracting the stage_id once from the description once and using the id to find the correct solution? The id is much more appropriate for your use-case anyway, as there is no strict unique-name assumption among the stages.

Alternatively, assuming you have the Task object in the same process you can also extract the id directly:

auto state = make_unique<Stage>();
auto* stage_ptr { &stage };
task.add(move(state))
task.init();
uint32_t stage_id = task.introspection().stageId(stage_ptr);

Should we add a simpler interface for that last line?

[felixvd]

If I understand correctly, you would have to store the task description along with the solution to know which stage_id to look up, which to me is just another opportunity to mix up numbers. A cleartext description in the solution to describe the purpose of each trajectory seems a lot less error-prone and more intuitive.

Also, I imagine that users will call action servers that return the solution for a task (Pick/Place/PickPlace). Even if the task description was returned alongside the solution, searching through the message manually is a hassle. By contrast, stepping through the trajectories and checking each stage name is a breeze.

Should we add a simpler interface for that last line?

I am not sure I even understood the introspection pattern, so I don't think I can answer that completely. But I'll note that we call action servers and execute the solutions in Python, so I don't think it will make a difference to us.

[v4hn]

If I understand correctly, you would have to store the task description along with the solution to know which stage_id to look up,

Yes if you want to infer the correct stage remotely from the description. But no, if you directly get the correct id from the task - probably using the new method in fae4520 as I outlined above.

which to me is just another opportunity to mix up numbers. A cleartext description in the solution to describe the purpose of each trajectory seems a lot less error-prone and more intuitive.

As long as you do not use the same stage description twice that's an option. From my perspective I think both approaches are equally error-prone, it's just a bit easier to debug discrepancies with string. :-)

For a better solution, what do you think about adding something like Property[] execution_properties to the SolutionInfo. My motivation is to allow user-defined flags like "enable_suction", "switch_display_mode", "change_safety_parameter" to be specified with any SubTrajectory. That way, you can have a sane execution management that simply acts according to these flags instead of hard-coding stage names and at the same time you could have a Propagator stage that sets these properties in your Task model already.

[felixvd]

if you directly get the correct id from the task

That is assuming that you have access to the task, which you would not if you get the solution from a move_group capability as described in #112.

From my perspective I think both approaches are equally error-prone, it's just a bit easier to debug discrepancies with string. :-)

For a better solution, what do you think about adding something like Property[] execution_properties to the SolutionInfo. My motivation is to allow user-defined flags like "enable_suction", "switch_display_mode", "change_safety_parameter" to be specified with any SubTrajectory. That way, you can have a sane execution management that simply acts according to these flags instead of hard-coding stage names and at the same time you could have a Propagator stage that sets these properties in your Task model already.

I like it, although the name execution_properties makes me expect modifying parameters or things I should not touch as a user. custom_flags or similar would probably be clearer.

That said, even though using the stage name to transmit flags/signals like that is definitely hacky, I think the stage name is also helpful for debugging and displaying, so including it in the solution makes sense to me.

[v4hn]

That is assuming that you have access to the task, which you would not if you get the solution from a move_group capability as described in #112.

Yes. But for any kind of interleaved-execution or custom hooks, you would need that anyway. The only other way to support similar functionality is by additional information in the solution message, as we discuss right now.

I like it, although the name execution_properties makes me expect modifying parameters or things I should not touch as a user.

The concept of properties is very general in MTC and you already modify the properties of stages to setup your problem. We could also go for Property[] external or external_properties, I would just like to avoid ambiguities with the Properties that are used to generate a specific solution (we do not export those), although maybe we should.

custom_flags or similar would probably be clearer.

Properties can be boolean flags, but providing arbitrary data, e.g. to specify the pressure for each element in a suction array, is supported here, so "flags" is no good name.

the stage name is also helpful for debugging and displaying, so including it in the solution makes sense to me.

In principle I agree. But then we would add redundant information to the messages and it also makes sense to avoid that. The information is now cleanly split between task description, statistical data and actual solutions and we decided not to use the stage name as linking key.

If you want to debug MTC plans, you should save the description/statistics topics anyway or debug from within the C++ code in which you run the Task.

[felixvd]

But for any kind of interleaved-execution or custom hooks, you would need [access to the Task] anyway. The only other way to support similar functionality is by additional information in the solution message, as we discuss right now.

I assumed that one should be able to execute the solution without knowing the task description, just like one can follow a manual without knowing all of the specifications and constraints that were needed to create the manual. I consider MTC a problem solving engine, so I would not expect to need the Task object (part of the solver) to execute the solution, and it seems reasonable to add information to the solution message. That's just my interpretation though!

The concept of properties is very general in MTC and you already modify the properties of stages to setup your problem. We could also go for Property[] external or external_properties, I would just like to avoid ambiguities with the Properties that are used to generate a specific solution (we do not export those), although maybe we should.

I'm not sure which properties are used where, and which properties are used to generate a "specific" solution rather than the returned message.

Properties can be boolean flags, but providing arbitrary data, e.g. to specify the pressure for each element in a suction array, is supported here, so "flags" is no good name.

Then maybe custom_properties? I know that technically everything can be customized, but the point is to convey that it's a setting the user should configure. When I read through MTC, I found it difficult to know what is essential to the framework (= "better not touch"), and what can/should be changed when making a new task. I would hope a name like this makes it easier to grok.

But then we would add redundant information to the messages and it also makes sense to avoid that. The information is now cleanly split between task description, statistical data and actual solutions and we decided not to use the stage name as linking key. If you want to debug MTC plans, you should save the description/statistics topics anyway or debug from within the C++ code in which you run the Task.

True, it would be somewhat redundant, which is why I had doubts about merging this. I guess it's a question of how self-contained you want the solution to be, and what users should be able to do with the solution message alone.

For completeness: a use case outside of debugging is to display the stage name to show what the robot is currently doing.

[AndyZe]

@v4hn I've tried something very similar to what you suggested above. You suggested this:

auto state = make_unique<Stage>();
auto* stage_ptr { &stage };
task.add(move(state))
task.init();
uint32_t stage_id = task.introspection().stageId(stage_ptr);

I tried this:

moveit::task_constructor::Stage* open_fingers_stage_;
...
// Note, I use moveit::task_constructor::stages::MoveTo rather than Stage
auto stage = std::make_unique<MoveTo>(
  PICK_STAGE_MAP.at(PickStageEnum::OPEN_FINGERS),
  sampling_planner);
stage->setGroup(finger_group_);
stage->setGoal(arm_prefix_ + "fingers_open");
open_fingers_stage_ = &stage;
task_.add(std::move(stage));

It doesn't compile because of the type difference.

 error: cannot convert ‘std::unique_ptr<moveit::task_constructor::stages::MoveTo, std::default_delete<moveit::task_constructor::stages::MoveTo> >*’ to ‘moveit::task_constructor::Stage*’ in assignment
  130 |     open_fingers_stage_ = &stage;

I believe the MoveTo type is correct, and I need the Stage* type to be able to retrieve the ID. So... I do not see a way around this.

Is there a way to convert MoveTo to Stage?

[rhaschke]

Obviously, stage is a unique_ptr. To get a raw address, you need to use stage.get()!

[AndyZe]

Thank you Robert!

I'm getting close but there is one more issue:

uint32_t stage_id = task_.introspection().stageId(open_fingers_stage_);

error: ‘uint32_t moveit::task_constructor::Introspection::stageId(const moveit::task_constructor::Stage*)’ is private within this context

[AndyZe]

Would you accept a PR to make stageId() public?

[rhaschke]

The const version of stageId() is public. Please don't hijack PRs / issues with unrelated questions, but open a separate thread instead.

[AndyZe]

Alright. I would not say the discussion is off-topic because it is related to accomplishing the purpose of this PR. It would be useful to anybody reading this PR.

Also, you said:

Obviously, stage is a unique_ptr

What isn't so obvious is that MoveTo inherits from Stage. If I read the header file, MoveTo inherits from PropagatingEitherWay. I guess there are several layers of inheritance, which can make it difficult to find things.

Edit, here's the final solution that works for me in retrieving a stage ID:

moveit::task_constructor::Stage* open_fingers_stage_;
...
    auto stage = std::make_unique<MoveTo>(
      PICK_STAGE_MAP.at(PickStageEnum::OPEN_FINGERS),
      sampling_planner);
    stage->setGroup(finger_group_);
    stage->setGoal(arm_prefix_ + "fingers_open");
    open_fingers_stage_ = stage.get();
    task_.add(std::move(stage));
...
  if (task_.plan()) {
    task_.introspection().publishSolution(*task_.solutions().front());
    RCLCPP_INFO(LOGGER, "Planning succeeded");
    const moveit::task_constructor::Introspection& const_introspection = task_.introspection();
    const uint32_t stage_id = const_introspection.stageId(open_fingers_stage_);
    RCLCPP_ERROR_STREAM(LOGGER, "OPEN FINGERS STAGE ID: " << stage_id);
    return true;
  }