[WIP] Quick access of stages by id/index

[captain-yoshi]

This is somewhat related to #194. For pre-execution purposes.

We can already achieve this without any modification to the current code. This PR is just for demonstration.

1- Create a flat buffer of stages in BFS/DFS order 2- The indexes represents (minus 1) the given stage_id from SolutionInfo.msg 3- Use the stage_id to get the stage from the vector directly in O(1) complexity 4- You can retrieve your properties

I think the stage_id is primarily used for RViz, thus instropection must be enabled.

task->fillFlatBufferStages();
task->plan();

...

// introspection needed to populate message
moveit_task_constructor_msgs::Solution solution_msg;
task.solutions().front()->toMsg(solution_msg, &task.introspection());

for (std::size_t i = 0; i < solution_msg.sub_trajectory.size(); ++i)
{
    auto& sub_trajectory = solution_msg.sub_trajectory[i];

    uint32_t id = sub_trajectory.info.id;
    uint32_t stage_id = sub_trajectory.info.stage_id;

    // retrieve stage
    const moveit::task_constructor::Stage* stage = task.stages(stage_id - 1);
    if(!stage)
        throw std::runtime_error("explosion")

    // retrieve properties
    int my_property = stage->properties().get<int>("my-awesome-property")
}

---

Would there be any interest in adding this sort of behavior inside MTC ?

If yes then:

• Could be always on, or enabled by the user.
• Must work without introspection, needs a new ID ?

If no:

• Must work without introspection, needs a new ID ?

[codecov-commenter]

:warning: Please install the to ensure uploads and comments are reliably processed by Codecov.

Codecov Report

Attention: Patch coverage is 0% with 12 lines in your changes missing coverage. Please review.

Project coverage is 57.91%. Comparing base (6b0f2c8) to head (0562494). Report is 47 commits behind head on master.

Files with missing lines	Patch %	Lines
core/src/task.cpp	0.00%	12 Missing :warning:

:exclamation: Your organization needs to install the Codecov GitHub app to enable full functionality.

Additional details and impacted files

```diff @@ Coverage Diff @@ ## master #626 +/- ## ========================================== - Coverage 58.82% 57.91% -0.90% ========================================== Files 91 132 +41 Lines 8623 11119 +2496 Branches 0 957 +957 ========================================== + Hits 5072 6439 +1367 - Misses 3551 4633 +1082 - Partials 0 47 +47 ```

:umbrella: View full report in Codecov by Sentry.
:loudspeaker: Have feedback on the report? Share it here.

[rhaschke]

As far as I can see, this PR attempts to provide access to a Stage* from the stage_id field in a SolutionInfo.msg. While the suggested approach (of filling a lookup vector) might currently work, it seems to be very brittle. If such functionality is desired, a corresponding lookup function should be added to Introspection instead. As solutions are processed/executed external to the generating MTC node, adding properties to the solution seems more reasonable to me.

[captain-yoshi]

Agreed that the current approach is best. But I think it cannot serialise complex objects stored in a property, e.x. std::function ?

Even though I could change my properties to be serialisable, is there a way to loop through the solution internally without even needing the lookup vector ? Or it would be too complicated.

// 1- current way using Msg
moveit_task_constructor_msgs::Solution solution_msg;
task.solutions().front()->toMsg(solution_msg, &task.introspection());

// 2- using directly the SolutionBase
auto solution = task.solutions().front();

// example
for(sub_solution : solution) {
    stage = sub_solution.stage;
    sub_trajectory = sub_solution.sub_trajectory;
}

[rhaschke]

Is there a way to loop through the solution internally without even needing the lookup vector ?

In principle, you could inspect the SolutionBase directly. However, you need to use dynamic casts to determine the actual type. Message generation relies on virtual functions.

[captain-yoshi]

Will probably just change my approach with post-planning. It will scale better. Thanks for the answers.