How can I 3D plot "geometry_msgs/Posestamped" waypoint with Matplotlib (or with any other method)? I am trying to plot the waypoint of the HTC VIVE's controllers.
I have tried to subscribe to the message and tried to extract x, y and z position and plot it with Matplotlib but it does not seem to work.
from geometry_msgs.msg import Pose, PoseStamped
from std_msgs.msg import Float32
import rospy
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
Class DataVive:
def __init__(self):
self.right = []
self.trigger = Float32()
self.sub_right = [rospy.Subscriber("/vive/right/pose", PoseStamped, self.rightCallback, queue_size=1)]
self.sub_trigger = [[rospy.Subscriber("/vive/right/trigger", Float32, self.triggerCallback, queue_size=1)]
def rightCallback(self,msg):
self.right.append(msg)
if len(self.right) > 1000:
self.right = self.right[len(self.right)-1000:]
def triggerCallback(self,msg):
self.trigger = msg
if __name__ == "__main__":
rospy.init_node("trajectory_plot")
path = DataVive()
vive_trajectory = path.right
trigger_pressed = path.trigger
while not rospy.is_shutdown():
for i in range(len(vive_trajectory)):
x = vive_trajectory[i].pose.position.x
y = vive_trajectory[i].pose.position.y
z = vive_trajectory[i].pose.position.z
x_ =[]
y_ =[]
z_ =[]
x_.append(x)
y_.append(y)
z_.append(z)
if trigger_pressed.data > 0.99:
ax = plt.figure().add_subplot(projection='3d')
ax.plot(x_, y_, z_, zdir='z', label='curve in (x, y)')
ax.legend()
ax.set_xlim(0, 1)
ax.set_ylim(0, 1)
ax.set_zlim(0, 1)
ax.set_xlabel('X')
ax.set_ylabel('Y')
ax.set_zlabel('Z')
ax.view_init(elev=20., azim=-35)
if trigger_pressed.data < 0.99
plt.show()
break
This is the Posestamped message from my vr_publisher
Do my methods have any mistakes (I am a newbie with matplotlib)? I would love to plot a line of the trajectory from the Posestamped message.