Description

As of #3077, RobotState::setFromIK() should allow subframe names as the tip. This does not appear to have fixed the issue however.

ROS Distro

Humble + #3077

OS and version

Ubuntu 22.04 (Docker)

Source or binary build?

Source

If binary, which release version?

N/A

If source, which branch?

humble + #3077

Which RMW are you using?

FastRTPS

Steps to Reproduce

1. Load a robot
2. Attach an object to the robot
3. Associate a subframe with the object
4. Call RobotState::setFromIK() with the subframe name as the tip

Expected behavior

The call should succeed. This is because with #3077, the chain of calls is now setFromIK() -> getRigidlyConnectedParentLinkModel() -> getFrameInfo(). The latter function appears to include a search for subframes.

Actual behavior

The call fails with the errors given below. The first error message suggests that getFrameInfo() is returning false and the second message suggests that this is occuring when querying the pose frame

Backtrace or Console output

[move_group-2] [ERROR] [1731596998.921984677] [moveit_robot_state.robot_state]: Unable to find link for frame 'tb_9_3-f686742d-b737-43b8-a712-5318e3f05abc/disc_centre'
[move_group-2] [ERROR] [1731596998.921987769] [moveit_robot_state.robot_state]: The following Pose Frame does not exist: tb_9_3-f686742d-b737-43b8-a712-5318e3f05abc/disc_centre