使用ROS的rqt_plot对任意语言的程序进行可视化

简介

经常做数据处理的同学可能比较熟悉MATLAB或者Python，而做图像或者机器人用到最多的其实是C和C++。经常需要在调试时实时看到某些数据的变化趋势，而C++却没有一套好的可视化库（或者需要很麻烦的配置。）于是写了这个工具。调用者只要在程序里打印需要实时显示的数据，然后将控制台信息通过管道传给Python程序即可自动在rqt_plot里显示，并自动发布ROS话题。

效果

使用方法及源码

#! /usr/bin/python
# coding:utf-8
"""
    本脚本将任意程序输出转为ROS话题发布出来，便于通过可视化程序直接查看数据。
    用法：
    cmd | python debug_in_ros.py [TOPIC_NAME] [PLOT_NUMS]
    调用者需要在自己的程序中将需要调试的数据按如下格式打印：
    [TOPIC_NAME] [DATA] [NUMBER]
    数据间用空格隔开。
    如，我的程序a输出为：
    odom 1 2 3
    调用 a | python debug_in_ros.py odom 3
    则本程序将发布ros话题odom，并实时用rqt_plot绘制出波形，类型为Float64MultiArray。
"""
import sys
import os
import numpy as np
import rospy
import std_msgs.msg as ros_msg
def debug_in_ros():

    array = ros_msg.Float64MultiArray()
    rospy.init_node('debug_in_ros')
    pub = rospy.Publisher(sys.argv[1], ros_msg.Float64MultiArray, queue_size=10)
    rate = rospy.Rate(100)
    while not rospy.is_shutdown():
        try:
            line = raw_input("")
        except:
            break
        if line == '':
            return
        line = line.strip()
        tokens = line.split(' ')
        if len(tokens) <= 2:
            continue

        if tokens[0] == sys.argv[1]:
            array.data = [float(token) for token in tokens[1:]]
            pub.publish(array)
        rate.sleep()


if __name__ == '__main__':
    if len(sys.argv) < 3:
        print __doc__
        sys.exit()
    data = ['%s/data[%d]' % (sys.argv[1],i) for i in range(int(sys.argv[2]))]
    data = ' '.join(data)
    os.system('rosrun rqt_plot rqt_plot topics %s&' % data)
    debug_in_ros()