def main():
    rospy.init_node('easy_handeye')
    while rospy.get_time() == 0.0:
        pass

    print('Handeye Calibration Commander')
    print('connecting to: {}'.format((rospy.get_namespace().strip('/'))))

    cmder = HandeyeCalibrationCommander()

    if cmder.client.eye_on_hand:
        print('eye-on-hand calibration')
        print('robot effector frame: {}'.format(cmder.client.robot_effector_frame))
    else:
        print('eye-on-base calibration')
        print('robot base frame: {}'.format(cmder.client.robot_base_frame))
    print('tracking base frame: {}'.format(cmder.client.tracking_base_frame))
    print('tracking target frame: {}'.format(cmder.client.tracking_marker_frame))

    cmder.spin_interactive() 

def publish_to_ros(self, mode='transform', service_name='rigid_transforms/rigid_transform_publisher', namespace=None):
        """Publishes RigidTransform to ROS
        If a transform referencing the same frames already exists in the ROS publisher, it is updated instead.
        This checking is not order sensitive
        
        Requires ROS rigid_transform_publisher service to be running. Assuming autolab_core is installed as a catkin package,
        this can be done with: roslaunch autolab_core rigid_transforms.launch
        
        Parameters
        ----------
        mode : :obj:`str`
            Mode in which to publish. In {'transform', 'frame'}
            Defaults to 'transform'
        service_name : string, optional
            RigidTransformPublisher service to interface with. If the RigidTransformPublisher services are started through
            rigid_transforms.launch it will be called rigid_transform_publisher
        namespace : string, optional
            Namespace to prepend to transform_listener_service. If None, current namespace is prepended.
        
        Raises
        ------
        rospy.ServiceException
            If service call to rigid_transform_publisher fails
        """
        if namespace == None:
            service_name = rospy.get_namespace() + service_name
        else:
            service_name = namespace + service_name
        
        rospy.wait_for_service(service_name, timeout = 10)
        publisher = rospy.ServiceProxy(service_name, RigidTransformPublisher)
        
        trans = self.translation
        rot = self.quaternion
        
        publisher(trans[0], trans[1], trans[2], rot[0], rot[1], rot[2], rot[3], self.from_frame, self.to_frame, mode) 

def delete_from_ros(self, service_name='rigid_transforms/rigid_transform_publisher', namespace=None):
        """Removes RigidTransform referencing from_frame and to_frame from ROS publisher.
        Note that this may not be this exact transform, but may that references the same frames (order doesn't matter)
        
        Also, note that it may take quite a while for the transform to disappear from rigid_transform_publisher's cache 
        
        Requires ROS rigid_transform_publisher service to be running. Assuming autolab_core is installed as a catkin package,
        this can be done with: roslaunch autolab_core rigid_transforms.launch
        
        Parameters
        ----------
        service_name : string, optional
            RigidTransformPublisher service to interface with. If the RigidTransformPublisher services are started through
            rigid_transforms.launch it will be called rigid_transform_publisher
        namespace : string, optional
            Namespace to prepend to transform_listener_service. If None, current namespace is prepended.
        
        Raises
        ------
        rospy.ServiceException
            If service call to rigid_transform_publisher fails
        """
        if namespace == None:
            service_name = rospy.get_namespace() + service_name
        else:
            service_name = namespace + service_name
            
        rospy.wait_for_service(service_name, timeout = 10)
        publisher = rospy.ServiceProxy(service_name, RigidTransformPublisher)
        
        publisher(0, 0, 0, 0, 0, 0, 0, self.from_frame, self.to_frame, 'delete') 

