handeye_calibration.py

https://github.com/IFL-CAMP/easy_handeye/blob/master/easy_handeye/src/easy_handeye/handeye_calibration.py

import os
import yaml
import rospy
from geometry_msgs.msg import Vector3, Quaternion, Transform, TransformStamped

class HandeyeCalibration(object):
    #用来存储手眼标定中的参数和变换
    DIRECTORY = os.path.expanduser('~/.ros/easy_handeye')
   #标定配置文件的默认存储位置
    def __init__(self,
                 eye_on_hand=False,#bool
                 robot_base_frame=None,#string
                 robot_effector_frame=None,#string
                 tracking_base_frame=None,#string
                 transformation=None):#((float, float, float), (float, float, float, float))
        """
        Creates a HandeyeCalibration object.
        :param eye_on_hand: if false, it is a eye-on-base calibration
        :param robot_base_frame: needed only for eye-on-base calibrations: robot base link tf name
        :param robot_effector_frame: needed only for eye-on-hand calibrations: robot tool tf name
        :param tracking_base_frame: tracking system tf name
        :param transformation: transformation between optical origin and base/tool robot frame as tf tuple
        :return: a HandeyeCalibration object
        :rtype: easy_handeye.handeye_calibration.HandeyeCalibration*
        """
        if transformation is None:
            transformation = ((0, 0, 0), (0, 0, 0, 1))
        self.eye_on_hand = eye_on_hand
        self.transformation = TransformStamped(transform=Transform(
            Vector3(*transformation[0]), Quaternion(*transformation[1])))
        if self.eye_on_hand:
            self.transformation.header.frame_id = robot_effector_frame
        else:
            self.transformation.header.frame_id = robot_base_frame
        self.transformation.child_frame_id = tracking_base_frame
        self.filename = HandeyeCalibration.DIRECTORY + '/' + rospy.get_namespace().rstrip('/').split('/')[-1] + '.yaml'

    def to_dict(self):
        """
        Returns a dictionary representing this calibration. 
        dictionary是python中的一种数据结构，每个字典元素都有键（key）和值（value）两个属性，键用于定义和标识字典元素，可以是字符串或者整数；值是字典元素对应的值。
        :return: a dictionary representing this calibration.
        :rtype: dict[string, string|dict[string,float]]
        """
        ret = {
            'eye_on_hand': self.eye_on_hand,
            'tracking_base_frame': self.transformation.child_frame_id,
            'transformation': {
                'x': self.transformation.transform.translation.x,
                'y': self.transformation.transform.translation.y,
                'z': self.transformation.transform.translation.z,
                'qx': self.transformation.transform.rotation.x,
                'qy': self.transformation.transform.rotation.y,
                'qz': self.transformation.transform.rotation.z,
                'qw': self.transformation.transform.rotation.w
            }
        }#定义为ret的字典
        if self.eye_on_hand:
            ret['robot_effector_frame'] = self.transformation.header.frame_id
        else:
            ret['robot_base_frame'] = self.transformation.header.frame_id
  	#直接在字典里添加元素 robot_effector_frame 或者 robot_base_frame
        return ret
	#所以这个函数的作用就是定义ret这个字典。
	
    def from_dict(self, in_dict):
        """
        Sets values parsed from a given dictionary.
        :param in_dict: input dictionary.
        :type in_dict: dict[string, string|dict[string,float]]
        :rtype: None
        """
        self.eye_on_hand = in_dict['eye_on_hand']
        self.transformation = TransformStamped(
            child_frame_id=in_dict['tracking_base_frame'],
            transform=Transform(
                Vector3(in_dict['transformation']['x'],
                        in_dict['transformation']['y'],
                        in_dict['transformation']['z']),
                Quaternion(in_dict['transformation']['qx'],
                           in_dict['transformation']['qy'],
                           in_dict['transformation']['qz'],
                           in_dict['transformation']['qw'])
            )
        )
        if self.eye_on_hand:
            self.transformation.header.frame_id = in_dict['robot_effector_frame']
        else:
            self.transformation.header.frame_id = in_dict['robot_base_frame']
	#感觉像是拷贝构造函数的意思....对ret进行重新赋值
	
    def to_yaml(self):
        """
        Returns a yaml string representing this calibration.
        :return: a yaml string
        :rtype: string
        """
        return yaml.dump(self.to_dict())

    def from_yaml(self, in_yaml):
        """
        Parses a yaml string and sets the contained values in this calibration.
        :param in_yaml: a yaml string
        :rtype: None
        """
        self.from_dict(yaml.load(in_yaml))

    def to_file(self):
        """
        Saves this calibration in a yaml file in the default path.
        The default path consists of the default directory and the namespace the node is running in.
        :rtype: None
        """
        if not os.path.exists(HandeyeCalibration.DIRECTORY):
            os.makedirs(HandeyeCalibration.DIRECTORY)

        with open(self.filename, 'w') as calib_file:
            calib_file.write(self.to_yaml())
	#with open() as的用法是在文件处理中获取一个文件句柄，从文件中读取数据，然后关闭文件句柄。
	#函数的作用是在文件中写入参数
	
    def from_file(self):
        """
        Parses a yaml file in the default path and sets the contained values in this calibration.
        The default path consists of the default directory and the namespace the node is running in.
        :rtype: None
        """
        with open(self.filename) as calib_file:
            self.from_yaml(calib_file.read())
            #函数的作用是从文件中读入参数
         

    def from_parameters(self):
        """
        Stores this calibration as ROS parameters in the namespace of the current node.
        :rtype: None
        """
        calib_dict = {}
        root_params = ['eye_on_hand', 'tracking_base_frame']
        for rp in root_params:
            calib_dict[rp] = rospy.get_param(rp)
        if calib_dict['eye_on_hand']:
            calib_dict['robot_effector_frame'] = rospy.get_param('robot_effector_frame')
        else:
            calib_dict['robot_base_frame'] = rospy.get_param('robot_base_frame')
        transf_params = 'x', 'y', 'z', 'qx', 'qy', 'qz', 'qw'
        calib_dict['transformation'] = {}
        for tp in transf_params:
            calib_dict['transformation'][tp] = rospy.get_param('transformation/'+tp)
        self.from_dict(calib_dict)

    def to_parameters(self):
        """
        Fetches a calibration from ROS parameters in the namespace of the current node into this object.
        :rtype: None
        """
        calib_dict = self.to_dict()
        root_params = ['eye_on_hand', 'tracking_base_frame']
        if calib_dict['eye_on_hand']:
            root_params.append('robot_effector_frame')
        else:
            root_params.append('robot_base_frame')

        for rp in root_params:
            rospy.set_param(rp, calib_dict[rp])

        transf_params = 'x', 'y', 'z', 'qx', 'qy', 'qz', 'qw'

        for tp in transf_params:
	rospy.set_param('transformation/'+tp, calib_dict['transformation'][tp])

