Robotics StackExchange | Archived questions

rqt_plot plot 2d data

Hi everyone.

I have a topic which publishes arrays of data. Is there a way to use rqt_plot to plot the arrays in real time?

This is not like visualizing a topic which publishes numbers and plot value vs time. What I need is just plotting the data (a plottable array of floats) anytime a message is published, and plot value vs index over and over.

Thanks

Asked by crpizarr on 2014-06-26 10:33:14 UTC

Comments

Answers

One very quick and dirty method is to use rviz as an oscilloscope, and have a node that converts the array message to a Marker with a LINE_STRIP in it. https://github.com/lucasw/sdl2_ros/blob/master/scripts/array_to_marker.py does this for the Int16MultiArray message (it doesn't do anything with the multi array variables, just plots the data).

    self.pub = rospy.Publisher("marker", Marker, queue_size=2)

    marker = Marker()
    marker.header.frame_id = rospy.get_param("~frame_id", "map")
    marker.ns = "plot"  # marker.header.frame_id
    marker.id = 0
    marker.type = Marker.LINE_STRIP
    marker.action = Marker.ADD
    marker.pose.orientation.w = 1.0
    # should be smaller then index_scale if individual points can vary a lot
    scale = rospy.get_param("~line_thickness", 0.01)
    marker.scale.x = scale
    marker.scale.y = scale
    marker.scale.z = scale

    marker.color.a = 1.0
    marker.color.r = rospy.get_param("~r", 1.0)
    marker.color.g = rospy.get_param("~g", 1.0)
    marker.color.b = rospy.get_param("~b", 1.0)
    self.marker = marker

    self.max_points = rospy.get_param("~max_points", 1000)
    self.step = rospy.get_param("~step", 1)
    self.index_scale = rospy.get_param("~index_scale", 0.001)
    self.y_scale = rospy.get_param("~y_scale", 1.0 / (2**15))
    self.sub = rospy.Subscriber("audio", Int16MultiArray,
                                self.callback, queue_size=1)

def callback(self, msg):
    self.marker.points = []
    for i in range(0, len(msg.data), self.step):
        if (i > self.max_points):
            break
        pt = Point()
        pt.x = i * self.index_scale
        pt.y = msg.data[i] * self.y_scale
        pt.z = 0
        self.marker.points.append(pt)

This method isn't efficient for thousands and thousands of points, though, and the line thickness needs to be manipulated depending on the data (being able to set Marker line width in distance independent pixels would help).

I might clean this up, generalize it, and maybe make it a standalone package.

Another very quick and more efficient method is to plot the data into an Image and publish it: https://github.com/lucasw/audio_common/blob/appsink/audio_to_float/scripts/view.py (uses ChannelFloat32)

def __init__(self):
    self.bridge = CvBridge()
    self.fade1 = rospy.get_param("~fade1", 0.9)
    self.fade2 = rospy.get_param("~fade2", 0.99)
    self.buffer = collections.deque(maxlen=8192)
    self.im = np.zeros((256, 1300, 3), np.uint8)
    self.pub = rospy.Publisher("image", Image, queue_size=1)
    self.sub = rospy.Subscriber("decoded", ChannelFloat32,
                                self.audio_callback, queue_size=1)
    self.timer = rospy.Timer(rospy.Duration(0.05), self.update)

def audio_callback(self, msg):
    for i in range(len(msg.values)):
        self.buffer.append(msg.values[i])

def update(self, event):
    self.im[:, :, 1:3] = (self.im[:, :, 1:3] * self.fade1).astype(np.uint8)
    self.im[:, :, 0] = (self.im[:, :, 0] * self.fade2).astype(np.uint8)
    width = self.im.shape[1]
    height = self.im.shape[0]
    last_y = 0
    for i in range(0, width):
        if i >= len(self.buffer):
            break
        sample = self.buffer[i]
        sample *= height/2
        sample += height/2
        y = int(sample) % height
        y0 = min(last_y, y)
        y1 = max(last_y, y)
        self.im[y0:y1+1, i, :] = 255
        last_y = y
    self.pub.publish(self.bridge.cv2_to_imgmsg(self.im, "bgr8"))

Maybe plotjuggler, rqt_plot, or the new graph_rviz_plugin https://gitlab.com/InstitutMaupertuis/graph_rviz_plugin/ can do it already or it wouldn't be hard to add it?

Asked by lucasw on 2018-08-12 15:56:31 UTC

Comments

Or you could just republish the array on single topics if the amount of data is reasonable. Even quicker, even dirtier.

Asked by fvd on 2018-08-12 19:27:41 UTC

With PlotJuggler you can see on the left side the most recent value of each field.

https://github.com/facontidavide/PlotJuggler

image description

If you are visualizing an array, you will be able to see the last received value of each of the indexes.

Asked by Davide Faconti on 2018-10-25 02:41:03 UTC

Comments

in case anyone else ends up here I've hacked a solution to this problem for the uint16 multi-array message type and added it to the graph_rviz_plugin.

https://github.com/flynneva/graph_rviz_plugin

right now it just counts up from 0 to the size of the array in the x axis and then puts the values of the multi-array on the y.

similar types of hacks could be done for the other multi-array types I think.

UPDATE: just added all the rest of the multi-array types to this. havent tested it but it "should" work i think.

Asked by flynneva on 2020-11-09 14:34:49 UTC

Comments