[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]

⚠️ 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 ⚠️

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

Additional details and impacted files

@@            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     

☔ View full report in Codecov by Sentry.
📢 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.