ROS仿真环境中如何获取hector_quadrotor的Z轴位置

问题根因 #

GPS返回的altitude是WGS84坐标系下的绝对海拔值，你当前环境的全局参考点海拔没有初始化，所以pose_estimation_->globalReference()->position().altitude返回NAN，直接使用gps->altitude得到的不是相对起飞点的高度，数值自然异常。

适配修改步骤 #

1. 确认编译宏配置 #

代码中高度相关的回调分两个分支，由编译宏USE_HECTOR_UAV_MSGS控制：

• 仿真环境默认开启该宏，走baroCallback气压计回调逻辑
• 关闭宏的情况下走heightCallback高度计回调逻辑

2. 针对气压计分支（开启USE_HECTOR_UAV_MSGS）修改 #

原baroCallback仅把气压数据喂给了状态估计器，没有更新sensor_pose_的Z轴数值，直接修改回调函数即可：

void PoseEstimationNode::baroCallback(const hector_uav_msgs::AltimeterConstPtr& altimeter) {
  boost::shared_ptr<Baro> m = boost::static_pointer_cast<Baro>(pose_estimation_->getMeasurement("baro"));
  m->add(Baro::Update(altimeter->pressure, altimeter->qnh));

  // 新增以下代码，用状态估计器融合后的高度赋值Z轴
  if (sensor_pose_publisher_) {
    sensor_pose_.pose.position.z = pose_estimation_->state().getPosition()(2);
  }
}

修改后不需要再在gpsCallback里手动给sensor_pose_.pose.position.z赋值，气压计回调会自动更新Z轴数值。

3. 针对高度计分支（关闭USE_HECTOR_UAV_MSGS）配置 #

原代码已经实现了高度赋值逻辑，只需要确保heightCallback订阅的话题是仿真环境输出的高度数据即可，不需要额外修改代码。

临时调试方案 #

如果需要临时用GPS获取Z轴数值，可以在节点初始化阶段手动设置全局参考点海拔：

pose_estimation_->globalReference()->position().altitude = 0.0;

这样原有GPS的Z轴计算逻辑gps_pose.pose.position.z = gps->altitude - pose_estimation_->globalReference()->position().altitude;就可以正常输出相对高度，但GPS Z轴噪声远大于气压计，不推荐正式使用。

内容的提问来源于stack exchange，提问作者YusufUcan