Prevent moveit_servo transforms between fixed frames from causing timeout #2418
Conversation
…causing target_pose timeout If target_pose must be transformed to be in planning_frame, and a URDF transform from the current frame to planning_frame is available, target_pose's timestamp was updated to 0, causing timeout aborts
Codecov Report
@@ Coverage Diff @@
## master #2418 +/- ##
==========================================
+ Coverage 60.22% 60.58% +0.37%
==========================================
Files 351 351
Lines 26434 26435 +1
==========================================
+ Hits 15916 16013 +97
+ Misses 10518 10422 -96
Continue to review full report at Codecov.
|
| @@ -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 = msg->header.stamp; | |||
There was a problem hiding this comment.
Since the target pose and planning frame are guaranteed to be different frames and transform between the frames is the latest possible (since ros::Time(0) is used), there are two cases here.
Either the transform is fixed or the transform is one between two moving frames. However, in both cases, the latest transform is used and the target pose is transformed into the planning frame. So, after the transform, isn't the situation equivalent to us having a target pose in the planning frame with a timestamp of ros::Time::now()?
Suppose we had a target pose in odom at timestamp 10, and we received it at timestamp 100 and transformed it to base_link with the latest transform, wouldn't the correct timestamp for the pose be 100 instead of 10? Probably doesn't make a world of difference... perhaps it doesn't make any difference, but it feels like it is more correct nonetheless.
There was a problem hiding this comment.
That is a good point. I don't think it will have impact here, but is the pattern people should follow elsewhere where it could make an impact.
Description
Currently, if target_pose must be transformed to be in planning_frame (in
targetPoseCallbackfunction), and a URDF transform from the current frame to planning_frame is available, then TransformStamped time returned by lookupTransform will be 0. 0 is then copied into target_pose_'s stamp by doTransform. This will result in the timestamp checking inhaveRecentTargetPoseto causemoveToPoseto always abort because it doesn't think it has received a target_pose recently.Two examples of when users could experience this behavior:
target_posein world frame, butplanning_frameis configured to bebase_link(and world to base_link is defined in the URDF)Solution:
I don't think we can assume that a TransformStamped timestamp of 0 will always means that it was a URDF defined transform. So instead of checking for that, I am simply copying back the target_pose's original timestamp back if a transform has to be applied. I think this is fine as the purpose of the timestamp checking in
haveRecentTargetPoseis to see if the pose was received recently, rather than how recent the transform is. The TransformException catch will still flag failures to get any transform in the provided time window.This work is sponsored by RE2 Robotics.
Checklist