/**
* Receives the messages from the IMU
*/
void imuHandler(const sensor_msgs::Imu::ConstPtr& imuIn)
{
// get RPY
double roll, pitch, yaw;
tf::Quaternion orientation;
tf::quaternionMsgToTF(imuIn->orientation, orientation);
tf::Quaternion transformation_q;
// -90 in y
transformation_q.setRotation( tf::Vector3(0,1,0), -tfScalar(PI/2));
tf::Quaternion parsed_orientation = orientation.operator*=(transformation_q);
geometry_msgs::PoseStamped imuVis;
// publish the imu quaternion
imuVis.pose.orientation.x = parsed_orientation.x();
imuVis.pose.orientation.y = parsed_orientation.y();
imuVis.pose.orientation.z = parsed_orientation.z();
imuVis.pose.orientation.w = parsed_orientation.w();
imuVis.pose.position.x = 0;
imuVis.pose.position.y = 0;
imuVis.pose.position.z = 0;
imuVis.header.stamp = imuIn->header.stamp;
imuVis.header.frame_id = "/camera_init_2";
pubImuVisPointer->publish(imuVis);
tf::Matrix3x3(parsed_orientation).getRPY(roll, pitch, yaw);
int imuPointerBack = imuPointerLast; // initially -1
imuPointerLast = (imuPointerLast + 1) % imuQueLength; // the array pointer 0 - imuQueueLength (100 in MS case)
imuTime[imuPointerLast] = imuIn->header.stamp.toSec(); // the time the imu measurement was taken for each of the imuQueueLength (100) points
double timeDiff = imuTime[imuPointerLast] - imuTime[imuPointerBack]; // difference in time between the two imu measurement
// if two consecutive measurement were taken within 0.1 seconds
if (timeDiff < 0.1) {
// imuAccuRoll += timeDiff * imuIn->angular_velocity.x;
// imuAccuPitch += timeDiff * imuIn->angular_velocity.y;
// imuAccuYaw += timeDiff * imuIn->angular_velocity.z;
imuRoll[imuPointerLast] = yaw;
imuPitch[imuPointerLast] = roll;
imuYaw[imuPointerLast] = -pitch;
// imuRoll[imuPointerLast] = imuAccuRoll;
// imuPitch[imuPointerLast] = -imuAccuPitch;
// imuYaw[imuPointerLast] = -imuAccuYaw;
// if there's monotonic IMU shift- accumulate it
// imuAccX[imuPointerLast] = -imuIn->linear_acceleration.y;
// imuAccY[imuPointerLast] = -imuIn->linear_acceleration.z - 9.81;
// imuAccZ[imuPointerLast] = imuIn->linear_acceleration.x;
AccumulateIMUShift();
}
}
void imuHandler(const sensor_msgs::Imu::ConstPtr& imuIn)
{
double roll, pitch, yaw;
tf::Quaternion orientation;
tf::quaternionMsgToTF(imuIn->orientation, orientation);
tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);
float accX = imuIn->linear_acceleration.y - sin(roll) * cos(pitch) * 9.81;
float accY = imuIn->linear_acceleration.z - cos(roll) * cos(pitch) * 9.81;
float accZ = imuIn->linear_acceleration.x + sin(pitch) * 9.81;
imuPointerLast = (imuPointerLast + 1) % imuQueLength;
imuTime[imuPointerLast] = imuIn->header.stamp.toSec() - 0.1068;
imuRoll[imuPointerLast] = roll;
imuPitch[imuPointerLast] = pitch;
imuYaw[imuPointerLast] = yaw;
imuAccX[imuPointerLast] = accX;
imuAccY[imuPointerLast] = accY;
imuAccZ[imuPointerLast] = accZ;
AccumulateIMUShift();
}