def closest_point_to_obstacle(point_test, polygon):
ref_point = []
distance = INITIAL_DISTANCE_VALUE
for n in range(polygon.get_length() + 1):
ref_point.append(n % polygon.get_length())
closest_point_on_obstacle = Point(0, 0)
for n in range(polygon.get_length()):
# line_temp = polygon[n].find_line_point_to_point(polygon[n + 1])
# distance.append(point_test, line_temp)
# print(ref_point[n], ref_point[n+1])
point_obstacle = closest_point_to_line(point_test,
polygon.poly[ref_point[n]],
polygon.poly[ref_point[n + 1]])
if point_test.find_distance_between_points(point_obstacle) < distance:
distance = point_test.find_distance_between_points(point_obstacle)
closest_point_on_obstacle.x = point_obstacle.x
closest_point_on_obstacle.y = point_obstacle.y
closest_point_on_obstacle.plot_point()
return closest_point_on_obstacle
def main():
'''
# Baxter Setup
"""RSDK Joint Position Waypoints Example
Records joint positions each time the navigator 'OK/wheel'
button is pressed.
Upon pressing the navigator 'Rethink' button, the recorded joint positions
will begin playing back in a loop.
"""
arg_fmt = argparse.RawDescriptionHelpFormatter
parser = argparse.ArgumentParser(formatter_class=arg_fmt,
description=main.__doc__)
required = parser.add_argument_group('required arguments')
required.add_argument(
'-l', '--limb', required=True, choices=['left', 'right'],
help='limb to record/playback waypoints'
)
parser.add_argument(
'-s', '--speed', default=0.3, type=float,
help='joint position motion speed ratio [0.0-1.0] (default:= 0.3)'
)
parser.add_argument(
'-a', '--accuracy',
default=baxter_interface.settings.JOINT_ANGLE_TOLERANCE, type=float,
help='joint position accuracy (rad) at which waypoints must achieve'
)
args = parser.parse_args(rospy.myargv()[1:])
print("Initializing node... ")
# initialized node
rospy.init_node("rsdk_joint_position_waypoints_%s" % (args.limb,))
'''
p_1 = Point(2, 1)
p_2 = Point(10, 1)
p_3 = Point(10, 10)
p_4 = Point(2, 10)
p_5 = Point(2, 8)
p_6 = Point(6, 6)
p_7 = Point(6, 4)
p_8 = Point(1, 2)
polygon = Polygon()
polygon.add_point(p_1)
polygon.add_point(p_2)
polygon.add_point(p_3)
polygon.add_point(p_4)
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()