def main():
nav = navigation.GPSComputing()
with adapters.GPSUbloxAdapter(config.GPS_PORT, config.GPS_BAUDRATE,
config.GPS_POSITIONS_TO_KEEP) as gps:
with adapters.VescAdapter(RPM, MOVING_TIME, config.VESC_ALIVE_FREQ,
config.VESC_CHECK_FREQ, config.VESC_PORT,
config.VESC_BAUDRATE) as vesc_engine:
print("Getting a starting point...")
starting_point = gps.get_last_position()
print("Moving forward...")
vesc_engine.start_moving()
vesc_engine.wait_for_stop()
print("Getting point A...")
time.sleep(1)
A = gps.get_last_position()
print("Computing rest points...")
B = nav.get_coordinate(A, starting_point, 180, FIELD_SIZE)
C = nav.get_coordinate(B, A, 90, FIELD_SIZE)
D = nav.get_coordinate(C, B, 90, FIELD_SIZE)
print("Saving field.txt file...")
save_gps_coordinates([A, B, C, D], "field.txt")
print("Done.")
def __th_gps():
print("Loading gps...")
with adapters.GPSUbloxAdapter(config.GPS_PORT, config.GPS_BAUDRATE,
config.GPS_POSITIONS_TO_KEEP) as gps:
print("Starting reading from gps...")
show_ok_msg = True
while KEEP_WORKING:
cur_pos = gps.get_last_position()
if show_ok_msg:
print(
"Point from GPS is received, seems like GPS is ok, keep reading."
)
show_ok_msg = False
print("Gps is stopped correctly.")
def main():
with adapters.GPSUbloxAdapter(config.GPS_PORT, config.GPS_BAUDRATE,
config.GPS_POSITIONS_TO_KEEP) as gps:
i = 1
points = []
while True:
cmd = input("Press enter to get point " + str(i) +
", type anything to exit.")
if cmd != "":
break
point = gps.get_fresh_position()
points.append(point)
print(point)
#print("Point", i, "saved.\n")
#save_gps_coordinates(points, config.INPUT_GPS_FIELD_FILE)
print("Exit !")
def __init__(self, socketio: SocketIO, logger: utility.Logger):
self.socketio = socketio
self.logger = logger
self.socketio.emit('field', {"status": "pushed"}, namespace='/button', broadcast=True)
try:
msg = f"[{self.__class__.__name__}] -> initSmoothie"
self.logger.write_and_flush(msg+"\n")
print(msg)
self.smoothie = initSmoothie(self.logger)
msg = f"[{self.__class__.__name__}] -> initGPS"
self.logger.write_and_flush(msg+"\n")
print(msg)
self.gps = adapters.GPSUbloxAdapter(config.GPS_PORT, config.GPS_BAUDRATE, config.GPS_POSITIONS_TO_KEEP)
msg = f"[{self.__class__.__name__}] -> initGPSComputing"
self.logger.write_and_flush(msg+"\n")
print(msg)
self.nav = navigation.GPSComputing()
except:
self.on_event(Events.ERROR)
self.statusOfUIObject = {
"joystick": True, #True or False
"fieldButton": "charging", #True or False or charging or validate or None
"startButton": False, #True or False or charging or None
"continueButton": False, #True or False or charging or None
"stopButton": True, #True or charging or None
"wheelButton": False, #True or False
"audit": 'disable', #True or False or use or not-use or disable
"slider": 25, #Int for slider value
"removeFieldButton": False #True or False
}
self.fieldCreator = FieldCreator(self.logger, self.gps, self.nav, self.smoothie, self.socketio)
self.field = None
self.manoeuvre = False
try:
self.notificationQueue = posix_ipc.MessageQueue(config.QUEUE_NAME_UI_NOTIFICATION)
except:
self.notificationQueue = None
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")
print("\n", end="")
if __name__ == '__main__':
ntripArgs = {}
if utility.get_ublox_address() is not None:
ntripArgs['output'] = utility.get_ublox_address()
else:
ntripArgs['output'] = config.NTRIP_OUTPUT_PORT
ntripArgs['baudrate'] = config.NTRIP_OUTPUT_BAUDRATE
with adapters.GPSUbloxAdapter(ntripArgs['output'], ntripArgs['baudrate'],
config.GPS_POSITIONS_TO_KEEP) as gps:
try:
pos = gps.get_fresh_position()
ntripArgs['lat'] = pos[0]
ntripArgs['long'] = pos[1]
print(
f"Current latitude : {pos[0]}, longitude : {pos[1]} for send to ntrip."
)
except:
pass
time.sleep(5)
ntripArgs['user'] = config.NTRIP_USER
ntripArgs['password'] = config.NTRIP_PASSWORD
ntripArgs['caster'] = config.NTRIP_CASTER
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.")
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 = []
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():
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()