def main():
print("Loading smoothie adapter")
smoothie = adapters.SmoothieAdapter(config.SMOOTHIE_HOST)
while True:
print("Calibrating cork")
smoothie.ext_calibrate_cork()
smoothie.wait_for_all_actions_done()
print("Aligning cork to center")
res = smoothie.ext_align_cork_center(config.XY_F_MAX)
smoothie.wait_for_all_actions_done()
print(res)
print("Extracting, cork down")
# extraction, cork down
res = smoothie.custom_move_for(F=1700, Z=-35)
smoothie.wait_for_all_actions_done()
print(res)
# extraction, cork up
print("Extracting, cork up")
res = smoothie.ext_cork_up()
smoothie.wait_for_all_actions_done()
print(res)
print("30 sec delay...")
time.sleep(30)
def main():
if not os.path.exists(LOG_DIR):
try:
os.mkdir(LOG_DIR)
except OSError:
print("Creation of the directory %s failed" % LOG_DIR)
logging.error("Creation of the directory %s failed" % LOG_DIR)
else:
print("Successfully created the directory %s " % LOG_DIR)
logging.info("Successfully created the directory %s " % LOG_DIR)
# create smoothieboard adapter (API for access and control smoothieboard)
while True:
try:
smoothie = adapters.SmoothieAdapter(config.SMOOTHIE_HOST)
log_msg = "Successfully connected to smoothie"
print(log_msg)
logging.info(log_msg)
break
except OSError as error:
logging.warning(repr(error))
print(repr(error))
detector_simulation = multiprocessing.Process(target=detection_process)
detector_simulation.start()
log_msg = "Started working at " + str(str(datetime.datetime.now()).split(".")[:-1])[2:-2].replace(":", "-")
print(log_msg)
logging.info(log_msg)
while True:
do_extractions(smoothie)
move_forward(smoothie)
def main():
try:
create_directories(WITH_PLANTS_DIR, WITHOUT_PLANTS_DIR)
working_zone_polygon = Polygon(config.WORKING_ZONE_POLY_POINTS)
with adapters.SmoothieAdapter(config.SMOOTHIE_HOST) as smoothie:
with adapters.CameraAdapterIMX219_170(config.CROP_W_FROM, config.CROP_W_TO, config.CROP_H_FROM,
config.CROP_H_TO, config.CV_ROTATE_CODE,
config.ISP_DIGITAL_GAIN_RANGE_FROM, config.ISP_DIGITAL_GAIN_RANGE_TO,
config.GAIN_RANGE_FROM, config.GAIN_RANGE_TO,
config.EXPOSURE_TIME_RANGE_FROM, config.EXPOSURE_TIME_RANGE_TO,
config.AE_LOCK, config.CAMERA_W, config.CAMERA_H, config.CAMERA_W,
config.CAMERA_H, config.CAMERA_FRAMERATE,
config.CAMERA_FLIP_METHOD) as camera:
with adapters.VescAdapter(config.VESC_RPM_SLOW, config.VESC_MOVING_TIME, config.VESC_ALIVE_FREQ,
config.VESC_CHECK_FREQ, config.VESC_PORT, config.VESC_BAUDRATE) as vesc_engine:
counter = 1
session_label = input("Set session label (added to names): ")
input("Press enter to start.")
print("Data gathering started.")
while True:
counter = gather_data(smoothie, camera, counter, session_label, working_zone_polygon)
# move_forward_smoothie(1100, -5.2, smoothie) # F1100, B-5.2 = 30 cm with max speed (B-104 F1900 for min speed 30 cm) # for smoothie moving forward control
vesc_engine.start_moving()
vesc_engine.wait_for_stop()
except KeyboardInterrupt:
print("Stopped by a keyboard interrupt")
except:
print(traceback.format_exc())
finally:
smoothie.disconnect()
print("Data gathering done.", str(counter - 1), "images collected at this session.")
def main():
smoothie = adapters.SmoothieAdapter(config.SMOOTHIE_HOST)
meters_multiplier = ask_meters_multiplier()
distance, force = ask_speed_mode(meters_multiplier)
moving = False
# main loop
while True:
command = input("Hit enter to " + ("STOP" if moving else "MOVE") +
", type anything to exit: ")
if command != "":
if moving:
print("Stopping movement...")
smoothie.halt()
smoothie.reset()
print("Done.")
break
if not moving:
response = smoothie.custom_move_for(force, B=distance)
if response == smoothie.RESPONSE_OK:
print("Moving forward for", meters_multiplier, "meters")
else:
print("Couldn't move forward, smoothie error occurred:",
response)
exit(1)
else:
smoothie.halt()
smoothie.reset()
print("Movement interrupted, standing")
moving = not moving
def main():
print("Loading smoothie adapter")
smoothie = adapters.SmoothieAdapter(config.SMOOTHIE_HOST)
print("Aligning cork to center")
res = smoothie.ext_align_cork_center(config.XY_F_MAX)
smoothie.wait_for_all_actions_done()
if res != smoothie.RESPONSE_OK:
print("Couldn't align cork to center, smoothie error occurred:", res)
exit(1)
print("Extracting")
# extraction, cork down
res = smoothie.custom_move_for(config.Z_F_MAX, Z=-30)
smoothie.wait_for_all_actions_done()
if res != smoothie.RESPONSE_OK:
print("Couldn't move the extractor down, smoothie error occurred:",
res)
exit(1)
# extraction, cork up
res = smoothie.ext_cork_up()
smoothie.wait_for_all_actions_done()
if res != smoothie.RESPONSE_OK:
print("Couldn't move the extractor up, smoothie error occurred:", res)
exit(1)
print("Done!")
def create_smoothie_connection(smoothie_ip):
"""Create smoothieboard adapter instance (API for access and smoothieboard control)"""
while True:
try:
smoothie = adapters.SmoothieAdapter(smoothie_ip)
log_msg = "Successfully connected to smoothie"
print(log_msg)
return smoothie
except OSError as error:
print(repr(error))
def initSmoothie(logger: utility.Logger):
smoothie_vesc_addr = utility.get_smoothie_vesc_addresses()
if config.SMOOTHIE_BACKEND == 1:
smoothie_address = config.SMOOTHIE_HOST
else:
if "smoothie" in smoothie_vesc_addr:
smoothie_address = smoothie_vesc_addr["smoothie"]
else:
msg = "Couldn't get smoothie's USB address!"
logger.write_and_flush(msg+"\n")
print(msg)
exit(1)
smoothie = adapters.SmoothieAdapter(smoothie_address)
return smoothie
def main():
output_dir = "gathered_data/"
if not os.path.exists(output_dir):
try:
os.mkdir(output_dir)
except OSError:
print("Creation of the directory %s failed" % output_dir)
else:
print("Successfully created the directory %s " % output_dir)
print("Loading...")
sma = adapters.SmoothieAdapter(config.SMOOTHIE_HOST)
if config.USE_X_AXIS_CALIBRATION and config.USE_Y_AXIS_CALIBRATION:
sma.ext_align_cork_center(config.XY_F_MAX)
pca = adapters.PiCameraAdapter()
print("Loading complete.")
dist = float(input("Move straight for (meters): "))
dist *= 1000 # convert to mm
dist = int(dist)
label = input("Label for that session: ")
sep = " "
# offset = int(input("Take image every (mm): "))
offset = 75 # 151 mm is full AOI
for B in range(0, dist, offset):
image = pca.get_image()
cv.imwrite(
output_dir + str(datetime.datetime.now()) + sep + str(B) + sep +
label + ".jpg", image)
sma.nav_move_forward(int(offset * config.B_ONE_MM_IN_SMOOTHIE),
config.B_F_MAX)
sma.wait_for_all_actions_done()
# time.sleep(2) # if function above is not working properly
# take last image
image = pca.get_image()
cv.imwrite(
output_dir + str(datetime.datetime.now()) + sep + "last" + sep +
label + ".jpg", image)
print("Done.")
def main():
sma = adapters.SmoothieAdapter(config.SMOOTHIE_HOST)
pca = adapters.PiCameraAdapter()
det = detection.YoloOpenCVDetection()
sma.ext_align_cork_center(config.XY_F_MAX)
sma.wait_for_all_actions_done()
img = pca.get_image()
plant_boxes = det.detect(img)
plant_boxes = sort_plant_boxes_dist(plant_boxes, config.CORK_CENTER_X, config.CORK_CENTER_Y)
for box in plant_boxes:
sma.ext_align_cork_center(config.XY_F_MAX)
sma.wait_for_all_actions_done()
box_x, box_y = box.get_center_points()
# if inside the working zone
if point_is_in_rect(box_x, box_y, wl, wt, wr, wb):
while True:
box_x, box_y = box.get_center_points()
sm_x = px_to_smoohie_value(box_x, config.CORK_CENTER_X, config.ONE_MM_IN_PX)
sm_y = px_to_smoohie_value(box_y, config.CORK_CENTER_Y, config.ONE_MM_IN_PX)
res = sma.custom_move_for(config.XY_F_MAX, X=sm_x, Y=sm_y)
sma.wait_for_all_actions_done()
# check if no movement errors
if res != sma.RESPONSE_OK:
print("Smoothie error occurred:", res)
exit()
# if inside undistorted zone
if point_is_in_rect(box_x, box_y, ul, ut, ur, ub):
input("Ready to plant extraction, press enter to begin")
sma.ext_do_extraction(config.Z_F_MAX)
sma.wait_for_all_actions_done()
break
# if outside undistorted zone but in working zone
else:
temp_img = pca.get_image()
temp_plant_boxes = det.detect(temp_img)
# get closest box
box = sort_plant_boxes_dist(temp_plant_boxes, config.CORK_CENTER_X, config.CORK_CENTER_Y)[0]
# if not in working zone
else:
print(str(box), "is not in working area, switching to next")
def __init__(self):
self.all_tests = 14
self.pass_tests = 0
self.smoothie = adapters.SmoothieAdapter(config.SMOOTHIE_HOST)
self.vesc = adapters.VescAdapter(-2800, 10, config.VESC_ALIVE_FREQ,
config.VESC_CHECK_FREQ,
config.VESC_PORT,
config.VESC_BAUDRATE)
self.gps = adapters.GPSUbloxAdapter(config.GPS_PORT,
config.GPS_BAUDRATE,
config.GPS_POSITIONS_TO_KEEP)
self.camera = adapters.CameraAdapterIMX219_170(
config.CROP_W_FROM, config.CROP_W_TO, config.CROP_H_FROM,
config.CROP_H_TO, config.CV_ROTATE_CODE,
config.ISP_DIGITAL_GAIN_RANGE_FROM,
config.ISP_DIGITAL_GAIN_RANGE_TO, config.GAIN_RANGE_FROM,
config.GAIN_RANGE_TO, config.EXPOSURE_TIME_RANGE_FROM,
config.EXPOSURE_TIME_RANGE_TO, config.AE_LOCK, config.CAMERA_W,
config.CAMERA_H, config.CAMERA_W, config.CAMERA_H,
config.CAMERA_FRAMERATE, config.CAMERA_FLIP_METHOD)
self.name = datetime.datetime.now().strftime("%d-%m-%Y %H:%M")
self.logger = utility.Logger(self.name + '.txt')
def main():
points_history = []
prev_point = None
try:
nav = navigation.GPSComputing()
print("Initializing...", end=" ")
with adapters.SmoothieAdapter(config.SMOOTHIE_HOST) as smoothie:
with adapters.VescAdapter(config.VESC_RPM, config.VESC_MOVING_TIME,
config.VESC_ALIVE_FREQ,
config.VESC_CHECK_FREQ, config.VESC_PORT,
config.VESC_BAUDRATE) as vesc_engine:
with adapters.GPSUbloxAdapter(
config.GPS_PORT, config.GPS_BAUDRATE,
config.GPS_POSITIONS_TO_KEEP) as gps:
print("done.")
field_gps_coords = ask_for_ab_points(gps)
save_gps_coordinates(
field_gps_coords,
"field " + get_current_time() + ".txt")
input("Press enter to start moving")
# start moving forward
vesc_engine.start_moving()
# main navigation control loop
# TO DO: this loop is working much faster than gps, need to evaluate sleep time or waiting for new P
while True:
time.sleep(0.5)
cur_pos = gps.get_last_position()
# if str(cur_pos) == prev_point:
# continue
# prev_point = str(cur_pos)
points_history.append(cur_pos.copy())
# check if arrived
# TO DO: it won't work if deviation > course destination diff, as robot will be far away on some
# side and will never get too close to the path ending point
distance = nav.get_distance(cur_pos,
field_gps_coords[1])
print("Distance to B:", distance)
if distance <= config.COURSE_DESTINATION_DIFF:
vesc_engine.stop_moving()
print(
"Arrived (allowed destination distance difference",
config.COURSE_DESTINATION_DIFF, "mm)")
break
# check for course deviation. if deviation is bigger than a threshold
deviation, side = nav.get_deviation(
field_gps_coords[0], field_gps_coords[1], cur_pos)
nav_wheels_position = smoothie.get_adapter_current_coordinates(
)["A"]
side_text = "(left)" if side == -1 else "(right)" if side == 1 else "(center)"
print("A:", field_gps_coords[0], "B:",
field_gps_coords[1], "Cur:", cur_pos)
print("Deviation:", deviation, "Max dev.:",
config.COURSE_SIDE_DEVIATION_MAX, "side flag:",
side, side_text)
if deviation > config.COURSE_SIDE_DEVIATION_MAX:
# deviation to the left side
if side == -1:
# if not turned wheels yet
# TO DO: try to make different wheels turning values for different deviation
if not (-config.COURSE_ADJ_SMC_VAL - 0.001 <
nav_wheels_position <
-config.COURSE_ADJ_SMC_VAL + 0.001):
# TO DO: check abs(COURSE_ADJ_SMC_VAL - nav_wheels_position) and set bigger or
# lesser turning value
print("Turning wheels left",
-config.COURSE_ADJ_SMC_VAL)
smoothie.nav_turn_wheels_to(
-config.COURSE_ADJ_SMC_VAL,
config.A_F_MAX)
else:
print("Wheels already at left side:",
nav_wheels_position)
# deviation to the right side
elif side == 1:
# if not turned wheels yet
# TO DO: try to make different wheels turning values for different deviation
if not (config.COURSE_ADJ_SMC_VAL - 0.001 <
nav_wheels_position <
config.COURSE_ADJ_SMC_VAL + 0.001):
# TO DO: check abs(COURSE_ADJ_SMC_VAL - nav_wheels_position) and set bigger or
# lesser turning value
print("Turning wheels right:",
config.COURSE_ADJ_SMC_VAL)
smoothie.nav_turn_wheels_to(
config.COURSE_ADJ_SMC_VAL,
config.A_F_MAX)
else:
print("Wheels already at right side:",
nav_wheels_position)
# it's an error if deviation is big but side flag is telling us that we're on moving vector
else:
print("Abnormal state: deviation =", deviation,
"at", side,
"side. Emergency stop applied.")
exit(1)
# if deviation is ok (0 or less than an adj. threshold)
else:
# if not turned wheels yet
# TO DO: try to make different wheels turning values for different deviation
if not (-0.001 < nav_wheels_position < 0.001):
# TO DO: check abs(COURSE_ADJ_SMC_VAL - nav_wheels_position) and set bigger or
# lesser turning value
print("Turning wheels to center")
smoothie.nav_align_wheels_center(
config.A_F_MAX)
else:
print("Wheels already at center:",
nav_wheels_position)
print()
print("Done!")
except KeyboardInterrupt:
print("Stopped by a keyboard interrupt (Ctrl + C)")
finally:
save_gps_coordinates(points_history,
"gps_history " + get_current_time() + ".txt")
def main():
log_counter = 1
log_dir = "log/"
if not os.path.exists(log_dir):
try:
os.mkdir(log_dir)
except OSError:
print("Creation of the directory %s failed" % log_dir)
else:
print("Successfully created the directory %s " % log_dir)
smoothie = adapters.SmoothieAdapter(config.SMOOTHIE_HOST)
detector = detection.YoloOpenCVDetection()
smoothie.ext_align_cork_center(config.XY_F_MAX)
smoothie.wait_for_all_actions_done()
with adapters.CameraAdapterIMX219_170() as camera:
time.sleep(5)
image = camera.get_image()
img_y_c, img_x_c = int(image.shape[0] / 2), int(image.shape[1] / 2)
plant_boxes = detector.detect(image)
plant_boxes = sort_plant_boxes_dist(plant_boxes, config.CORK_CENTER_X,
config.CORK_CENTER_Y)
# check if no plants detected
if len(plant_boxes) < 1:
print("No plants detected on view scan, exiting.")
cv.imwrite(
log_dir + str(log_counter) + " starting - see no plants.jpg",
image)
exit(0)
# log
log_img = image.copy()
log_img = detection.draw_boxes(log_img, plant_boxes)
log_img = draw_zones_circle(log_img, img_x_c, img_y_c,
undistorted_zone_radius,
working_zone_radius)
cv.imwrite(log_dir + str(log_counter) + " starting.jpg", log_img)
log_counter += 1
for box in plant_boxes:
smoothie.ext_align_cork_center(
config.XY_F_MAX) # camera in real center
smoothie.wait_for_all_actions_done()
box_x, box_y = box.get_center_points()
# if inside the working zone
if is_point_in_circle(box_x, box_y, img_x_c, img_y_c,
working_zone_radius):
while True:
box_x, box_y = box.get_center_points()
# if inside undistorted zone
if is_point_in_circle(box_x, box_y, img_x_c, img_y_c,
undistorted_zone_radius):
# calculate values to move camera over a plant
sm_x = -px_to_smoohie_value(
box_x, config.CORK_CENTER_X, config.ONE_MM_IN_PX)
sm_y = px_to_smoohie_value(box_y, config.CORK_CENTER_Y,
config.ONE_MM_IN_PX)
# move camera over a plant
res = smoothie.custom_move_for(config.XY_F_MAX,
X=sm_x,
Y=sm_y)
smoothie.wait_for_all_actions_done()
if res != smoothie.RESPONSE_OK:
print(
"Couldn't move camera over plant, smoothie error occurred:",
res)
exit(1)
# move cork to the camera position
res = smoothie.custom_move_for(config.XY_F_MAX, Y=57)
smoothie.wait_for_all_actions_done()
if res != smoothie.RESPONSE_OK:
print(
"Couldn't move cork over plant, smoothie error occurred:",
res)
exit(1)
# waiting confirmation for extraction (just to make people see how it's going on)
input(
"Ready to plant extraction, press enter to begin")
# extraction, cork down
res = smoothie.custom_move_for(config.Z_F_MAX, Z=-30)
smoothie.wait_for_all_actions_done()
if res != smoothie.RESPONSE_OK:
print(
"Couldn't move the extractor down, smoothie error occurred:",
res)
exit(1)
# extraction, cork up
res = smoothie.ext_cork_up()
smoothie.wait_for_all_actions_done()
if res != smoothie.RESPONSE_OK:
print(
"Couldn't move the extractor up, smoothie error occurred:",
res)
exit(1)
break
# if outside undistorted zone but in working zone
else:
# calculate values for move camera closer to a plant
sm_x = -px_to_smoohie_value(box_x, img_x_c,
config.ONE_MM_IN_PX)
sm_y = px_to_smoohie_value(box_y, img_y_c,
config.ONE_MM_IN_PX)
# move for a half distance, dist is not < 10
sm_x = int(sm_x / 2) if sm_x / 2 > 10 else 10
sm_y = int(sm_y / 2) if sm_y / 2 > 10 else 10
# move camera closer to a plant
res = smoothie.custom_move_for(config.XY_F_MAX,
X=sm_x,
Y=sm_y)
smoothie.wait_for_all_actions_done()
if res != smoothie.RESPONSE_OK:
print(
"Couldn't move to plant, smoothie error occurred:",
res)
exit(1)
# make new photo and re-detect plants
temp_img = camera.get_image()
temp_plant_boxes = detector.detect(temp_img)
# check if no plants detected
if len(temp_plant_boxes) < 1:
print(
"No plants detected (plant was in undistorted zone before), trying to move on next item"
)
temp_img = draw_zones_circle(
temp_img, img_x_c, img_y_c,
undistorted_zone_radius, working_zone_radius)
cv.imwrite(
log_dir + str(log_counter) +
" in undistorted branch - see no plants.jpg",
temp_img)
log_counter += 1
break
# log
log_img = temp_img.copy()
log_img = detection.draw_boxes(log_img,
temp_plant_boxes)
log_img = draw_zones_circle(log_img, img_x_c, img_y_c,
undistorted_zone_radius,
working_zone_radius)
cv.imwrite(
log_dir + str(log_counter) +
" in undistorted branch - all.jpg", log_img)
# get closest box (exactly update current box from main list coordinates after moving closer)
box = min_plant_box_dist(temp_plant_boxes, img_x_c,
img_y_c)
# log
log_img = temp_img.copy()
log_img = detection.draw_box(log_img, box)
log_img = draw_zones_circle(log_img, img_x_c, img_y_c,
undistorted_zone_radius,
working_zone_radius)
cv.imwrite(
log_dir + str(log_counter) +
" in undistorted branch - closest.jpg", log_img)
log_counter += 1
# if not in working zone
else:
print("skipped", str(box), "(not in working area)")
print("Script executing is done.")
def main():
time.sleep(5)
log_counter = 1
if not os.path.exists(LOG_DIR):
try:
os.mkdir(LOG_DIR)
except OSError:
print("Creation of the directory %s failed" % LOG_DIR)
logging.error("Creation of the directory %s failed" % LOG_DIR)
else:
print("Successfully created the directory %s " % LOG_DIR)
logging.info("Successfully created the directory %s " % LOG_DIR)
# working zone pre-calculations
# these points list is changed for usage in matplotlib (it has differences in the coords system)
# working_zone_points_plt = list(
# map(lambda item: [item[0], config.CROP_H_TO - config.CROP_H_FROM - item[1]], WORKING_ZONE_POLY_POINTS))
working_zone_polygon = Polygon(WORKING_ZONE_POLY_POINTS)
# these points array is used for drawing zone using OpenCV
working_zone_points_cv = np.array(WORKING_ZONE_POLY_POINTS,
np.int32).reshape((-1, 1, 2))
# remove old images from log dir
print("Removing .jpg images from log directory")
logging.debug("Removing .jpg images from log directory")
clear_log_dir()
# create smoothieboard adapter (API for access and control smoothieboard)
while True:
try:
smoothie = adapters.SmoothieAdapter(config.SMOOTHIE_HOST)
print("Successfully connected to smoothie")
logging.info("Successfully connected to smoothie")
break
except OSError as error:
logging.warning(repr(error))
print(repr(error))
detector = detection.YoloOpenCVDetection()
with adapters.CameraAdapterIMX219_170() as camera:
print("Warming up the camera")
logging.debug("Warming up the camera")
time.sleep(5)
# main loop, detection and motion
while True:
logging.debug("Starting detection and motion main loop iteration")
# go to scan position
# smoothie.ext_align_cork_center(config.XY_F_MAX)
smoothie.custom_move_to(config.XY_F_MAX,
X=config.X_MAX / 2,
Y=config.Y_MIN)
smoothie.wait_for_all_actions_done()
image = camera.get_image()
img_y_c, img_x_c = int(image.shape[0] / 2), int(image.shape[1] / 2)
plant_boxes = detector.detect(image)
plant_boxes = sort_plant_boxes_dist(plant_boxes, config.X_MAX / 2,
config.Y_MIN)
# check if no plants detected
if len(plant_boxes) < 1:
print("No plants detected on view scan (img " +
str(log_counter) + "), moving forward")
logging.info("No plants detected on view scan (img " +
str(log_counter) + "), moving forward")
log_img = image.copy()
log_img = draw_zones(log_img, img_x_c, img_y_c,
UNDISTORTED_ZONE_RADIUS,
working_zone_points_cv)
if config.SAVE_DEBUG_IMAGES:
cv.imwrite(
LOG_DIR + str(log_counter) +
" overview scan (see no plants).jpg", log_img)
log_counter += 1
# move forward for 30 sm
res = smoothie.custom_move_for(1000, B=5.43)
smoothie.wait_for_all_actions_done()
if res != smoothie.RESPONSE_OK:
print(
"Couldn't move forward (for 30 sm), smoothie error occurred:",
res)
logging.critical(
"Couldn't move forward (for 30 sm), smoothie error occurred: "
+ res)
# exit(1)
continue
else:
print("Found " + str(len(plant_boxes)) +
" plants on view scan (img " + str(log_counter) + ")")
logging.info("Found " + str(len(plant_boxes)) +
" plants on view scan (img " + str(log_counter) +
")")
log_img = image.copy()
log_img = draw_zones(log_img, img_x_c, img_y_c,
UNDISTORTED_ZONE_RADIUS,
working_zone_points_cv)
log_img = detection.draw_boxes(log_img, plant_boxes)
if config.SAVE_DEBUG_IMAGES:
cv.imwrite(
LOG_DIR + str(log_counter) + " overview scan (see " +
str(len(plant_boxes)) + " plants).jpg", log_img)
log_counter += 1
# loop over all detected plants
for box in plant_boxes:
logging.debug("Starting loop over plants list iteration")
# smoothie.ext_align_cork_center(config.XY_F_MAX) # camera in real center
smoothie.custom_move_to(config.XY_F_MAX,
X=config.X_MAX / 2,
Y=config.Y_MIN)
smoothie.wait_for_all_actions_done()
box_x, box_y = box.get_center_points()
"""
print("Processing plant on x=" + str(box_x) + " y=" + str(box_y))
logging.info("Processing plant on x=" + str(box_x) + " y=" + str(box_y))
"""
# if plant is in working zone and can be reached by cork
if is_point_in_poly(box_x, box_y, working_zone_polygon):
print("Plant is in working zone")
logging.info("Plant is in working zone")
while True:
print("Starting extraction loop")
logging.info("Starting extraction loop")
box_x, box_y = box.get_center_points()
print("Processing plant in x=" + str(box_x) + " y=" +
str(box_y))
logging.info("Processing plant in x=" + str(box_x) +
" y=" + str(box_y))
# if inside undistorted zone
if is_point_in_circle(box_x, box_y, img_x_c, img_y_c,
UNDISTORTED_ZONE_RADIUS):
print("Plant is in undistorted zone")
logging.info("Plant is in undistorted zone")
# calculate values to move camera over a plant
sm_x = px_to_smoothie_value(
box_x, img_x_c, config.ONE_MM_IN_PX)
sm_y = -px_to_smoothie_value(
box_y, img_y_c, config.ONE_MM_IN_PX)
# swap camera and cork for extraction immediately
sm_y += CORK_CAMERA_DISTANCE
print("Calculated smoothie moving coordinates X=" +
str(sm_x) + " Y=" + str(sm_y))
logging.debug(
"Calculated smoothie moving coordinates X=" +
str(sm_x) + " Y=" + str(sm_y))
ad_cur_coord = smoothie.get_adapter_current_coordinates(
)
print("Adapter coordinates: " + str(ad_cur_coord))
logging.info("Adapter coordinates: " +
str(ad_cur_coord))
sm_cur_coord = smoothie.get_smoothie_current_coordinates(
)
print("Smoothie coordinates: " + str(sm_cur_coord))
logging.info("Smoothie coordinates: " +
str(sm_cur_coord))
# move cork over a plant
print("Moving cork to the plant")
logging.info("Moving cork to the plant")
res = smoothie.custom_move_for(config.XY_F_MAX,
X=sm_x,
Y=sm_y)
smoothie.wait_for_all_actions_done()
if res != smoothie.RESPONSE_OK:
print(
"Couldn't move cork over plant, smoothie error occurred:",
res)
logging.critical(
"Couldn't move cork over plant, smoothie error occurred: "
+ str(res))
# exit(1)
# temp debug 1
log_img = camera.get_image()
if config.SAVE_DEBUG_IMAGES:
cv.imwrite(
LOG_DIR + str(log_counter) +
" extracting (cork in upper position).jpg",
log_img)
log_counter += 1
# extraction, cork down
print("Extracting plant (cork down)")
logging.info("Extracting plant (cork down)")
res = smoothie.custom_move_for(config.Z_F_MAX,
Z=-30)
smoothie.wait_for_all_actions_done()
if res != smoothie.RESPONSE_OK:
print(
"Couldn't move the extractor down, smoothie error occurred:",
res)
logging.critical(
"Couldn't move the extractor down, smoothie error occurred:"
+ str(res))
# exit(1)
# temp debug 2
log_img = camera.get_image()
if config.SAVE_DEBUG_IMAGES:
cv.imwrite(
LOG_DIR + str(log_counter) +
" extracting (cork in lower position).jpg",
log_img)
log_counter += 1
# extraction, cork up
print("Extracting plant (cork up)")
logging.info("Extracting plant (cork up)")
res = smoothie.ext_cork_up()
smoothie.wait_for_all_actions_done()
if res != smoothie.RESPONSE_OK:
print(
"Couldn't move the extractor up, smoothie error occurred:",
res)
logging.critical(
"Couldn't move the extractor up, smoothie error occurred:"
+ str(res))
# exit(1)
break
# if outside undistorted zone but in working zone
else:
print(
"Plant is outside undistorted zone, moving to")
logging.info(
"Plant is outside undistorted zone, moving to")
# calculate values for move camera closer to a plant
control_point = get_closest_control_point(
box_x, box_y, IMAGE_CONTROL_POINTS_MAP)
sm_x = control_point[2]
sm_y = control_point[3]
debug_text = "Moving to px x=" + str(control_point[0]) + " y=" + str(control_point[1]) + \
" (control point #" + str(control_point[4]) + ")"
print(debug_text)
logging.info(debug_text)
# move camera closer to a plant
res = smoothie.custom_move_for(config.XY_F_MAX,
X=sm_x,
Y=sm_y)
smoothie.wait_for_all_actions_done()
if res != smoothie.RESPONSE_OK:
print(
"Couldn't move to plant, smoothie error occurred:",
res)
logging.critical(
"Couldn't move to plant, smoothie error occurred: "
+ str(res))
# exit(1)
# make new photo and re-detect plants
image = camera.get_image()
temp_plant_boxes = detector.detect(image)
# check if no plants detected
if len(temp_plant_boxes) < 1:
print(
"No plants detected (plant was in working zone before), trying to move on\
next item")
logging.info(
"No plants detected (plant was in working zone before), trying to move on\
next item")
log_img = image.copy()
log_img = draw_zones(log_img, img_x_c, img_y_c,
UNDISTORTED_ZONE_RADIUS,
working_zone_points_cv)
if config.SAVE_DEBUG_IMAGES:
cv.imwrite(
LOG_DIR + str(log_counter) +
" in working zone branch - see no plants.jpg",
log_img)
log_counter += 1
break
# log
log_img = image.copy()
log_img = draw_zones(log_img, img_x_c, img_y_c,
UNDISTORTED_ZONE_RADIUS,
working_zone_points_cv)
log_img = detection.draw_boxes(
log_img, temp_plant_boxes)
if config.SAVE_DEBUG_IMAGES:
cv.imwrite(
LOG_DIR + str(log_counter) +
" in working zone branch - all plants.jpg",
log_img)
log_counter += 1
# get closest box (exactly update current box from main list coordinates after moving closer)
box = min_plant_box_dist(temp_plant_boxes, img_x_c,
img_y_c)
# log
log_img = image.copy()
log_img = draw_zones(log_img, img_x_c, img_y_c,
UNDISTORTED_ZONE_RADIUS,
working_zone_points_cv)
log_img = detection.draw_box(log_img, box)
if config.SAVE_DEBUG_IMAGES:
cv.imwrite(
LOG_DIR + str(log_counter) +
" in working zone branch - closest plant.jpg",
log_img)
log_counter += 1
# if not in working zone
else:
print("Skipped", str(box), "(not in working area)")
logging.info("Skipped " + str(box) +
" (not in working area)")
# move forward for 30 sm
res = smoothie.custom_move_for(1000, B=5.43)
smoothie.wait_for_all_actions_done()
if res != smoothie.RESPONSE_OK:
print(
"Couldn't move forward (for 30 sm), smoothie error occurred:",
res)
logging.critical(
"Couldn't move forward (for 30 sm), smoothie error occurred: "
+ str(res))
def main():
create_directories(config.DEBUG_IMAGES_PATH)
working_zone_polygon = Polygon(config.WORKING_ZONE_POLY_POINTS)
working_zone_points_cv = np.array(config.WORKING_ZONE_POLY_POINTS,
np.int32).reshape((-1, 1, 2))
view_zone_polygon = Polygon(config.VIEW_ZONE_POLY_POINTS)
view_zone_points_cv = np.array(config.VIEW_ZONE_POLY_POINTS,
np.int32).reshape((-1, 1, 2))
nav = navigation.GPSComputing()
used_points_history = []
raw_angles_history = []
logger_full = utility.Logger("full log " + get_current_time() + ".txt")
logger_table = utility.Logger("table log " + get_current_time() + ".csv")
# sensors picking
report_field_names = [
'temp_fet_filtered', 'temp_motor_filtered', 'avg_motor_current',
'avg_input_current', 'rpm', 'input_voltage'
]
report_file = open('report.csv', 'w', newline='')
report_writer = csv.DictWriter(report_file, fieldnames=report_field_names)
report_writer.writeheader()
# QGIS and sensor data transmitting
path = os.path.abspath(os.getcwd())
sensor_processor = SensorProcessing.SensorProcessing(path, 0)
sensor_processor.startServer()
client = socketForRTK.Client.Client(4000)
time.sleep(1)
if not client.connectionToServer():
msg = "Connection refused for Server RTK."
print(msg)
logger_full.write(msg + "\n")
sensor_processor.startSession()
try:
msg = "Initializing..."
print(msg)
logger_full.write(msg + "\n")
print("Loading periphery detector...")
periphery_detector = detection.YoloOpenCVDetection(
config.PERIPHERY_CLASSES_FILE, config.PERIPHERY_CONFIG_FILE,
config.PERIPHERY_WEIGHTS_FILE, config.PERIPHERY_INPUT_SIZE,
config.PERIPHERY_CONFIDENCE_THRESHOLD,
config.PERIPHERY_NMS_THRESHOLD, config.PERIPHERY_DNN_BACKEND,
config.PERIPHERY_DNN_TARGET)
print("Loading smoothie...")
smoothie = adapters.SmoothieAdapter(config.SMOOTHIE_HOST)
print("Loading vesc...")
vesc_engine = adapters.VescAdapter(config.VESC_RPM_SLOW,
config.VESC_MOVING_TIME,
config.VESC_ALIVE_FREQ,
config.VESC_CHECK_FREQ,
config.VESC_PORT,
config.VESC_BAUDRATE)
print("Loading GPS...")
gps = adapters.GPSUbloxAdapter(config.GPS_PORT, config.GPS_BAUDRATE,
config.GPS_POSITIONS_TO_KEEP)
print("Loading camera...")
camera = adapters.CameraAdapterIMX219_170(
config.CROP_W_FROM, config.CROP_W_TO, config.CROP_H_FROM,
config.CROP_H_TO, config.CV_ROTATE_CODE,
config.ISP_DIGITAL_GAIN_RANGE_FROM,
config.ISP_DIGITAL_GAIN_RANGE_TO, config.GAIN_RANGE_FROM,
config.GAIN_RANGE_TO, config.EXPOSURE_TIME_RANGE_FROM,
config.EXPOSURE_TIME_RANGE_TO, config.AE_LOCK, config.CAMERA_W,
config.CAMERA_H, config.CAMERA_W, config.CAMERA_H,
config.CAMERA_FRAMERATE, config.CAMERA_FLIP_METHOD)
# set smoothie's A axis to 0 (nav turn wheels)
response = smoothie.set_current_coordinates(A=0)
if response != smoothie.RESPONSE_OK:
msg = "Failed to set A=0 on smoothie (turning wheels init position), response message:\n" + response
print(msg)
logger_full.write(msg + "\n")
# load field coordinates
field_gps_coords = load_coordinates(
config.INPUT_GPS_FIELD_FILE) # [A, B, C, D]
if len(field_gps_coords) != 4:
msg = "Expected 4 gps points in " + config.INPUT_GPS_FIELD_FILE + ", got " + str(
len(field_gps_coords))
print(msg)
logger_full.write(msg + "\n")
exit(1)
# generate path points
path_points = build_path(field_gps_coords, nav, logger_full)
if len(path_points) > 0:
save_gps_coordinates(
path_points, "generated_path " + get_current_time() + ".txt")
else:
msg = "List of path points is empty, saving canceled."
print(msg)
logger_full.write(msg + "\n")
if len(path_points) < 2:
msg = "Expected at least 2 points in path, got " + str(len(path_points)) + \
" instead (1st point is starting point)."
print(msg)
logger_full.write(msg + "\n")
exit(1)
# ask permission to start moving
msg = "Initializing done. Press enter to start moving."
input(msg)
logger_full.write(msg + "\n")
msg = 'GpsQ|Raw ang|Res ang|Ord ang|Sum ang|Distance |Adapter|Smoothie|'
print(msg)
logger_full.write(msg + "\n")
msg = 'GpsQ,Raw ang,Res ang,Ord ang,Sum ang,Distance,Adapter,Smoothie,'
for field_name in report_field_names:
msg += field_name + ","
msg = msg[:-1]
logger_table.write(msg + "\n")
# path points visiting loop
for i in range(1, len(path_points)):
from_to = [path_points[i - 1], path_points[i]]
from_to_dist = nav.get_distance(from_to[0], from_to[1])
msg = "Current movement vector: " + str(
from_to) + " Vector size: " + str(from_to_dist)
# print(msg)
logger_full.write(msg + "\n")
move_to_point(from_to, used_points_history, gps, vesc_engine,
smoothie, logger_full, client, nav,
raw_angles_history, report_writer,
report_field_names, logger_table, periphery_detector,
camera, working_zone_points_cv)
msg = "Done!"
print(msg)
logger_full.write(msg + "\n")
except KeyboardInterrupt:
msg = "Stopped by a keyboard interrupt (Ctrl + C)"
print(msg)
logger_full.write(msg + "\n")
except Exception:
msg = "Exception occurred:\n" + traceback.format_exc()
print(msg)
logger_full.write(msg + "\n")
finally:
try:
print("Stopping movement...")
vesc_engine.stop_moving()
except:
pass
try:
print("Releasing camera...")
camera.release()
except:
pass
# save log data
if len(used_points_history) > 0:
save_gps_coordinates(
used_points_history,
"used_gps_history " + get_current_time() + ".txt")
else:
msg = "used_gps_history list has 0 elements!"
print(msg)
logger_full.write(msg + "\n")
adapter_points_history = gps.get_last_positions_list()
if len(adapter_points_history) > 0:
save_gps_coordinates(
adapter_points_history,
"adapter_gps_history " + get_current_time() + ".txt")
else:
msg = "adapter_gps_history list has 0 elements!"
print(msg)
logger_full.write(msg + "\n")
# close log and hardware connections
logger_full.close()
logger_table.close()
report_file.close()
smoothie.disconnect()
vesc_engine.disconnect()
gps.disconnect()
# close transmitting connections
sensor_processor.endSession()
client.closeConnection()
sensor_processor.stopServer()
def main():
nav = navigation.GPSComputing()
used_points_history = []
raw_angles_history = []
logger = utility.Logger("console " + get_current_time() + ".txt")
# QGIS and sensor data transmitting
path = os.path.abspath(os.getcwd())
sensor_processor = SensorProcessing.SensorProcessing(path, 0)
sensor_processor.startServer()
client = socketForRTK.Client.Client(4000)
time.sleep(1)
if not client.connectionToServer():
msg = "Connection refused for Server RTK."
print(msg)
logger.write(msg + "\n")
sensor_processor.startSession()
try:
msg = "Initializing..."
print(msg)
logger.write(msg + "\n")
smoothie = adapters.SmoothieAdapter(config.SMOOTHIE_HOST)
vesc_engine = adapters.VescAdapter(int(config.VESC_RPM / 2),
config.VESC_MOVING_TIME,
config.VESC_ALIVE_FREQ,
config.VESC_CHECK_FREQ,
config.VESC_PORT,
config.VESC_BAUDRATE)
gps = adapters.GPSUbloxAdapter(config.GPS_PORT, config.GPS_BAUDRATE,
config.GPS_POSITIONS_TO_KEEP)
# set smoothie's A axis to 0 (nav turn wheels)
response = smoothie.set_current_coordinates(A=0)
if response != smoothie.RESPONSE_OK:
msg = "Failed to set A=0 on smoothie (turning wheels init position), response message:\n" + response
print(msg)
logger.write(msg + "\n")
msg = "Initializing done."
print(msg)
logger.write(msg + "\n")
# pick from gps
field_gps_coords = load_coordinates(
config.INPUT_GPS_FIELD_FILE) # [A, B, C, D]
if len(field_gps_coords) != 4:
msg = "Expected 4 gps points in " + config.INPUT_GPS_FIELD_FILE + ", got " + str(
len(field_gps_coords))
print(msg)
logger.write(msg)
field_gps_coords.append(field_gps_coords[0].copy(
)) # this makes robot move back to starting point (point A)
field_gps_coords.append(field_gps_coords[1].copy())
# start moving forward
msg = "Press enter to start moving"
input(msg)
logger.write(msg + "\n")
# perimeter bypass loop
for i in range(1, len(field_gps_coords) - 1):
# current vector is marked as XY where X2 and Y1 is corner maneuver points
# example: current side is AB which marked as XY, then point X2 is on AB, Y1 is on BC and
# X2 and Y1 near ^ABC angle (XYZ). ABCD are "global" points which corresponding field.txt file
# while XYZ displays currently processed points
# XY vector
current_vector = [field_gps_coords[i - 1], field_gps_coords[i]]
cur_vec_dist = nav.get_distance(current_vector[0],
current_vector[1])
msg = "Current movement vector: " + str(
current_vector) + " Vector size: " + str(cur_vec_dist)
print(msg)
logger.write(msg)
# check if moving vector is too small for maneuvers
if config.MANEUVER_START_DISTANCE >= cur_vec_dist:
msg = "No place for maneuvers; config distance before maneuver is " + \
str(config.MANEUVER_START_DISTANCE) + " current moving vector distance is " + str(cur_vec_dist)
print(msg)
logger.write(msg + "\n")
break
# compute X2 point
else:
dist_to_x2 = cur_vec_dist - config.MANEUVER_START_DISTANCE
point_x2 = nav.get_point_on_vector(current_vector[0],
current_vector[1],
dist_to_x2)
msg = "Point X2: " + str(point_x2) + "[X X2] size: " + str(
dist_to_x2)
print(msg)
logger.write(msg)
# move to X2
from_to = [current_vector[0], point_x2]
move_to_point(from_to, used_points_history, gps, vesc_engine,
smoothie, logger, client, nav, raw_angles_history)
# TO DO: BUG - we checking YZ to be less than maneuver dist but robot will be in Y1, not in Y, so it's not
# correct. Maybe config.MANEUVER_START_DISTANCE should be multiplied by 2 when checking that vector size is
# big enough
# go from X2 to Y1 point (see description in comments above)
# YZ vector
current_vector = [field_gps_coords[i], field_gps_coords[i + 1]]
cur_vec_dist = nav.get_distance(current_vector[0],
current_vector[1])
msg = "Next movement vector: " + str(
current_vector) + " Vector size: " + str(cur_vec_dist)
print(msg)
logger.write(msg)
# check if moving vector is too small for maneuvers
if config.MANEUVER_START_DISTANCE >= cur_vec_dist:
msg = "No place for maneuvers; config distance before maneuver is " + \
str(config.MANEUVER_START_DISTANCE) + " next moving vector distance is " + str(cur_vec_dist)
print(msg)
logger.write(msg + "\n")
break
# compute Y1 point to move (current vector is marked as XY where X2 and Y1 is corner maneuver points)
# example: next side is BC which marked as XY, then point Y1 is on BC (YZ)
else:
dist_to_y1 = config.MANEUVER_START_DISTANCE
point_y1 = nav.get_point_on_vector(current_vector[0],
current_vector[1],
dist_to_y1)
msg = "Point Y1: " + str(point_y1) + "[Y Y1] size: " + str(
dist_to_y1)
print(msg)
logger.write(msg)
# move to Y1
from_to = [current_vector[0], point_y1]
move_to_point(from_to, used_points_history, gps, vesc_engine,
smoothie, logger, client, nav, raw_angles_history)
msg = "Done!"
print(msg)
logger.write(msg + "\n")
except KeyboardInterrupt:
msg = "Stopped by a keyboard interrupt (Ctrl + C)"
print(msg)
logger.write(msg + "\n")
except Exception:
msg = "Exception occurred:\n" + traceback.format_exc()
print(msg)
logger.write(msg)
finally:
# save log data
if len(used_points_history) > 0:
save_gps_coordinates(
used_points_history,
"used_gps_history " + get_current_time() + ".txt")
else:
msg = "used_gps_history list has 0 elements!"
print(msg)
logger.write(msg)
adapter_points_history = gps.get_last_positions_list()
if len(adapter_points_history) > 0:
save_gps_coordinates(
adapter_points_history,
"adapter_gps_history " + get_current_time() + ".txt")
else:
msg = "adapter_gps_history list has 0 elements!"
print(msg)
logger.write(msg)
# close log and hardware connections
logger.close()
smoothie.disconnect()
vesc_engine.disconnect()
gps.disconnect()
# close transmitting connections
sensor_processor.endSession()
client.closeConnection()
sensor_processor.stopServer()
def main():
create_directories(config.DEBUG_IMAGES_PATH)
working_zone_polygon = Polygon(config.WORKING_ZONE_POLY_POINTS)
working_zone_points_cv = np.array(config.WORKING_ZONE_POLY_POINTS,
np.int32).reshape((-1, 1, 2))
view_zone_polygon = Polygon(config.VIEW_ZONE_POLY_POINTS)
view_zone_points_cv = np.array(config.VIEW_ZONE_POLY_POINTS,
np.int32).reshape((-1, 1, 2))
nav = navigation.GPSComputing()
used_points_history = []
logger_full = utility.Logger("full log " + get_current_time() + ".txt")
logger_table = utility.Logger("table log " + get_current_time() + ".csv")
# sensors picking
report_field_names = [
'temp_fet_filtered', 'temp_motor_filtered', 'avg_motor_current',
'avg_input_current', 'rpm', 'input_voltage'
]
# QGIS and sensor data transmitting
path = os.path.abspath(os.getcwd())
sensor_processor = SensorProcessing.SensorProcessing(path, 0)
sensor_processor.startServer()
client = socketForRTK.Client.Client(4000)
time.sleep(1)
if not client.connectionToServer():
msg = "Connection refused for Server RTK."
print(msg)
logger_full.write(msg + "\n")
sensor_processor.startSession()
try:
msg = "Initializing..."
print(msg)
logger_full.write(msg + "\n")
# load field coordinates
if config.RECEIVE_FIELD_FROM_RTK:
msg = "Loading field coordinates from RTK"
logger_full.write(msg + "\n")
try:
field_gps_coords = load_coordinates(rtk.CURRENT_FIELD_PATH)
except AttributeError:
msg = "Couldn't get field file name from RTK script as it is wasn't assigned there."
print(msg)
logger_full.write(msg + "\n")
exit(1)
except FileNotFoundError:
msg = "Couldn't not find " + rtk.CURRENT_FIELD_PATH + " file."
print(msg)
logger_full.write(msg + "\n")
exit(1)
if len(field_gps_coords) < 5:
msg = "Expected at least 4 gps points in " + rtk.CURRENT_FIELD_PATH + ", got " + \
str(len(field_gps_coords))
print(msg)
logger_full.write(msg + "\n")
exit(1)
field_gps_coords = nav.corner_points(field_gps_coords,
config.FILTER_MAX_DIST,
config.FILTER_MIN_DIST)
else:
msg = "Loading " + config.INPUT_GPS_FIELD_FILE
logger_full.write(msg + "\n")
field_gps_coords = load_coordinates(
config.INPUT_GPS_FIELD_FILE) # [A, B, C, D]
# check field corner points count
if len(field_gps_coords) != 4:
msg = "Expected 4 gps corner points, got " + str(
len(field_gps_coords)) + "\nField:\n" + str(field_gps_coords)
print(msg)
logger_full.write(msg + "\n")
exit(1)
field_gps_coords = reduce_field_size(field_gps_coords,
config.FIELD_REDUCE_SIZE, nav)
# generate path points
path_points = build_path(field_gps_coords, nav, logger_full)
if len(path_points) > 0:
save_gps_coordinates(
path_points, "generated_path " + get_current_time() + ".txt")
else:
msg = "List of path points is empty, saving canceled."
print(msg)
logger_full.write(msg + "\n")
if len(path_points) < 2:
msg = "Expected at least 2 points in path, got " + str(len(path_points)) + \
" instead (1st point is starting point)."
print(msg)
logger_full.write(msg + "\n")
exit(1)
# load and connect to everything
print("Loading periphery detector...")
periphery_detector = detection.YoloOpenCVDetection(
config.PERIPHERY_CLASSES_FILE, config.PERIPHERY_CONFIG_FILE,
config.PERIPHERY_WEIGHTS_FILE, config.PERIPHERY_INPUT_SIZE,
config.PERIPHERY_CONFIDENCE_THRESHOLD,
config.PERIPHERY_NMS_THRESHOLD, config.PERIPHERY_DNN_BACKEND,
config.PERIPHERY_DNN_TARGET)
print("Loading precise detector...")
precise_detector = detection.YoloOpenCVDetection(
config.PRECISE_CLASSES_FILE, config.PRECISE_CONFIG_FILE,
config.PRECISE_WEIGHTS_FILE, config.PRECISE_INPUT_SIZE,
config.PRECISE_CONFIDENCE_THRESHOLD, config.PRECISE_NMS_THRESHOLD,
config.PRECISE_DNN_BACKEND, config.PRECISE_DNN_TARGET)
print("Loading smoothie...")
smoothie = adapters.SmoothieAdapter(config.SMOOTHIE_HOST)
print("Loading vesc...")
vesc_engine = adapters.VescAdapter(config.VESC_RPM_SLOW,
config.VESC_MOVING_TIME,
config.VESC_ALIVE_FREQ,
config.VESC_CHECK_FREQ,
config.VESC_PORT,
config.VESC_BAUDRATE)
print("Loading gps...")
gps = adapters.GPSUbloxAdapter(config.GPS_PORT, config.GPS_BAUDRATE,
config.GPS_POSITIONS_TO_KEEP)
# gps = stubs.GPSStub(config.GPS_PORT, config.GPS_BAUDRATE, config.GPS_POSITIONS_TO_KEEP)
print("Loading camera...")
camera = adapters.CameraAdapterIMX219_170(
config.CROP_W_FROM, config.CROP_W_TO, config.CROP_H_FROM,
config.CROP_H_TO, config.CV_ROTATE_CODE,
config.ISP_DIGITAL_GAIN_RANGE_FROM,
config.ISP_DIGITAL_GAIN_RANGE_TO, config.GAIN_RANGE_FROM,
config.GAIN_RANGE_TO, config.EXPOSURE_TIME_RANGE_FROM,
config.EXPOSURE_TIME_RANGE_TO, config.AE_LOCK, config.CAMERA_W,
config.CAMERA_H, config.CAMERA_W, config.CAMERA_H,
config.CAMERA_FRAMERATE, config.CAMERA_FLIP_METHOD)
# set smoothie's A axis to 0 (nav turn wheels)
response = smoothie.set_current_coordinates(A=0)
if response != smoothie.RESPONSE_OK:
msg = "Failed to set A=0 on smoothie (turning wheels init position), response message:\n" + response
print(msg)
logger_full.write(msg + "\n")
# ask permission to start moving
msg = "Initializing done. Press enter to start moving."
input(msg)
logger_full.write(msg + "\n")
msg = 'GpsQ|Raw ang|Res ang|Ord ang|Sum ang|Distance |Adapter|Smoothie|'
print(msg)
logger_full.write(msg + "\n")
msg = 'GpsQ,Raw ang,Res ang,Ord ang,Sum ang,Distance,Adapter,Smoothie,'
for field_name in report_field_names:
msg += field_name + ","
msg = msg[:-1]
logger_table.write(msg + "\n")
# path points visiting loop
msg = "Generated " + str(len(path_points)) + " points."
logger_full.write(msg + "\n")
for i in range(1, len(path_points)):
from_to = [path_points[i - 1], path_points[i]]
from_to_dist = nav.get_distance(from_to[0], from_to[1])
msg = "Current movement vector: " + str(
from_to) + " Vector size: " + str(from_to_dist)
# print(msg)
logger_full.write(msg + "\n\n")
move_to_point_and_extract(
from_to, gps, vesc_engine, smoothie, camera,
periphery_detector, precise_detector, client, logger_full,
logger_table, report_field_names, used_points_history,
config.UNDISTORTED_ZONE_RADIUS, working_zone_polygon,
working_zone_points_cv, view_zone_polygon, view_zone_points_cv,
config.DEBUG_IMAGES_PATH, nav)
msg = "Work is done."
print(msg)
logger_full.write(msg + "\n")
except KeyboardInterrupt:
msg = "Stopped by a keyboard interrupt (Ctrl + C)\n" + traceback.format_exc(
)
print(msg)
logger_full.write(msg + "\n")
except:
msg = "Exception occurred:\n" + traceback.format_exc()
print(msg)
logger_full.write(msg + "\n")
finally:
# try emergency stop
try:
vesc_engine.stop_moving()
except:
pass
print("Releasing camera...")
camera.release()
# save log data
print("Saving positions histories...")
if len(used_points_history) > 0:
save_gps_coordinates(
used_points_history,
"used_gps_history " + get_current_time() + ".txt"
) # TODO: don't accumulate a lot of points - write each of them to file as soon as they come
else:
msg = "used_gps_history list has 0 elements!"
print(msg)
logger_full.write(msg + "\n")
adapter_points_history = gps.get_last_positions_list(
) # TODO: reduce history positions to 1 to save RAM
if len(adapter_points_history) > 0:
save_gps_coordinates(
adapter_points_history,
"adapter_gps_history " + get_current_time() + ".txt")
else:
msg = "adapter_gps_history list has 0 elements!"
print(msg)
logger_full.write(msg + "\n")
# close log and hardware connections
print("Closing loggers...")
logger_full.close()
logger_table.close()
print("Disconnecting hardware...")
smoothie.disconnect()
vesc_engine.disconnect()
gps.disconnect()
# close transmitting connections
print("Closing transmitters...")
sensor_processor.endSession()
client.closeConnection()
sensor_processor.stopServer()
print("Safe disable is done.")
import adapters
from config import config
sma = adapters.SmoothieAdapter(config.SMOOTHIE_HOST)
smc = sma.get_connector()
tn = smc.get_telnet()
def test_switch_to_relative():
r = sma.try_get_response()
assert r == ""
r = sma.switch_to_relative()
assert r == sma.RESPONSE_OK
r = sma.try_get_response()
assert r == ""
def test_halt():
r = sma.try_get_response()
assert r == ""
r = sma.halt()
assert r == "ok Emergency Stop Requested - reset or M999 required to exit HALT state\r\n"
r = sma.try_get_response()
assert r == ""
r = sma.switch_to_relative()
assert r == sma.RESPONSE_ALARM_LOCK
def main():
#meters_multiplier = ask_meters_multiplier()
#distance, force = ask_speed_mode(meters_multiplier)
rpm = float(input("Set RPM: "))
moving_time = float(input("Set moving time (seconds): "))
print("Initializing detector...")
create_directories(WITH_PLANTS_OUTPUT_PATH, NO_PLANTS_OUTPUT_PATH,
DRAWN_BOXES_OUTPUT_PATH)
detector = detection.YoloOpenCVDetection()
print("Initializing smoothie...")
smoothie = adapters.SmoothieAdapter(config.SMOOTHIE_HOST)
# move camera to the low-center position
print("Moving camera to position...")
smoothie.custom_move_to(config.XY_F_MAX,
X=config.X_MAX / 2,
Y=config.Y_MIN)
smoothie.wait_for_all_actions_done()
print("Initializing camera...")
with CameraAdapterIMX219_170(ISP_DIGITAL_GAIN_RANGE_FROM,
ISP_DIGITAL_GAIN_RANGE_TO, GAIN_RANGE_FROM,
GAIN_RANGE_TO, EXPOSURE_TIME_RANGE_FROM,
EXPOSURE_TIME_RANGE_TO, AE_LOCK) as camera:
time.sleep(2)
counter = 1
try:
with adapters.VescAdapter(rpm, moving_time, VESC_ALIVE_FREQ,
VESC_CHECK_FREQ, VESC_PORT,
VESC_BAUDRATE) as vesc_engine:
# start moving forward
vesc_engine.start_moving()
# get and save images until keyboard interrupt
print("Starting to take photos...")
while vesc_engine.is_movement_allowed():
frame = camera.get_image()
plants = detector.detect(frame)
# sort and save photo
if len(plants) > 0:
save_image(WITH_PLANTS_OUTPUT_PATH, frame, counter,
"With plants")
if SAVE_WITH_BOXES:
detection.draw_boxes(frame, plants)
save_image(DRAWN_BOXES_OUTPUT_PATH, frame, counter,
"With boxes")
else:
save_image(NO_PLANTS_OUTPUT_PATH, frame, counter,
"No plants")
counter += 1
if counter % 100 == 0:
print("Saved", counter, "photos")
except KeyboardInterrupt:
vesc_engine.stop_moving()
print("Stopped by a keyboard interrupt (Ctrl + C)")
print("Saved", counter, "photos")
exit(0)
print("Saved", counter, "photos")
print("Done!")
def main():
mode = int(
input("""
1 = X
2 = X, Y
3 = X, Y, A
4 = X, Y, A, vesc forward
5 = X, Y, A, vesc forward, camera reading
6 = X, Y, A, vesc forward, camera reading, detection
7 = X, Y, A, vesc forward, camera reading, detection, gps
"""))
# check settings and mode
if mode not in range(1, 8):
print("Wrong mode. Exiting.")
exit(1)
if X < 0 or Y < 0 or A < 0 or F < 0:
print(
"Bad value detected in settings (at the script beginning). Exiting."
)
exit(1)
try:
print("Loading smoothie...")
with adapters.SmoothieAdapter(config.SMOOTHIE_HOST) as smoothie:
# start threads if needed
threads = []
if mode > 3:
print("Loading threads...")
threads.append(threading.Thread(target=__th_vesc,
daemon=False))
if mode > 4:
threads.append(
threading.Thread(target=__th_camera, daemon=False))
if mode > 5:
threads.append(
threading.Thread(target=__th_detection, daemon=False))
if mode > 6:
threads.append(
threading.Thread(target=__th_gps, daemon=False))
print("Starting threads...")
for thread in threads:
thread.start()
# start smoothie
print("Starting smoothie movement...")
res = smoothie.ext_align_cork_center(F=config.XY_F_MAX)
smoothie.wait_for_all_actions_done()
if res != smoothie.RESPONSE_OK:
print("Smoothie cork center aligning was failed:\n", res)
movement_positive = True
while True:
if movement_positive:
movement_positive = False
if mode == 1:
res = smoothie.custom_move_for(F=F, X=X)
if mode == 2:
res = smoothie.custom_move_for(F=F, X=X, Y=Y)
if mode > 2:
res = smoothie.custom_move_for(F=F, X=X, Y=Y, A=A)
else:
movement_positive = True
if mode == 1:
res = smoothie.custom_move_for(F=F, X=-X)
if mode == 2:
res = smoothie.custom_move_for(F=F, X=-X, Y=-Y)
if mode > 2:
res = smoothie.custom_move_for(F=F, X=-X, Y=-Y, A=-A)
smoothie.wait_for_all_actions_done()
if res != smoothie.RESPONSE_OK:
print("Smoothie movement failed:\n", res)
except KeyboardInterrupt:
print("Stop request received.")
except:
print("Unexpected exception occurred:")
print(traceback.format_exc())
finally:
print("Stopping everything, please wait for correct shutdown...")
global KEEP_WORKING
KEEP_WORKING = False
print("Halting smoothie...")
smoothie.halt()
smoothie.reset()
print("Waiting for threads...")
for thread in threads:
thread.join()
print("Done.")
def main():
with adapters.SmoothieAdapter(config.SMOOTHIE_HOST) as smoothie:
with adapters.VescAdapter(config.VESC_RPM_SLOW, config.VESC_MOVING_TIME, config.VESC_ALIVE_FREQ,
config.VESC_CHECK_FREQ, config.VESC_PORT, config.VESC_BAUDRATE) as vesc:
name = datetime.datetime.now().strftime("%d-%m-%Y %H:%M")
with utility.Logger(name + '.txt') as logger:
msg = 'Starting durability test...\n'
logger.write(msg)
print(msg)
msg = 'RPM: {0}'.format(config.VESC_RPM_SLOW)
logger.write(msg + "\n")
print(msg)
for i in range(COUNT_OF_ITERATION):
msg = 'Iteration {0}...'.format(i)
logger.write(msg + "\n")
print(msg)
try:
A_rand = rnd.randint(config.A_MIN, config.A_MAX) # 1
msg = 'Start moving the steering wheels to {0}...'.format(A_rand)
logger.write(msg + "\n")
print(msg)
response = smoothie.custom_move_to(config.A_F_MAX, A=A_rand)
# if response != smoothie.RESPONSE_OK:
logger.write(response + '\n')
print(response)
smoothie.wait_for_all_actions_done()
moving_time_rand = rnd.uniform(MOVE_FORWARD_FROM, MOVE_FORWARD_TO) # 2
msg = 'Start moving the robot forward, VESC_MOVING_TIME {0}...'.format(moving_time_rand)
logger.write(msg + '\n')
print(msg)
vesc.set_moving_time(moving_time_rand)
vesc.start_moving()
vesc.wait_for_stop()
msg = 'Stop moving the robot forward, VESC_MOVING_TIME {0}...'.format(moving_time_rand)
logger.write(msg + '\n')
print(msg)
for j in range(COUNT_OF_EXTRACTION):
msg = 'Go to the extraction position Y min...'
logger.write(msg + '\n')
print(msg)
response = smoothie.custom_move_to(config.XY_F_MAX, X=config.X_MAX / 2 / config.XY_COEFFICIENT_TO_MM,
Y=config.Y_MIN)
# if response != smoothie.RESPONSE_OK:
logger.write(response + '\n')
print(response)
smoothie.wait_for_all_actions_done()
point_rand = rnd.choice(config.IMAGE_CONTROL_POINTS_MAP) # 3
msg = 'Start the corkscrew movement to the control point {0}...'.format(point_rand)
logger.write(msg + '\n')
print(msg)
response = smoothie.custom_move_for(config.XY_F_MAX, X=point_rand[2] / config.XY_COEFFICIENT_TO_MM,
Y=point_rand[3] / config.XY_COEFFICIENT_TO_MM)
# if response != smoothie.RESPONSE_OK:
logger.write(response + '\n')
print(response)
smoothie.wait_for_all_actions_done()
cur_coord = smoothie.get_adapter_current_coordinates()
rand_sm_x = rnd.randint(SHIFT_MIN, SHIFT_MAX) # 4
rand_sm_y = rnd.randint(SHIFT_MIN, SHIFT_MAX)
error_msg_x = smoothie.validate_value(cur_coord['X'] * config.XY_COEFFICIENT_TO_MM,
rand_sm_x * config.XY_COEFFICIENT_TO_MM,
"X", config.X_MIN, config.X_MAX, "X_MIN", "X_MAX")
error_msg_y = smoothie.validate_value(cur_coord['Y'] * config.XY_COEFFICIENT_TO_MM,
rand_sm_y * config.XY_COEFFICIENT_TO_MM,
"Y", config.Y_MIN, config.Y_MAX, "Y_MIN", "Y_MAX")
if error_msg_x:
rand_sm_x = -rand_sm_x
if error_msg_y:
rand_sm_y = -rand_sm_y
msg = 'Start pointing the corkscrew by {0} mm along the X axis, and {1} mm along the Y axis...'.format(
rand_sm_x, rand_sm_y)
logger.write(msg + '\n')
print(msg)
response = smoothie.custom_move_for(config.XY_F_MAX, X=rand_sm_x, Y=rand_sm_y)
logger.write(response + '\n')
print(response)
smoothie.wait_for_all_actions_done()
msg = 'Starting extraction movements...' # 5
logger.write(msg + '\n')
print(msg)
response = smoothie.custom_move_for(config.Z_F_EXTRACTION_DOWN, Z=config.EXTRACTION_Z)
print(response)
logger.write(response + "\n")
smoothie.wait_for_all_actions_done()
response = smoothie.ext_cork_up()
print(response)
logger.write(response + "\n")
smoothie.wait_for_all_actions_done()
except KeyboardInterrupt:
msg = "Keyboard interrupt. Exiting."
print(msg)
logger.write(msg + "\n")
exit(0)
except:
error = traceback.format_exc()
logger.write(error + "\n")
print(error)
else:
error = 'No software errors found'
logger.write(error + '\n')
print(error)
msg = 'Done.'
logger.write(msg + '\n')
print(msg)
def main():
used_points_history = []
adapter_points_history = []
raw_angles_history = []
logger = utility.Logger("console " + get_current_time() + ".txt")
# """
path = os.path.abspath(os.getcwd())
sP = SensorProcessing(path, 0)
sP.startServer()
client = Client(4000)
time.sleep(1)
if not client.connectionToServer():
msg = "Connection refused for Server RTK."
print(msg)
logger.write(msg + "\n")
sP.startSession()
# """
try:
nav = navigation.GPSComputing()
msg = "Initializing..."
print(msg)
logger.write(msg + "\n")
with adapters.SmoothieAdapter(config.SMOOTHIE_HOST) as smoothie:
with adapters.VescAdapter(config.VESC_RPM, config.VESC_MOVING_TIME, config.VESC_ALIVE_FREQ,
config.VESC_CHECK_FREQ, config.VESC_PORT, config.VESC_BAUDRATE) as vesc_engine:
with adapters.GPSUbloxAdapter(config.GPS_PORT, config.GPS_BAUDRATE,
config.GPS_POSITIONS_TO_KEEP) as gps:
# set smoothie's A axis to 0 (nav turn wheels)
response = smoothie.set_current_coordinates(A=0)
if response != smoothie.RESPONSE_OK:
msg = "Failed to set A=0 on smoothie (turning wheels init position), response message:\n" + response
print(msg)
logger.write(msg + "\n")
msg = "Initializing done."
print(msg)
logger.write(msg + "\n")
# get route (field) and save it
msg = "Enter 1 to create and save field.txt points file, 2 to load existing file: "
command = input(msg)
msg += command
logger.write(msg + "\n")
# pick from gps
if command == "1":
msg = "Press enter to save point B"
input(msg)
logger.write(msg)
point_b = gps.get_fresh_position()
msg = "Point B saved."
print(msg)
logger.write(msg)
# ask info for moving to next point and move there
rpm = 11000
msg = "RPM = " + str(rpm) + "; set moving time (seconds; will start moving immediately): "
moving_time = input(msg)
msg += moving_time
moving_time = float(moving_time)
vesc_engine.set_rpm(11000)
vesc_engine.set_moving_time(moving_time)
vesc_engine.start_moving()
vesc_engine.wait_for_stop()
vesc_engine.set_moving_time(config.VESC_MOVING_TIME)
vesc_engine.set_rpm(config.VESC_RPM)
msg = "Press enter to save point A"
input(msg)
logger.write(msg)
point_a = gps.get_fresh_position()
msg = "Point A saved."
print(msg)
logger.write(msg)
field_gps_coords = [point_a, point_b]
save_gps_coordinates(field_gps_coords, "field " + get_current_time() + ".txt")
# load from file
elif command == "2":
field_gps_coords = load_coordinates(config.INPUT_GPS_FIELD_FILE)
else:
msg = "Wrong command, exiting."
print(msg)
logger.write(msg + "\n")
exit(1)
# start moving forward
msg = "Press enter to start moving"
input(msg)
logger.write(msg + "\n")
prev_maneuver_time = time.time()
prev_point = gps.get_fresh_position()
vesc_engine.start_moving()
# main navigation control loop
while True:
cur_pos = gps.get_fresh_position()
used_points_history.append(cur_pos.copy())
# """
if not client.sendData("{};{}".format(cur_pos.copy()[0], cur_pos.copy()[1])):
msg = "[Client] Connection closed !"
print(msg)
logger.write(msg + "\n")
# """
if str(cur_pos) == str(prev_point):
msg = "Got the same position, added to history, calculations skipped"
print(msg)
logger.write(msg + "\n")
continue
# check if arrived
# TO DO: it won't work if deviation > course destination diff, as robot will be far away on some
# side and will never get too close to the path ending point
distance = nav.get_distance(cur_pos, field_gps_coords[1])
# reduce speed if near the target point
if USE_SPEED_LIMIT:
if distance <= DECREASE_SPEED_TRESHOLD:
vesc_engine.apply_rpm(int(config.VESC_RPM / 2))
else:
vesc_engine.apply_rpm(config.VESC_RPM)
msg = "Distance to B: " + str(distance)
print(msg)
logger.write(msg + "\n")
if distance <= config.COURSE_DESTINATION_DIFF:
vesc_engine.stop_moving()
msg = "Arrived (allowed destination distance difference " + \
str(config.COURSE_DESTINATION_DIFF) + " mm)"
print(msg)
logger.write(msg + "\n")
break
# do maneuvers not more often than specified value
cur_time = time.time()
if cur_time - prev_maneuver_time < config.MANEUVERS_FREQUENCY:
continue
prev_maneuver_time = cur_time
msg = "Timestamp: " + str(cur_time)
print(msg)
logger.write(msg + "\n")
msg = "Prev: " + str(prev_point) + " Cur: " + str(cur_pos) + " A: " + str(field_gps_coords[0]) \
+ " B: " + str(field_gps_coords[1])
print(msg)
logger.write(msg + "\n")
raw_angle = nav.get_angle(prev_point, cur_pos, cur_pos, field_gps_coords[1])
if len(raw_angles_history) >= config.WINDOW:
raw_angles_history.pop(0)
raw_angles_history.append(raw_angle)
# angle_kp = raw_angle * config.KP
sum_angles = sum(raw_angles_history)
angle_kp = raw_angle * config.KP + sum_angles * config.KI
target_angle_sm = angle_kp * -config.A_ONE_DEGREE_IN_SMOOTHIE # smoothie -Value == left, Value == right
ad_wheels_pos = smoothie.get_adapter_current_coordinates()["A"]
sm_wheels_pos = smoothie.get_smoothie_current_coordinates()["A"]
# compute order angle (smoothie can't turn for huge values immediately also as cancel movement,
# so we need to do nav. actions in steps)
order_angle_sm = target_angle_sm - ad_wheels_pos
# check for out of update frequency and smoothie execution speed (for nav wheels)
if order_angle_sm > config.MANEUVERS_FREQUENCY * config.A_DEGREES_PER_SECOND * \
config.A_ONE_DEGREE_IN_SMOOTHIE:
msg = "Order angle changed from " + str(order_angle_sm) + " to " + str(
config.MANEUVERS_FREQUENCY * config.A_DEGREES_PER_SECOND +
config.A_ONE_DEGREE_IN_SMOOTHIE) + " due to exceeding degrees per tick allowed range."
print(msg)
logger.write(msg + "\n")
order_angle_sm = config.MANEUVERS_FREQUENCY * config.A_DEGREES_PER_SECOND * \
config.A_ONE_DEGREE_IN_SMOOTHIE
elif order_angle_sm < -(config.MANEUVERS_FREQUENCY * config.A_DEGREES_PER_SECOND *
config.A_ONE_DEGREE_IN_SMOOTHIE):
msg = "Order angle changed from " + str(order_angle_sm) + " to " + str(-(
config.MANEUVERS_FREQUENCY * config.A_DEGREES_PER_SECOND *
config.A_ONE_DEGREE_IN_SMOOTHIE)) + " due to exceeding degrees per tick allowed range."
print(msg)
logger.write(msg + "\n")
order_angle_sm = -(config.MANEUVERS_FREQUENCY * config.A_DEGREES_PER_SECOND *
config.A_ONE_DEGREE_IN_SMOOTHIE)
# convert to global coordinates
order_angle_sm += ad_wheels_pos
# checking for out of smoothie supported range
if order_angle_sm > config.A_MAX:
msg = "Global order angle changed from " + str(order_angle_sm) + " to config.A_MAX = " + \
str(config.A_MAX) + " due to exceeding smoothie allowed values range."
print(msg)
logger.write(msg + "\n")
order_angle_sm = config.A_MAX
elif order_angle_sm < config.A_MIN:
msg = "Global order angle changed from " + str(order_angle_sm) + " to config.A_MIN = " + \
str(config.A_MIN) + " due to exceeding smoothie allowed values range."
print(msg)
logger.write(msg + "\n")
order_angle_sm = config.A_MIN
msg = "Adapter wheels pos (target): " + str(ad_wheels_pos) + " Smoothie wheels pos (current): "\
+ str(sm_wheels_pos)
print(msg)
logger.write(msg + "\n")
msg = "KI: " + str(config.KI) + " Sum angles: " + str(sum_angles) + " Sum angles history: " + \
str(raw_angles_history)
print(msg)
logger.write(msg + "\n")
msg = "KP: " + str(config.KP) + " Raw angle: " + str(raw_angle) + " Angle * KP: " + \
str(angle_kp) + " Smoothie target angle: " + str(target_angle_sm) + \
" Smoothie absolute order angle: " + str(order_angle_sm)
print(msg)
logger.write(msg + "\n")
prev_point = cur_pos
response = smoothie.nav_turn_wheels_to(order_angle_sm, config.A_F_MAX)
msg = "Smoothie response: " + response
print(msg)
logger.write(msg)
# next tick indent
print()
logger.write("\n")
adapter_points_history = gps.get_last_positions_list()
msg = "Done!"
print(msg)
logger.write(msg + "\n")
except KeyboardInterrupt:
msg = "Stopped by a keyboard interrupt (Ctrl + C)"
print(msg)
logger.write(msg + "\n")
except Exception:
msg = "Exception occurred:\n" + traceback.format_exc()
print(msg)
logger.write(msg)
finally:
if len(used_points_history) > 0:
save_gps_coordinates(used_points_history, "used_gps_history " + get_current_time() + ".txt")
else:
msg = "used_gps_history list has 0 elements!"
print(msg)
logger.write(msg)
if len(adapter_points_history) > 0:
save_gps_coordinates(adapter_points_history, "adapter_gps_history " + get_current_time() + ".txt")
else:
msg = "adapter_gps_history list has 0 elements!"
print(msg)
logger.write(msg)
logger.close()
# """
sP.endSession()
client.closeConnection()
sP.stopServer()
