img = Image.open(sys.argv[3])
m = SimpleMap(0.0, float(img.width), 0.0, float(img.height), img)
print("Loading collision data from %s..." % sys.argv[3])
#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)
# This program is free software: you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation, either version 3 of the License, or
# (at your option) any later version.
#
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
# GNU General Public License for more details.
#
# You should have received a copy of the GNU General Public License
# along with this program. If not, see <http://www.gnu.org/licenses/>.
from holonomic_turtle import HolonomicTurtleRRT
import map as smap
m = smap.SimpleMap(-10, 10, -10, 10, [])
h = HolonomicTurtleRRT((0.0,)*5, 0.5, 0.5, 15, 15, 0.5, m)
for _ in range(100):
print h.extend_randomly()
def print_tree(t, depth):
print(" "*depth+" + "+str(t.data))
for child in t.children:
print_tree(child, depth+1)
print_tree(h.root, 0)
