ROS知识(20)----使用Master_API查询Master管理的节点话题服务内容
在一些应用中会需要获取master的uri地址,发布的话题,订阅的话题,发布的服务,节点的信息等等。这些功能我们通常可一通过rosnode list, rosnode info, rostopic list, rostopic info, rosservice list和 rosservice info 等命令查询,而这些功能是怎么做到的呢? 这就需要用到Master_API进行查询,ROS官方使用了python来实现(在ros_comm包中),这里我们使用c++来实现这些查询的功能。
1.实现
参考[1]API,功能描述细节不累赘了,下面为实现的代码:
#include "ros/ros.h" #include "std_msgs/String.h" #include <ros/master.h> #include <iostream> /* * Get the URI of the master. */ bool getUri(std::string&uri) { XmlRpc::XmlRpcValue args, result, payload; args[0] = ros::this_node::getName(); if (!ros::master::execute("getUri", args, result, payload, true)) { std::cout << "Failed!" << std::endl; return false; } std::cout << "----------Master URI----------" << std::endl; uri = std::string(payload); std::cout << std::string(payload) << std::endl; } /* * Get list of topics that can be subscribed to. * This does not return topics that have no publishers. * See getSystemState() to get more comprehensive list. * */ bool getPublishedTopics(const std::string& subgraph, ros::master::V_TopicInfo& topics) { XmlRpc::XmlRpcValue args, result, payload; args[0] = ros::this_node::getName(); args[1] = subgraph; if (!ros::master::execute("getPublishedTopics", args, result, payload, true)) { std::cout << "Failed!" << std::endl; return false; } topics.clear(); std::cout << "----------PublishedTopics----------" << std::endl; std::cout << "published_topic_name \t message_name" << std::endl; for (int i = 0; i < payload.size(); ++i) { topics.push_back( ros::master::TopicInfo(std::string(payload[i][0]), std::string(payload[i][1]))); std::string v1 = std::string(payload[i][0]); std::string v2 = std::string(payload[i][1]); std::cout << v1.c_str() << "\t" << v2.c_str() << std::endl; } return true; } /* * Retrieve list topic names and their types. * */ bool getTopicTypes(ros::master::V_TopicInfo& topics) { XmlRpc::XmlRpcValue args, result, payload; args[0] = ros::this_node::getName(); if (!ros::master::execute("getTopicTypes", args, result, payload, true)) { std::cout << "Failed!" << std::endl; return false; } topics.clear(); std::cout << "----------TopicTypes----------" << std::endl; std::cout << "topic_name\t message_name" << std::endl; for (int i = 0; i < payload.size(); ++i) { topics.push_back( ros::master::TopicInfo(std::string(payload[i][0]), std::string(payload[i][1]))); std::string v1 = std::string(payload[i][0]); std::string v2 = std::string(payload[i][1]); std::cout << v1.c_str() << "\t" << v2.c_str() << std::endl; } return true; } /* * Get the XML-RPC URI of the node with the associated name/caller_id. * This API is for looking information about publishers and subscribers. * Use lookupService instead to lookup ROS-RPC URIs. */ bool lookupNode(const std::string& node, std::string&uri) { XmlRpc::XmlRpcValue args, result, payload; args[0] = ros::this_node::getName(); args[1] = node; if (!ros::master::execute("lookupNode", args, result, payload, true)) { std::cout << "Failed!" << std::endl; return false; } std::cout << "----------LookupedNode----------" << std::endl; uri = std::string(payload); std::cout << node << ":" << std::string(payload) << std::endl; } /* * Lookup all provider of a particular service. */ bool lookupService(const std::string& service, std::string&uri) { XmlRpc::XmlRpcValue args, result, payload; args[0] = ros::this_node::getName(); args[1] = service; if (!ros::master::execute("lookupService", args, result, payload, true)) { std::cout << "Failed!" << std::endl; return false; } std::cout << "----------LookupedService----------" << std::endl; uri = std::string(payload); std::cout << service << ":" << std::string(payload) << std::endl; } /* * Retrieve list representation of system state * (i.e. publishers, subscribers, and services). * */ bool getSystemState() { XmlRpc::XmlRpcValue args, result, payload; args[0] = ros::this_node::getName(); if (!ros::master::execute("getSystemState", args, result, payload, true)) { std::cout << "Failed!" << std::endl; return false; } std::cout << "----------SystemState----------" << std::endl; //publishers int t = 0; std::cout << "Published Topics:" << std::endl; for (int j = 0; j < payload[t].size(); ++j) { //topics std::cout << " *" << std::string(payload[t][j][0]) << ":" << std::endl; for (int k = 0; k < payload[t][j][1].size(); ++k) { //publisher std::cout << " *" << std::string(payload[t][j][1][k]) << std::endl; } } t = 1; std::cout << "Subscribed Topics:" << std::endl; for (int j = 0; j < payload[t].size(); ++j) { //topics std::cout << " *" << std::string(payload[t][j][0]) << ":" << std::endl; for (int k = 0; k < payload[t][j][1].size(); ++k) { //publisher std::cout << " *" << std::string(payload[t][j][1][k]) << std::endl; } } t = 2; std::cout << "Services:" << std::endl; for (int j = 0; j < payload[t].size(); ++j) { //topics std::cout << " *" << std::string(payload[t][j][0]) << ":" << std::endl; for (int k = 0; k < payload[t][j][1].size(); ++k) { //publisher std::cout << " *" << std::string(payload[t][j][1][k]) << std::endl; } } return true; } int main(int argc, char **argv) { ros::init(argc, argv, "listener"); ros::NodeHandle n; //get master uri std::string host_uri; getUri(host_uri); //get published topics std::string subgraph; ros::master::V_TopicInfo published_topics; getPublishedTopics(subgraph, published_topics); //get topic types ros::master::V_TopicInfo topics; getTopicTypes(topics); //get node uri std::string node, node_uri; node = "/talker"; lookupNode(node, node_uri); //get service uri std::string service, service_uri; service = "/talker"; lookupService(service, service_uri); //get all published topics,subscribed topics and services getSystemState(); ros::spin(); return 0; }
查询的结果如下:
----------Master URI---------- http://192.168.1.21:11311/ ----------PublishedTopics---------- published_topic_name message_name /rosout rosgraph_msgs/Log /rosout_agg rosgraph_msgs/Log /talker/message std_msgs/String ----------TopicTypes---------- topic_name message_name /rosout rosgraph_msgs/Log /rosout_agg rosgraph_msgs/Log /talker/message std_msgs/String ----------LookupedNode---------- /talker:http://192.168.1.21:44625/ Failed! ----------SystemState---------- Published Topics: */rosout: */talker */listener */rosout_agg: */rosout */talker/message: */talker Subscribed Topics: */rosout: */rosout Services: */talker/set_logger_level: */talker */listener/set_logger_level: */listener */rosout/get_loggers: */rosout */talker/get_loggers: */talker */rosout/set_logger_level: */rosout */listener/get_loggers: */listener
2.参考资料
[1]. Master_API
Make Change - Focus on Computer Vision and Pattern Recognition
版权声明:本文为博主原创文章,未经博主允许不得转载
版权声明:本文为博主原创文章,未经博主允许不得转载