action_database.py 文件源码

def back_forth_search_xy(self):
        rate = rospy.Rate(100)
        for i in range(100):
            if np.all(np.array(self.finger_detect) == np.array(self.detect_goal)):
                return 'found'
            msg = self.create_pose_velocity_msg([0.0,-0.05,0.0,0.0,0.0,0.0])
            self.jn.kinematic_control(msg)
            rate.sleep()
        for i in range(200):
            if np.all(np.array(self.finger_detect)[[self.fingers]] == np.array(self.detect_goal)):
                return 'found'
            msg = self.create_pose_velocity_msg([0.0,0.05,0.0,0.0,0.0,0.0])
            self.jn.kinematic_control(msg)
            rate.sleep()
        for i in range(100):
            if np.all(np.array(self.finger_detect) == np.array(self.detect_goal)):
                return 'found'
            msg = self.create_pose_velocity_msg([0.0,-0.05,0.0,0.0,0.0,0.0])
            self.jn.kinematic_control(msg)
            rate.sleep()
        return 'not_found'