Exemplo n.º 1
0
	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()
Exemplo n.º 2
0
 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()
Exemplo n.º 3
0
 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()