如何在 ROS 中获取 hector_quadrotor 的 z 位置？

Question

我正在尝试在模拟中获得 hector_quadrotor 的 z 位置。我可以获得 X 和 Y 轴坐标，但无法获得 Z 坐标。我试图通过使用 GPS 获取它，但值不正确。所以我想通过使用气压计或其他传感器来获取 Z 坐标。

这里是 pose_estimation_node.cpp 的一部分（您可以在 github 源上找到完整版本）：

void PoseEstimationNode::gpsCallback(const sensor_msgs::NavSatFixConstPtr& gps, const 
geometry_msgs::Vector3StampedConstPtr& gps_velocity) {
  boost::shared_ptr<GPS> m = boost::static_pointer_cast<GPS>(pose_estimation_->getMeasurement("gps"));

  if (gps->status.status == sensor_msgs::NavSatStatus::STATUS_NO_FIX) {
    if (m->getStatusFlags() > 0) m->reset(pose_estimation_->state());
    return;
  }

  GPS::Update update;
  update.latitude = gps->latitude * M_PI/180.0;
  update.longitude = gps->longitude * M_PI/180.0;
  update.velocity_north =  gps_velocity->vector.x;
  update.velocity_east  = -gps_velocity->vector.y;
  m->add(update);

  if (gps_pose_publisher_ || sensor_pose_publisher_) {
    geometry_msgs::PoseStamped gps_pose;
    pose_estimation_->getHeader(gps_pose.header);
    gps_pose.header.stamp = gps->header.stamp;
    GPSModel::MeasurementVector y = m->getVector(update, pose_estimation_->state());

    if (gps_pose_publisher_) {
      gps_pose.pose.position.x = y(0);
      gps_pose.pose.position.y = y(1);
      gps_pose.pose.position.z = gps->altitude - pose_estimation_->globalReference()- >position().altitude;
      double track = atan2(gps_velocity->vector.y, gps_velocity->vector.x);
      gps_pose.pose.orientation.w = cos(track/2);
      gps_pose.pose.orientation.z = sin(track/2);
      gps_pose_publisher_.publish(gps_pose);
    }

    sensor_pose_.pose.position.x = y(0);
    sensor_pose_.pose.position.y = y(1);
    """I add it here""" 
  }
}

如果我添加 -----> sensor_pose_.pose.position.z = gps->altitude,

我可以在 RVIZ 模拟或 gnome 终端上获得 Z 坐标。但是正如我所说，这些值非常没有意义（负值）。

还有 ------> gps_pose.pose.position.z = gps->altitude - pose_estimation_->globalReference()- >position().altitude;

它不起作用，因为 position().altitude return NAN。 pose_estimation_node.cpp 中还有另一种测量方法，如气压计。如何使用气压计值。

这是pose_estimation_node.cpp的另一部分：

#if defined(USE_HECTOR_UAV_MSGS)
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));
}

#else
void PoseEstimationNode::heightCallback(const geometry_msgs::PointStampedConstPtr& 
height) {
boost::shared_ptr<Height> m = boost::static_pointer_cast<Height>(pose_estimation_->getMeasurement("height"));

Height::MeasurementVector update;
update(0) = height->point.z;
m->add(Height::Update(update));

if (sensor_pose_publisher_) {
sensor_pose_.pose.position.z = height->point.z - m->getElevation();
  }
}
#endif

void PoseEstimationNode::magneticCallback(const geometry_msgs::Vector3StampedConstPtr& magnetic) {
boost::shared_ptr<Magnetic> m = boost::static_pointer_cast<Magnetic>(pose_estimation_->getMeasurement("magnetic"));

Magnetic::MeasurementVector update;
update.x() = magnetic->vector.x;
update.y() = magnetic->vector.y;
update.z() = magnetic->vector.z;
m->add(Magnetic::Update(update));

if (sensor_pose_publisher_) {
sensor_pose_yaw_ = -(m->getModel()->getTrueHeading(pose_estimation_->state(), update) - pose_estimation_->globalReference()->heading());
}
}

Answer 1

要使用气压计，您需要将该传感器实际添加到您的机器人中。您正在查看的只是回调，这意味着支持此类消息。如果您想为机器人添加新传感器，我建议您查看 this tutorial.

综上所述，如果您得到的高度值不正确，您需要返回并重新检查您的转换，因为内置于 hector 中的本地化应该可以正常工作。

如何在 ROS 中获取 hector_quadrotor 的 z 位置？

How can I get z position of hector_quadrotor in ROS?

c++

simulation

ros

robotics