Terrain Following Not Working in PX4 Gazebo Simulation Environment

PX4 Autopilot
1

I conduct tests in the PX4 simulation environment before transitioning to the real system. In this case, I added a distance sensor in the simulation and publish its data at 30 Hz to /fmu/in/distance_sensor via ROS2. I can verify that the vehicle receives this data through QGroundControl (QGC). However, even though the sensor values change in uneven terrain, the drone in the simulation does not follow these changes and continues to fly at a constant altitude.

PX4-Autopilot 1.15.0 version
Gazebo Harmonic
QGC Param:

  • EKF2_HGT_REF: 2 (range)
  • MPC_ALT_MODE: 1 (Terrain following)

Distance Sensor Code

using distanceSensorMsg = px4_msgs::msg::DistanceSensor;
...
void DistanceCallback(const laserScanMsg::SharedPtr new_msg)
    {
        RCLCPP_INFO(this->get_logger(), "Distance: %.2f", new_msg->ranges[0]);

        distanceSensorMsg msg;
        msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
        msg.device_id = 201;
        msg.min_distance = 0.10f;
        msg.max_distance = 220.0f;
        msg.current_distance = new_msg->ranges[0]; // Convert distance to meters
        msg.type = 0;                                             // MAV_DISTANCE_SENSOR_LASER
        msg.h_fov = 0.5 * DEG_TO_RAD;
        msg.v_fov = 0.5 * DEG_TO_RAD;
        msg.orientation = distanceSensorMsg::ROTATION_DOWNWARD_FACING;
        msg.signal_quality = 100;
    
        tf_pub->publish(msg);
    }

image
image1246×502 121 KB

  • Although the distance_sensor message is published at 30 Hz, it appears as approximately 0.2 Hz in QGroundControl (QGC). What could be the reason for this?

image
image1696×1007 99.7 KB

image
image1869×963 249 KB

My tests show that even though the distance sensor values change, the vehicle does not increase its altitude.