robot_width = config["drivetrain"]["width"]
robot_length = config["drivetrain"]["length"]
robot_rot = config["robot"]["rot"]
robot_x = config["robot"]["x"]
robot_y = config["robot"]["y"]
drivetrain = DifferentialDrive(robot_width)
robot = Robot(drivetrain, robot_width * ppi, robot_length * ppi, position=[robot_x, robot_y], rotation=robot_rot)
dt = 0
last_time = datetime.now().microsecond / 1000000
while True:
simfield = np.copy(field)
robot.update(dt)
pts = np.array(robot.points, np.int32)
pts = pts.reshape((-1, 1, 2))
cv2.fillPoly(simfield, [pts], (0, 255, 255))
cv2.putText(simfield, f"L: {drivetrain.l_speed:.2f}", (15, 50), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255))
cv2.putText(simfield, f"R: {drivetrain.r_speed:.2f}", (15, 100), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255))
cv2.putText(simfield, f"F: {robot.forward:.2f}", (200, 50), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255))
cv2.putText(simfield, f"T: {robot.turn:.2f}", (200, 100), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255))
cv2.imshow("Sim", simfield)
key = cv2.waitKey(5)
if key != -1:
print(key)
if key == 113:
class AspectsTuner:
def __init__(self, window, window_title):
self.window = window
self.robot = Robot('tkinter')
self.window.title(window_title)
# register behaviour on clicking red cross 'close' button
self.window.protocol("WM_DELETE_WINDOW", self.close_window)
# create two notebooks side by side , one for plain eye images, second for aspects
self.eye_notebook = ttk.Notebook(window)
self.aspect_notebook = ttk.Notebook(window)
# create notebook frame for each eye and add it to notebook
for eye in self.robot.cognition.eyes:
frame = EyeFrame(self.eye_notebook, eye,
self.eyeframe_click_callback)
self.eye_notebook.add(frame, text=eye.name)
# bind eye frame notebook tab change routine
frame.bind("<Visibility>", self.on_eye_tab_changed)
current_eye = self.get_current_eye_frame().eye
# make tabs for first eye aspects
self.populate_aspect_notebook(current_eye)
current_aspect = self.get_current_aspect_frame().aspect
# create aspect details frame
self.aspect_details = AspectDetails(window, 'Details', current_aspect)
self.btn_save_all_aspects = tkinter.Button(
window,
text="Save all aspects",
width=20,
command=self.save_all_aspects)
self.btn_load_all_aspects = tkinter.Button(
window,
text="Load all aspects",
width=20,
command=self.load_all_aspects)
self.btn_start_record = tkinter.Button(window,
text="Start Recorder",
width=20,
command=self.start_recorder)
self.btn_start_chaser = tkinter.Button(window,
text="Start Chaser",
width=20,
command=self.start_chaser)
# grid manager
self.eye_notebook.grid(column=0, row=0, columnspan=3)
self.aspect_notebook.grid(column=4, row=0, columnspan=3)
self.btn_save_all_aspects.grid(column=0, row=1)
self.btn_load_all_aspects.grid(column=1, row=1)
self.btn_start_record.grid(column=2, row=1)
self.btn_start_chaser.grid(column=3, row=1)
self.aspect_details.grid(column=4, row=1, columnspan=3)
# # Define the codec and create VideoWriter object
# self.fourcc = cv2.VideoWriter_fourcc(*"MJPG")
# self.out = cv2.VideoWriter('output.avi', self.fourcc, 15.0, (600, 450))
# After it is called once, the update method will be automatically called every delay milliseconds
# should be about 30FPS
self.delay = 30
self.update()
self.window.mainloop()
def eyeframe_click_callback(self, event):
current_aspect = self.get_current_aspect_frame().aspect
x = event.x
y = event.y
# get bgr colour from plain aspect of current eye
notebook_frame = self.get_current_eye_frame()
image_frame = notebook_frame.eye.get_aspect('plain').frame
# define colour boundaries based on BGR colour LAB
current_aspect.BGR_colour_acquired(image_frame[y, x])
self.aspect_details.haul_aspect_details(current_aspect)
def on_eye_tab_changed(self, event):
# which eye become visible
self.populate_aspect_notebook(event.widget.eye)
def populate_aspect_notebook(self, eye):
# clear aspect frames notebook
for i, item in enumerate(self.aspect_notebook.tabs()):
self.aspect_notebook.forget(item)
# create notebook frame for each aspect and add it to notebook
for aspect in eye.aspects_list:
# add every aspect excluding 'plain'
if aspect.name is not 'plain':
frame = AspectFrame(self.aspect_notebook, eye, aspect)
self.aspect_notebook.add(frame, text=aspect.name)
frame.bind("<Visibility>", self.on_aspect_tab_changed)
def on_aspect_tab_changed(self, event):
# get current aspect
self.aspect_details.haul_aspect_details(event.widget.aspect)
def update(self):
# update robot state including taking images from cameras
self.robot.update()
# show active eye image on tab
notebook_frame = self.get_current_eye_frame()
image_frame = notebook_frame.eye.get_aspect('plain').frame
image_canvas = notebook_frame.canvas
notebook_frame.photo = PIL.ImageTk.PhotoImage(
image=PIL.Image.fromarray(image_frame))
image_canvas.tag_lower(
image_canvas.create_image(0,
0,
image=notebook_frame.photo,
anchor=tkinter.NW))
# # save to file
# self.out.write(image_frame)
# show active eye aspect image on tab
notebook_frame = self.get_current_aspect_frame()
image_frame = notebook_frame.aspect.frame
image_canvas = notebook_frame.canvas
notebook_frame.photo = PIL.ImageTk.PhotoImage(
image=PIL.Image.fromarray(image_frame))
image_canvas.tag_lower(
image_canvas.create_image(0,
0,
image=notebook_frame.photo,
anchor=tkinter.NW))
self.window.after(self.delay, self.update)
def get_current_eye_frame(self):
index = self.eye_notebook.index("current")
tab_id = self.eye_notebook.tabs()[index]
tab_id = tab_id[tab_id.rindex('.') + 1:]
return self.eye_notebook.children[tab_id]
def get_current_aspect_frame(self):
index = self.aspect_notebook.index("current")
tab_id = self.aspect_notebook.tabs()[index]
tab_id = tab_id[tab_id.rindex('.') + 1:]
return self.aspect_notebook.children[tab_id]
def close_window(self):
self.robot.shut_down()
self.out.release()
self.window.destroy()
def save_all_aspects(self):
self.robot.cognition.save_all_aspects()
def load_all_aspects(self):
self.robot.cognition.load_all_aspects()
# refresh aspect details
self.aspect_details
def start_recorder(self):
self.robot.start_recorder()
def start_chaser(self):
self.robot.start_chaser()
