How to measure how long a callback takes.

edit

I'm publishing a custom message at 200Hz from a node, which is rather large, when I subscribe to the publisher (at 1000Hz) and attempt to print/write to file info from the publisher, I am dropping messages, I believe this is due to how long the call back takes. I am dealing in real time so the buffer on both the publisher and subscriber is 1.

Is this a correct way to message how long the callback is taking?

class data_functions{
public:
    nodal_system_baxter::leap_msgs current_leap_msg;
    void leap_call_back(const  nodal_system_baxter::leap_msgs& msg);
    ros::Time call_lat_start;
};
void data_functions::leap_call_back(const  nodal_system_baxter::leap_msgs& msg){
call_lat_start = ros::Time::now();
current_leap_msg = msg;
}

int main(int argc, char **argv) {
    ros::init(argc, argv, "data_collection");
    ros::NodeHandle node;
    ros::Rate loop_rate(1000);
    data_functions data_functions;
    ros::Subscriber leap_sub;
    leap_sub = node.subscribe("left_leap_data", 1, &data_functions::leap_call_back, &data_functions);
    while(ros::ok()){
        ros::spinOnce();
        loop_rate.sleep();
        ros::Time call_lat_end = ros::Time::now();
        double call_time =  call_lat_end.toSec() - data_functions.call_lat_start.toSec();
        std::cout  << std::setprecision(20) << call_time << std::endl;
    }
    ros::shutdown();
    return 0;
}

It currently states that the callback is taking 40ms, I am missing 8 messages published every 5ms, so it adds up.