Prevent moveit_servo transforms between fixed frames from causing tim…

…eout (moveit#2418)
tylerjw · Apr 29, 2021 · f59b399 · f59b399
1 parent 54857b6
commit f59b399
Showing 1 changed file with 3 additions and 0 deletions.
diff --git a/moveit_ros/moveit_servo/src/pose_tracking.cpp b/moveit_ros/moveit_servo/src/pose_tracking.cpp
@@ -238,6 +238,9 @@ void PoseTracking::targetPoseCallback(const geometry_msgs::PoseStampedConstPtr&
       geometry_msgs::TransformStamped target_to_planning_frame = transform_buffer_.lookupTransform(
           planning_frame_, target_pose_.header.frame_id, ros::Time(0), ros::Duration(0.1));
       tf2::doTransform(target_pose_, target_pose_, target_to_planning_frame);
+
+      // Prevent doTransform from copying a stamp of 0, which will cause the haveRecentTargetPose check to fail servo motions
+      target_pose_.header.stamp = ros::Time::now();
     }
     catch (const tf2::TransformException& ex)
     {