in simulation_ws/src/rover/hector_gazebo_plugins/src/gazebo_ros_imu.cpp [282:501]
void GazeboRosIMU::Update()
{
// Get Time Difference dt
#if (GAZEBO_MAJOR_VERSION >= 8)
common::Time cur_time = world->SimTime();
#else
common::Time cur_time = world->GetSimTime();
#endif
double dt = updateTimer.getTimeSinceLastUpdate().Double();
boost::mutex::scoped_lock scoped_lock(lock);
// Get Pose/Orientation
#if (GAZEBO_MAJOR_VERSION >= 8)
ignition::math::Pose3d pose = link->WorldPose();
// ignition::math::Vector3d pos = pose.pos + this->offset_.pos;
ignition::math::Quaterniond rot = this->offset_.Rot() * pose.Rot();
#else
math::Pose pose = link->GetWorldPose();
// math::Vector3 pos = pose.pos + this->offset_.pos;
math::Quaternion rot = this->offset_.rot * pose.rot;
#endif
rot.Normalize();
// get Gravity
#if (GAZEBO_MAJOR_VERSION >= 8)
gravity = world->Gravity();
double gravity_length = gravity.Length();
ROS_DEBUG_NAMED("gazebo_ros_imu", "gravity_world = [%g %g %g]", gravity.X(), gravity.Y(), gravity.Z());
#else
gravity = world->GetPhysicsEngine()->GetGravity();
double gravity_length = gravity.GetLength();
ROS_DEBUG_NAMED("gazebo_ros_imu", "gravity_world = [%g %g %g]", gravity.x, gravity.y, gravity.z);
#endif
// get Acceleration and Angular Rates
// the result of GetRelativeLinearAccel() seems to be unreliable (sum of forces added during the current simulation step)?
//accel = myBody->GetRelativeLinearAccel(); // get acceleration in body frame
#if (GAZEBO_MAJOR_VERSION >= 8)
ignition::math::Vector3d temp = link->WorldLinearVel(); // get velocity in world frame
#else
math::Vector3 temp = link->GetWorldLinearVel(); // get velocity in world frame
#endif
if (dt > 0.0) accel = rot.RotateVectorReverse((temp - velocity) / dt - gravity);
velocity = temp;
// calculate angular velocity from delta quaternion
// note: link->GetRelativeAngularVel() sometimes return nan?
// rate = link->GetRelativeAngularVel(); // get angular rate in body frame
#if (GAZEBO_MAJOR_VERSION >= 8)
ignition::math::Quaterniond delta = this->orientation.Inverse() * rot;
#else
math::Quaternion delta = this->orientation.GetInverse() * rot;
#endif
this->orientation = rot;
if (dt > 0.0) {
#if (GAZEBO_MAJOR_VERSION >= 8)
rate = this->offset_.Rot().Inverse()
* (2.0 * acos(std::max(std::min(delta.W(), 1.0), -1.0)) * ignition::math::Vector3d(delta.X(), delta.Y(), delta.Z()).Normalize() / dt);
#else
rate = this->offset_.rot.GetInverse()
* (2.0 * acos(std::max(std::min(delta.w, 1.0), -1.0)) * math::Vector3(delta.x, delta.y, delta.z).Normalize() / dt);
#endif
}
// update sensor models
accel = accelModel(accel, dt);
rate = rateModel(rate, dt);
yawModel.update(dt);
#if (GAZEBO_MAJOR_VERSION >= 8)
ROS_DEBUG_NAMED("gazebo_ros_imu", "Current bias errors: accel = [%g %g %g], rate = [%g %g %g], yaw = %g",
accelModel.getCurrentBias().X(), accelModel.getCurrentBias().Y(), accelModel.getCurrentBias().Z(),
rateModel.getCurrentBias().X(), rateModel.getCurrentBias().Y(), rateModel.getCurrentBias().Z(),
yawModel.getCurrentBias());
ROS_DEBUG_NAMED("gazebo_ros_imu", "Scale errors: accel = [%g %g %g], rate = [%g %g %g], yaw = %g",
accelModel.getScaleError().X(), accelModel.getScaleError().Y(), accelModel.getScaleError().Z(),
rateModel.getScaleError().X(), rateModel.getScaleError().Y(), rateModel.getScaleError().Z(),
yawModel.getScaleError());
#else
ROS_DEBUG_NAMED("gazebo_ros_imu", "Current bias errors: accel = [%g %g %g], rate = [%g %g %g], yaw = %g",
accelModel.getCurrentBias().x, accelModel.getCurrentBias().y, accelModel.getCurrentBias().z,
rateModel.getCurrentBias().x, rateModel.getCurrentBias().y, rateModel.getCurrentBias().z,
yawModel.getCurrentBias());
ROS_DEBUG_NAMED("gazebo_ros_imu", "Scale errors: accel = [%g %g %g], rate = [%g %g %g], yaw = %g",
accelModel.getScaleError().x, accelModel.getScaleError().y, accelModel.getScaleError().z,
rateModel.getScaleError().x, rateModel.getScaleError().y, rateModel.getScaleError().z,
yawModel.getScaleError());
#endif
// apply accelerometer and yaw drift error to orientation (pseudo AHRS)
#if (GAZEBO_MAJOR_VERSION >= 8)
ignition::math::Vector3d accelDrift = pose.Rot().RotateVector(accelModel.getCurrentBias());
double yawError = yawModel.getCurrentBias();
ignition::math::Quaterniond orientationError(
ignition::math::Quaterniond(cos(yawError/2), 0.0, 0.0, sin(yawError/2)) * // yaw error
ignition::math::Quaterniond(1.0, 0.5 * accelDrift.Y() / gravity_length, 0.5 * -accelDrift.X() / gravity_length, 0.0) // roll and pitch error
);
#else
math::Vector3 accelDrift = pose.rot.RotateVector(accelModel.getCurrentBias());
double yawError = yawModel.getCurrentBias();
math::Quaternion orientationError(
math::Quaternion(cos(yawError/2), 0.0, 0.0, sin(yawError/2)) * // yaw error
math::Quaternion(1.0, 0.5 * accelDrift.y / gravity_length, 0.5 * -accelDrift.x / gravity_length, 0.0) // roll and pitch error
);
#endif
orientationError.Normalize();
rot = orientationError * rot;
// copy data into pose message
imuMsg.header.frame_id = frame_id_;
imuMsg.header.stamp.sec = cur_time.sec;
imuMsg.header.stamp.nsec = cur_time.nsec;
// orientation quaternion
#if (GAZEBO_MAJOR_VERSION >= 8)
imuMsg.orientation.x = rot.X();
imuMsg.orientation.y = rot.Y();
imuMsg.orientation.z = rot.Z();
imuMsg.orientation.w = rot.W();
#else
imuMsg.orientation.x = rot.x;
imuMsg.orientation.y = rot.y;
imuMsg.orientation.z = rot.z;
imuMsg.orientation.w = rot.w;
#endif
// pass angular rates
#if (GAZEBO_MAJOR_VERSION >= 8)
imuMsg.angular_velocity.x = rate.X();
imuMsg.angular_velocity.y = rate.Y();
imuMsg.angular_velocity.z = rate.Z();
#else
imuMsg.angular_velocity.x = rate.x;
imuMsg.angular_velocity.y = rate.y;
imuMsg.angular_velocity.z = rate.z;
#endif
// pass accelerations
#if (GAZEBO_MAJOR_VERSION >= 8)
imuMsg.linear_acceleration.x = accel.X();
imuMsg.linear_acceleration.y = accel.Y();
imuMsg.linear_acceleration.z = accel.Z();
#else
imuMsg.linear_acceleration.x = accel.x;
imuMsg.linear_acceleration.y = accel.y;
imuMsg.linear_acceleration.z = accel.z;
#endif
// fill in covariance matrix
imuMsg.orientation_covariance[8] = yawModel.gaussian_noise*yawModel.gaussian_noise;
if (gravity_length > 0.0) {
#if (GAZEBO_MAJOR_VERSION >= 8)
imuMsg.orientation_covariance[0] = accelModel.gaussian_noise.X()*accelModel.gaussian_noise.X()/(gravity_length*gravity_length);
imuMsg.orientation_covariance[4] = accelModel.gaussian_noise.Y()*accelModel.gaussian_noise.Y()/(gravity_length*gravity_length);
#else
imuMsg.orientation_covariance[0] = accelModel.gaussian_noise.x*accelModel.gaussian_noise.x/(gravity_length*gravity_length);
imuMsg.orientation_covariance[4] = accelModel.gaussian_noise.y*accelModel.gaussian_noise.y/(gravity_length*gravity_length);
#endif
} else {
imuMsg.orientation_covariance[0] = -1;
imuMsg.orientation_covariance[4] = -1;
}
// publish to ros
pub_.publish(imuMsg);
ROS_DEBUG_NAMED("gazebo_ros_imu", "Publishing IMU data at t = %f", cur_time.Double());
// publish bias
if (bias_pub_) {
biasMsg.header = imuMsg.header;
#if (GAZEBO_MAJOR_VERSION >= 8)
biasMsg.orientation.x = orientationError.X();
biasMsg.orientation.y = orientationError.Y();
biasMsg.orientation.z = orientationError.Z();
biasMsg.orientation.w = orientationError.W();
biasMsg.angular_velocity.x = rateModel.getCurrentBias().X();
biasMsg.angular_velocity.y = rateModel.getCurrentBias().Y();
biasMsg.angular_velocity.z = rateModel.getCurrentBias().Z();
biasMsg.linear_acceleration.x = accelModel.getCurrentBias().X();
biasMsg.linear_acceleration.y = accelModel.getCurrentBias().Y();
biasMsg.linear_acceleration.z = accelModel.getCurrentBias().Z();
#else
biasMsg.orientation.x = orientationError.x;
biasMsg.orientation.y = orientationError.y;
biasMsg.orientation.z = orientationError.z;
biasMsg.orientation.w = orientationError.w;
biasMsg.angular_velocity.x = rateModel.getCurrentBias().x;
biasMsg.angular_velocity.y = rateModel.getCurrentBias().y;
biasMsg.angular_velocity.z = rateModel.getCurrentBias().z;
biasMsg.linear_acceleration.x = accelModel.getCurrentBias().x;
biasMsg.linear_acceleration.y = accelModel.getCurrentBias().y;
biasMsg.linear_acceleration.z = accelModel.getCurrentBias().z;
#endif
bias_pub_.publish(biasMsg);
}
// debug output
#ifdef DEBUG_OUTPUT
if (debugPublisher) {
geometry_msgs::PoseStamped debugPose;
debugPose.header = imuMsg.header;
debugPose.header.frame_id = "/map";
debugPose.pose.orientation.w = imuMsg.orientation.w;
debugPose.pose.orientation.x = imuMsg.orientation.x;
debugPose.pose.orientation.y = imuMsg.orientation.y;
debugPose.pose.orientation.z = imuMsg.orientation.z;
#if (GAZEBO_MAJOR_VERSION >= 8)
ignition::math::Pose3d pose = link->WorldPose();
debugPose.pose.position.x = pose.Pos().X();
debugPose.pose.position.y = pose.Pos().Y();
debugPose.pose.position.z = pose.Pos().Z();
#else
math::Pose pose = link->GetWorldPose();
debugPose.pose.position.x = pose.pos.x;
debugPose.pose.position.y = pose.pos.y;
debugPose.pose.position.z = pose.pos.z;
#endif
debugPublisher.publish(debugPose);
}
#endif // DEBUG_OUTPUT
}