ROS中gazebo仿真时通过ModelStates获取模型状态的方法

1、C++代码

1.1 先引入所需的头文件：

#include <ros/ros.h>
#include <gazebo_msgs/ModelStates.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/Twist.h>

重点是那个ModelStates.h

1.2 创建 ROS 节点并订阅模型状态话题

ros::NodeHandle nh;
ros::Subscriber sub = nh.subscribe("/gazebo/model_states", 1000, modelStatesCallback);

其中，“/gazebo/model_states” 是 Gazebo 仿真中的话题名字，1000 是接受该话题的消息队列长度（即 1s 内接受的最大消息数量），第三个参数是回调函数。

1.3 定义回调函数 modelStatesCallback，处理接受的状态信息

void modelStatesCallback(const gazebo_msgs::ModelStates::ConstPtr& msg)
{
    // 遍历模型列表
    for (int i = 0; i < msg->name.size(); i++)
    {
    	// 如果不确定模型的名字，可以先打印出来看一看
    	cout << "model_name: " << msg->name[i] << endl;
        // 如果当前模型是 our_model
        if (msg->name[i] == "our_model")
        {
            // 获取模型的位置和姿态
            geometry_msgs::Pose pose = msg->pose[i];
            // 处理信息
            double x = pose.position.x;
            double y = pose.position.y;
            double z = pose.position.z;
            
            // 获取模型的速度和角速度
            geometry_msgs::Twist twist = msg->twist[i];
            // 处理信息
            double linear_x = twist.linear.x;    // 模型线速度在 x 轴的分量
            double linear_y = twist.linear.y;    // 模型线速度在 y 轴的分量
            double linear_z = twist.linear.z;    // 模型线速度在 z 轴的分量
            double angular_x = twist.angular.x;  // 模型角速度在 x 轴的分量
            double angular_y = twist.angular.y;  // 模型角速度在 y 轴的分量
            double angular_z = twist.angular.z;  // 模型角速度在 z 轴的分量
        }
    }
}

这样，当有新的模型状态发布时，会调用回调函数处理状态信息。在回调函数中，遍历 ModelStates 消息列表，寻找指定名称的模型，从而获取该模型的状态等信息。
这里我是用话题通信的方式，其实也可以用服务通信的方式实现。需要注意的是，在使用订阅话题的方式获取模型状态时，需要遍历消息列表查找指定模型，相对于服务接口来说，效率会较低。但其优点是，可以实时监听模型状态变化，适用于需要频繁获取模型状态的场景。

2、Python代码

其实Python里也是同理的

2.1 先引入所需的头文件：

# -*- coding: utf-8 -*-
import pandas as pd
import os
import rospy
import datetime

from std_msgs.msg import String
from geometry_msgs.msg import Pose
from geometry_msgs.msg import Twist
from gazebo_msgs.msg import ModelStates

2.2 建立节点和订阅话题

def listener():
    rospy.init_node('get_gazebo',anonymous=True) #建立节点
    rospy.Subscriber('/gazebo/model_states',ModelStates,callback) #订阅话题
    rospy.spin() #调用回调函数，并阻塞，直到程序结束

2.3 回调函数

def callback(data):
    print 'Begin callback'
    for i,x in enumerate(data.name):
        #print('i: ', i)
        #print('x: ', x) #测试用
        #修改这里来确定所需要记录的模型，以gazebo界面左侧显示的模型名字为准
        if x == 'model_name':
            print 'recording model_name'
            pose=data.pose[i]
            quat=pose.orientation
            pos=pose.position
            pos_x = pos.x
            pos_y = pos.y
            pos_z = pos.z

2.4 主函数

if __name__ == '__main__':
    listener()