Fix TF_DENORMALIZED_QUATERNION

[gui2dev]

With version 0.5.15 release of tf2, it now checks for quaternion normalization. As far as I am concerned, imu_filter_madgwick does not provide a normalized quaternion, hence an error :

Error:   TF_DENORMALIZED_QUATERNION: Ignoring transform for child_frame_id "imu" from authority "unknown_publisher" because of an invalid quaternion in the transform (-0.018815 0.013563 -1.000259 0.046847)
         at line 253 in /tmp/binarydeb/ros-kinetic-tf2-0.5.15/src/buffer_core.cpp

For now, I fixed it that way :

void ImuFilterRos::publishTransform(const ImuMsg::ConstPtr& imu_msg_raw)
{
  double q0,q1,q2,q3;
  filter_.getOrientation(q0,q1,q2,q3);
  geometry_msgs::TransformStamped transform;
  transform.header.stamp = imu_msg_raw->header.stamp;
  double d = sqrt(q0*q0+q1*q1+q2*q2+q3*q3);

  if (reverse_tf_)
  {
    transform.header.frame_id = imu_frame_;
    transform.child_frame_id = fixed_frame_;
    transform.transform.rotation.w = q0/d;
    transform.transform.rotation.x = -q1/d;
    transform.transform.rotation.y = -q2/d;
    transform.transform.rotation.z = -q3/d;
  }
  else {
    transform.header.frame_id = fixed_frame_;
    transform.child_frame_id = imu_frame_;
    transform.transform.rotation.w = q0/d;
    transform.transform.rotation.x = q1/d;
    transform.transform.rotation.y = q2/d;
    transform.transform.rotation.z = q3/d;
  }
  tf_broadcaster_.sendTransform(transform);

}

which might be unsafe to some extend given the possibilty of d=0.

[mintar]

Thanks for the heads up, I've just fixed the issue!