Thanks to visit codestin.com
Credit goes to github.com

Skip to content

It seems to give the wrong attitude. #137

@LTHlidar

Description

@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.

Metadata

Metadata

Assignees

No one assigned

    Labels

    No labels
    No labels

    Projects

    No projects

    Milestone

    No milestone

    Relationships

    None yet

    Development

    No branches or pull requests

    Issue actions