def main(use_ros=False):
starting_position, starting_angle = starting_procedure()
path_holder.update_path(
dubins_path(starting_position[0],
starting_position[1],
starting_angle - 90,
FINAL_POINT_X,
FINAL_POINT_Y,
FINAL_THETA,
turning_radius=HOST_TURNING_RADIUS,
step_size=5))
print("PATH ESTABLISHED")
# path_holder.update_path(dubins_path(0, 0, 180,
# -100,100,180,
# turning_radius = HOST_TURNING_RADIUS, step_size = 5))
name_prefix = "Prosto, lewo"
now = datetime.datetime.now()
path = path_holder.path
path_as_np = np.asarray(path)
# pd.DataFrame(path_as_np).to_csv("path/to/file.csv")
np.savetxt(
f'{now.hour}:{now.minute}__{starting_position[0]}-{starting_position[1]}-{starting_angle}_to_{FINAL_POINT_X}-{FINAL_POINT_Y}-{FINAL_THETA}.csv',
path_as_np,
delimiter=",")
x_list = [x[0] for x in path]
y_list = [x[1] for x in path]
# if (starting_position[0] < FINAL_POINT_X):
# path.reverse()
plt.plot(x_list, y_list)
plt.plot(x_list[0],
y_list[0],
'r',
marker=(2, 0, starting_angle + 90),
markersize=20,
linestyle='None')
plt.axis('equal')
plt.savefig(
f'path_{starting_position[0]}-{starting_position[1]}-{starting_angle}_to_{FINAL_POINT_X}-{FINAL_POINT_Y}-{FINAL_THETA}.png'
)
plt.show()
# demo_rotating_all(path, x_list, y_list)
# starting_angle = Helper.get_angle_from_3pts(path[0], path[1], path[2])
# rotated_initial_points = rotate_list_of_points(path[2:5], 180, 0)
# print("rotated_initial_points", rotated_initial_points)
# first=Helper.get_angle_from_3pts(path[0], path[1], path[2])
# rotated = Helper.get_angle_from_3pts(rotated_initial_points[0], rotated_initial_points[1], rotated_initial_points[2])
# print("f", first,"r", rotated)
curvature = []
angles = []
radius_list = []
right_left = []
angle_sum = 0
for i in range(len(path) - 3):
rotation_angle = Helper.get_angle_from_3pts(path[i], path[i + 1],
path[i + 2])
angles.append(rotation_angle)
angle_sum += rotation_angle
rotated_path = rotate_list_of_points(path[i:i + 3], angle_sum, 0)
right_left.append(
Helper.determine_right_left(path[i], path[i + 1], path[i + 2]))
# x_list_rot= [x[0] for x in rotated_path[i:i+3]]
# y_list_rot= [x[1] for x in rotated_path[i:i+3]]
# plt.plot(x_list_rot, y_list_rot, 'r')
# plt.show()
try:
circle_center, circle_radius = Helper.find_circle(
path[i][0], path[i][1], path[i + 1][0], path[i + 1][1],
path[i + 2][0], path[i + 2][1])
radius_list.append(math.floor(radius_to_pwm(circle_radius)))
except:
print("failed establishing circle")
xr = [x[0] for x in rotated_path]
yr = [x[1] for x in rotated_path]
polynomial, x_poly_space = get_xy_as_polynomial(rotated_path)
x_space, y = poly_to_coordinates(polynomial, x_poly_space)
curv = coordinates_to_curvature(polynomial, x_space)
curvature.append(np.mean(curv))
# reset = Helper.generate_stopping_frames(10)
run = Helper.generate_straight_run_frames(10)
# radius_list.insert()
plt.plot(curvature)
# plt.plot(angles)
plt.plot(right_left)
plt.show()
radius_list = [[rad, curv] for rad, curv in zip(radius_list, right_left)]
print(radius_list)
frames = Helper.speeds_to_json_frames(radius_list)
for r in run:
frames.insert(0, r)
# for res in reset:
# frames.insert(0, res)
print("curvature: ", len(curvature), "Sradius_list: ", len(radius_list))
json_vals = json.dumps(frames)
with open(
'/home/wojciech/catkin_ws/src/beginner_tutorials/scripts/dubins_output.json',
'w') as outfile:
json.dump(json_vals, outfile)
print("FINISHED!")
populate_to_ros(use_ros, frames)
