def compute_x2_spiral(point_a: list, point_b: list,
nav: navigation.GPSComputing, logger: utility.Logger):
"""
Computes p. x2 with distance + spiral interval distance from p. B. Distances are loaded from
config file. Returns None if AB <= distance * 2 + interval (as there's no place for robot maneuvers).
:param point_a:
:param point_b:
:param nav:
:param logger:
:return:
"""
cur_vec_dist = nav.get_distance(point_a, point_b)
# check if moving vector is too small for maneuvers
if config.MANEUVER_START_DISTANCE * 2 + config.SPIRAL_SIDES_INTERVAL >= cur_vec_dist:
msg = "No place for maneuvers; Config maneuver distance is (that will be multiplied by 2): " + \
str(config.MANEUVER_START_DISTANCE) + " Config spiral interval: " + str(config.SPIRAL_SIDES_INTERVAL) + \
" Current moving vector distance is: " + str(cur_vec_dist) + " Given points are: " + str(point_a) + \
" " + str(point_b)
# print(msg)
logger.write(msg + "\n")
return None
return nav.get_point_on_vector(
point_a, point_b, cur_vec_dist - config.MANEUVER_START_DISTANCE -
config.SPIRAL_SIDES_INTERVAL)
def compute_x1_x2_int_points(point_a: list, point_b: list,
nav: navigation.GPSComputing,
logger: utility.Logger):
"""
Computes spiral interval points x1, x2
:param point_a:
:param point_b:
:param nav:
:param logger:
:return:
"""
cur_vec_dist = nav.get_distance(point_a, point_b)
# check if moving vector is too small for maneuvers
if config.SPIRAL_SIDES_INTERVAL * 2 >= cur_vec_dist:
msg = "No place for maneuvers; Config spiral interval (that will be multiplied by 2): " + \
str(config.SPIRAL_SIDES_INTERVAL) + " Current moving vector distance is: " + str(cur_vec_dist) + \
" Given points are: " + str(point_a) + " " + str(point_b)
# print(msg)
logger.write(msg + "\n")
return None
point_x1_int = nav.get_point_on_vector(point_a, point_b,
config.SPIRAL_SIDES_INTERVAL)
point_x2_int = nav.get_point_on_vector(
point_a, point_b, cur_vec_dist - config.SPIRAL_SIDES_INTERVAL)
return point_x1_int, point_x2_int
def compute_x1_x2_points(point_a: list, point_b: list,
nav: navigation.GPSComputing, logger: utility.Logger):
"""
Computes p. x1 with config distance from p. A and p. x2 with the same distance from p. B. Distance is loaded from
config file. Returns None if AB <= that distance (as there's no place for robot maneuvers).
:param point_a:
:param point_b:
:param nav:
:param logger:
:return:
"""
cur_vec_dist = nav.get_distance(point_a, point_b)
# check if moving vector is too small for maneuvers
if config.MANEUVER_START_DISTANCE * 2 >= cur_vec_dist:
msg = "No place for maneuvers; config start maneuver distance is (that will be multiplied by 2): " + \
str(config.MANEUVER_START_DISTANCE) + " current moving vector distance is: " + str(cur_vec_dist) + \
" Given points are: " + str(point_a) + " " + str(point_b)
# print(msg)
logger.write(msg + "\n")
return None, None
point_x1 = nav.get_point_on_vector(point_a, point_b,
config.MANEUVER_START_DISTANCE)
point_x2 = nav.get_point_on_vector(
point_a, point_b, cur_vec_dist - config.MANEUVER_START_DISTANCE)
return point_x1, point_x2
def move_to_point_and_extract(
coords_from_to: list, gps: adapters.GPSUbloxAdapter,
vesc_engine: adapters.VescAdapter, smoothie: adapters.SmoothieAdapter,
camera: adapters.CameraAdapterIMX219_170,
periphery_det: detection.YoloOpenCVDetection,
precise_det: detection.YoloOpenCVDetection, client,
logger_full: utility.Logger, logger_table: utility.Logger,
report_field_names, used_points_history: list, undistorted_zone_radius,
working_zone_polygon, working_zone_points_cv, view_zone_polygon,
view_zone_points_cv, img_output_dir, nav: navigation.GPSComputing):
"""
Moves to the given target point and extracts all weeds on the way.
:param coords_from_to:
:param gps:
:param vesc_engine:
:param smoothie:
:param camera:
:param periphery_det:
:param precise_det:
:param client:
:param logger_full:
:param logger_table:
:param report_field_names:
:param used_points_history:
:param nav:
:param working_zone_polygon:
:param undistorted_zone_radius:
:param working_zone_points_cv:
:param img_output_dir:
:return:
"""
vesc_engine.apply_rpm(config.VESC_RPM_SLOW)
slow_mode_time = -float("inf")
current_working_mode = working_mode_slow = 1
working_mode_switching = 2
working_mode_fast = 3
close_to_end = True # True if robot is close to one of current movement vector points, False otherwise
raw_angles_history = []
stop_helping_point = nav.get_coordinate(coords_from_to[1],
coords_from_to[0], 90, 1000)
prev_maneuver_time = time.time()
prev_pos = gps.get_last_position()
# set camera to the Y min
res = smoothie.custom_move_to(config.XY_F_MAX,
X=config.X_MAX / 2 /
config.XY_COEFFICIENT_TO_MM,
Y=config.Y_MIN)
if res != smoothie.RESPONSE_OK:
msg = "INIT: Failed to move camera to Y min, smoothie response:\n" + res
logger_full.write(msg + "\n")
smoothie.wait_for_all_actions_done()
# main navigation control loop
while True:
# EXTRACTION CONTROL
start_t = time.time()
frame = camera.get_image()
frame_t = time.time()
plants_boxes = periphery_det.detect(frame)
per_det_t = time.time()
debug_save_image(
img_output_dir,
"(periphery view scan M=" + str(current_working_mode) + ")", frame,
plants_boxes, undistorted_zone_radius, working_zone_points_cv
if current_working_mode == 1 else view_zone_points_cv)
msg = "View frame time: " + str(
frame_t - start_t) + "\t\tPer. det. time: " + str(per_det_t -
frame_t)
logger_full.write(msg + "\n")
# slow mode
if current_working_mode == working_mode_slow:
if any_plant_in_zone(plants_boxes, working_zone_polygon):
vesc_engine.stop_moving()
time.sleep(0.2)
start_work_t = time.time()
frame = camera.get_image()
frame_t = time.time()
plants_boxes = precise_det.detect(frame)
pre_det_t = time.time()
debug_save_image(img_output_dir, "(precise view scan M=1)",
frame, plants_boxes, undistorted_zone_radius,
working_zone_points_cv)
msg = "Work frame time: " + str(
frame_t -
start_work_t) + "\t\tPrec. det. time: " + str(pre_det_t -
frame_t)
logger_full.write(msg + "\n")
if any_plant_in_zone(plants_boxes, working_zone_polygon):
extract_all_plants(smoothie, camera, precise_det,
working_zone_polygon, frame,
plants_boxes, undistorted_zone_radius,
working_zone_points_cv, img_output_dir)
elif not any_plant_in_zone(plants_boxes, view_zone_polygon) and \
time.time() - slow_mode_time > config.SLOW_MODE_MIN_TIME:
# set camera to the Y max
res = smoothie.custom_move_to(
config.XY_F_MAX,
X=config.X_MAX / 2 / config.XY_COEFFICIENT_TO_MM,
Y=config.Y_MAX / config.XY_COEFFICIENT_TO_MM)
if res != smoothie.RESPONSE_OK:
msg = "M=" + str(
current_working_mode
) + ": " + "Failed to move to Y max, smoothie response:\n" + res
logger_full.write(msg + "\n")
smoothie.wait_for_all_actions_done()
current_working_mode = working_mode_switching
vesc_engine.start_moving()
# switching to fast mode
elif current_working_mode == working_mode_switching:
if any_plant_in_zone(plants_boxes, view_zone_polygon):
vesc_engine.stop_moving()
# set camera to the Y min
res = smoothie.custom_move_to(config.XY_F_MAX,
X=config.X_MAX / 2 /
config.XY_COEFFICIENT_TO_MM,
Y=config.Y_MIN)
if res != smoothie.RESPONSE_OK:
msg = "M=" + str(
current_working_mode
) + ": " + "Failed to move to Y min, smoothie response:\n" + res
logger_full.write(msg + "\n")
smoothie.wait_for_all_actions_done()
current_working_mode = working_mode_slow
slow_mode_time = time.time()
elif smoothie.get_smoothie_current_coordinates(False)[
"Y"] + config.XY_COEFFICIENT_TO_MM * 20 > config.Y_MAX:
current_working_mode = working_mode_fast
if not close_to_end:
vesc_engine.apply_rpm(config.VESC_RPM_FAST)
# fast mode
else:
if any_plant_in_zone(plants_boxes, view_zone_polygon):
vesc_engine.stop_moving()
# set camera to the Y min
res = smoothie.custom_move_to(config.XY_F_MAX,
X=config.X_MAX / 2 /
config.XY_COEFFICIENT_TO_MM,
Y=config.Y_MIN)
if res != smoothie.RESPONSE_OK:
msg = "M=" + str(
current_working_mode
) + ": " + "Failed to move to Y min, smoothie response:\n" + res
logger_full.write(msg + "\n")
smoothie.wait_for_all_actions_done()
current_working_mode = working_mode_slow
slow_mode_time = time.time()
vesc_engine.apply_rpm(config.VESC_RPM_SLOW)
elif close_to_end:
vesc_engine.apply_rpm(config.VESC_RPM_SLOW)
else:
vesc_engine.apply_rpm(config.VESC_RPM_FAST)
# NAVIGATION CONTROL
nav_start_t = time.time()
cur_pos = gps.get_last_position()
if str(cur_pos) == str(prev_pos):
# msg = "Got the same position, added to history, calculations skipped. Am I stuck?"
# print(msg)
# logger_full.write(msg + "\n")
continue
used_points_history.append(cur_pos.copy())
if not client.sendData("{};{}".format(cur_pos[0], cur_pos[1])):
msg = "[Client] Connection closed !"
print(msg)
logger_full.write(msg + "\n")
distance = nav.get_distance(cur_pos, coords_from_to[1])
msg = "Distance to B: " + str(distance)
# print(msg)
logger_full.write(msg + "\n")
# check if arrived
_, side = nav.get_deviation(coords_from_to[1], stop_helping_point,
cur_pos)
# if distance <= config.COURSE_DESTINATION_DIFF: # old way
if side != 1: # TODO: maybe should use both side and distance checking methods at once
vesc_engine.stop_moving()
# msg = "Arrived (allowed destination distance difference " + str(config.COURSE_DESTINATION_DIFF) + " mm)"
msg = "Arrived to " + str(coords_from_to[1])
# print(msg)
logger_full.write(msg + "\n")
break
# pass by cur points which are very close to prev point to prevent angle errors when robot is staying
# (too close points in the same position can produce false huge angles)
if nav.get_distance(prev_pos,
cur_pos) < config.PREV_CUR_POINT_MIN_DIST:
continue
# reduce speed if near the target point
if config.USE_SPEED_LIMIT:
distance_from_start = nav.get_distance(coords_from_to[0], cur_pos)
close_to_end = distance < config.DECREASE_SPEED_TRESHOLD or distance_from_start < config.DECREASE_SPEED_TRESHOLD
# 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 = "Prev: " + str(prev_pos) + " Cur: " + str(cur_pos) + " A: " + str(coords_from_to[0]) \
+ " B: " + str(coords_from_to[1])
# print(msg)
logger_full.write(msg + "\n")
raw_angle = nav.get_angle(prev_pos, cur_pos, cur_pos,
coords_from_to[1])
# sum(e)
if len(raw_angles_history) >= config.WINDOW:
raw_angles_history.pop(0)
raw_angles_history.append(raw_angle)
sum_angles = sum(raw_angles_history)
if sum_angles > config.SUM_ANGLES_HISTORY_MAX:
msg = "Sum angles " + str(sum_angles) + " is bigger than max allowed value " + \
str(config.SUM_ANGLES_HISTORY_MAX) + ", setting to " + str(config.SUM_ANGLES_HISTORY_MAX)
# print(msg)
logger_full.write(msg + "\n")
sum_angles = config.SUM_ANGLES_HISTORY_MAX
elif sum_angles < -config.SUM_ANGLES_HISTORY_MAX:
msg = "Sum angles " + str(sum_angles) + " is less than min allowed value " + \
str(-config.SUM_ANGLES_HISTORY_MAX) + ", setting to " + str(-config.SUM_ANGLES_HISTORY_MAX)
# print(msg)
logger_full.write(msg + "\n")
sum_angles = -config.SUM_ANGLES_HISTORY_MAX
angle_kp_ki = raw_angle * config.KP + sum_angles * config.KI
target_angle_sm = angle_kp_ki * -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 range (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_full.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_full.write(msg + "\n")
order_angle_sm = -(config.MANEUVERS_FREQUENCY *
config.A_DEGREES_PER_SECOND *
config.A_ONE_DEGREE_IN_SMOOTHIE)
# convert to global smoothie 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_full.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_full.write(msg + "\n")
order_angle_sm = config.A_MIN
raw_angle = round(raw_angle, 2)
angle_kp_ki = round(angle_kp_ki, 2)
order_angle_sm = round(order_angle_sm, 2)
sum_angles = round(sum_angles, 2)
distance = round(distance, 2)
ad_wheels_pos = round(ad_wheels_pos, 2)
sm_wheels_pos = round(sm_wheels_pos, 2)
gps_quality = cur_pos[2]
msg = str(gps_quality).ljust(5) + str(raw_angle).ljust(8) + str(
angle_kp_ki).ljust(8) + str(order_angle_sm).ljust(8) + str(
sum_angles).ljust(8) + str(distance).ljust(13) + str(
ad_wheels_pos).ljust(8) + str(sm_wheels_pos).ljust(9)
print(msg)
logger_full.write(msg + "\n")
# load sensors data to csv
s = ","
msg = str(gps_quality) + s + str(raw_angle) + s + str(angle_kp_ki) + s + str(order_angle_sm) + s + \
str(sum_angles) + s + str(distance) + s + str(ad_wheels_pos) + s + str(sm_wheels_pos)
vesc_data = vesc_engine.get_sensors_data(report_field_names)
if vesc_data is not None:
msg += s
for key in vesc_data:
msg += str(vesc_data[key]) + s
msg = msg[:-1]
logger_table.write(msg + "\n")
prev_pos = cur_pos
response = smoothie.nav_turn_wheels_to(order_angle_sm, config.A_F_MAX)
if response != smoothie.RESPONSE_OK: # TODO: what if response is not ok?
msg = "Smoothie response is not ok: " + response
print(msg)
logger_full.write(msg + "\n")
msg = "Nav calc time: " + str(time.time() - nav_start_t)
logger_full.write(msg + "\n\n")
def move_to_point(coords_from_to: list, used_points_history: list,
gps: adapters.GPSUbloxAdapter,
vesc_engine: adapters.VescAdapter,
smoothie: adapters.SmoothieAdapter, logger: utility.Logger,
client, nav: navigation.GPSComputing,
raw_angles_history: list):
"""
Moves to given point.
:param coords_from_to:
:param used_points_history:
:param gps:
:param vesc_engine:
:param smoothie:
:param logger:
:param client:
:param nav:
:return:
:param raw_angles_history:
"""
raw_angles_history = []
stop_helping_point = nav.get_coordinate(coords_from_to[1],
coords_from_to[0], 90, 1000)
prev_maneuver_time = time.time()
prev_point = gps.get_fresh_position(
) # TODO: maybe it's ok to get last position instead of waiting for fresh
vesc_engine.set_rpm(int(config.VESC_RPM / 2))
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[0], cur_pos[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
distance = nav.get_distance(cur_pos, coords_from_to[1])
msg = "Distance to B: " + str(distance)
print(msg)
logger.write(msg + "\n")
# check if arrived
_, side = nav.get_deviation(coords_from_to[1], stop_helping_point,
cur_pos)
# if distance <= config.COURSE_DESTINATION_DIFF: # old way
if side != 1: # TODO: maybe should use both side and distance checking methods at once
vesc_engine.stop_moving()
# msg = "Arrived (allowed destination distance difference " + str(config.COURSE_DESTINATION_DIFF) + " mm)"
msg = "Arrived."
print(msg)
logger.write(msg + "\n")
break
# reduce speed if near the target point
if config.USE_SPEED_LIMIT:
distance_from_start = nav.get_distance(coords_from_to[0], cur_pos)
if distance < config.DECREASE_SPEED_TRESHOLD or distance_from_start < config.DECREASE_SPEED_TRESHOLD:
vesc_engine.apply_rpm(int(config.VESC_RPM / 2))
else:
vesc_engine.apply_rpm(config.VESC_RPM)
# 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(coords_from_to[0]) \
+ " B: " + str(coords_from_to[1])
print(msg)
logger.write(msg + "\n")
raw_angle = nav.get_angle(prev_point, cur_pos, cur_pos,
coords_from_to[1])
# sum(e)
if len(raw_angles_history) >= config.WINDOW:
raw_angles_history.pop(0)
raw_angles_history.append(raw_angle)
sum_angles = sum(raw_angles_history)
if sum_angles > config.SUM_ANGLES_HISTORY_MAX:
msg = "Sum angles " + str(sum_angles) + " is bigger than max allowed value " + \
str(config.SUM_ANGLES_HISTORY_MAX) + ", setting to " + str(config.SUM_ANGLES_HISTORY_MAX)
print(msg)
logger.write(msg)
sum_angles = config.SUM_ANGLES_HISTORY_MAX
elif sum_angles < -config.SUM_ANGLES_HISTORY_MAX:
msg = "Sum angles " + str(sum_angles) + " is less than min allowed value " + \
str(-config.SUM_ANGLES_HISTORY_MAX) + ", setting to " + str(-config.SUM_ANGLES_HISTORY_MAX)
print(msg)
logger.write(msg)
sum_angles = -config.SUM_ANGLES_HISTORY_MAX
angle_kp_ki = raw_angle * config.KP + sum_angles * config.KI
target_angle_sm = angle_kp_ki * -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 + sum(angles) * KI: " + \
str(angle_kp_ki) + " 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")
def move_to_point(
coords_from_to: list, used_points_history: list,
gps: adapters.GPSUbloxAdapter, vesc_engine: adapters.VescAdapter,
smoothie: adapters.SmoothieAdapter, logger_full: utility.Logger,
client, nav: navigation.GPSComputing, raw_angles_history: list,
report_writer, report_field_names, logger_table: utility.Logger,
periphery_detector: detection.YoloOpenCVDetection,
camera: adapters.CameraAdapterIMX219_170, working_zone_points_cv):
"""
Moves to given point.
:param coords_from_to:
:param used_points_history:
:param gps:
:param vesc_engine:
:param smoothie:
:param logger_full:
:param client:
:param nav:
:param raw_angles_history:
:return:
"""
raw_angles_history = []
stop_helping_point = nav.get_coordinate(coords_from_to[1],
coords_from_to[0], 90, 1000)
prev_maneuver_time = time.time()
prev_point = gps.get_fresh_position(
) # TODO: maybe it's ok to get last position instead of waiting for fresh
vesc_engine.set_rpm(config.VESC_RPM_SLOW)
vesc_engine.start_moving()
# main navigation control loop
while True:
# DETECTION
frame = camera.get_image()
plants_boxes = periphery_detector.detect(frame)
if len(plants_boxes) > 0:
if config.SAVE_DEBUG_IMAGES:
debug_save_image(config.DEBUG_IMAGES_PATH, "", frame,
plants_boxes, config.UNDISTORTED_ZONE_RADIUS,
working_zone_points_cv)
# NAVIGATION
cur_pos = gps.get_fresh_position()
used_points_history.append(cur_pos.copy())
if not client.sendData("{};{}".format(cur_pos[0], cur_pos[1])):
msg = "[Client] Connection closed !"
print(msg)
logger_full.write(msg + "\n")
if str(cur_pos) == str(prev_point):
msg = "Got the same position, added to history, calculations skipped. Am I stuck?"
print(msg)
logger_full.write(msg + "\n")
continue
distance = nav.get_distance(cur_pos, coords_from_to[1])
msg = "Distance to B: " + str(distance)
# print(msg)
logger_full.write(msg + "\n")
# check if arrived
_, side = nav.get_deviation(coords_from_to[1], stop_helping_point,
cur_pos)
# if distance <= config.COURSE_DESTINATION_DIFF: # old way
if side != 1: # TODO: maybe should use both side and distance checking methods at once
vesc_engine.stop_moving()
# msg = "Arrived (allowed destination distance difference " + str(config.COURSE_DESTINATION_DIFF) + " mm)"
msg = "Arrived to " + str(coords_from_to[1])
# print(msg)
logger_full.write(msg + "\n")
break
# reduce speed if near the target point
if config.USE_SPEED_LIMIT:
distance_from_start = nav.get_distance(coords_from_to[0], cur_pos)
if distance < config.DECREASE_SPEED_TRESHOLD or distance_from_start < config.DECREASE_SPEED_TRESHOLD:
vesc_engine.apply_rpm(config.VESC_RPM_SLOW)
else:
vesc_engine.apply_rpm(config.VESC_RPM_FAST)
# 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_full.write(msg + "\n")
msg = "Prev: " + str(prev_point) + " Cur: " + str(cur_pos) + " A: " + str(coords_from_to[0]) \
+ " B: " + str(coords_from_to[1])
# print(msg)
logger_full.write(msg + "\n")
raw_angle = nav.get_angle(prev_point, cur_pos, cur_pos,
coords_from_to[1])
# sum(e)
if len(raw_angles_history) >= config.WINDOW:
raw_angles_history.pop(0)
raw_angles_history.append(raw_angle)
sum_angles = sum(raw_angles_history)
if sum_angles > config.SUM_ANGLES_HISTORY_MAX:
msg = "Sum angles " + str(sum_angles) + " is bigger than max allowed value " + \
str(config.SUM_ANGLES_HISTORY_MAX) + ", setting to " + str(config.SUM_ANGLES_HISTORY_MAX)
# print(msg)
logger_full.write(msg + "\n")
sum_angles = config.SUM_ANGLES_HISTORY_MAX
elif sum_angles < -config.SUM_ANGLES_HISTORY_MAX:
msg = "Sum angles " + str(sum_angles) + " is less than min allowed value " + \
str(-config.SUM_ANGLES_HISTORY_MAX) + ", setting to " + str(-config.SUM_ANGLES_HISTORY_MAX)
# print(msg)
logger_full.write(msg + "\n")
sum_angles = -config.SUM_ANGLES_HISTORY_MAX
angle_kp_ki = raw_angle * config.KP + sum_angles * config.KI
target_angle_sm = angle_kp_ki * -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 range (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_full.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_full.write(msg + "\n")
order_angle_sm = -(config.MANEUVERS_FREQUENCY *
config.A_DEGREES_PER_SECOND *
config.A_ONE_DEGREE_IN_SMOOTHIE)
# convert to global smoothie 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_full.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_full.write(msg + "\n")
order_angle_sm = config.A_MIN
raw_angle = round(raw_angle, 2)
angle_kp_ki = round(angle_kp_ki, 2)
order_angle_sm = round(order_angle_sm, 2)
sum_angles = round(sum_angles, 2)
distance = round(distance, 2)
ad_wheels_pos = round(ad_wheels_pos, 2)
sm_wheels_pos = round(sm_wheels_pos, 2)
gps_quality = cur_pos[2]
msg = str(gps_quality).ljust(5) + str(raw_angle).ljust(8) + str(
angle_kp_ki).ljust(8) + str(order_angle_sm).ljust(8) + str(
sum_angles).ljust(8) + str(distance).ljust(13) + str(
ad_wheels_pos).ljust(8) + str(sm_wheels_pos).ljust(9)
print(msg)
logger_full.write(msg + "\n")
# load sensors data to csv
s = ","
msg = str(gps_quality) + s + str(raw_angle) + s + str(angle_kp_ki) + s + str(order_angle_sm) + s + \
str(sum_angles) + s + str(distance) + s + str(ad_wheels_pos) + s + str(sm_wheels_pos)
vesc_data = vesc_engine.get_sensors_data(report_field_names)
if vesc_data is not None:
msg += s
for key in vesc_data:
msg += str(vesc_data[key]) + s
msg = msg[:-1]
logger_table.write(msg + "\n")
prev_point = cur_pos
response = smoothie.nav_turn_wheels_to(order_angle_sm, config.A_F_MAX)
if response != smoothie.RESPONSE_OK:
msg = "Smoothie response is not ok: " + response + "\n"
print(msg)
logger_full.write(msg + "\n\n")
def extract_all_plants(smoothie: adapters.SmoothieAdapter, camera: adapters.CameraAdapterIMX219_170,
detector: detection.YoloOpenCVDetection, working_zone_polygon: Polygon, frame,
plant_boxes: list, undistorted_zone_radius, working_zone_points_cv, img_output_dir,
logger_full: utility.Logger, data_collector: datacollection.DataCollector):
"""Extract all plants found in current position"""
msg = "Extracting " + str(len(plant_boxes)) + " plants"
logger_full.write(msg + "\n")
# loop over all detected plants
for box in plant_boxes:
# go to the extraction position Y min
smoothie.custom_move_to(config.XY_F_MAX, X=config.X_MAX / 2 / config.XY_COEFFICIENT_TO_MM, Y=config.Y_MIN)
smoothie.wait_for_all_actions_done()
box_x, box_y = box.get_center_points()
# if plant is in working zone (can be reached by cork)
if is_point_in_poly(box_x, box_y, working_zone_polygon):
# extraction loop
for _ in range(config.EXTRACTION_TUNING_MAX_COUNT):
box_x, box_y = box.get_center_points()
# if plant inside undistorted zone
if is_point_in_circle(box_x, box_y, config.SCENE_CENTER_X, config.SCENE_CENTER_Y, undistorted_zone_radius):
msg = "Plant " + str(box) + " is in undistorted zone"
logger_full.write(msg + "\n")
# TODO: use plant box from precise NN for movement calculations
# calculate values to move camera over a plant
sm_x = px_to_smoothie_value(box_x, config.SCENE_CENTER_X, config.ONE_MM_IN_PX)
sm_y = -px_to_smoothie_value(box_y, config.SCENE_CENTER_Y, config.ONE_MM_IN_PX)
# swap camera and cork for extraction immediately
sm_x += config.CORK_TO_CAMERA_DISTANCE_X
sm_y += config.CORK_TO_CAMERA_DISTANCE_Y
# move cork 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:
msg = "Couldn't move cork over plant, smoothie error occurred:\n" + res
logger_full.write(msg + "\n")
break
# debug image saving
frame = camera.get_image()
debug_save_image(img_output_dir, "(before first cork down)", frame, [],
undistorted_zone_radius, working_zone_points_cv)
# extraction
if hasattr(extraction.ExtractionMethods, box.get_name()):
# TODO: it's temporary log (1)
msg = "Trying extractions: 5" # only Daisy implemented, it has 5 drops
logger_full.write(msg + "\n")
res, cork_is_stuck = getattr(extraction.ExtractionMethods, box.get_name())(smoothie, box)
else:
# TODO: it's temporary log (2)
# 5 drops is default, also 1 center drop is possible
drops = 5 if config.EXTRACTION_DEFAULT_METHOD == "five_drops_near_center" else 1
msg = "Trying extractions: " + str(drops)
logger_full.write(msg + "\n")
res, cork_is_stuck = getattr(extraction.ExtractionMethods, config.EXTRACTION_DEFAULT_METHOD)(smoothie, box)
if res != smoothie.RESPONSE_OK:
logger_full.write(res + "\n")
if cork_is_stuck: # danger flag is True if smoothie couldn't pick up cork
msg = "Cork is stuck! Emergency stopping."
logger_full.write(msg + "\n")
exit(1)
else:
data_collector.add_extractions_data(box.get_name(), 1)
break
# if outside undistorted zone but in working zone
else:
msg = "Plant is in working zone, trying to get closer"
logger_full.write(msg + "\n")
# calculate values for move camera closer to a plant
control_point = get_closest_control_point(box_x, box_y, config.IMAGE_CONTROL_POINTS_MAP)
# fixing cork tube view obscuring
if config.AVOID_CORK_VIEW_OBSCURING:
# compute target point x
C_H = box_x - control_point[0] # may be negative
H_x = control_point[0] + C_H
target_x = H_x
# compute target point y
T1_y = control_point[1] - config.UNDISTORTED_ZONE_RADIUS
T1_P = box_y - T1_y # always positive
target_y = control_point[1] + T1_P - config.DISTANCE_FROM_UNDIST_BORDER
# transfer that to millimeters
sm_x = px_to_smoothie_value(target_x, control_point[0], config.ONE_MM_IN_PX)
sm_y = -px_to_smoothie_value(target_y, control_point[1], config.ONE_MM_IN_PX)
# move camera closer to a plant (and trying to avoid obscuring)
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:
msg = "Couldn't apply cork obscuring, smoothie's response:\n" + res + "\n" + \
"(box_x: " + str(box_x) + " box_y: " + str(box_y) + " target_x: " + str(target_x) + \
" target_y: " + str(target_y) + " cp_x: " + str(control_point[0]) + " cp_y: " + \
str(control_point[1]) + ")"
logger_full.write(msg + "\n")
sm_x, sm_y = control_point[2], control_point[3]
# 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:
msg = "Couldn't move camera closer to plant, smoothie error occurred:\n" + res
logger_full.write(msg + "\n")
break
else:
sm_x, sm_y = control_point[2], control_point[3]
# 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:
msg = "Couldn't move camera closer to plant, smoothie error occurred:\n" + res
logger_full.write(msg + "\n")
break
# make new photo and re-detect plants
frame = camera.get_image()
temp_plant_boxes = detector.detect(frame)
# debug image saving
debug_save_image(img_output_dir, "(extraction specify)", frame, temp_plant_boxes,
undistorted_zone_radius, working_zone_points_cv)
# check case if no plants detected
if len(temp_plant_boxes) == 0:
msg = "No plants detected (plant was in working zone before), trying to move on next item"
logger_full.write(msg + "\n")
break
# get closest box (update current box from main list coordinates after moving closer)
box = min_plant_box_dist(temp_plant_boxes, config.SCENE_CENTER_X, config.SCENE_CENTER_Y)
else:
msg = "Too much extraction attempts, trying to extract next plant if there is."
logger_full.write(msg)
# if not in working zone
else:
msg = "Skipped " + str(box) + " (not in working area)"
logger_full.write(msg + "\n")
# set camera back to the Y min
smoothie.custom_move_to(config.XY_F_MAX, X=config.X_MAX / 2 / config.XY_COEFFICIENT_TO_MM, Y=config.Y_MIN)
smoothie.wait_for_all_actions_done()