def rigid_transform_from_ros(from_frame, to_frame, service_name='rigid_transforms/rigid_transform_listener', namespace=None):
        """Gets transform from ROS as a rigid transform
        
        Requires ROS rigid_transform_publisher service to be running. Assuming autolab_core is installed as a catkin package,
        this can be done with: roslaunch autolab_core rigid_transforms.launch
        
        Parameters
        ----------
        from_frame : :obj:`str`
        to_frame : :obj:`str`
        service_name : string, optional
            RigidTransformListener service to interface with. If the RigidTransformListener services are started through
            rigid_transforms.launch it will be called rigid_transform_listener
        namespace : string, optional
            Namespace to prepend to transform_listener_service. If None, current namespace is prepended.
        
        Raises
        ------
        rospy.ServiceException
            If service call to rigid_transform_listener fails
        """
        if namespace == None:
            service_name = rospy.get_namespace() + service_name
        else:
            service_name = namespace + service_name
        
        rospy.wait_for_service(service_name, timeout = 10)
        listener = rospy.ServiceProxy(service_name, RigidTransformListener)
        
        ret = listener(from_frame, to_frame)
        
        quat = np.asarray([ret.w_rot, ret.x_rot, ret.y_rot, ret.z_rot])
        trans = np.asarray([ret.x_trans, ret.y_trans, ret.z_trans])
        
        rot = RigidTransform.rotation_from_quaternion(quat)
        
        return RigidTransform(rotation=rot, translation=trans, from_frame=from_frame, to_frame=to_frame) 

def cb_h264_frame(self, event, sender, data, **args):
        frame, seq_id, frame_secs = data
        pkt_msg = H264Packet()
        pkt_msg.header.seq = seq_id
        pkt_msg.header.frame_id = rospy.get_namespace()
        pkt_msg.header.stamp = rospy.Time.from_sec(frame_secs)
        pkt_msg.data = frame
        self.pub_image_h264.publish(pkt_msg) 

def framegrabber_loop(self):
        # Repeatedly try to connect
        vs = self.get_video_stream()
        while self.state != self.STATE_QUIT:
            try:
                container = av.open(vs)
                break
            except BaseException as err:
                rospy.logerr('fgrab: pyav stream failed - %s' % str(err))
                time.sleep(1.0)

        # Once connected, process frames till drone/stream closes
        while self.state != self.STATE_QUIT:
            try:
                # vs blocks, dies on self.stop
                for frame in container.decode(video=0):
                    img = np.array(frame.to_image())
                    try:
                        img_msg = self.bridge.cv2_to_imgmsg(img, 'rgb8')
                        img_msg.header.frame_id = rospy.get_namespace()
                    except CvBridgeError as err:
                        rospy.logerr('fgrab: cv bridge failed - %s' % str(err))
                        continue
                    self.pub_image_raw.publish(img_msg)
                break
            except BaseException as err:
                rospy.logerr('fgrab: pyav decoder failed - %s' % str(err)) 

def __init__(self, arm_service, namespace = None, timeout = YMC.ROS_TIMEOUT):
        if namespace == None:
            self.arm_service = rospy.get_namespace() + arm_service
        else:
            self.arm_service = namespace + arm_service

        self.timeout = timeout 

def __init__(self, depth_image_buffer= None, depth_absolute=False, color_image_buffer=None, color_absolute=False,
                 flip_images=True, frame=None, staleness_limit=10., timeout=10):  
        import rospy
        from rospy import numpy_msg
        from perception.srv import ImageBufferResponse
        ImageBufferResponse = rospy.numpy_msg.numpy_msg(ImageBufferResponse)
        ImageBuffer._response_class = ImageBufferResponse
    
        self._flip_images = flip_images
        self._frame = frame
        
        self.staleness_limit = staleness_limit
        self.timeout = timeout

        if self._frame is None:
            self._frame = 'primesense'
        self._color_frame = '%s_color' %(self._frame)
        self._ir_frame = self._frame # same as color since we normally use this one
        
        # Set image buffer locations
        self._depth_image_buffer = ('{0}/depth/stream_image_buffer'.format(frame)
                                    if depth_image_buffer == None else depth_image_buffer)
        self._color_image_buffer = ('{0}/rgb/stream_image_buffer'.format(frame)
                                    if color_image_buffer == None else color_image_buffer)
        if not depth_absolute:
            self._depth_image_buffer = rospy.get_namespace() + self._depth_image_buffer
        if not color_absolute:
            self._color_image_buffer = rospy.get_namespace() + self._color_image_buffer 

