polygon.add_point(p_5)
polygon.add_point(p_6)
polygon.add_point(p_7)
polygon.add_point(p_8)
polygon.polygon_plot('r-')
convex_polygon = polygon.concave_convex_conversion()
convex_polygon.polygon_plot('g-')
safe_range_polygon = convex_polygon.repulsive_poly(REPULSIVE_POLY_MULTIPLIER)
safe_range_polygon.polygon_plot('b-')
point_centroid = safe_range_polygon.get_polygon_centroid()
# p = Point(-5, -5)
point_centroid.plot_point()
point_start = Point(-5, 5)
p_obs = closest_point_to_obstacle(point_start, safe_range_polygon)
point_start.plot_point()
p_obs.plot_point()
point_goal = Point(15, 0)
point_goal.plot_point()
path = potential_get_path(safe_range_polygon, point_start, point_goal, point_centroid)
# plot_path(path)
# plt.show()
move = Waypoints(args.limb, args.speed, args.accuracy)
# Register clean shutdown
rospy.on_shutdown(move.clean_shutdown())
for p in path:
x = p.x
y = p.y
