class Operator: def __init__(self, r_w_j_p): rospy.init_node(NODE_NAME) #目標地点リスト 名前, 座標, 向き jsonファイルで読み込み decoder = json.JSONDecoder(object_pairs_hook=collections.OrderedDict) room_waypoints_jsonfile_path = r_w_j_p with open(room_waypoints_jsonfile_path) as f: df = decoder.decode(f.read()) initial_point = df["initial_point"] room_waypoints = df["room_waypoints"] #waypointsから目標地点名のみ抽出 room_names = [] for w in room_waypoints: room_names.append(w) self.operator = StateMachine( ['success', 'reception', 'auto_dock', 'move_to_reception'] + room_names) reception_transitions = {} for r in room_names: reception_transitions[r] = r with self.operator: #受けつけ、受付まで移動状態を追加 StateMachine.add('move_to_reception', Waypoint(initial_point["position"], initial_point["orientation"]), transitions={'success': 'reception'}) StateMachine.add('auto_dock', AutoDock(), transitions={'success': 'reception'}) StateMachine.add('reception', Reception(room_names), transitions=reception_transitions) for r in room_names: waypoints = room_waypoints[r] next_move_state_names = [ ] # [Navigate_Room01_door_key_1, Room01_room] next_wait_state_names = [ ] # [Navigate_Room01_door_key_1_wait, Room01_room_wait] for w_n in waypoints: next_move_state_names.append('Navigate_' + r + '_' + w_n) #Navigate_ next_wait_state_names.append('Navigate_' + r + '_' + w_n + '_wait') #Navigate_ rospy.loginfo('Navigate_' + r + '_' + w_n) #Navigate_ self.operator.register_outcomes(next_move_state_names + next_wait_state_names) next_move_state_names.append('move_to_reception') StateMachine.add( r, MoveToRoom(), transitions={'success': next_move_state_names[0]}) for i, (w_n, w) in enumerate(waypoints.items()): w_n_split = w_n.split("_") if (w_n_split[0] == "room"): StateMachine.add( next_move_state_names[i], Waypoint(w["position"], w["orientation"]), transitions={'success': next_wait_state_names[i]}) StateMachine.add(next_wait_state_names[i], WaitStartFlag( next_move_state_names[i]), transitions={ 'success': next_move_state_names[i + 1] }) elif (w_n_split[0] == "door"): if (w_n_split[1] == "areascan"): areascan_state_scan_name = next_move_state_names[ i] + "_scan" areascan_state_move_name = next_move_state_names[ i] + "_move" self.operator.register_outcomes([ areascan_state_scan_name, areascan_state_move_name ]) StateMachine.add(next_move_state_names[i], Waypoint(w[0]["position"], w[0]["orientation"]), transitions={ 'success': next_wait_state_names[i] }) StateMachine.add(next_wait_state_names[i], WaitStartFlag( next_move_state_names[i]), transitions={ 'success': areascan_state_move_name }) StateMachine.add(areascan_state_move_name, Waypoint(w[1]["position"], w[1]["orientation"]), transitions={ 'success': areascan_state_scan_name }) StateMachine.add(areascan_state_scan_name, AreaScan(r), transitions={ 'success': next_move_state_names[i + 1] }) else: StateMachine.add(next_move_state_names[i], Waypoint(w[0]["position"], w[0]["orientation"]), transitions={ 'success': next_wait_state_names[i] }) StateMachine.add(next_wait_state_names[i], WaitStartFlag( next_move_state_names[i]), transitions={ 'success': next_move_state_names[i + 1] }) def run(self): self.operator.execute() return
class Operator: def __init__(self): rospy.init_node(NODE_NAME) self.operator = StateMachine( ['success', 'reception', 'auto_dock', 'move_to_reception'] + room_names) reception_transitions = {} for r in room_names: reception_transitions[r] = r with self.operator: #受けつけ、受付まで移動状態を追加 StateMachine.add('move_to_reception', Waypoint(initial_point["position"], initial_point["orientation"]), transitions={'success': 'reception'}) StateMachine.add('auto_dock', AutoDock(), transitions={'success': 'reception'}) StateMachine.add('reception', Reception(), transitions=reception_transitions) for r in room_names: waypoints = room_waypoints[r] next_move_state_names = [ ] # [Navigate_Room01_door_key_1, Room01_room] next_wait_state_names = [ ] # [Navigate_Room01_door_key_1_wait, Room01_room_wait] for w_n in waypoints: next_move_state_names.append('Navigate_' + r + '_' + w_n) #Navigate_ next_wait_state_names.append('Navigate_' + r + '_' + w_n + '_wait') #Navigate_ rospy.loginfo('Navigate_' + r + '_' + w_n) #Navigate_ self.operator.register_outcomes(next_move_state_names + next_wait_state_names) next_move_state_names.append('move_to_reception') StateMachine.add( r, MoveToRoom(), transitions={'success': next_move_state_names[0]}) for i, (w_n, w) in enumerate(waypoints.items()): w_n_split = w_n.split("_") if (w_n_split[0] == "room"): StateMachine.add( next_move_state_names[i], Waypoint(w["position"], w["orientation"]), transitions={'success': next_wait_state_names[i]}) StateMachine.add(next_wait_state_names[i], WaitStartFlag( next_move_state_names[i]), transitions={ 'success': next_move_state_names[i + 1] }) elif (w_n_split[0] == "door"): if (w_n_split[1] == "areascan"): areascan_state_scan_name = next_move_state_names[ i] + "_scan" areascan_state_move_name = next_move_state_names[ i] + "_move" self.operator.register_outcomes([ areascan_state_scan_name, areascan_state_move_name ]) StateMachine.add(next_move_state_names[i], Waypoint(w[0]["position"], w[0]["orientation"]), transitions={ 'success': next_wait_state_names[i] }) StateMachine.add(next_wait_state_names[i], WaitStartFlag( next_move_state_names[i]), transitions={ 'success': areascan_state_move_name }) StateMachine.add(areascan_state_move_name, Waypoint(w[1]["position"], w[1]["orientation"]), transitions={ 'success': areascan_state_scan_name }) StateMachine.add(areascan_state_scan_name, AreaScan(r), transitions={ 'success': next_move_state_names[i + 1] }) else: StateMachine.add(next_move_state_names[i], Waypoint(w[0]["position"], w[0]["orientation"]), transitions={ 'success': next_wait_state_names[i] }) StateMachine.add(next_wait_state_names[i], WaitStartFlag( next_move_state_names[i]), transitions={ 'success': next_move_state_names[i + 1] }) def run(self): self.operator.execute() return