簡介

經常做資料處理的同學可能比較熟悉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()