def __init__(self):
rospy.loginfo("INIT MAIN ROBOT STRATEGY ---------------------------")
self.move_pub = rospy.Publisher("/main_robot/move_command",
String,
queue_size=100)
self.cmd_pub = rospy.Publisher("/main_robot/stm_command",
String,
queue_size=100)
self.map_pub = rospy.Publisher("/map_server/cmd",
String,
queue_size=10)
self.res_sub = SubscriberHandler("/main_robot/response")
self.pf_cmd_pub = rospy.Publisher("/main_robot/pf_cmd",
String,
queue_size=10)
self.rospack = rospkg.RosPack()
self.bts = {}
with open(
self.rospack.get_path('eurobot_decision_maker') +
"/scripts/cubes_paths_beta_3.bin", "rb") as f:
heap_strats = pickle.load(f)
for i in range(N_STR):
btb = BehaviorTreeBuilder("main_robot",
self.move_pub,
self.cmd_pub,
self.map_pub,
self.res_sub,
self.res_sub,
move_type='standard')
btb.add_strategy(MAIN_ROBOT_STRATEGY)
if SIDE == "orange":
btb.add_cubes_sequence_new(heap_strats[i]['012'])
else:
btb.add_cubes_sequence_new(heap_strats[i]['543'])
btb.create_tree_from_strategy(wire_start=False)
self.bts[i] = btb.bt
self.current_bt = self.bts[0]
self.is_active = False
btb = BehaviorTreeBuilder("main_robot",
self.move_pub,
self.cmd_pub,
self.map_pub,
self.res_sub,
self.res_sub,
move_type='standard')
btb.add_strategy(EMERGENCY_MAIN_ROBOT_STRATEGY)
btb.create_tree_from_strategy(wire_start=False)
self.emerge_bt = btb.bt
self.is_emerge = False
self.done_bts = []
def prepare_new_bt(self, new_strategy, new_plan_index, new_heaps_order):
self.strategy = new_strategy
self.plan_index = new_plan_index
self.heaps_order = new_heaps_order
self.cubes_sequence = self.heap_strats[self.plan_index][self.heaps_order]
self.new_btb = BehaviorTreeBuilder(self.name, self.move_pub, self.cmd_pub, self.map_pub,
self.res_sub, self.res_sub, move_type='standard')
self.new_btb.add_strategy(self.strategy)
self.new_btb.add_cubes_sequence_new(self.cubes_sequence)
self.new_btb.create_tree_from_strategy(wire_start=False)
def __init__(self):
rospy.loginfo(
"INIT SECONDARY ROBOT STRATEGY ---------------------------")
self.move_pub = rospy.Publisher("/secondary_robot/move_command",
String,
queue_size=100)
self.cmd_pub = rospy.Publisher("/secondary_robot/stm_command",
String,
queue_size=100)
self.map_pub = rospy.Publisher("/map_server/cmd",
String,
queue_size=10)
self.res_sub = SubscriberHandler("/secondary_robot/response")
self.btb = BehaviorTreeBuilder("secondary_robot",
self.move_pub,
self.cmd_pub,
self.map_pub,
self.res_sub,
self.res_sub,
move_type='standard')
self.pf_cmd_pub = rospy.Publisher("/secondary_robot/pf_cmd",
String,
queue_size=10)
self.btb.add_strategy(SMALL_ROBOT_STRATEGY)
self.btb.create_tree_from_strategy(wire_start=False)
self.current_bt = self.btb.bt
self.is_active = False
self.done_bts = []
btb = BehaviorTreeBuilder("main_robot",
self.move_pub,
self.cmd_pub,
self.map_pub,
self.res_sub,
self.res_sub,
move_type='standard')
btb.add_strategy(EMERGENCY_SECONDARY_ROBOT_STRATEGY)
btb.create_tree_from_strategy(wire_start=False)
self.emerge_bt = btb.bt
self.is_emerge = False
if __name__ == "__main__":
rospy.init_node("btb_node", anonymous=True)
move_pub = rospy.Publisher("/secondary_robot/move_command",
String,
queue_size=100)
cmd_pub = rospy.Publisher("/secondary_robot/stm_command",
String,
queue_size=100)
map_pub = rospy.Publisher("/map_server/cmd", String, queue_size=10)
move_type = 'standard'
res_sub = SubscriberHandler("/secondary_robot/response")
btb = BehaviorTreeBuilder("secondary_robot",
move_pub,
cmd_pub,
map_pub,
res_sub,
res_sub,
move_type=move_type)
# btb.add_strategy([("heaps",1),("funny",1),("heaps",2),("heaps",0),("disposal",0),("funny",0)])
# btb.add_strategy([("heaps", 0), ("heaps", 1), ("heaps", 2), ("disposal", 0)])
# btb.add_strategy([("disposal",0)])
# btb.add_strategy([("heaps",0)])
action_name = "release"
action_num = "left"
if len(sys.argv) > 1:
action_name = sys.argv[1]
if len(sys.argv) > 2:
action_num = sys.argv[2]
btb.add_strategy([(action_name, action_num)])
# so = StrategyOperator(file='first_bank.txt')
if __name__ == "__main__":
rospy.init_node("btb_node", anonymous=True)
move_pub = rospy.Publisher("/main_robot/move_command",
String,
queue_size=100)
cmd_pub = rospy.Publisher("/main_robot/stm_command",
String,
queue_size=100)
map_pub = rospy.Publisher("/map_server/cmd", String, queue_size=10)
move_type = 'standard'
res_sub = SubscriberHandler("/main_robot/response")
btb = BehaviorTreeBuilder("main_robot",
move_pub,
cmd_pub,
map_pub,
res_sub,
res_sub,
move_type=move_type)
# btb.add_strategy([("heaps",1),("funny",1),("heaps",2),("heaps",0),("disposal",0),("funny",0)])
# btb.add_strategy([("heaps", 0), ("heaps", 1), ("heaps", 2), ("disposal", 0)])
# btb.add_strategy([("disposal",0)])
# btb.add_strategy([("heaps",0)])
action_name = "help"
action_num = "open"
if len(sys.argv) > 1:
action_name = sys.argv[1]
if len(sys.argv) > 2:
action_num = int(sys.argv[2])
btb.add_strategy([(action_name, action_num)])
# so = StrategyOperator(file='first_bank.txt')