mag data not affecting orientation

[tylerjw]

I'm having the problem where the data coming out of the /imu/mag field is not affecting the orientation output on /imu/data. The output on /imu/data is nearly the same as it is on /imu/raw and the robot is not pointing north or east (as you can see in the bagfile).

OS: Ubuntu 18.04 ROS: melodic imu_filter_madgwich Version: 1.2.2 IMU: xsens mti-630

Bag file: bagfile.zip

imu_test.launch

<launch>
  <node name="mag_convert" pkg="prototype_config" type="mag_convert.py" output="screen">     
    </node>

  #### IMU Driver ###########################################################

  <group>
    <remap from="/imu/data" to="/imu/data_raw"/>
    <remap from="/imu/mag" to="/imu/magnetic_field"/>
    <include file="$(find xsens_mti_driver)/launch/xsens_mti_node.launch" />
  </group>

  #### Nodelet manager ######################################################

  <node pkg="nodelet" type="nodelet" name="imu_manager"
    args="manager" output="screen" />

  #### IMU Orientation Filter ###############################################

  <node pkg="nodelet" type="nodelet" name="ImuFilterNodelet"
    args="load imu_filter_madgwick/ImuFilterNodelet imu_manager"
    output="screen">

    <param name="use_mag" value="true"/>
    <param name="publish_tf" value="false"/>
  </node>

</launch>

mag_convert.py

#!/usr/bin/env python
import rospy

from geometry_msgs.msg import Vector3Stamped
from sensor_msgs.msg import MagneticField

def vector3Callback(msg):
    global mag_pub

    mag_msg = MagneticField()
    mag_msg.header = msg.header
    mag_msg.magnetic_field = msg.vector

    mag_pub.publish(mag_msg)

def main():
    global mag_pub

    rospy.init_node('mag_convert')
    rospy.Subscriber('/imu/magnetic_field', Vector3Stamped, vector3Callback)
    mag_pub = rospy.Publisher('/imu/mag', MagneticField, queue_size=10)
    rospy.spin()

if __name__ == "__main__":
    try:
        main()
    except rospy.ROSInterruptException:
        pass

[mintar]

Sorry for not responding to this issue earlier; I was on vacation when you opened it and then I was too busy and then I forgot about it...

and the robot is not pointing north or east (as you can see in the bagfile).

Actually, the IMU is pointing almost exactly east in the bagfile:

magnetic_field: 
  x: -0.0672955513
  y: 1.17481422424
  z: -0.979140520096

In the ENU convention, x=East, y=North, z=Up. So in the neutral orientation (yaw, pitch and roll = 0), the magnetic field is pointing north and down (which is expected in the northern hemisphere). This is why the filter correctly returns the neutral orientation. If I edit your mag_convert.py so that x and y are switched and play back the bagfile without the imu/mag and imu/data topics, the filter correctly returns the new orientation.