Fix RobotState::getRigidlyConnectedParentLinkModel() (#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.

(cherry picked from commit 1f23344)

# Conflicts:
#	moveit_core/robot_state/src/robot_state.cpp

Author: rhaschke

moveit_core/robot_state/src/robot_state.cpp

@@ -801,6 +801,7 @@ void RobotState::updateStateWithLinkAt(const LinkModel* link, const Eigen::Isome
 
 const LinkModel* RobotState::getRigidlyConnectedParentLinkModel(const std::string& frame) const
 {
+<<<<<<< HEAD
   const moveit::core::LinkModel* link{ nullptr };
 
   size_t idx = 0;
@@ -821,6 +822,13 @@ const LinkModel* RobotState::getRigidlyConnectedParentLinkModel(const std::strin
   else if (getRobotModel()->hasLinkModel(frame))
     link = getLinkModel(frame);
 
+=======
+  bool found;
+  const LinkModel* link{ nullptr };
+  getFrameInfo(frame, link, found);
+  if (!found)
+    RCLCPP_ERROR(getLogger(), "Unable to find link for frame '%s'", frame.c_str());
+>>>>>>> 1f23344de (Fix RobotState::getRigidlyConnectedParentLinkModel() (#2985))
   return getRobotModel()->getRigidlyConnectedParentLinkModel(link);
 }