原标题： ROS通过话题订阅得到自己需要的信息——Gazebo仿真软件中模型的位姿信息为例

来源：CSDN博客    链接：https://blog.csdn.net/qq_38427871/article/details/105042871

前言

最近毕业设计需要用到ROS去控制机械臂，且现在因为疫情被困家中只能用仿真的形式来完成，于是不得已通过仿真的形式完成实验。由于话题（Topic）是ROS系统中最常用的通信手段之一，ROS系统在运行时各个节点间的信息交互、参数传递等都需要用到发布和订阅话题的手段完成，但很多Fresh fish like me（像我一样的菜鸟）并不能在短时间内很好地从官方的教程提炼出其中的用法和思路。

授人以鱼不如授人以渔，本文重点讲解在一个系统或者一套（开源）代码运行起来之后，找到自己想要的信息，然后一步一步通过话题订阅获得自己想要的信息的思路。Gazebo是常用的物理仿真环境之一，本文就以Gazebo中对应模型的位姿信息为例。

笔者环境信息为： Ubuntu18.04 + ros-melodic 虽然本文内容基本不受环境影响，但笔者认为在相关话题中没有环境信息的话是不完整的。

查找所需信息所在的话题——若已知目标话题名可略过该节

首先我们需要将这个系统运行起来，如果是开源代码往往通过运行launch文件，自己写的代码也还有可能是运行自己的节点，如此我们才能在系统的消息列表中找到我们需要的信息。系统运行之后Gazebo界面如下图所示，在左侧可以看到当前环境中存在的模型信息。本文的目标模型为 box

系统开始运行之后，首先通过指令 rostopic list 得到当前系统中所有的消息名称

rostopic list

找到这时候往往会发现系统的消息名称列表特别多，很难找到自己想要的那一部分信息，于是我们快速拖动滑块浏览一下列表，快速查找有没有那一部分可能是我们需要的信息。不难发现在消息列表偏上的不分有几个话题是以/gazebo为开头的，这里面很可能就包含我们需要的信息。于是重新通过指令rostopic list /gazebo来查看所有以/gazebo为开头的话题列表。

rostopic list /gazebo

找到相关的消息列表后，其中第二行的消息为/gazebo/model_states,看名字就很像我们需要的消息内容，但如果并不确定的话可以继续往下查看该话题的相关信息。

rostopic info /gazebo/model_states

通过该指令查看话题信息后我们得到了该话题的类型、发布者和订阅者等信息，这更加笃信了我们的想法，最后还可以通过echo一下话题发布出来的内容最终检查一下话题中是否包含我们所需要的信息，以及所需信息在话题中的存在形式。

rostopic echo /gazebo/model_states

通过与Gazebo仿真界面中的信息对比，不难发现该话题发布的内容中存在我们需要的模型的名称信息name: box以及我么真正需要的位姿信息pose。到这一步就可以完全确认该话题就是我们所需要的那一个了。

订阅目标话题以获取所需参数

找到目标话题之后当然还没有完成任务，我们真正的目标是要从话题中获得我们需要的信息并传递到程序参数中，这就需要我们编写一个消息订阅节点以订阅目标话题，但在订阅目标话题之前我们还需要先搞清楚该话题的类型以及各个成员的类型。

获取目标话题类型信息以及成员信息

在上一节中，我们通过rostopic info /gazebo/model_states指令得知该话题的类型（Type）为gazebo_msg/ModelStates,这个信息就为我们在编写程序时需要声明何种标量类型来存储消息提供了指引。但若此处是一个我们从未见过的，比较陌生的消息类型，我们很难搞清楚我们所需要的那一部分信息在话题中所属的成员位置，于是我们将该消息类型复制到百度或者谷歌的搜索栏中，往往这时候会出现一个最后一个单词为Documentation的链接，点击进去之后就可找到该消息的参考信息。这里有经验的玩家可以直接通过查找ROS Pack 中的消息源文件查看。

找到具体消息类型之后，我们就可以轻而易举地索引到我们所需要的参数所在的位置。如果消息参考中仍然有不认识的消息类型，可以再次采用上述方法递归。到这里我们就可以开始着手写程序了。

消息订阅的C++代码

#include <gazebo_msgs/ModelStates.h>

#include <ros/ros.h>

#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

ros::Subscriber box_state_sub;

geometry_msgs::Pose pose;

void boxStatesCallback(const gazebo_msgs::ModelStates &box_state_current)

{

    int box_index = -1;

    std::vector<std::string> model_names = box_state_current.name;

    for(size_t i = 0; i < model_names.size(); i++)

    {

        if(model_names[i] == "box")

            box_index = i;

    }

    pose = box_state_current.pose[box_index];

    ROS_INFO_STREAM("Position:" << std::endl << pose);

}

int main(int argc, char **argv)

{

  ros::init(argc, argv, "box_subscriber_node");

  ros::NodeHandle node_handle;

  ros::AsyncSpinner spinner(1);

  spinner.start();

  ros::Subscriber box_states_sub = node_handle.subscribe("/gazebo/model_states", 1, boxStatesCallback);

  ROS_INFO_STREAM("END!");

  ros::waitForShutdown();

  return 0;

}

讲到代码之后事情反而变得简单了许多，毕竟可能许多人就直接复制过去用了，也不必明白太多。值得一提的是pose中包含的是模型的位姿信息，而不仅仅是位置信息（position）或姿态信息（orientation）。若要获取位置信息可以用pose.position或pose.position.x解决，姿态信息同理。

消息订阅的Python代码

因为在ROS开发中，C++语言会更常用一些，用Python代码的需求可能不多，所以这一段先空着，如果有需求留言的话我再补上。 另外本人也是初学者，若有文中存在错误，欢迎指出；若有问题不解，大家也可以在评论区留言交流。拜谢。

————————————————

版权声明：本文为CSDN博主「时雨月五」的原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接及本声明。

原文链接：https://blog.csdn.net/qq_38427871/article/details/105042871

一THE END一

免责声明：本文来自互联网新闻客户端自媒体，不代表本网的观点和立场。

合作及投稿邮箱：E-mail:editor@tusaishared.com