#!/usr/bin/env python3
import rospy
import time

#定义一个装饰器，用来计算函数运行时间
#使用方法：在需要计算运行时间的函数前面加上@decorator_time
#回调函数

def decorator_time(func):
    def wrapper(*args, **kwargs):
        start_time = time.time()
        result = func(*args, **kwargs)
        end_time = time.time()
        elapsed_time = end_time - start_time
        rospy.loginfo(f"Time taken by {func.__name__}: {elapsed_time:.4f} seconds")
        return result
    return wrapper