Gazebo VIO Setup - Commander Takeoff Denied Due to Invalid Position Estimate

[honestGDUT]

Hi PX4 community,
I’m setting up vision-based navigation in Gazebo using a virtual camera and VINS, but encountering a preflight error when attempting takeoff. Here’s my setup:

1. Simulation Environment

• Using iris_realsense_camera.sdf model in Gazebo
• VINS-Fusion processing simulated camera images
• Custom ROS node (vins_to_mavros) converts VINS output to /mavros/vision_pose/pose
• Conversion handles FLU→FRD coordinate transform:

1. EKF2 Configuration
   Parameters set via QGroundControl:
   EKF2_AID_MASK = 24
   EKF2_HGT_MODE = 3
   EKF2_EV_DELAY = 175

2. Observed Behavior

• VINS output appears correct in Rviz

• /mavros/vision_pose/pose publishes at 30Hz with valid data

• commander check shows Prearm OK:
  [HealthFlags] VIO: // No EN/OK indicator! [HealthFlags] POS: // Empty status

• Takeoff command fails:
  commander takeoff # ERROR: Takeoff denied! Please disarm and retry # ERROR: Check for a valid position estimate

3. Diagnostics Performed

• Verified coordinate frames (VINS FLU → MAVROS FRD)
• Checked EKF innovations: uorb top shows no vision rejection
• Sensor logs show zero message drops
• Tested with both simulated and static VINS output
  Environment
• PX4 v1.13.1
• ROS Noetic, Gazebo 11
• VINS-Fusion (simulation-modified)
• Iris + Realsense camera model

where I modified

1. VINS–config
   add iris_stereo_config.yaml left.yaml right.yaml
   
   
2. NODE–vins_to_mavros
   add ros node to transform positon data

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Odometry.h>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <mutex>


Eigen::Vector3d p_mav = Eigen::Vector3d::Zero();
Eigen::Quaterniond q_mav = Eigen::Quaterniond::Identity();
std::mutex data_mutex;
bool received_first_msg = false;
std::string target_frame = "world";

const Eigen::Quaterniond FLU_TO_FRD(
    Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitX())
);

void vins_callback(const nav_msgs::Odometry::ConstPtr &msg)
{
    if (msg->header.frame_id == target_frame) {
        std::lock_guard<std::mutex> lock(data_mutex);
        
        
        p_mav = Eigen::Vector3d(
            msg->pose.pose.position.x,
            msg->pose.pose.position.y,
            msg->pose.pose.position.z
        );
        
      
        Eigen::Quaterniond q_flu(
            msg->pose.pose.orientation.w,
            msg->pose.pose.orientation.x,
            msg->pose.pose.orientation.y,
            msg->pose.pose.orientation.z
        );
        
        // q_frd = FLU_TO_FRD * q_flu
        q_mav = FLU_TO_FRD * q_flu;
        q_mav.normalize(); 
        
        received_first_msg = true;
    }
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "vins_to_mavros");
    ros::NodeHandle nh("~");
    

    nh.param<std::string>("target_frame", target_frame, "world");
    
    ros::Subscriber slam_sub = nh.subscribe<nav_msgs::Odometry>(
        "/vins_estimator/camera_pose", 100, vins_callback);
        
    ros::Publisher vision_pub = nh.advertise<geometry_msgs::PoseStamped>(
        "/mavros/vision_pose/pose", 10);
    
    ros::Rate rate(30.0);
    ros::Time last_print = ros::Time::now();

    while (ros::ok()) {
        if (!received_first_msg) {
            ROS_WARN_THROTTLE(1.0, "Waiting for first VINS odometry message...");
            ros::spinOnce();
            rate.sleep();
            continue;
        }

        geometry_msgs::PoseStamped vision;
        
        {
            std::lock_guard<std::mutex> lock(data_mutex);
            vision.pose.position.x = p_mav[0];
            vision.pose.position.y = p_mav[1];
            vision.pose.position.z = p_mav[2];
            
            vision.pose.orientation.x = q_mav.x();
            vision.pose.orientation.y = q_mav.y();
            vision.pose.orientation.z = q_mav.z();
            vision.pose.orientation.w = q_mav.w();

            vision.pose.covariance[0] = 0.01;  // x
            vision.pose.covariance[7] = 0.01;  // y
            vision.pose.covariance[14] = 0.01; // z
            vision.pose.covariance[21] = 0.01; // roll
            vision.pose.covariance[28] = 0.01; // pitch
            vision.pose.covariance[35] = 0.01; // yaw
        }
        

        vision.header.stamp = ros::Time::now(); 
        vision.header.frame_id = "vision";
        vision_pub.publish(vision);


        if ((ros::Time::now() - last_print).toSec() >= 1.0) {
            ROS_INFO("Published to /mavros/vision_pose/pose");
            ROS_INFO("Position: x=%.3f y=%.3f z=%.3f", 
                     p_mav[0], p_mav[1], p_mav[2]);
            ROS_INFO("Orientation: x=%.3f y=%.3f z=%.3f w=%.3f", 
                     q_mav.x(), q_mav.y(), q_mav.z(), q_mav.w());
            last_print = ros::Time::now();
        }
        
        ros::spinOnce();
        rate.sleep();
    }
    
    return 0;
}

Screen shot

1. rostopic echo /mavros/vision_pose/pose
   
   
2. commander takeoff
   
   
3. QGC
   
   

Is there any information I can provide?
What should I do for this problem?