def __init__(self, socketio):
utility.create_directories("logs/")
self.logger = utility.Logger("logs/" + utility.get_current_time())
sys.stderr = ErrorLogger(self.logger)
self.currentState = CheckState(socketio, self.logger)
msg = f"Current state : {self.currentState}."
self.logger.write_and_flush(msg + "\n")
print(msg)
def __init__(self, sess, env, agent, logdir):
self._sess = sess
self._env = env
self._agent = agent
self._logger = U.Logger(self._sess, self._agent, logdir)
self._eval_seeds = [
42, 125, 356, 466, 689, 1024, 1337, 2490, 3809, 6667
]
self._sess.run(tf.global_variables_initializer())
self._logger.load()
self._agent.hard_update_target_networks()
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')
import utility as ut
import datetime as dt
from pandas import plotting as pltng
from torch.utils.data import DataLoader
pwd = os.getcwd()
graphfolder = pwd + "/graphs/"
modelfolder = pwd + "/models/"
logfolder = pwd + "/logs/"
LINESPLIT = "-" * 100
sys.stdout = ut.Logger("{}{}.log".format(logfolder,
dt.datetime.ctime(dt.datetime.now())))
sys.stderr = sys.stdout
seed = 1
torch.manual_seed(seed)
MAX_EPOCHS = 10000
BATCH_SIZE = 4
epochs = 800
learning_rate = 0.001
eps = 1e-6
PRINT_FREQ = 5
SAVE_FREQ = 100
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():
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 setup(self):
# CONFIG PARAMETERS
try:
with open('config.ini') as self._cfg:
self._config.read_file(self._cfg)
self._hname = self._config.get("TestHost", "hostname")
self._dd_dir = self._config.get("Dir", "dd_dir")
self._src = self._config.get("Sources", "sources")
self._a_filters = (self._config.get("Filters",
"filters")).split(', ')
self._n_batch = self._config.get("Batch", "n_batch")
if self._n_batch:
self._n_batch = int(self._n_batch)
print("\n[NOTICE] Check your settings in 'config.ini' file. ")
except (IOError, OSError):
print("[ERROR] Unable to find config file (config.ini)")
sys.exit()
# NETWORK TEST
try:
print("\n>>Test network connection...")
self._hostname = hostname.Test_hostname()
self._hostname.set_hostname(self._hname)
self._hostname.get_hostname()
self._hostname.test_conn()
except (IOError, OSError):
print("[ERROR] Unable to get test_hostname.")
# Logging...
self._setup_log = utility.Logger()
self._setup_log.set_file('log.txt')
self._setup_log.get_file()
# BEGIN SETUP
#########################
try:
# DIRECTORY
self._dd_directory = directory.Download_dir()
self._dd_directory.set_directory(self._dd_dir)
self._dd_directory = self._dd_directory.get_directory()
# BATCH FOR TASK POOL
self._batch = batch.Batch()
self._batch.set_batch(self._n_batch)
self._batch = self._batch.get_batch()
# FILTERS
self._filters = filters.Filters()
self._filters.set_filters(self._a_filters)
self._filters = self._filters.get_filters()
# SOURCES
# There is no need to set a default action. If the source file doesn't exist or it's corrupted,
# the script will terminate its operation instantly.
print("\n>>Source File:")
print(self._src)
self._sources = sources.Sources()
self._sources.set_urls(self._src)
self._sources = self._sources.get_urls()
print("\n>>Options")
print("Download directory: ", self._dd_dir)
# If filters is empty in config.ini, don't apply filters.
if self._a_filters[0] == '' and len(self._a_filters) == 1:
self._filtered_urls = self._sources
print("No filter rules applied.\n")
else:
print("Filter rules: ", self._a_filters, "\n")
self._filtered_urls = utility.Filtered_urls()
self._filtered_urls = self._filtered_urls.apply_filters(
self._a_filters, self._sources)
# for logging...
self._conf_log = "\n[CONFIG] [time: " + str(
(datetime.now()
).strftime('%Y/%m/%d %H:%M:%S')) + "] Setup complete. [OK]"
self._setup_log.logger(self._conf_log)
except BaseException:
print("[ERROR] Unable to complete script setup.")
self._fail_conf_log = "\n[ERROR] [time: " + str(
(datetime.now()
).strftime('%Y/%m/%d %H:%M:%S')) + "] Setup Failed!!!"
self._setup_log.logger(self._fail_conf_log)
sys.exit()
return self._filtered_urls, self._dd_directory, self._n_batch
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()
log.info(msg)
with open(os.path.join(path, filename), "w") as writer:
if with_count:
for record in records:
writer.write(record[1]+","+str(record[0])+"\n")
else:
for record in records:
writer.write(record[1]+"\n")
except:
log.info("Fail to output to file. \n{0}".format(traceback.format_exc()))
# Main Program
infile = sys.argv[1]
outdir = sys.argv[2]
log = utility.Logger("./")
# Initialization for the feature classes
hosts = host.HostActivity()
resources = resource.ResourceStatistics()
time_stat = time_statistics.TimeStatistics()
num_busy_hours = 10
time_window = time_window.TimeWindow(hours=1, n_top=num_busy_hours)
blocked = block_hosts.BlockedHosts(monitor_seconds=20, block_seconds=300, chances=3)
blocked_entries = []
server_errs = []
resources_not_found = set()
log.info("Start to read and process the entries in input file {0}:".format(infile))
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()
jList = []
rList = []
cList = []
total_steps = 0
if not os.path.exists(path):
os.makedirs(path)
# init log header
log_header = ["episode"]
log_header.append("steps")
log_header.extend(["reward_{}".format(i) for i in range(env.n)])
log_header.extend(["collisions_{}".format(i) for i in range(env.n)])
print("Collecting statistics {}:".format(" ".join(log_header)))
performance_logs = utility.Logger(log_header)
with tf.Session() as sess:
sess.run(init)
if load_model == True:
print('Loading Model...')
ckpt = tf.train.get_checkpoint_state(path)
saver.restore(sess, ckpt.model_checkpoint_path)
for episode in range(num_episodes):
if episode % 10 == 0:
print("Running episode ", episode)
episodeBuffer = [experience_buffer() for i in range(env.n)]
states = env.reset()