Using NodeHandle in callback

edit

Hello, I am making an initialization node for AHRS and I'm trying to get the initial bias of gyroscope by averaging the first 1000 readings that I get. I need to advertise the service only after I have received the correct amount of data points, but I can't figure out how to do this. In the version below, I tried making a global pointer to the node handle and while

        if(n.ok()){  
            ROS_INFO("NodeHandle working");
        }

runs properly and prints out "NodeHandle working", the service ros::ServiceServer service = n.advertiseService("initialize_ahrs", initialization_handle); is not being advertised. I have also tried using boost::bind as suggested in one of the answers to a similar question but it didn't work either. I'm new to ROS and I don't know how else to get it working, could you, please, point out where am I getting it wrong?

#include "ros/ros.h"
#include "ekf_imu_odom/initRequest.h"
#include "sensor_msgs/Imu.h"
#include "Eigen/Dense"
#include <geometry_msgs/Quaternion.h>
#include <tf/transform_datatypes.h>
#include "std_msgs/String.h"
#include <boost/bind.hpp>    

ros::Subscriber sub;
ros::NodeHandle * n_ptr;

int n_imu = 0;
int num_data = 1000;
double sum_accel [3] = {0};
double sum_gyro [3] = {0};

bool initialization_handle(ekf_imu_odom::initRequest::Request &req,ekf_imu_odom::initRequest::Response &res);

void imu_callback(const sensor_msgs::Imu::ConstPtr& data){
    ros::NodeHandle n = *n_ptr;
    // Getting data from the accelerometer
    double accel_data [3];
    accel_data[0] = data -> linear_acceleration.x;
    accel_data[1] = data -> linear_acceleration.y;
    accel_data[2] = data -> linear_acceleration.z;    

    // Getting data from the gyro
    double gyro_data [3];
    gyro_data[0] = data -> angular_velocity.x;
    gyro_data[1] = data -> angular_velocity.y;
    gyro_data[2] = data -> angular_velocity.z;

    // Making sure we get a 1000 points to get the avg
    if (n_imu < num_data){
        // Adding latest values to existing to the avg later on            
        sum_accel[0] -= accel_data[0];
        sum_accel[1] -= accel_data[1];
        sum_accel[2] -= accel_data[2];

        sum_gyro[0] += gyro_data[0];
        sum_gyro[1] += gyro_data[1];
        sum_gyro[2] += gyro_data[2];

        n_imu += 1;
    }
    else {    
        if(n.ok()){  
        ROS_INFO("NodeHandle working");
        }     
        ros::ServiceServer service = n.advertiseService("initialize_ahrs", initialization_handle);
        sub.shutdown();
    }   
}

bool initialization_handle(ekf_imu_odom::initRequest::Request &req,ekf_imu_odom::initRequest::Response &res){
    // Estimating gravity vector
    double g_v[3] = {0,0,0};
    g_v[0] = sum_accel[0] / num_data;
    g_v[1] = sum_accel[1] / num_data;
    g_v[2] = sum_accel[2] / num_data; 

    // Initial roll(phi), pitch(theta), yaw(psi)
    double phi = atan2(-g_v[1], -g_v[2]);
    double theta = atan2(g_v[0], sqrt(pow(g_v[1],2) + pow(g_v[2],2)));
    double psi = 0;

    // Creating quaternion from RPY to pass to ekf
    tf::Quaternion quat = tf::createQuaternionFromRPY(-phi,-theta, -psi);

    // Computing bias of the gyroscope
    double gyro_bias[3] = {0,0,0};

    res.gyro_bias[0] = sum_gyro[0] / num_data;
    res.gyro_bias[1] = sum_gyro[1] / num_data;
    res.gyro_bias[2] = sum_gyro[2] / num_data;

    // Converting from tf::Quaternion to geometry_msgs::Quaternion to pass as a msg
    quaternionTFToMsg(quat , res.init_orientation);

    ROS_INFO("sending back response: \n gyro_bias[1]: %f gyro_bias[2]: %f gyro_bias[3]: %f ", (double)res.gyro_bias[0], 
(double)res.gyro_bias[1], (double)res.gyro_bias[2]);
    ROS_INFO("quat x: %f y: %f z: %f ...