def create_artists(self, legend, orig_handle, xdescent, ydescent, width,
height, fontsize, trans):
center = 0.5 * width - 0.5 * xdescent, 0.5 * height - 0.5 * ydescent
p = Wedge(center,
height / 1.5,
orig_handle.theta1,
orig_handle.theta2,
width=orig_handle.width)
self.update_prop(p, orig_handle, legend)
p.set_transform(trans)
return [p]
def main_master_plot():
global X, Y, THETA, trajectory_x, trajectory_y
global sigma_matrix
rospy.loginfo("Starting master plot node")
rospy.init_node('main_master_plot', anonymous=True)
rospy.Subscriber('robot_position', Pose, positionCallback)
rospy.Subscriber('robot_uncertainty', Covariance, uncertaintyCallback)
rate = rospy.Rate(30)
if matplotlib.__version__ < '2.2.4':
rospy.logfatal(
"Update matplotlib to run the plot node: Version 2.2.4 is required"
)
kill_node()
f, ax1 = plt.subplots()
ax1.grid(b=True, which='major', color='#AAAAAA', linestyle='-')
plt.xlim(0, 250)
plt.ylim(0, 250)
plt.ion()
win = plt.gcf().canvas.manager.window
win.protocol("WM_DELETE_WINDOW", kill_node)
f.canvas.set_window_title('Robot position')
size = f.get_size_inches() * f.dpi
windowWidth = size[0]
windowHeight = size[1]
positionRight = int(win.winfo_screenwidth() / 2 - windowWidth / 2)
positionDown = int(win.winfo_screenheight() / 2.3 - windowHeight / 2)
win.geometry("+{}+{}".format(positionRight, positionDown))
plt.show()
addedImg = False
while not rospy.is_shutdown():
x_unc = np.sqrt(sigma_matrix[0, 0]) / 10
y_unc = np.sqrt(sigma_matrix[1, 1]) / 10
theta_unc = np.sqrt(sigma_matrix[2, 2]) * 180 / np.pi
if addedImg:
robotImgMPL.remove()
trajectory.remove()
oval_uncertainty.remove()
wedge_uncertainty.remove()
del robotImgMPL
del trajectory
del oval_uncertainty
del wedge_uncertainty
robotImgMPL = ax1.imshow(img, zorder=10)
tr = transforms.Affine2D().scale(0.3, 0.3)
tr.rotate_around(132 * 0.3 / 2, 132 * 0.3 / 2, THETA)
tr.translate(X - 132 * 0.3 / 2, Y - 132 * 0.3 / 2)
tr = tr + ax1.transData
robotImgMPL.set_transform(tr)
lock.acquire()
trajectory, = ax1.plot(trajectory_x, trajectory_y, c='firebrick')
lock.release()
oval_uncertainty = Ellipse((0, 0),
width=x_unc * 2,
height=y_unc * 2,
facecolor='lightblue',
edgecolor='midnightblue',
alpha=0.3,
zorder=20)
tr_oval = transforms.Affine2D().translate(X, Y) #rotate(THETA)
oval_uncertainty.set_transform(tr_oval + ax1.transData)
ax1.add_patch(oval_uncertainty)
wedge_uncertainty = Wedge((0, 0),
45,
theta1=-theta_unc,
theta2=theta_unc,
zorder=5,
color='cornflowerblue',
alpha=0.5)
tr_wedge = transforms.Affine2D().rotate(THETA).translate(X, Y)
wedge_uncertainty.set_transform(tr_wedge + ax1.transData)
ax1.add_patch(wedge_uncertainty)
addedImg = True
plt.draw()
plt.pause(0.001)
rate.sleep()
