Prefixed frame being considered as a subframe results in failure

[mbusy]

Description

When using the plan or the compute_cartesian_path method of a moveit_commander.MoveGroupCommander object, I get the following error:

Pose frame 'ur5_robot/wrist_3_link' does not exist.

To add more context, I work with a model containing links that can be prefixed (eg ur5_robot/wrist_3_link instead of wrist_3_link).
This error arises when calling the plan method using a PoseStamped or compute_cartesian_path, in both cases after changing the end effector of the MoveGroupCommander (using set_end_effector_link) to a link within the group chain that is not the tip link.

I eventually tracked down what was responsible for that, and it has been brought by PR #2918 and #3388.

This first check is triggered when the end effector has been modified and now calls getRigidlyConnectedParentLinkModel, and fails in here as ur5_robot/wrist_3_link is assumed to be a subframe.

What would be the best way of updating this to be able to work with prefixed frames, without having them considered as subframes?

Your environment

• ROS Distro: [Noetic]
• OS Version: e.g. Ubuntu 20.04
• Commit a6f8bb9
• Brought by PR Generalize RobotState::setFromIK() #3388 and add RobotState::getRigidlyAttachedParentLink #2918

Steps to reproduce

• Use a robot description containing prefixed frame (eg ur5_robot/wrist_3_link instead of wrist_3_link)
• Create a moveit_commander.MoveGroupCommander using a specific move group
• Set the end effector of the moveit_commander.MoveGroupCommander to a different link than the tip link of the group chain
• Call the plan method with a PoseStamped or the compute_cartesian_path method

Expected behaviour

The methods should execute smoothly and consider the prefixed link as a standard link

Actual behaviour

The methods fail, throwing

Pose frame 'ur5_robot/wrist_3_link' does not exist.

(When 'ur5_robot/wrist_3_link' has been selected as the end effector, through the set_end_effector_link link method)

[welcome]

Thanks for reporting an issue. Because we're a volunteer community, providing a pull request with suggested changes is always welcomed.

[rhaschke]

Slashes are considered by MoveIt as a subframe separator. Thus, just use link names w/o slashes. Hence, you should use another last character for your prefix, e.g. replacing / with \ or :.