Fix RobotState::getRigidlyConnectedParentLinkModel() (#2985, #2993)

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.

moveit_core/robot_state/src/robot_state.cpp

@@ -801,26 +801,11 @@ void RobotState::updateStateWithLinkAt(const LinkModel* link, const Eigen::Isome
 
 const LinkModel* RobotState::getRigidlyConnectedParentLinkModel(const std::string& frame) const
 {
-  const moveit::core::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);
-
+  bool found;
+  const LinkModel* link{ nullptr };
+  getFrameInfo(frame, link, found);
+  if (!found)
+    RCLCPP_ERROR(LOGGER, "Unable to find link for frame '%s'", frame.c_str());
   return getRobotModel()->getRigidlyConnectedParentLinkModel(link);
 }
 

moveit_core/robot_state/test/robot_state_test.cpp

@@ -691,6 +691,7 @@ TEST_F(OneRobot, rigidlyConnectedParent)
   EXPECT_EQ(robot_model_->getRigidlyConnectedParentLinkModel(link_b), link_a);
 
   moveit::core::RobotState state(robot_model_);
+  state.updateLinkTransforms();
 
   EXPECT_EQ(state.getRigidlyConnectedParentLinkModel("link_b"), link_a);