/** * 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(); }