It seems to give the wrong attitude.

[LTHlidar]

Hello.

I'm trying to use the VectorNAV package with the ROS2 humble version to get the attitude of an IMU.

I understand that the default reference frame of the VectorNav sensor is the NED frame.

So, if I get the attitude of IMU wrt NED frame (set use_enu = false), I don't need ENU - > NED conversion, right?

In the code (vn_sensor_msgs.cc), it looks like this.

    // IMU
    {
      sensor_msgs::msg::Imu msg;
      msg.header = msg_in->header;
      
      if(use_enu) {
        convert_to_enu(msg_in, msg);
      } else {
        msg.angular_velocity = msg_in->angularrate;
        msg.linear_acceleration = msg_in->accel;

        **// Quaternion ENU -> NED
        tf2::Quaternion q, q_ned2enu;
        fromMsg(msg_in->quaternion, q);
        q_ned2enu.setRPY(M_PI, 0.0, -M_PI / 2);
        msg.orientation = toMsg(q_ned2enu * q);**
      }

      fill_covariance_from_param("orientation_covariance", msg.orientation_covariance);
      fill_covariance_from_param("angular_velocity_covariance", msg.angular_velocity_covariance);
      fill_covariance_from_param(
        "linear_acceleration_covariance", msg.linear_acceleration_covariance);

      pub_imu_->publish(msg);
    }

In other words, I think the Quaternion ENU -> NED code in the above code should be removed. In fact, when I set the vn-100 sensor flat and rotate the z-axis clockwise to match the body-frame(x: forward, y: right, z: down), NED-frame definition, the yaw should increase, but with the current code it is decreasing.
When I delete the Quaternion ENU -> NED, the yaw increases.

Also, the pose given by the sensor is NED-frame, so I think there should be a NED to ENU-frame conversion in the convert_to_enu() function. (q_ned2enu.setRPY(PI, 0.0, PI / 2) quaternion multiplication)

Currently, inside the convert_to_enu() function, the sign of the quaternion z-component is swapped, but I'm not sure why.

I would like to know if my thinking is correct. I would appreciate any feedback from users.

Thanks.

[dawonn]

PR is appreciated; make sure to include details for how you test it when submitting.