def leaderElection_ID(list, includeSelf):
leader_id = 0
leader = 0
if list and len(list) > 0:
for nbr in list:
nbr_id = neighbors.get_nbr_id(nbr)
if (nbr_id > leader_id):
leader = nbr
leader_id = temp_id
if includeSelf:
if rone.get_id() > leader_id:
return (0, rone.get_id())
else:
return (leader, leader_id)
def leaderElection_ID(list, includeSelf):
leader_id = 0
leader = 0
if list and len(list) > 0:
for nbr in list:
nbr_id = neighbors.get_nbr_id(nbr)
if (nbr_id > leader_id):
leader = nbr
leader_id = temp_id
if includeSelf:
if rone.get_id() > leader_id:
return (0, rone.get_id())
else:
return (leader, leader_id)
def FTL_sorted():
tv = 0
rv = 0
nbr_list = neighborsX.get_neighbors()
if len(nbr_list) > 0:
# You have neighbors. Follow max-min robot-id. If you're the lowest, become leader.
follow_id = -1
follow_nbr = None
for nbr in nbr_list:
nbr_id = neighborsX.get_nbr_id(nbr)
if nbr_id < rone.get_id() and nbr_id > follow_id:
follow_id = nbr_id
follow_nbr = nbr
if follow_id == -1: # leader
leds.set_pattern('rg', 'blink_fast', LED_BRIGHTNESS)
tv = FTL_TV
rv = 0
#bump_avoid()
else: # follower
leds.set_pattern('b', 'blink_fast', LED_BRIGHTNESS)
(tv, rv) = follow_motion_controller(follow_nbr)
else:
# no neighbors. do nothing
leds.set_pattern('r', 'circle', LED_BRIGHTNESS)
velocity.set_tvrv(tv, rv)
def spring():
at_tree_odo = None
tree_pose = None
followers = 0
have_seen_leader = False
beh.init(0.22, 40, 0.5, 0.1)
motion.init_rv(1000, MOTION_RV, MOTION_CAPTURE_DISTANCE,
MOTION_RELEASE_DISTANCE, MOTION_CAPTURE_ANGLE,
MOTION_RELEASE_ANGLE)
state = STATE_IDLE
while True:
# run the system updates
new_nbrs = beh.update()
nbr_list = neighbors.get_neighbors()
if new_nbrs:
print state
beh_out = beh.BEH_INACTIVE
# set colors, because why not do it at the top
color_counts = [0, 0, 0]
for i in range(3):
color_counts[i] = min([5, (state - 5 * i)])
if state == STATE_IDLE:
leds.set_pattern('rb', 'group', LED_BRIGHTNESS)
elif state == STATE_SUCCESS:
leds.set_pattern('g', 'circle', LED_BRIGHTNESS)
else:
leds.set_pattern(color_counts, 'count', LED_BRIGHTNESS)
if rone.button_get_value('g'):
tree_pose = None
followers = 0
have_seen_leader = False
state = STATE_IDLE
# this is the main finite-state machine
if not state in [
STATE_IDLE, STATE_LEADER, STATE_SUCCESS, STATE_FOLLOW
]:
for nbr in nbr_list:
nbr_state = hba.get_msg_from_nbr(nbr, new_nbrs)[MSG_IDX_STATE]
if nbr_state in [STATE_LEADER, STATE_SUCCESS]:
start_time = sys.time()
state = STATE_FOLLOW
if state == STATE_IDLE:
if rone.button_get_value('r'):
pose.set_pose(0, 0, 0)
state = STATE_WANDER
elif rone.button_get_value('b'):
state = STATE_QUEEN
elif state == STATE_QUEEN:
pass
elif state == STATE_WANDER:
## leds.set_pattern('r', 'circle', LED_BRIGHTNESS)
nav = hba.find_nav_tower_nbr(NAV_ID)
beh_out = beh.avoid_nbr(nav, MOTION_TV)
queen = None
for nbr in nbr_list:
nbr_state = hba.get_msg_from_nbr(nbr, new_nbrs)[MSG_IDX_STATE]
if nbr_state == STATE_QUEEN:
queen = nbr
if bump_front():
if queen != None:
state = STATE_SUCCESS
else:
tree_pose = pose.get_pose()
motion.set_goal((0.0, 0.0), MOTION_TV)
at_tree_odo = pose.get_odometer()
state = STATE_RETURN
elif nav == None:
motion.set_goal((0.0, 0.0), MOTION_TV)
state = STATE_RETURN
elif state == STATE_RETURN:
## nav_tower = hba.find_nav_tower_nbr(NAV_ID)
queen = None
recruiter = None
for nbr in nbr_list:
nbr_state = hba.get_msg_from_nbr(nbr, new_nbrs)[MSG_IDX_STATE]
if nbr_state == STATE_QUEEN:
queen = nbr
elif nbr_state == STATE_RECRUIT:
recruiter = nbr
if queen != None:
if (recruiter == None) and (tree_pose != None) and \
close_to_nbr(queen):
start_time = sys.time()
dist_traveled = pose.get_odometer() - at_tree_odo
at_queen_odo = pose.get_odometer()
state = STATE_RECRUIT
elif not closer_to_nbr(queen):
beh_out = beh.follow_nbr(queen, MOTION_TV)
else:
start_time = sys.time()
state = STATE_FOLLOW
else:
(tv, rv) = motion.update()
beh_out = beh.tvrv(tv, rv)
elif state == STATE_RECRUIT:
new_followers = 0
for nbr in nbr_list:
nbr_state = hba.get_msg_from_nbr(nbr, new_nbrs)[MSG_IDX_STATE]
if nbr_state == STATE_RECRUIT:
if neighbors.get_nbr_id(nbr) > rone.get_id():
state = STATE_FOLLOW
elif nbr_state in [STATE_FOLLOW, STATE_QUEEN]:
new_followers += 1
if new_followers > followers:
print 'reset timer'
start_time = sys.time()
followers = max([followers, new_followers])
if followers == 4 or sys.time() > start_time + WAIT_TIME:
tree_pos = (tree_pose[0], tree_pose[1])
motion.set_goal(tree_pos, MOTION_TV)
state = STATE_LEADER
elif state == STATE_FOLLOW:
recruiter = None
leader = None
success = False
new_followers = 1
for nbr in nbr_list:
nbr_state = hba.get_msg_from_nbr(nbr, new_nbrs)[MSG_IDX_STATE]
if nbr_state == STATE_RECRUIT:
recruiter = nbr
elif nbr_state == STATE_LEADER:
leader = nbr
elif nbr_state == STATE_SUCCESS:
leader = nbr
success = True
elif nbr_state in [STATE_FOLLOW, STATE_WANDER]:
new_followers += 1
if success:
have_seen_leader = True
if new_followers > followers:
start_time = sys.time()
followers = max([followers, new_followers])
if recruiter == None:
if leader == None:
if have_seen_leader:
beh_out = beh.tvrv(MOTION_TV, 0)
elif followers == 5 or sys.time() > start_time + WAIT_TIME:
followers = 0
state = STATE_WANDER
else:
if success and bump() and close_to_nbr(leader):
state = STATE_SUCCESS
beh_out = beh.follow_nbr(leader, MOTION_TV)
elif state == STATE_LEADER:
## (tv, rv) = motion.update()
## beh_out = beh.tvrv(tv, rv)
## if motion.is_done():
## state = STATE_SUCCESS
beh_out = beh.tvrv(-MOTION_TV, 0)
if bump() or (pose.get_odometer() - at_queen_odo) > dist_traveled:
state = STATE_SUCCESS
elif state == STATE_SUCCESS:
pass
# end of the FSM
if state not in [
STATE_IDLE, STATE_RECRUIT, STATE_LEADER, STATE_SUCCESS,
STATE_QUEEN
]:
bump_beh_out = beh.bump_beh(MOTION_TV)
beh_out = beh.subsume([beh_out, bump_beh_out])
# set the beh velocities
beh.motion_set(beh_out)
#set the HBA message
msg = [0, 0, 0]
msg[MSG_IDX_STATE] = state
hba.set_msg(msg[0], msg[1], msg[2])
def message_demo():
beh.init(0.22, 40, 0.5, 0.1)
state = STATE_LEADER
num_r = 1
num_g = 1
num_b = 1
button_red_old = 0
button_green_old = 0
button_blue_old = 0
while True:
# run the system updates
new_nbrs = beh.update()
nbr_list = neighbors.get_neighbors()
# init the velocities
beh_out = beh.BEH_INACTIVE
# read the buttons and update the local counts
(num_r, button_red_old) = check_button('r', num_r, button_red_old)
(num_g, button_green_old) = check_button('g', num_g, button_green_old)
(num_b, button_blue_old) = check_button('b', num_b, button_blue_old)
# build this robot's neighbor message
distance_local = num_r
mode_local = num_g
quality_local = num_b
hba.set_msg(distance_local, mode_local, quality_local)
# find the neighbor with the lowest ID
nbr_leader = nbrList_getRobotWithLowID(nbr_list)
# are you the leader?
if nbr_leader == None:
# no neighbors. you are the leader
state = STATE_LEADER
else:
low_ID = neighbors.get_nbr_id(nbr_leader)
# if new_nbrs:
# print 'nbr lowID=',nbr_leader
if low_ID < rone.get_id():
state = STATE_MINION
else:
state = STATE_LEADER
# this is the main finite-state machine
if state == STATE_LEADER:
msg = (distance_local, mode_local, quality_local)
if new_nbrs:
print "leader:", msg
brightness = LED_BRIGHTNESSER
elif state == STATE_MINION:
if nbr_leader != None:
msg = hba.get_msg_from_nbr(nbr_leader, new_nbrs)
if new_nbrs:
print "minion. leader = ", str(low_ID), " msg=", msg
brightness = LED_BRIGHTNESS
# end of the FSM
# unpack the message and display it on the LEDs
(distance, mode, quality) = msg
leds.set_pattern((distance, mode, quality), 'count', brightness)
# set the velocities
beh.motion_set(beh_out)
def spring():
at_tree_odo = None
tree_pose = None
followers = 0
have_seen_leader = False
beh.init(0.22, 40, 0.5, 0.1)
motion.init_rv(1000, MOTION_RV, MOTION_CAPTURE_DISTANCE, MOTION_RELEASE_DISTANCE
, MOTION_CAPTURE_ANGLE, MOTION_RELEASE_ANGLE)
state = STATE_IDLE
while True:
# run the system updates
new_nbrs = beh.update()
nbr_list = neighbors.get_neighbors()
if new_nbrs:
print state
beh_out = beh.BEH_INACTIVE
# set colors, because why not do it at the top
color_counts = [0, 0, 0]
for i in range(3):
color_counts[i] = min([5, (state - 5 * i)])
if state == STATE_IDLE:
leds.set_pattern('rb', 'group', LED_BRIGHTNESS)
elif state == STATE_SUCCESS:
leds.set_pattern('g', 'circle', LED_BRIGHTNESS)
else:
leds.set_pattern(color_counts, 'count', LED_BRIGHTNESS)
if rone.button_get_value('g'):
tree_pose = None
followers = 0
have_seen_leader = False
state = STATE_IDLE
# this is the main finite-state machine
if not state in [STATE_IDLE, STATE_LEADER, STATE_SUCCESS, STATE_FOLLOW]:
for nbr in nbr_list:
nbr_state = hba.get_msg_from_nbr(nbr, new_nbrs)[MSG_IDX_STATE]
if nbr_state in [STATE_LEADER, STATE_SUCCESS]:
start_time = sys.time()
state = STATE_FOLLOW
if state == STATE_IDLE:
if rone.button_get_value('r'):
pose.set_pose(0, 0, 0)
state = STATE_WANDER
elif rone.button_get_value('b'):
state = STATE_QUEEN
elif state == STATE_QUEEN:
pass
elif state == STATE_WANDER:
## leds.set_pattern('r', 'circle', LED_BRIGHTNESS)
nav = hba.find_nav_tower_nbr(NAV_ID)
beh_out = beh.avoid_nbr(nav, MOTION_TV)
queen = None
for nbr in nbr_list:
nbr_state = hba.get_msg_from_nbr(nbr, new_nbrs)[MSG_IDX_STATE]
if nbr_state == STATE_QUEEN:
queen = nbr
if bump_front():
if queen != None:
state = STATE_SUCCESS
else:
tree_pose = pose.get_pose()
motion.set_goal((0.0, 0.0), MOTION_TV)
at_tree_odo = pose.get_odometer()
state = STATE_RETURN
elif nav == None:
motion.set_goal((0.0, 0.0), MOTION_TV)
state = STATE_RETURN
elif state == STATE_RETURN:
## nav_tower = hba.find_nav_tower_nbr(NAV_ID)
queen = None
recruiter = None
for nbr in nbr_list:
nbr_state = hba.get_msg_from_nbr(nbr, new_nbrs)[MSG_IDX_STATE]
if nbr_state == STATE_QUEEN:
queen = nbr
elif nbr_state == STATE_RECRUIT:
recruiter = nbr
if queen != None:
if (recruiter == None) and (tree_pose != None) and \
close_to_nbr(queen):
start_time = sys.time()
dist_traveled = pose.get_odometer() - at_tree_odo
at_queen_odo = pose.get_odometer()
state = STATE_RECRUIT
elif not closer_to_nbr(queen):
beh_out = beh.follow_nbr(queen, MOTION_TV)
else:
start_time = sys.time()
state = STATE_FOLLOW
else:
(tv, rv) = motion.update()
beh_out = beh.tvrv(tv, rv)
elif state == STATE_RECRUIT:
new_followers = 0
for nbr in nbr_list:
nbr_state = hba.get_msg_from_nbr(nbr,new_nbrs)[MSG_IDX_STATE]
if nbr_state == STATE_RECRUIT:
if neighbors.get_nbr_id(nbr) > rone.get_id():
state = STATE_FOLLOW
elif nbr_state in [STATE_FOLLOW, STATE_QUEEN]:
new_followers += 1
if new_followers > followers:
print 'reset timer'
start_time = sys.time()
followers = max([followers, new_followers])
if followers == 4 or sys.time() > start_time + WAIT_TIME:
tree_pos = (tree_pose[0], tree_pose[1])
motion.set_goal(tree_pos, MOTION_TV)
state = STATE_LEADER
elif state == STATE_FOLLOW:
recruiter = None
leader = None
success = False
new_followers = 1
for nbr in nbr_list:
nbr_state = hba.get_msg_from_nbr(nbr,new_nbrs)[MSG_IDX_STATE]
if nbr_state == STATE_RECRUIT:
recruiter = nbr
elif nbr_state == STATE_LEADER:
leader = nbr
elif nbr_state == STATE_SUCCESS:
leader = nbr
success = True
elif nbr_state in [STATE_FOLLOW, STATE_WANDER]:
new_followers += 1
if success:
have_seen_leader = True
if new_followers > followers:
start_time = sys.time()
followers = max([followers, new_followers])
if recruiter == None:
if leader == None:
if have_seen_leader:
beh_out = beh.tvrv(MOTION_TV, 0)
elif followers == 5 or sys.time() > start_time + WAIT_TIME:
followers = 0
state = STATE_WANDER
else:
if success and bump() and close_to_nbr(leader):
state = STATE_SUCCESS
beh_out = beh.follow_nbr(leader, MOTION_TV)
elif state == STATE_LEADER:
## (tv, rv) = motion.update()
## beh_out = beh.tvrv(tv, rv)
## if motion.is_done():
## state = STATE_SUCCESS
beh_out = beh.tvrv(-MOTION_TV, 0)
if bump() or (pose.get_odometer() - at_queen_odo) > dist_traveled:
state = STATE_SUCCESS
elif state == STATE_SUCCESS:
pass
# end of the FSM
if state not in [STATE_IDLE, STATE_RECRUIT, STATE_LEADER, STATE_SUCCESS, STATE_QUEEN]:
bump_beh_out = beh.bump_beh(MOTION_TV)
beh_out = beh.subsume([beh_out, bump_beh_out])
# set the beh velocities
beh.motion_set(beh_out)
#set the HBA message
msg = [0, 0, 0]
msg[MSG_IDX_STATE] = state
hba.set_msg(msg[0], msg[1], msg[2])
def message_demo():
beh.init(0.22, 40, 0.5, 0.1)
state = STATE_LEADER
num_r = 1
num_g = 1
num_b = 1
button_red_old = 0
button_green_old = 0
button_blue_old = 0
while True:
# run the system updates
new_nbrs = beh.update()
nbr_list = neighbors.get_neighbors()
# init the velocities
beh_out = beh.BEH_INACTIVE
# read the buttons and update the local counts
(num_r, button_red_old) = check_button("r", num_r, button_red_old)
(num_g, button_green_old) = check_button("g", num_g, button_green_old)
(num_b, button_blue_old) = check_button("b", num_b, button_blue_old)
# build this robot's neighbor message
distance_local = num_r
mode_local = num_g
quality_local = num_b
hba.set_msg(distance_local, mode_local, quality_local)
# find the neighbor with the lowest ID
nbr_leader = nbrList_getRobotWithLowID(nbr_list)
# are you the leader?
if nbr_leader == None:
# no neighbors. you are the leader
state = STATE_LEADER
else:
low_ID = neighbors.get_nbr_id(nbr_leader)
# if new_nbrs:
# print 'nbr lowID=',nbr_leader
if low_ID < rone.get_id():
state = STATE_MINION
else:
state = STATE_LEADER
# this is the main finite-state machine
if state == STATE_LEADER:
msg = (distance_local, mode_local, quality_local)
if new_nbrs:
print "leader:", msg
brightness = LED_BRIGHTNESSER
elif state == STATE_MINION:
if nbr_leader != None:
msg = hba.get_msg_from_nbr(nbr_leader, new_nbrs)
if new_nbrs:
print "minion. leader = ", str(low_ID), " msg=", msg
brightness = LED_BRIGHTNESS
# end of the FSM
# unpack the message and display it on the LEDs
(distance, mode, quality) = msg
leds.set_pattern((distance, mode, quality), "count", brightness)
# set the velocities
beh.motion_set(beh_out)
def update():
current_time = sys.time()
if current_time < _nbr_state['time_ir_xmit']:
# not time yet to update the _nbr_state
return False
_nbr_state['time_ir_xmit'] += _nbr_state['nbr_period']
while _nbr_state['time_ir_xmit'] < current_time:
_nbr_state['time_ir_xmit'] += _nbr_state['nbr_period']
# transmit your announce message
if _nbr_state['xmit_enable']:
#The IR message only contains the robot ID
rone.ir_comms_send_message()
# add a '@' to the front of the radio message to mark it as a nbr message
rone.radio_send_message('@' + chr(rone.get_id()) + _nbr_state['message'])
# walk over neighbor list and timeout old neighbors
nbr_list = _nbr_state['nbr_list']
nbr_idx = 0
while nbr_idx < len(nbr_list):
if current_time > (nbr_get_update_time(nbr_list[nbr_idx]) + _nbr_state['nbr_timeout']):
nbr_list.pop(nbr_idx)
else:
nbr_idx += 1
# time out old obstacles
if current_time > (_nbr_state['obstacles_time'] + _nbr_state['nbr_timeout']):
_nbr_state['obstacles'] = None
# process new messages and update current neighbors
while True:
ir_msg = rone.ir_comms_get_message()
if ir_msg == None:
break
(nbr_ID, nbr_bearing, nbr_orientation, nbr_range) = _process_nbr_message(ir_msg)
#print 'msg recv', nbr_ID
if nbr_ID == rone.get_id():
# this is your own message. Don't make a neighbor, but process it for obstacles
(nbr_id, receivers_list, transmitters_list, nbr_range) = ir_msg
_nbr_state['obstacles'] = receivers_list
_nbr_state['obstacles_time'] = current_time
continue
# this message is from a neighbor. look for a previous message from this neighbor
new_nbr = True
nbr_idx = 0
while nbr_idx < len(nbr_list):
if get_nbr_id(nbr_list[nbr_idx]) == nbr_ID:
new_nbr = False
#print 'update nbr ', nbr_ID
break
else:
nbr_idx += 1
# Add or replace the nbr on the nbr list
# note: the order of this tuple is important. It needs to match the getters above
if new_nbr:
nbr = (nbr_ID, '', nbr_bearing, nbr_orientation, nbr_range, current_time)
nbr_list.append(nbr)
else:
nbr_msg = get_nbr_message(nbr_list[nbr_idx])
nbr = (nbr_ID, nbr_msg, nbr_bearing, nbr_orientation, nbr_range, current_time)
nbr_list[nbr_idx] = nbr
#print 'obs',_nbr_state['obstacles']
# update the radio messages every time, in case they arrive out-of-sync with the IR
while True:
#Look for neighbor radio messages on the radio queue and if the msg ID is there, make that robot's
#msg the incoming message
#TODO check for '@' char, push other messages back on gueue
radio_msg = rone.radio_get_message_nbr()
if radio_msg == None:
# There are no more radio messages, finished updates
break
else:
# radio_msg[0] = '@', radio_msg[1] = robot ID, radio_msg[2] is the message
radio_msg_id = ord(radio_msg[1])
nbr_idx = 0
while nbr_idx < len(nbr_list):
if get_nbr_id(nbr_list[nbr_idx]) == radio_msg_id:
#make the radio message the nbr's message
radio_msg = radio_msg[2:-1]
#print '>',radio_msg,'<'
(nbr_ID, old_msg, nbr_bearing, nbr_orientation, nbr_range, current_time) = nbr_list[nbr_idx]
nbr = (nbr_ID, radio_msg, nbr_bearing, nbr_orientation, nbr_range, current_time)
nbr_list[nbr_idx] = nbr
break
nbr_idx += 1
return True
def update():
current_time = sys.time()
if current_time < _nbr_state['time_ir_xmit']:
# not time yet to update the _nbr_state
return False
_nbr_state['time_ir_xmit'] += _nbr_state['nbr_period']
while _nbr_state['time_ir_xmit'] < current_time:
_nbr_state['time_ir_xmit'] += _nbr_state['nbr_period']
# transmit your announce message
if _nbr_state['xmit_enable']:
#IR_msg = chr(rone.get_id()) + _nbr_state['message']
#rone.ir_comms_send_message(IR_msg)
rone.ir_comms_send_message()
rone.radio_send_message(chr(rone.get_id()) + _nbr_state['message'])
# walk over neighbor list and timeout old neighbors
nbr_list = _nbr_state['nbr_list']
nbr_idx = 0
while nbr_idx < len(nbr_list):
if current_time > (nbr_get_update_time(nbr_list[nbr_idx]) + _nbr_state['nbr_timeout']):
nbr_list.pop(nbr_idx)
else:
nbr_idx += 1
# time out old obstacles
if current_time > (_nbr_state['obstacles_time'] + _nbr_state['nbr_timeout']):
_nbr_state['obstacles'] = None
# process new messages and update current neighbors
while True:
ir_msg = rone.ir_comms_get_message()
if ir_msg == None:
break
(nbr_ID, nbr_bearing, nbr_orientation, nbr_range_bits) = _process_nbr_message(ir_msg)
#print 'msg recv', nbr_ID
if nbr_ID == rone.get_id():
# this is your own message. Don't make a neighbor, but process it for obstacles
(nbr_id, receivers_list, transmitters_list, nbr_range_bits) = ir_msg
_nbr_state['obstacles'] = receivers_list
_nbr_state['obstacles_time'] = current_time
continue
# this message is from a neighbor. look for a previous message from this neighbor
new_nbr = True
nbr_idx = 0
while nbr_idx < len(nbr_list):
if get_nbr_id(nbr_list[nbr_idx]) == nbr_ID:
new_nbr = False
#print 'update nbr ', nbr_ID
break
else:
nbr_idx += 1
# Add or replace the nbr on the nbr list
# note: the order of this tuple is important. It needs to match the getters above
if new_nbr:
nbr = (nbr_ID, '', nbr_bearing, nbr_orientation, nbr_range_bits, current_time)
nbr_list.append(nbr)
else:
nbr_msg = get_nbr_message(nbr_list[nbr_idx])
nbr = (nbr_ID, nbr_msg, nbr_bearing, nbr_orientation, nbr_range_bits, current_time)
nbr_list[nbr_idx] = nbr
while True:
#Look for neighbor on the radio queue and if the msg ID is there, make that robot's
#msg the incoming message
radio_msg = rone.radio_get_message()
if radio_msg == None:
# There are no more radio messages, finished updates
break
else:
radio_msg_id = ord(radio_msg[0])
nbr_idx = 0
while nbr_idx < len(nbr_list):
if get_nbr_id(nbr_list[nbr_idx]) == radio_msg_id:
#make the radio message the nbr's message
radio_msg = radio_msg[1:-1]
#print '>',radio_msg,'<'
(nbr_ID, old_msg, nbr_bearing, nbr_orientation, nbr_range_bits, current_time) = nbr_list[nbr_idx]
nbr = (nbr_ID, radio_msg, nbr_bearing, nbr_orientation, nbr_range_bits, current_time)
nbr_list[nbr_idx] = nbr
break
nbr_idx += 1
#print 'obs',_nbr_state['obstacles']
return True
