ROS知识（20）----使用Master_API查询Master管理的节点话题服务内容

#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