CODEWITHSUNDEEP
pythontransformationframesros
ekptwtos
ekptwtos

Reputation: 101

ROS frame transformation (camera to base)

I am working with Baxter robot and what I am trying to do is get the position of an object using augmented reality markers and move the hand to that position in order to grasp it.

I am using the ar_tools package to get the position/orientation of the object, but that with respect to the head_camera which I am using. The last couple of days I've been experimenting with how to change that reference frame (head_camera) to the base frame as this frame is used by moveit to make the plans. I have tried to set the frame_id of the header of the message I receive from the ar_tools manually to 'base':

 pose_target.header.frame_id = 'base'

but the position and orientation I am getting are still WRT the head_camera. I also tried to do:

self.tl.lookupTransform("/base", "/head_camera", self.t)

where self.t = self.tl.getLatestCommonTime("/head_camera", "/base"), but I was getting an extrapolation error. It was something like

the transformation requires to extrapolate in the past

(I can't really remember now and I'm not in the lab.) Then I thought I might need to run the lookupTransform from the head_cam to head, from head to torso and from torso to Baxter's base.

Could someone guide me on how to change the frame of the marker of the ar_tools from head_camera to base?

Also, for the extrapolation error, is there a way to do this in a static way?

Upvotes: 3

Views: 6682

Answers (1)

aruisdante
aruisdante

Reputation: 9105

There is a slightly more straightforwards way to do this, presuming you're reviving a PoseStamped message from ar_tools:

on_received_pose(pose):
    '''
    Handler for your poses from ar_tools
    '''
    if self.tl.waitForTransform(pose.header.frame_id, "/base", pose.header.stamp, rospy.Duration(0.1)): # this line prevents your extrapolation error, it waits up to 0.1 seconds for the transform to become valid
        transd_pose = self.tl.transformPose("/base",pose)
        # do whatever with the transformed pose here
    else:
        rospy.logwarn('Couldn\'t Transform from "{}" to "/base"  before timeout. Are you updating TF tree sufficiently?'.format(pose.header.frame_id))

You're getting that extrapolation error likely because the transform network wasn't fully formed at the time you got your first message; tf refuses to extrapolate, it will only interpolate, so if you haven't received at least one transform message for every frame both before and after (or exactly at) the timestamp you're trying to transform to, it will throw an exception. That added if statement checks to see if it can actually perform the transform before trying to do so. You could of course also just surround the transformPose() call in a try/catch block instead, but I feel that for tf this makes it more explicit what you're trying to do.

In general, check out the simple ROS tf Python Tutorial for more examples/modes of operation.

Upvotes: 1

Related Questions