码迷,mamicode.com
首页 > Windows程序 > 详细

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

时间:2017-06-29 13:33:27      阅读:573      评论:0      收藏:0      [点我收藏+]

标签:sage   init   eve   upn   标题   org   地址   ctypes   参考资料   

在一些应用中会需要获取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

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

标签:sage   init   eve   upn   标题   org   地址   ctypes   参考资料   

原文地址:http://www.cnblogs.com/cv-pr/p/7093861.html

(0)
(0)
   
举报
评论 一句话评论(0
登录后才能评论!
© 2014 mamicode.com 版权所有  联系我们:gaon5@hotmail.com
迷上了代码!