handeye_calibrator.py

https://github.com/IFL-CAMP/easy_handeye/blob/master/easy_handeye/src/easy_handeye/handeye_calibrator.py

import rospy
import tf
from tf import transformations as tfs
from geometry_msgs.msg import Vector3, Quaternion, Transform
from visp_hand2eye_calibration.msg import TransformArray
from visp_hand2eye_calibration.srv import compute_effector_camera_quick
from easy_handeye.handeye_calibration import HandeyeCalibration


class HandeyeCalibrator(object):
    """
    Connects tf and ViSP hand2eye to provide an interactive mean of calibration.
    """

    MIN_SAMPLES = 2  
    # TODO: correct? this is what is stated in the paper, but sounds strange
    #Minimum samples required for a successful calibration.

    def __init__(self):
        self.eye_on_hand = rospy.get_param('eye_on_hand', False)
        self.robot_effector_frame = rospy.get_param('robot_effector_frame', 'tool0')
        self.robot_base_frame = rospy.get_param('robot_base_frame', 'base_link')
        self.tracking_base_frame = rospy.get_param('tracking_base_frame', 'optical_origin')
        self.tracking_marker_frame = rospy.get_param('tracking_marker_frame', 'optical_target')
        self.listener = tf.TransformListener()
        self.broadcaster = tf.TransformBroadcaster()
        self.transformer = tf.TransformerROS()
        self.samples = []
        # calibration service
        rospy.wait_for_service('compute_effector_camera_quick')
        self.calibrate = rospy.ServiceProxy( 'compute_effector_camera_quick', compute_effector_camera_quick)
        

    def _wait_for_tf_init(self):
        """
        Waits until all needed frames are present in tf.
        :rtype: None
        """
        self.listener.waitForTransform(self.robot_base_frame, self.robot_effector_frame, rospy.Time(0), rospy.Duration(10))
        self.listener.waitForTransform(self.tracking_base_frame, self.tracking_marker_frame, rospy.Time(0), rospy.Duration(60))

    def _wait_for_transforms(self):
        """
        Waits until the needed transformations are recent in tf.
        :rtype: rospy.Time
        """
        now = rospy.Time.now()
        self.listener.waitForTransform(self.robot_base_frame, self.robot_effector_frame, now, rospy.Duration(10))
        self.listener.waitForTransform(self.tracking_base_frame, self.tracking_marker_frame, now, rospy.Duration(10))
        return now

    def _get_transforms(self, time=None):
        """
        Samples the transforms at the given time.
        :param time: sampling time (now if None)
        :type time: None|rospy.Time
        :rtype: dict[str, ((float, float, float), (float, float, float, float))]
        """
        if time is None:
            time = self._wait_for_transforms()

        # here we trick the library (it is actually made for eye_on_hand only). Trust me, I'm an engineer
        if self.eye_on_hand:
            rob = self.listener.lookupTransform(self.robot_base_frame, self.robot_effector_frame, time)
        else:
            rob = self.listener.lookupTransform(self.robot_effector_frame, self.robot_base_frame, time)
        opt = self.listener.lookupTransform(self.tracking_base_frame, self.tracking_marker_frame, time)
        return {'robot': rob, 'optical': opt}

    def take_sample(self):
        """
        Samples the transformations and appends the sample to the list.
        对转换进行采样并将样本附加到列表中。
        :rtype: None
        """
        rospy.loginfo("Taking a sample...")
        transforms = self._get_transforms()
        rospy.loginfo("Got a sample")
        self.samples.append(transforms)

    def remove_sample(self, index):
        """
        Removes a sample from the list.
        :type index: int
        :rtype: None
        """
        if 0 <= index < len(self.samples):
            del self.samples[index]

    @staticmethod
    def _tuple_to_msg_transform(tf_t):
        """
        Converts a tf tuple into a geometry_msgs/Transform message
        将tf元组转换为geometry_msgs / Transform消息
        :type tf_t: ((float, float, float), (float, float, float, float))
        :rtype: geometry_msgs.msg.Transform
        """
        transl = Vector3(*tf_t[0])
        rot = Quaternion(*tf_t[1])
        return Transform(transl, rot)

    def get_visp_samples(self):
        """
        Returns the sample list as a TransformArray.
        :rtype: visp_hand2eye_calibration.msg.TransformArray
        """
        hand_world_samples = TransformArray()
        #hand_world_samples.header.frame_id = self.robot_base_frame
        hand_world_samples.header.frame_id = self.tracking_base_frame
        # DONTFIXME: yes, I know, it should be like the line above.
        # thing is, otherwise the results of the calibration are wrong. don't ask me why

        camera_marker_samples = TransformArray()
        camera_marker_samples.header.frame_id = self.tracking_base_frame

        for s in self.samples:
            to = HandeyeCalibrator._tuple_to_msg_transform(s['optical'])
            camera_marker_samples.transforms.append(to)
            tr = HandeyeCalibrator._tuple_to_msg_transform(s['robot'])
            hand_world_samples.transforms.append(tr)

        return hand_world_samples, camera_marker_samples

    def compute_calibration(self):
        """
        Computes the calibration through the ViSP service and returns it.
        :rtype: easy_handeye.handeye_calibration.HandeyeCalibration
        """
        if len(self.samples) < HandeyeCalibrator.MIN_SAMPLES:
            rospy.logwarn("{} more samples needed! Not computing the calibration".format(
                HandeyeCalibrator.MIN_SAMPLES - len(self.samples)))
            return

        # Update data
        hand_world_samples, camera_marker_samples = self.get_visp_samples()

        if len(hand_world_samples.transforms) != len(camera_marker_samples.transforms):
            rospy.logerr("Different numbers of hand-world and camera-marker samples!")
            raise AssertionError

        rospy.loginfo("Computing from %g poses..." % len(self.samples))

        try:
            result = self.calibrate(camera_marker_samples, hand_world_samples)
            rospy.loginfo("Computed calibration: {}".format(str(result)))
            transl = result.effector_camera.translation
            rot = result.effector_camera.rotation
            result_tuple = ((transl.x, transl.y, transl.z),
                            (rot.x, rot.y, rot.z, rot.w))

            ret = HandeyeCalibration(self.eye_on_hand,
                                     self.robot_base_frame,
                                     self.robot_effector_frame,
                                     self.tracking_base_frame,
                                     result_tuple)

            return ret

        except rospy.ServiceException as ex:
            rospy.logerr("Calibration failed: " + str(ex))
return None

关于@staticmethod的说明
一般来说，要使用某个类的方法，需要先实例化一个对象再调用方法。staticmethod不需要表示自身对象的self和自身类的cls参数，就跟使用函数一样。而@classmethod因为持有cls参数，可以来调用类的属性，类的方法，实例化对象等，避免硬编码。
而使用@staticmethod或@classmethod，就可以不需要实例化，直接类名.方法名()来调用。 这有利于组织代码，把某些应该属于某个类的函数给放到那个类里去，同时有利于命名空间的整洁。如果在@staticmethod中要调用到这个类的一些属性方法，只能直接类名.属性名或类名.方法名。

参考博客：https://blog.csdn.net/handsomekang/article/details/9615239