rqt_plot plot 2d data

edit

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/bl... 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_commo... (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... can ... (more)