#for y in range(img.height):
# for x in range(img.width):
# if img.getpixel((x, y)) == (255, 255, 255, 255):
# m.add_obstacle((x, y), (x, y))
print("Initializing RRT...")
rrt = HolonomicTurtleRRT(init, 0.5, math.pi/32, 3.0, math.pi/3, 1.0, m)
rospy.init_node('rrt_planner')
p = rospy.Publisher('/rrt/new_node', String, queue_size=10)
print("Here we go!")
i = 0
rate = rospy.Rate(500)
while not rospy.is_shutdown():
state, new_node = rrt.extend_randomly()
if new_node is not None:
p.publish("(%s, %s, %s)" % (new_node.parent.data, (new_node.edge_data[0], new_node.edge_data[1]), new_node.data))
i += 1
if i % 50 == 0:
state, new_node = rrt.extend(goal)
if new_node is not None:
p.publish("(%s, %s, %s)" % (new_node.parent.data, (new_node.edge_data[0], new_node.edge_data[1]), new_node.data))
if state is REACHED:
print "We made it!"
break
rate.sleep()
