fixup! ComputeIK supports attached-object ik frame

rhaschke · rhaschke · commit 8408fba17d75 · 2021-11-06T20:56:05.000+01:00
Avoid double lookup of transform
diff --git a/core/src/stages/compute_ik.cpp b/core/src/stages/compute_ik.cpp
@@ -288,11 +288,12 @@ void ComputeIK::compute() {
 		Eigen::Isometry3d ik_pose;
 		tf2::fromMsg(ik_pose_msg.pose, ik_pose);
 
-		if (!scene->getCurrentState().knowsFrameTransform(ik_pose_msg.header.frame_id)) {
+		bool found = false;
+		ik_pose = scene->getCurrentState().getFrameTransform(ik_pose_msg.header.frame_id, &found) * ik_pose;
+		if (!found) {
 			ROS_WARN_STREAM_NAMED("ComputeIK", "ik frame unknown in robot: '" << ik_pose_msg.header.frame_id << "'");
 			return;
 		}
-		ik_pose = scene->getCurrentState().getFrameTransform(ik_pose_msg.header.frame_id) * ik_pose;
 
 #if MOVEIT_HAS_STATE_RIGID_PARENT_LINK
 		link = scene->getCurrentState().getRigidlyConnectedParentLinkModel(ik_pose_msg.header.frame_id);
