def remove_outliers(utm_data, max_speed=15., plot_data=False):
utm_data_temp = utm_data[:, 3:6].astype(np.float)
utm_c2 = np.vstack((utm_data_temp[0, :], utm_data_temp))
diff = utm_data_temp - utm_c2[:-1]
speed = np.hypot(diff[:, 0], diff[:, 1]) / diff[:, 2]
utm_data = utm_data[speed < max_speed]
# import pdb; pdb.set_trace()
if plot_data:
utm = utm_data_temp[speed < max_speed]
# fig = plt.figure()
# plt.hold(True)
# plt.xlabel("UTM N", fontsize=20)
# plt.ylabel("UTM E", fontsize=20)
# plt.title("Result after removing outliers", fontsize=23)
#
# plt.plot(utm_data_temp[:,0], utm_data_temp[:,1], color="blue",
# linewidth=1.5, linestyle=":", label='Original data')
# plt.plot(utm[:,0], utm[:,1], color="red", linewidth=1.5,
# label='Outlier filtered data')
# plt.legend(loc=0)
#plt.show()
plotter.path_plot(utm_data_temp[:, 0:2], utm[:, 0:2])
#fig.savefig("resources/result_removing_outliers.png")
#plt.close()
return utm_data[:, 0:5]
def main(plots=False):
gps_data = parse_data()
times = gps_data[:, 0].reshape((-1, 1))
geo_coordinates = gps_data[:, 1:3]
utm_values = utm_geo_convert.geodetic_to_utm(geo_coordinates)
utm_coordinates = utm_values[:, 3:].astype(np.float)
sp = simplify_polygons()
vertices = sp.skimage_rdp(utm_coordinates)
mask, marker = sp.simplify_lang(10, utm_coordinates,
step=100)
simplified = utm_coordinates[mask]
utm_data = np.hstack((utm_values, times))
# ex 4.3
ro = remove_outliers()
filtered_utm_data = ro.remove_outliers(utm_data,15, plot_data=False)
# ex. 4.4 - simplify path
filtered_utm_coordinates = filtered_utm_data[:,3:5].astype(np.float)
vertices = sp.skimage_rdp(filtered_utm_coordinates)
wyatt = sp.wyatt(utm_values, 10)
min_dist_path = sp.minimize_distance_and_angle(filtered_utm_coordinates,
0.2)
min_dist_path_angle = sp.minimize_distance_and_angle(
filtered_utm_coordinates, 0.2, True, 45)
if(plots):
plotter.path_plot(filtered_utm_coordinates, vertices, wyatt,
min_dist_path, min_dist_path_angle)
# ex. 4.5 - convert back to geo
filtered_geo_coordinates = utm_geo_convert.utm_to_geodetic(utm_data)
# ex 4.7 - fixed wing path cubic hermite spline
if(plots):
chs = cubic_hermite_spline();
splines = chs.get_path(vertices)
plotter.path_plot(vertices, splines)
# convert vertices to geodetic_coordinates
left_side = np.zeros((vertices.shape[0],3)).astype(str)
left_side[:, 0] = "N"
left_side[:, 1] = 32
left_side[:, 2] = "U"
result = np.hstack((left_side, vertices))
coordinates_for_planner = utm_geo_convert.utm_to_geodetic(result)
# import pdb; pdb.set_trace()
# write to file
new_left_side = np.zeros((vertices.shape[0],1)).astype(str)
new_result = np.hstack((new_left_side, coordinates_for_planner))
new_left_side = np.zeros((vertices.shape[0],1)).astype(str)
new_left_side[:, 0] = "15"
new_result = np.hstack((new_result, new_left_side))
np.savetxt("resources/new_log_wps.log", new_result, "%s", ",")
return utm_coordinates
def main(plots=False):
gps_data = parse_data()
times = gps_data[:, 0].reshape((-1, 1))
geo_coordinates = gps_data[:, 1:3]
utm_values = utm_geo_convert.geodetic_to_utm(geo_coordinates)
utm_coordinates = utm_values[:, 3:].astype(np.float)
sp = simplify_polygons()
vertices = sp.skimage_rdp(utm_coordinates)
mask, marker = sp.simplify_lang(10, utm_coordinates,
step=100)
simplified = utm_coordinates[mask]
utm_data = np.hstack((utm_values, times))
# ex 4.3
ro = remove_outliers()
filtered_utm_data = ro.remove_outliers(utm_data,15, plot_data=False)
# ex. 4.4 - simplify path
filtered_utm_coordinates = filtered_utm_data[:,3:5].astype(np.float)
vertices = sp.skimage_rdp(filtered_utm_coordinates)
wyatt = sp.wyatt(utm_values, 10)
min_dist_path = sp.minimize_distance_and_angle(filtered_utm_coordinates,
0.2)
min_dist_path_angle = sp.minimize_distance_and_angle(
filtered_utm_coordinates, 0.2, True, 45)
if(plots):
plotter.path_plot(filtered_utm_coordinates, vertices, wyatt,
min_dist_path, min_dist_path_angle)
# import pdb; pdb.set_trace()
# ex. 4.5 - convert back to geo
filtered_geo_coordinates = utm_geo_convert.utm_to_geodetic(utm_data)
# ex 4.7 - fixed wing path cubic hermite spline
if(plots):
chs = cubic_hermite_spline();
splines = chs.get_path(vertices)
plotter.path_plot(vertices, splines)
return utm_coordinates
def main():
logger.info("BEGINNING EXECUTION")
# SIGINT handling:
# -Create a global flag to check if the execution should keep running.
# -Whenever SIGINT is received, set the global flag to False.
global run_program
run_program = True
def sigint_handler(signal, frame):
global run_program
logger.info("Shutting down")
run_program = False
return
signal.signal(signal.SIGINT, sigint_handler)
# This exception forces to give the robot_id argument within run command.
rectangle_path = False
help_msg = ('Usage: controller.py [-r <robot_id>], [--robotid=<robot_id>], '
'[--rectangle]')
try:
opts, args = getopt.getopt(sys.argv[1:], "hr:",
["robotid=", "rectangle"])
except getopt.GetoptError:
print help_msg
sys.exit()
if not opts:
print help_msg
sys.exit()
for opt, arg in opts:
if opt == '-h':
print help_msg
sys.exit()
if opt in ("-r", "--robotid"):
robot_id = int(arg)
if opt == "--rectangle":
rectangle_path = True
# Calls the main function
my_robot = RobotController(robot_id)
# Open listening sockets
sockets = init_sockets(robot_id)
# Until the first pose is not published, the robot instance is not
# initialized. Keep trying to receive the initial position with a
# no blocking recv until the instance is initialized or the run flag
# has been set to False.
logger.info("Waiting for first position")
while run_program and not my_robot.init:
try:
position = sockets['position'].recv_json(zmq.NOBLOCK)
except zmq.ZMQError:
pass
else:
logger.debug("Received first position: {}".format(position))
my_robot.set_speed(position)
# This function sends 4 rectangle points to the robot path.
if rectangle_path:
make_a_rectangle(my_robot)
# Listen sockets
listen_sockets(sockets, my_robot)
# Once the run flag has been set to False, shutdown
my_robot.on_shutdown()
if my_robot.QCTracker.path is not None:
# Print the log output to files and plot it
script_path = os.path.dirname(os.path.realpath(__file__))
# A file identifier is generated from the current time value
file_id = time.strftime('%Y%m%d_%H%M')
with open('{}/tmp/path_{}.log'.format(script_path, file_id), 'a') as f:
np.savetxt(f, my_robot.QCTracker.route, fmt='%.2f')
# Plots the robot ideal path.
plotter.path_plot(my_robot.QCTracker.path, my_robot.QCTracker.route)
return