task3.py 文件源码

def start(self, _=None):
        """ Start the macro """
        global LAST_LEAK
        self.leak = None
        self.best_leak = 0.01

        sub = rospy.Subscriber("/task3/checkpoint5/leak", Leak,
                               self._leak_update)
        self.fc.zarj.neck.neck_control([0.35, 1.0, 0], True)
        self._check_distance()
        found, stage = self._full_cycle(0)
        if self.stop:
            return
        while found is not None and not found:
            log('Leak not found at : {}'.format(stage))
            found, stage = self._full_cycle(stage)
            if self.stop:
                break
        if found:
            self.fc.process_get_palm_msg(ZarjGetPalmCommand('left'))
            joint_values = self.fc.zarj.hands.get_joint_values('left')
            ack = ZarjGetArmJointsResponse('left', joint_values)
            self.fc.zarj_comm.push_message(ack)
            log('leak found! stage {}'.format(stage))
            palm_transform = self.fc.zarj.hands.get_current_hand_center_transform(
                'left', 'world')
            LAST_LEAK = PointStamped()
            LAST_LEAK.header.frame_id = 'world'
            LAST_LEAK.point = palm_transform.translation
            log('Leak at {}'.format(LAST_LEAK))
            log('Arm and palm positions updated')

        sub.unregister()