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 __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
class RobotBrain(object):
def __init__(self, default_name="server", add_str="", server_strategy=None):
self.name = rospy.get_param("robot_name", default_name)
self.move_pub = rospy.Publisher(add_str + "move_command", String, queue_size=100)
self.cmd_pub = rospy.Publisher(add_str + "stm_command", String, queue_size=100)
self.map_pub = rospy.Publisher("/map_server/cmd", String, queue_size=10)
self.res_sub = SubscriberHandler(add_str + "response")
self.pf_cmd_pub = rospy.Publisher(add_str + "pf_cmd", String, queue_size=10)
self.points_pub = rospy.Publisher(add_str + "point", String, queue_size=100)
self.statuses_pub = rospy.Publisher(add_str + "action_statuses", String, queue_size=100)
self.rospack = rospkg.RosPack()
self.bts = {}
self.done_bts = []
self.heap_strats = None
with open(self.rospack.get_path('eurobot_decision_maker') + "/scripts/" + CUBES_STRATEGY_FILE, "rb") as f:
self.heap_strats = pickle.load(f)
self.statuses = []
# default, about robot
self.strategy = []
if self.name == "main_robot":
self.strategy = MAIN_ROBOT_STRATEGY
elif self.name == "secondary_robot":
self.strategy = SMALL_ROBOT_STRATEGY
self.plan_index = 0
self.heaps_order = HEAPS_ORDER[SIDE]
self.new_btb = None
self.prepare_new_bt(self.strategy, 0, self.heaps_order)
self.current_btb = self.new_btb
self.current_bt = self.current_btb.bt
self.new_btb = None
self.is_active = False
self.is_finished = False
# default, about server
if server_strategy:
self.server_strategy = server_strategy
else:
if self.name == "main_robot":
self.server_strategy = [""]
# points
self.points = {'secondary_robot': 145, 'main_robot': 0}
self.sub_points = {name: rospy.Subscriber("/" + name + "/point", String, self.refresh_points(name))
for name in self.points}
self.change_bt_sub = rospy.Subscriber("change_strategy", String, self.change_strategy_callback)
self.change_plan_sub = rospy.Subscriber("/server/plan", String, self.change_plan_callback)
def refresh_points(self, whose):
def cb(msg):
self.points[whose] = int(msg.data)
return cb
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 change_strategy_callback(self, msg):
msg_splitted = msg.data.split()
heaps_order = msg_splitted[0]
strategy = json.loads(reduce(lambda a, b: a + ' ' + b, msg_splitted[1:]))
self.prepare_new_bt(strategy, self.plan_index, heaps_order)
def change_plan_callback(self, msg):
plan_str = msg.data.split()
new_index = self.plan_index
if plan_str in POSSIBLE_PLANS:
new_index = POSSIBLE_PLANS.index(plan_str)
if plan_str in INV_POSSIBLE_PLANS:
new_index = INV_POSSIBLE_PLANS.index(plan_str)
if new_index != self.plan_index and not self.is_active:
self.prepare_new_bt(self.strategy, new_index, self.heaps_order)
def start_strategy(self, changed_only=False):
if not changed_only:
for i in range(3):
self.cmd_pub.publish("start_" + self.name + " 164")
time.sleep(0.05)
self.is_active = True
self.current_bt.root_node.start()
return "finished"
def stop_strategy(self, is_finished=True):
self.is_active = False
if is_finished:
self.is_finished = True
self.move_pub.publish("stop_main_cmd stop")
self.current_bt.root_node.finish()
self.done_bts.append(self.current_bt)
return "finished"
def change_bt(self):
if self.new_btb:
if self.is_active:
self.stop_strategy(False)
self.current_btb = self.new_btb
self.current_bt = self.current_btb.bt
self.new_btb = None
self.start_strategy(True)
else:
self.current_btb = self.new_btb
self.current_bt = self.current_btb.bt
self.new_btb = None
return "finished"
def fail_until_end(self):
if self.is_finished:
return "finished"
else:
return "failed"
def monitor_big_actions(self):
prev_statuses = self.statuses
self.statuses = [node.status for node in self.current_bt.root_node.child.children_list]
statuses_str = reduce(lambda x, y: x + ' ' + y, self.statuses)
prev_statuses_str = reduce(lambda x, y: x + ' ' + y, prev_statuses)
if statuses_str != prev_statuses_str:
self.statuses_pub.publish(statuses_str)
def calculate_points(self):
bts = self.done_bts + [self.current_bt] # + brain_secondary.done_bts
is_disposal = False
is_bee = False
is_button = False
is_wastewater_tower = False
is_wastewater_reservoir = False
is_move_wastewater_tower = False
is_move_cleanwater_tower = False
is_cleanwater_tower = False
is_disposal_secondary = False
heap_points = 0
balls = 0
for bt1 in bts:
for child in bt1.root_node.child.children_list:
print(child.name)
print(child.status)
if child.status == "finished":
if child.name.find("parallel_shift_down_man") != -1:
is_disposal = True
elif child.name.find("bee") != -1:
is_bee = True
elif child.name.find("switch_main") != -1:
is_button = True
elif child.name.find("strategy") != -1:
ind = child.name.find("strategy")
p = int(child.name[ind:].split("_")[1])
heap_points = max(heap_points, p)
elif child.name.find("wastewater_tower") != -1:
is_wastewater_tower = True
elif child.name.find("wastewater_reservoir") != -1:
is_wastewater_reservoir = True
elif child.name.find("disposal_secondary") != -1:
is_disposal_secondary = True
if child.name.find("wastewater_tower") != -1:
for child1 in child.children_list:
if child1.name.find("move") != -1 and child1.status == "finished":
is_move_wastewater_tower = True
if child.name.find("cleanwater_tower") != -1:
if child.status == "finished":
is_cleanwater_tower = True
for child1 in child.children_list:
if child1.name.find("sort_and_shoot") != -1:
print(child1.name)
print(child1.status)
if child1.status == "finished":
balls += 1
if child1.name.find("move") != -1:
print(child1.name)
print(child1.status)
if child1.status == "finished":
is_move_cleanwater_tower = True
is_wastewater_reservoir = IS_WASTEWATER_SHOOT
points = 5 + \
is_disposal * heap_points + \
is_cleanwater_tower * 40 + \
is_wastewater_tower * is_wastewater_reservoir * 30 + \
is_bee * 50 + \
is_button * 25 + \
is_move_wastewater_tower * 10 + \
is_move_cleanwater_tower * 10 + \
is_disposal_secondary * 4
self.points[self.name] = points
self.points_pub.publish(str(points))
global_points_pub.publish(str(sum([v for k, v in self.points.items()])))
# rospy.loginfo("POINTS " + str(points))
return "finished"
def set_robot_server_strategy(self):
self.bt = BehaviorTree("server_" + self.name)
general = SequenceNode("general")
self.bt.add_node(general, self.bt.root_node.name)
self.bt.add_node(ActionNode("start_plan_recognition", camera_cmd_pub, "start", None, True), "general")
main_cycle_if = TryUntilSuccessNode("main_cycle_if")
self.bt.add_node(main_cycle_if, "general")
# The main cycle if
main_cycle = SequenceNode("main_cycle")
main_cycle_if.set_child(main_cycle)
self.bt.nodes[main_cycle.name] = main_cycle
self.bt.add_node(TimeoutNode("half_sec_wait", 0.1), "main_cycle")
self.bt.add_node(ActionFunctionNode("init_robot", self.change_bt), "main_cycle")
self.bt.add_node(ActionFunctionNode("wait_wire", wait_wire), "main_cycle")
# The main sequence after start
self.bt.add_node(ActionNode("stop_plan_recognition", camera_cmd_pub, "finish", None, True), "general")
self.bt.add_node(ActionFunctionNode("start_main", self.start_strategy), "general")
self.bt.add_node(ActionNode("stop_wait_wire", stm_node_cmd_pub, "stop_wire", None, True), "general")
active_work = ParallelNode("active_work")
self.bt.add_node(active_work, "general")
wait_and_stop = SequenceNode("wait_and_stop")
self.bt.add_node(wait_and_stop, "active_work")
change_bt_cycle_if = TryUntilSuccessNode("change_bt_cycle_if")
self.bt.add_node(change_bt_cycle_if, "active_work")
change_bt_cycle = SequenceNode("change_bt_cycle")
change_bt_cycle_if.set_child(change_bt_cycle)
self.bt.nodes[change_bt_cycle.name] = change_bt_cycle
self.bt.add_node(TimeoutNode("little_wait", 0.5), "change_bt_cycle")
self.bt.add_node(ActionFunctionNode("change_bt_if_needed", self.change_bt), "change_bt_cycle")
self.bt.add_node(ActionFunctionNode("refresh_points", self.calculate_points), "change_bt_cycle")
self.bt.add_node(ActionFunctionNode("fail_until_end", self.fail_until_end), "change_bt_cycle")
self.bt.add_node(TimeoutNode("100_sec_wait", 98.5), "wait_and_stop")
self.bt.add_node(ActionFunctionNode("stop_main", self.stop_strategy), "wait_and_stop")
def start_server(self):
self.bt.root_node.start()
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')
# btb.add_cubes_sequence(so.get_cubes_strategy(['orange','black','green'])[0])
# with open("very_important_bt_paths2.bin","rb") as f:
# heap_strats = pickle.load(f)
# btb.add_cubes_sequence([[[], [0], []],
# [[3], [2], [1]],
# [[4], [], []],
# [[2], [], []],
# [[1], [0], [3]],
# [[], [], [4]],
# [[], [], [3]],
# [[2], [1], [0]],
# [[], [4], []]])
# # [[], [], [4]],
class SecondaryRobotBrain(object):
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
def init_strategy(self):
return "finished"
def start_strategy(self):
for i in range(3):
self.cmd_pub.publish("start_secondary 164")
time.sleep(0.05)
# self.pf_cmd_pub.publish("reset")
self.is_active = True
self.current_bt.root_node.start()
return "finished"
def emergency_strategy(self):
if not self.is_active:
return "finished"
else:
if self.current_bt.root_node.status in ["failed", "error"
] and not self.is_emerge:
self.done_bts.append(self.current_bt)
self.current_bt = self.emerge_bt
self.current_bt.root_node.start()
self.is_emerge = True
return "active"
def stop_strategy(self):
self.is_active = False
self.move_pub.publish("stop_secondary_cmd stop")
self.current_bt.root_node.finish()
self.done_bts.append(self.current_bt)
return "finished"
queue_size=100)
map_pub = rospy.Publisher("/map_server/cmd", String, queue_size=10)
move_type = 'standard'
if len(sys.argv) > 1 and sys.argv[1] in ['simple', 'standard']:
move_type = sys.argv[1]
response_sub = SubscriberHandler("/secondary_robot/response")
btb = BehaviorTreeBuilder("secondary_robot",
move_pub,
cmd_pub,
map_pub,
response_sub,
response_sub,
move_type=move_type)
# btb.add_strategy([("heaps",1),("funny",1),("heaps",2),("heaps",0),("disposal",0),("funny",0)])
#btb.add_strategy([("bee_secondary",0)])
btb.add_strategy([("cleanwater_tower_before_waste", 0),
("bee_secondary", 0), ("wastewater_tower", 0)])
#btb.add_strategy([("bee_secondary",0)])
# btb.add_strategy([("cleanwater_tower_before_waste",0),("switch_secondary",0),("bee_secondary",0)])
# btb.add_strategy([("cleanwater_tower_before_waste",0), ("switch_secondary",0), ("wastewater_tower", 0), ("wastewater_reservoir", 0), ("bee_secondary",0)])
# btb.add_strategy([("cleanwater_tower_before_waste",0), ("switch_secondary",0), ("wastewater_tower",0), ("wastewater_reservoir",0), ("bee_secondary", 0)])
# btb.add_strategy([("bee_secondary", 0)])
#so = StrategyOperator(file='first_bank.txt')
# btb.add_cubes_sequence(so.get_cubes_strategy(['orange','black','green'])[0])
# btb.add_cubes_sequence([[[], [0], []],
# [[3], [2], [1]],
# [[4], [], []],
# [[2], [], []],
# [[1], [0], [3]],