def __init__(self,
                 eye_on_hand=False,
                 robot_base_frame=None,
                 robot_effector_frame=None,
                 tracking_base_frame=None,
                 transformation=None):
        """
        Creates a HandeyeCalibration object.

        :param eye_on_hand: if false, it is a eye-on-base calibration
        :type eye_on_hand: bool
        :param robot_base_frame: needed only for eye-on-base calibrations: robot base link tf name
        :type robot_base_frame: string
        :param robot_effector_frame: needed only for eye-on-hand calibrations: robot tool tf name
        :type robot_effector_frame: string
        :param tracking_base_frame: tracking system tf name
        :type tracking_base_frame: string
        :param transformation: transformation between optical origin and base/tool robot frame as tf tuple
        :type transformation: ((float, float, float), (float, float, float, float))
        :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
        """
        if false, it is a eye-on-base calibration

        :type: bool
        """

        self.transformation = TransformStamped(transform=Transform(
            Vector3(*transformation[0]), Quaternion(*transformation[1])))
        """
        transformation between optical origin and base/tool robot frame

        :type: geometry_msgs.msg.TransformedStamped
        """

        # tf names
        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 __init__(self, context):
        super(RqtHandeyeCalibration, self).__init__(context)
        # Give QObjects reasonable names
        self.setObjectName('HandeyeCalibration')

        # Process standalone plugin command-line arguments
        from argparse import ArgumentParser
        parser = ArgumentParser()
        # Add argument(s) to the parser.
        parser.add_argument("-q", "--quiet", action="store_true",
                            dest="quiet",
                            help="Put plugin in silent mode")
        args, unknowns = parser.parse_known_args(context.argv())
        if not args.quiet:
            print 'arguments: ', args
            print 'unknowns: ', unknowns

        # Create QWidget
        self._widget = QWidget()
        # Get path to UI file which should be in the "resource" folder of this package
        ui_file = os.path.join(rospkg.RosPack().get_path('rqt_easy_handeye'), 'resource', 'rqt_handeye.ui')
        # Extend the widget with all attributes and children from UI file
        loadUi(ui_file, self._widget)
        # Give QObjects reasonable names
        self._widget.setObjectName('RqtHandeyeCalibrationUI')
        # Show _widget.windowTitle on left-top of each plugin (when 
        # it's set in _widget). This is useful when you open multiple 
        # plugins at once. Also if you open multiple instances of your 
        # plugin at once, these lines add number to make it easy to 
        # tell from pane to pane.
        if context.serial_number() > 1:
            self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
        # Add widget to the user interface
        context.add_widget(self._widget)

        self.client = HandeyeClient()

        self._widget.calibNameLineEdit.setText(rospy.get_namespace())
        self._widget.trackingBaseFrameLineEdit.setText(self.client.tracking_base_frame)
        self._widget.trackingMarkerFrameLineEdit.setText(self.client.tracking_marker_frame)
        self._widget.robotBaseFrameLineEdit.setText(self.client.robot_base_frame)
        self._widget.robotEffectorFrameLineEdit.setText(self.client.robot_effector_frame)
        if self.client.eye_on_hand:
            self._widget.calibTypeLineEdit.setText("eye on hand")

        else:
            self._widget.calibTypeLineEdit.setText("eye on base")

        self._widget.takeButton.clicked[bool].connect(self.handle_take_sample)
        self._widget.removeButton.clicked[bool].connect(self.handle_remove_sample)
        self._widget.computeButton.clicked[bool].connect(self.handle_compute_calibration)
        self._widget.saveButton.clicked[bool].connect(self.handle_save_calibration)

        self._widget.removeButton.setEnabled(False)
        self._widget.computeButton.setEnabled(False)
        self._widget.saveButton.setEnabled(False)