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()
def main():
global corner1, corner2, numCells
global searcher, newPath
rospy.init_node('astar_alpha_main')
# set the parameters specified within the launch files
# corner 1 and corner 2 specify the area in the map
# that astar will consider
# Corner1
if rospy.has_param('corner1x'):
corner1x = rospy.get_param('corner1x')
else:
corner1x = -6.25
if rospy.has_param('corner1y'):
corner1y = rospy.get_param('corner1y')
else:
corner1y = 8.2
# Corner2
if rospy.has_param('corner2x'):
corner2x = rospy.get_param('corner2x')
else:
corner2x = 15.75
if rospy.has_param('corner2y'):
corner2y = rospy.get_param('corner2y')
else:
corner2y = 28.2
corner1 = (corner1x,corner1y)
corner2 = (corner2x,corner2y)
# numCells specifies the number of cells to divide the
# specified astar search space into
if rospy.has_param('numCells'):
numCells = rospy.get_param('numCells')
else:
numCells = 100
# topic that the node looks for the closed points on
if rospy.has_param('inflatedTopic'):
inflatedTopic = rospy.get_param('inflatedTopic')
else:
inflatedTopic = '/costmap_local_alpha/costmap_local/inflated_obstacles'
# topic that the node looks for goal messages on
if rospy.has_param('goalTopic'):
goalTopic = rospy.get_param('goalTopic')
else:
goalTopic = 'goal_point'
# initialize an instance of the Astar class
searcher = Astar(corner1,corner2,numCells)
naptime = rospy.Rate(RATE)
print "corner1: "
print corner1
print ""
print "corner2: "
print corner2
print ""
print "numCells: %i" % numCells
print ""
print "goal topics: %s" % goalTopic
print ""
print "inflatedTopic: %s" % inflatedTopic
rospy.Subscriber(goalTopic,GoalMsg,goalCallback)
rospy.Subscriber(inflatedTopic,GridCellsMsg,inflatedObstaclesCallback)
rospy.Subscriber('map_pos', PoseStampedMsg, poseCallback)
pathPointPub = rospy.Publisher('point_list', PointListMsg)
first_run = True
pointList = PointListMsg()
while not rospy.is_shutdown():
pointList.new = newPath
print "searcher.start"
print searcher.start
print ""
print "searcher.goal"
print searcher.goal
print ""
print "newPath"
print newPath
print ""
print "searcher.path"
print searcher.path
print ""
pointList.points = []
for point in searcher.path:
pathPoint = PointMsg()
pathPoint.x = point[0]
pathPoint.y = point[1]
pointList.points.append(pathPoint)
pathPointPub.publish(pointList)
if newPath:
newPath = False
naptime.sleep()
def main():
global corner1, corner2, numCells
global searcher, newPath
rospy.init_node('astar_alpha_main')
# set the parameters specified within the launch files
# corner 1 and corner 2 specify the area in the map
# that astar will consider
# Corner1
if rospy.has_param('corner1x'):
corner1x = rospy.get_param('corner1x')
else:
corner1x = -6.25
if rospy.has_param('corner1y'):
corner1y = rospy.get_param('corner1y')
else:
corner1y = 8.2
# Corner2
if rospy.has_param('corner2x'):
corner2x = rospy.get_param('corner2x')
else:
corner2x = 15.75
if rospy.has_param('corner2y'):
corner2y = rospy.get_param('corner2y')
else:
corner2y = 28.2
corner1 = (corner1x, corner1y)
corner2 = (corner2x, corner2y)
# numCells specifies the number of cells to divide the
# specified astar search space into
if rospy.has_param('numCells'):
numCells = rospy.get_param('numCells')
else:
numCells = 100
# topic that the node looks for the closed points on
if rospy.has_param('inflatedTopic'):
inflatedTopic = rospy.get_param('inflatedTopic')
else:
inflatedTopic = '/costmap_local_alpha/costmap_local/inflated_obstacles'
# topic that the node looks for goal messages on
if rospy.has_param('goalTopic'):
goalTopic = rospy.get_param('goalTopic')
else:
goalTopic = 'goal_point'
# initialize an instance of the Astar class
searcher = Astar(corner1, corner2, numCells)
naptime = rospy.Rate(RATE)
print "corner1: "
print corner1
print ""
print "corner2: "
print corner2
print ""
print "numCells: %i" % numCells
print ""
print "goal topics: %s" % goalTopic
print ""
print "inflatedTopic: %s" % inflatedTopic
rospy.Subscriber(goalTopic, GoalMsg, goalCallback)
rospy.Subscriber(inflatedTopic, GridCellsMsg, inflatedObstaclesCallback)
rospy.Subscriber('map_pos', PoseStampedMsg, poseCallback)
pathPointPub = rospy.Publisher('point_list', PointListMsg)
first_run = True
pointList = PointListMsg()
while not rospy.is_shutdown():
pointList.new = newPath
print "searcher.start"
print searcher.start
print ""
print "searcher.goal"
print searcher.goal
print ""
print "newPath"
print newPath
print ""
print "searcher.path"
print searcher.path
print ""
pointList.points = []
for point in searcher.path:
pathPoint = PointMsg()
pathPoint.x = point[0]
pathPoint.y = point[1]
pointList.points.append(pathPoint)
pathPointPub.publish(pointList)
if newPath:
newPath = False
naptime.sleep()