Fix RobotState::getRigidlyConnectedParentLinkModel() (moveit#2985)

Simplify function using getFrameInfo() to yield the link corresponding to the given frame.

The order of frame resolution was wrong here:
If a link frame containing a slash was given, the code was expecting an attached body with a subframe.
As these didn't exist, the function falsely returned NULL.

Author: rhaschke

moveit_core/robot_state/src/robot_state.cpp

@@ -909,26 +909,11 @@ void RobotState::updateStateWithLinkAt(const LinkModel* link, const Eigen::Isome
 
 const LinkModel* RobotState::getRigidlyConnectedParentLinkModel(const std::string& frame) const
 {
+  bool found;
   const LinkModel* link{ nullptr };
-
-  size_t idx = 0;
-  if ((idx = frame.find('/')) != std::string::npos)
-  {  // resolve sub frame
-    std::string object{ frame.substr(0, idx) };
-    if (!hasAttachedBody(object))
-      return nullptr;
-    auto body{ getAttachedBody(object) };
-    if (!body->hasSubframeTransform(frame))
-      return nullptr;
-    link = body->getAttachedLink();
-  }
-  else if (hasAttachedBody(frame))
-  {
-    link = getAttachedBody(frame)->getAttachedLink();
-  }
-  else if (getRobotModel()->hasLinkModel(frame))
-    link = getLinkModel(frame);
-
+  getFrameInfo(frame, link, found);
+  if (!found)
+    RCLCPP_ERROR(getLogger(), "Unable to find link for frame '%s'", frame.c_str());
   return getRobotModel()->getRigidlyConnectedParentLinkModel(link);
 }