如何在 ROS 中获取 hector_quadrotor 的 z 位置?
How can I get z position of hector_quadrotor in ROS?
我正在尝试在模拟中获得 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());
}
}
要使用气压计,您需要将该传感器实际添加到您的机器人中。您正在查看的只是回调,这意味着支持此类消息。如果您想为机器人添加新传感器,我建议您查看 this tutorial.
综上所述,如果您得到的高度值不正确,您需要返回并重新检查您的转换,因为内置于 hector 中的本地化应该可以正常工作。
我正在尝试在模拟中获得 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());
}
}
要使用气压计,您需要将该传感器实际添加到您的机器人中。您正在查看的只是回调,这意味着支持此类消息。如果您想为机器人添加新传感器,我建议您查看 this tutorial.
综上所述,如果您得到的高度值不正确,您需要返回并重新检查您的转换,因为内置于 hector 中的本地化应该可以正常工作。