diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp index 91810f6f0e..0d96e1755c 100644 --- a/moveit_core/robot_state/src/robot_state.cpp +++ b/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); } diff --git a/moveit_core/robot_state/test/robot_state_test.cpp b/moveit_core/robot_state/test/robot_state_test.cpp index a102e49ee2..8d8fe4ce81 100644 --- a/moveit_core/robot_state/test/robot_state_test.cpp +++ b/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);