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
