ROS2 branch: difficulty in retrieving Stage ID

[AndyZe]

I'm having some trouble retrieving stage ID's. I've tried 4 different ways (attached).

For example, here I don't explicitly set the stage ID. When printed, all of the id fields are 0!

  {
    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");
    stage->setName(PICK_STAGE_MAP.at(PickStageEnum::OPEN_FINGERS));
    task_.add(std::move(stage));
  }

...

moveit_task_constructor_msgs::msg::Solution solution;
task_.solutions().front()->fillMessage(solution);
RCLCPP_INFO_STREAM(node->get_logger(), "Solution: " << moveit_task_constructor_msgs::msg::to_yaml(solution));

The other ways I tried (in brief) are:

• Using Stage::introspectionId() where the stage is a member variabe unique_ptr

• Setting the id explicity with task_.introspection().stageId(*open_fingers_stage_);. This does not compile.

• Using Stage::introspectionId() where the stage is a local unique_ptr

introspectionid_member_variable.txt
print_message.txt
set_introspection_id_explicitly.txt
stage_introspectionId.txt

[AndyZe]

If I do ros2 topic echo /solution, the stage_id field is populated.

[AndyZe]

I would like to have the id available when I create the task so a state machine can execute the stages individually (by ID).

[JafarAbdi]

I think you need to pass a pointer to the introspection when filling the message task_.solutions().front()->fillMessage(solution, &task_.introspection()); otherwise it's setting the id to zero https://github.com/ros-planning/moveit_task_constructor/blob/master/core/src/storage.cpp#L175

[rhaschke]

Exactly. The IDs were intended for communication with rviz only. If this is not required, the IDs stay zero.

[AndyZe]

Thanks @JafarAbdi. That does work for filling the message. It doesn't fix the bigger problem of how to correlate the sub-trajectories to a Stage ID somehow, so that the state machine can call the stages individually.

I see some stuff in remote_task_model.cpp that might help, like:

visualization/motion_planning_tasks/src/remote_task_model.cpp: auto it = id_to_stage_.find(stage_id);

I have to say that this API feels clunky. I guess that what I am trying is not common.

[rhaschke]

Hi Andy,
I think you are looking for something like #192/#194.
The MTC messages were primarily designed for internal communication between MTC and its rviz display. The stage_id is published once in the TaskDescription/StageDescription message together with clear-test names of stages. For efficiency, the Solution messages (which are much more frequently exchanged) don't comprise this info anymore, but only the stage_id.

[AndyZe]

Yep, Felix's idea is what I'm looking for. Thanks.