Using NodeHandle in callback
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 ...
Thank you, that worked.
Awesome! Converted the comment to an answer so that this shows up as answered