def main():
global corner1, corner2, numCells
global brush
global position
global pointList
rospy.init_node('brushfire_alpha_main')
corner1 = (-6.25, 8.2)
corner2 = (15.75, 28.2)
numCells = 100
brush = BrushFire(corner1, corner2, numCells, size=15)
naptime = rospy.Rate(RATE)
print "corner1: "
print corner1
print ""
print "corner2: "
print corner2
print ""
print "numCels: "
print numCells
print ""
rospy.Subscriber('goal_point', GoalMsg, goalCallback)
rospy.Subscriber('/costmap_alpha/costmap/obstacles', GridCellsMsg,
obstaclesCallback)
rospy.Subscriber('map_pos', PoseStampedMsg, poseCallback)
pathPointPub = rospy.Publisher('point_list', PointListMsg)
pointList = PointListMsg()
t = Timer(2.0, resetPath)
t.start()
while not rospy.is_shutdown():
if (position is None or brush is None or brush.goal is None):
naptime.sleep()
continue
brush.extractLocal(position.x, position.y)
brush.brushfire()
brush.computePath()
pointList.points = []
for point in brush.pathList:
pathPoint = PointMsg()
pathPoint.x = point[0]
pathPoint.y = point[1]
pointList.points.append(pathPoint)
pathPointPub.publish(pointList)
pointList.new = False
naptime.sleep()
def main():
global corner1, corner2, numCells
global brush
global position
global pointList
rospy.init_node('brushfire_alpha_main')
corner1 = (-6.25,8.2)
corner2 = (15.75,28.2)
numCells = 100
brush = BrushFire(corner1,corner2,numCells,size=15)
naptime = rospy.Rate(RATE)
print "corner1: "
print corner1
print ""
print "corner2: "
print corner2
print ""
print "numCels: "
print numCells
print ""
rospy.Subscriber('goal_point',GoalMsg,goalCallback)
rospy.Subscriber('/costmap_alpha/costmap/obstacles', GridCellsMsg,obstaclesCallback)
rospy.Subscriber('map_pos',PoseStampedMsg, poseCallback)
pathPointPub = rospy.Publisher('point_list',PointListMsg)
pointList = PointListMsg()
t = Timer(2.0, resetPath)
t.start()
while not rospy.is_shutdown():
if(position is None or brush is None or brush.goal is None):
naptime.sleep()
continue
brush.extractLocal(position.x,position.y)
brush.brushfire()
brush.computePath()
pointList.points = []
for point in brush.pathList:
pathPoint = PointMsg()
pathPoint.x = point[0]
pathPoint.y = point[1]
pointList.points.append(pathPoint)
pathPointPub.publish(pointList)
pointList.new = False
naptime.sleep()
#!/usr/bin/env python
if __name__ == "__main__":
from brushfire import BrushFire
brush = BrushFire((0,0),(10,10),20,5)
obstacles = [(0,0),(1,1),(2,2),(3,3),(4,4)]
brush.updateGlobalGrid(obstacles)
obstacles = [(.5,.5),(1.5,1.5),(2.5,2.5),(3.5,3.5),(4.5,4.5)]
brush.updateGlobalGrid(obstacles)
brush.extractLocal(5,5)
brush.brushfire()
print brush
brush.updateGoal((9.5,9.5))
brush.computePath()
print brush.pathList
