Exemplo n.º 1
0
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()
Exemplo n.º 2
0
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()
Exemplo n.º 3
0
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()
Exemplo n.º 4
0
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()