def loop(self): # Main Plotting Loop. Updates plot with info from subscriber callbacks. r = rospy.Rate(10) while not rospy.is_shutdown(): # Update MPC OL + Reference Trajectories. self.l2.set_xdata(self.x_ref_traj); self.l2.set_ydata(self.y_ref_traj) self.l3.set_xdata(self.x_mpc_traj); self.l3.set_ydata(self.y_mpc_traj) self.zl2.set_xdata(self.x_ref_traj); self.zl2.set_ydata(self.y_ref_traj) self.zl3.set_xdata(self.x_mpc_traj); self.zl3.set_ydata(self.y_mpc_traj) # Update Vehicle Plot. FrontBody, RearBody, FrontAxle, RearAxle, RightFrontTire, RightRearTire, LeftFrontTire, LeftRearTire = \ plotVehicle(self.x_vehicle, self.y_vehicle, self.psi_vehicle, self.df_vehicle, self.a, self.b, self.vW, self.rW) self.vl0.set_xdata(self.x_vehicle) self.vl0.set_ydata(self.y_vehicle) self.vl1.set_xdata(FrontBody[0,:]) self.vl1.set_ydata(FrontBody[1,:]) self.vl2.set_xdata(RearBody[0,:]) self.vl2.set_ydata(RearBody[1,:]) self.vl3.set_xdata(FrontAxle[0,:]) self.vl3.set_ydata(FrontAxle[1,:]) self.vl4.set_xdata(RearAxle[0,:]) self.vl4.set_ydata(RearAxle[1,:]) self.vl5.set_xdata(RightFrontTire[0,:]) self.vl5.set_ydata(RightFrontTire[1,:]) self.vl6.set_xdata(RightRearTire[0,:]) self.vl6.set_ydata(RightRearTire[1,:]) self.vl7.set_xdata(LeftFrontTire[0,:]) self.vl7.set_ydata(LeftFrontTire[1,:]) self.vl8.set_xdata(LeftRearTire[0,:]) self.vl8.set_ydata(LeftRearTire[1,:]) self.zvl0.set_xdata(self.x_vehicle) self.zvl0.set_ydata(self.y_vehicle) self.zvl1.set_xdata(FrontBody[0,:]) self.zvl1.set_ydata(FrontBody[1,:]) self.zvl2.set_xdata(RearBody[0,:]) self.zvl2.set_ydata(RearBody[1,:]) self.zvl3.set_xdata(FrontAxle[0,:]) self.zvl3.set_ydata(FrontAxle[1,:]) self.zvl4.set_xdata(RearAxle[0,:]) self.zvl4.set_ydata(RearAxle[1,:]) self.zvl5.set_xdata(RightFrontTire[0,:]) self.zvl5.set_ydata(RightFrontTire[1,:]) self.zvl6.set_xdata(RightRearTire[0,:]) self.zvl6.set_ydata(RightRearTire[1,:]) self.zvl7.set_xdata(LeftFrontTire[0,:]) self.zvl7.set_ydata(LeftFrontTire[1,:]) self.zvl8.set_xdata(LeftRearTire[0,:]) self.zvl8.set_ydata(LeftRearTire[1,:]) self.ax_zoom.set_xlim(self.x_vehicle - self.window, self.x_vehicle + self.window) self.ax_zoom.set_ylim(self.y_vehicle - self.window, self.y_vehicle + self.window) self.f.canvas.draw() plt.pause(0.001) r.sleep()
def loop(self):
r = rospy.Rate(10)
while not rospy.is_shutdown():
# Update Plot with fresh data.
self.l_arr[1].set_xdata(self.x_ref_traj)
self.l_arr[1].set_ydata(self.y_ref_traj)
self.l_arr[2].set_xdata(self.x_vehicle)
self.l_arr[2].set_ydata(self.y_vehicle)
self.l_arr[3].set_xdata(self.x_mpc_traj)
self.l_arr[3].set_ydata(self.y_mpc_traj)
self.f.canvas.draw()
plt.pause(0.001)
r.sleep()
def loop(self):
# Main Plotting Loop. Updates plot with info from subscriber callbacks.
r = rospy.Rate(10)
while not rospy.is_shutdown():
# Update Plot with fresh data.
self.l_arr[1].set_xdata(self.x_ref_traj)
self.l_arr[1].set_ydata(self.y_ref_traj)
self.l_arr[2].set_xdata(self.x_vehicle)
self.l_arr[2].set_ydata(self.y_vehicle)
self.l_arr[3].set_xdata(self.x_mpc_traj)
self.l_arr[3].set_ydata(self.y_mpc_traj)
self.f.canvas.draw()
plt.pause(0.001)
r.sleep()