Use external odometry in enu for px4

Hello,
As I am trying to apply vo from rtabmap, I do have problems converting the ros enu odometry message from rtabmap to feed into px4 vio. The general setup is to use gz (harmonic) as the simulator, and the x500_depth model with 5002 configuration in a warehouse (previously tugbot in a warehouse in which I removed tugbot), initially towards east. The drone is at some position else than (0,0,0) (the original tugbot location), and I use this position as the ros map and drone’s odom frame. rtabmap works well with an extra camera I added in the gz x500_depth model in rgbd mode. If I run the rtabmap as well as other simulation setups, the drone is flying normally and rtabmap showing somehow correct trajectory with drifts. But, once I bring up my node subscribing the odometry from rtabmap and feed it to fmu/in/visual_odometry with the navigation interface from ros2_cpp_lib, the vo would only work without rotation. If I takeoff and move around without rotation (always towards east), the vo works well, and the drone is much more stable than that without vo. But as long as I horizontally rotate the drone to point some direction else than east, the drone would start circling


And the larger the angle I yawed, the larger the radius of the circle is:

So I doubt if I had my vo node wrong. My conversion code:

    auto onRtabmapOdom = [this](const nav_msgs::msg::Odometry &odomMsg) {
      // PoseWithCovariance
      Eigen::Affine3d poseEnu;
      tf2::fromMsg(odomMsg.pose.pose, poseEnu);

      Eigen::Quaterniond qNed{frame_transforms::ros_to_px4_orientation(
          Eigen::Quaterniond(poseEnu.linear()))};
      qNed.normalize();

      Eigen::Vector3d poseNed{frame_transforms::enu_to_ned_local_frame(
          Eigen::Vector3d(poseEnu.translation()))};

      const frame_transforms::Covariance6d &poseCovEnu =
          odomMsg.pose.covariance;
      frame_transforms::Covariance6d poseCovNed =
          frame_transforms::transform_static_frame(
              poseCovEnu, frame_transforms::StaticTF::ENU_TO_NED);

      // TwistWithCovariance
      Eigen::Matrix<double, 6, 1> twistEnu;
      tf2::fromMsg(odomMsg.twist.twist, twistEnu);
      Eigen::Vector3d linearVelocityEnu{twistEnu.head(3)};
      Eigen::Vector3d linearVelocityNed{
          frame_transforms::enu_to_ned_local_frame(linearVelocityEnu)};

      // struct LocalPositionMeasurement does not contain angular velocity so
      // ignore the angualr velocity
      Eigen::Quaterniond qAngularVelocityEnu{
          frame_transforms::utils::quaternion::quaternion_from_euler(
              twistEnu.tail(3))};
      Eigen::Quaterniond qAngularVelocityNed{
          frame_transforms::ros_to_px4_orientation(qAngularVelocityEnu)};

      const frame_transforms::Covariance6d &twistCovEnu =
          odomMsg.twist.covariance;
      frame_transforms::Covariance6d twistCovNed =
          frame_transforms::transform_static_frame(
              twistCovEnu, frame_transforms::StaticTF::ENU_TO_NED);

      // Updating localPose

      px4_ros2::LocalPositionMeasurement localPose;
      localPose.timestamp_sample = odomMsg.header.stamp;

      // Pose part
      localPose.position_xy = poseNed.head(2).cast<float>();
      localPose.position_xy_variance =
          Eigen::Vector2f{poseCovNed[0], poseCovNed[1 + 1 * 6]};
      localPose.position_z = poseNed(2);
      localPose.position_z_variance = poseCovNed[2 + 2 * 6];

      localPose.velocity_xy = linearVelocityNed.head(2).cast<float>();
      localPose.velocity_xy_variance =
          Eigen::Vector2f{twistCovNed[0], twistCovNed[1 + 1 * 6]};
      localPose.velocity_z = linearVelocityNed(2);
      localPose.velocity_z_variance = twistCovNed[2 + 2 * 6];

      localPose.attitude_quaternion = qNed.cast<float>();
      localPose.attitude_variance =
          Eigen::Vector3f{static_cast<float>(poseCovNed[3 + 3 * 6]),
                          static_cast<float>(poseCovNed[4 + 4 * 6]),
                          static_cast<float>(poseCovNed[5 + 5 * 6])};

      this->_interface->updateLocalPosition(localPose);
    };

The pose frame and velocity frame of the vehicleodometry are both set to localned.

So I would doubt that there is something wrong in the conversion here. And if not I would appreciate any suggestion on how to further debug this problem. Thanks

Hey, I’m trying something similar. Having issues with converting NED to ENU frames. Did you find any fix? It would be quite helpful.

Thanks!