task3.py 文件源码

def _ensure_leak(self):
        """ If we do not have a leak detected assume our hand is in the
            right place"""

        global LAST_LEAK, LEAK_SIDE

        if LAST_LEAK is None:
            palm = self.fc.zarj.hands.get_current_hand_center_transform(
                'left', 'world')
            LAST_LEAK = PointStamped()
            LAST_LEAK.header.frame_id = 'world'
            LAST_LEAK.point = palm.translation
            log("No leak previously detected.. Assuming left hand is correct")
            log('Leak at {}'.format(LAST_LEAK))

            current = self.fc.zarj.hands.get_joint_states('left')
            LEAK_SIDE = current[0]['leftShoulderPitch'][0] > 0
            log('Leak is behind us: '.format(LEAK_SIDE))