I have a sensor_msgs/PointCloud2 with [x,y,z] and how can I plot it in real-time in matplotlib like this code here. I already changed the type from Odometry to pointcloud2 but I don't know what to change in odom_callback or how to change the code in order to plot it in matplotlib. Can someone has an idea how to plot pointcloud2 in matplotlib
import matplotlib.pyplot as plt
import rospy
import tf
from sensor_msgs.msg import PointCloud2
from tf.transformations import quaternion_matrix
import numpy as np
from matplotlib.animation import FuncAnimation
class Visualiser:
def __init__(self):
self.fig, self.ax = plt.subplots()
self.ln, = plt.plot([], [], 'ro')
self.x_data, self.y_data = [] , []
def plot_init(self):
self.ax.set_xlim(0, 10000)
self.ax.set_ylim(-7, 7)
return self.ln
def getYaw(self, pose):
quaternion = (pose.orientation.x, pose.orientation.y, pose.orientation.z,
pose.orientation.w)
euler = tf.transformations.euler_from_quaternion(quaternion)
yaw = euler[2]
return yaw
def odom_callback(self, msg):
yaw_angle = self.getYaw(msg.pose.pose)
self.y_data.append(yaw_angle)
x_index = len(self.x_data)
self.x_data.append(x_index+1)
def update_plot(self, frame):
self.ln.set_data(self.x_data, self.y_data)
return self.ln
rospy.init_node('publisher_node')
vis = Visualiser()
sub = rospy.Subscriber('/scan3dd', PointCloud2, vis.odom_callback)
ani = FuncAnimation(vis.fig, vis.update_plot, init_func=vis.plot_init)
plt.show(block=True)
