Transformation de trame ROS (caméra à base)

Question

Je travaille avec le robot Baxter et j'essaie de trouver la position d'un objet en utilisant des marqueurs de réalité augmentée et de déplacer la main dans cette position pour la saisir.

J'utilise le paquet ar_tools pour obtenir la position/l'orientation de l'objet, mais cela par rapport au head_camera que j'utilise. Les deux derniers jours, j'ai expérimenté avec la façon de changer ce cadre de référence (head_camera) à l'image de base que ce cadre est utilisé par moveit pour faire les plans. J'ai essayé de régler le frame_id de l'en-tête du message que je reçois de la ar_tools manuellement 'base':

pose_target.header.frame_id = 'base' 

mais la position et l'orientation que je reçois sont toujours le WRT head_camera. J'ai aussi essayé de le faire:

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

où self.t = self.tl.getLatestCommonTime("/head_camera", "/base"), mais je recevais une erreur d'extrapolation. Il était quelque chose comme

la transformation nécessite d'extrapoler dans le passé

(je ne me souviens pas maintenant et je ne suis pas dans le laboratoire.) Je pensais que je pourrais avoir besoin de courir le lookupTransform du head_cam au head, du head au torso et du torso au base de Baxter.

Quelqu'un pourrait me guider sur la façon de changer le cadre du marqueur de ar_tools de head_camera à base?

De même, pour l'erreur d'extrapolation, existe-t-il un moyen de le faire de manière statique?

Answer 1

Il y a un peu plus straightforwards façon de le faire, en supposant que vous êtes faire revivre un message PoseStamped de 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)) 

Vous obtenez cette erreur d'extrapolation probablement parce que le réseau transformation n'a pas été complètement formé au moment où vous avez reçu votre premier message; tf refuse d'extrapoler, il ne fera qu'interpoler, donc si vous n'avez pas reçu au moins un message de transformation pour chaque image avant et après (ou exactement à) l'horodatage que vous essayez de transformer, il lèvera une exception. Cela a ajouté if instruction vérifie pour voir si elle peut réellement effectuer la transformation avant d'essayer de le faire. Bien sûr, vous pouvez aussi simplement entourer l'appel transformPose() dans un bloc try/catch, mais je pense que pour tf cela rend plus explicite ce que vous essayez de faire.

En général, consultez le ROS tf Python Tutorial simple pour plus d'exemples/modes de fonctionnement.