如何使用hector_gazebo_plugin和Python2获取机器人(x,y,z)坐标

1. 新增Gazebo位姿插件（推荐方案，精度更高） #

hector_gazebo_plugins包自带libhector_gazebo_ros_pose.so插件，可直接输出世界坐标系下的精准位姿，无需额外坐标转换。
在你的四旋翼URDF/SDF模型文件中加入以下插件配置：

<plugin name="quadrotor_pose_plugin" filename="libhector_gazebo_ros_pose.so">
  <robotNamespace>/quadrotor</robotNamespace> <!-- 替换为你的四旋翼命名空间 -->
  <frameName>world</frameName> <!-- 输出位姿的参考坐标系 -->
  <topicName>ground_truth/pose</topicName> <!-- 自定义位姿发布话题名 -->
  <updateRate>50.0</updateRate> <!-- 话题发布频率，单位Hz -->
</plugin>

启动仿真后，可通过rostopic list命令验证对应话题是否正常发布。

2. Python订阅话题获取坐标 #

创建ROS节点订阅上一步配置的位姿话题，即可实时获取坐标值，示例代码如下：

#!/usr/bin/env python3
import rospy
from geometry_msgs.msg import PoseStamped

def pose_callback(msg):
    # 提取xyz坐标，单位为米
    x = msg.pose.position.x
    y = msg.pose.position.y
    z = msg.pose.position.z
    # 此处可添加自定义业务逻辑
    rospy.loginfo(f"四旋翼当前坐标：x={x:.2f}m, y={y:.2f}m, z={z:.2f}m")

if __name__ == "__main__":
    rospy.init_node("quadrotor_pose_receiver", anonymous=True)
    # 替换为你配置的实际位姿话题路径
    rospy.Subscriber("/quadrotor/ground_truth/pose", PoseStamped, pose_callback)
    rospy.spin()

3. 基于现有GPS插件的替代方案 #

如果你不想新增插件，也可以对现有GPS插件输出的经纬度数据做坐标转换得到xyz坐标：

• 用libhector_gazebo_ros_gps.so发布的sensor_msgs/NavSatFix类型话题，获取经度、纬度、海拔数据
• 借助Python的utm库将经纬度转换为UTM平面坐标，得到x、y值
• z值取GPS输出的海拔减去仿真初始点的海拔，即可得到相对高度

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