def next_move_plan(req):
'''
The function deals with the request of next_move_planner_server()
:param req: string status
:return: poseseq.poses = PoseArray[], contains the sequence of planned poses
'''
rospy.Subscriber('ltl_task',
String,
task_callback,
queue_size=1,
buff_size=2**24)
# Subscribe task_publisher to get LTL task
rospy.Subscriber('amcl_pose',
PoseWithCovarianceStamped,
pose_callback,
queue_size=1,
buff_size=2**24)
# Subscribe amcl to get current estimated pose as initial pose. Planning will be given based on initial pose
# Subscribing needs time and next sentences will run first. Hence, here needs a delay.
rospy.sleep(1.0)
robot_motion.set_initial(current_pose[1])
robot_model = MotActModel(robot_motion, robot_action)
robot_planner = ltl_planner(robot_model, hard_task, soft_task)
start = time.time()
robot_planner.optimal(10, 'static')
# Synthesize motion FTS, action model and task to plan a pose sequence
if req.status == 'PoseQueue':
poseseq = PoseArray()
poseseq.header.frame_id = "map"
poseseq.header.stamp = rospy.Time.now()
for index in range(len(robot_planner.run.pre_plan) - 2):
qua_angle = quaternion_from_euler(
0, 0, robot_planner.run.pre_plan[index][2], 'rxyz')
poseseq.poses.append(
Pose(
Point(robot_planner.run.pre_plan[index][0],
robot_planner.run.pre_plan[index][1], 0.000),
Quaternion(qua_angle[0], qua_angle[1], qua_angle[2],
qua_angle[3])))
# Here can not directly use qua_angle value cuz it is not a quaternion type but only a tuple type
# Data structure is given in P_MAG_TS, please read the instruction in the package
return NextMoveResponse(poseseq.poses)
else:
print 'No acceptable move'
return None
def next_move_plan(req):
'''
The function deals with the request of next_move_planner_server()
:param req: string status
:return: poseseq.poses: PoseArray[], contains the sequence of planned poses
'''
global robot_planner
poseseq = PoseStamped()
poseseq.header.frame_id = "map"
poseseq.header.stamp = rospy.Time.now()
if req.status == 'PlanSynthesis':
# Synthesize motion FTS, action model and task to plan a pose sequence
robot_motion.set_initial(current_pose[1])
robot_model = MotActModel(robot_motion, robot_action)
robot_planner = ltl_planner(robot_model, hard_task, soft_task)
start = time.time()
robot_planner.optimal(10, 'static')
next_move = robot_planner.run.pre_plan[0]
qua_angle = quaternion_from_euler(0, 0, next_move[2], 'rxyz')
poseseq.pose = Pose(
Point(next_move[0], next_move[1], 0.000),
Quaternion(qua_angle[0], qua_angle[1], qua_angle[2], qua_angle[3]))
return NextMoveResponse(poseseq.pose)
elif req.status == 'FirstMove':
next_move = robot_planner.next_move
qua_angle = quaternion_from_euler(0, 0, next_move[2], 'rxyz')
poseseq.pose = Pose(
Point(next_move[0], next_move[1], 0.000),
Quaternion(qua_angle[0], qua_angle[1], qua_angle[2], qua_angle[3]))
return NextMoveResponse(poseseq.pose)
elif req.status == 'NextMove':
next_move = robot_planner.find_next_move()
qua_angle = quaternion_from_euler(0, 0, next_move[2], 'rxyz')
poseseq.pose = Pose(
Point(next_move[0], next_move[1], 0.000),
Quaternion(qua_angle[0], qua_angle[1], qua_angle[2], qua_angle[3]))
# Here can not directly use qua_angle value cuz it is not a quaternion type but only a tuple type
# Data structure is given in P_MAG_TS, please read the instruction in the package
return NextMoveResponse(poseseq.pose)
else:
print 'Please synthesis plan first and then navigate.'
return None
def _plan_syn(self, map_name, ap, regions, edges, action, hard_task):
self.PlanViewer.clear()
#self.s.login('12.0.5.2', 'ubuntu', 'ubuntu', original_prompt='$$S', port=22, auto_prompt_reset=True)
if not (map_name and hard_task and self.InitPosInputR1.text()):
if not map_name:
self.PlanViewer.append('Please select a map first')
elif not hard_task:
self.PlanViewer.append('Please input a task first')
else:
self.PlanViewer.append('Please input a initial position')
else:
start_pos = regions.keys()[regions.values().index(
set([str(self.InitPosInputR1.text())]))]
# here the region must start with lower-case r instead of capital R
robot_motion = MotionFts(regions, ap, map_name)
robot_motion.set_initial(start_pos)
robot_motion.add_un_edges(edges, unit_cost=0.1)
robot_action = ActionModel(action)
robot_model = MotActModel(robot_motion, robot_action)
sys.stdout = EmittingStream(textWritten=self.text_written)
robot_planner = ltl_planner(robot_model, hard_task, None)
robot_planner.optimal(10, 'static')
sys.stdout = sys.__stdout__
self.PlanViewer.append('The start point is: ' + str(start_pos))
self.PlanViewer.append('It will move to the next point: ' +
str(robot_planner.next_move))
stream = file(
os.path.join(self.rp.get_path('rqt_ltl'), 'map', 'task.yaml'),
'w')
data = dict(total_task=hard_task + ',' + '',
start_point=start_pos,
map_name=map_name)
yaml.dump(data,
stream,
default_flow_style=False,
allow_unicode=False)
def construct_product(M, N):
# create motion model
robot_motion = create_rectworld(M, N, True)
draw_world(robot_motion, M, N)
robot_motion.set_initial((0, 0))
# empty action model
robot_action = ActionModel(dict())
# complete robot model
robot_model = MotActModel(robot_motion, robot_action)
# task formula
hard_task = '[]! x%dy%d' % (floor(0.5 * M), floor(0.5 * N))
soft_task = '([]<> x%dy%d) && ([]<> x%dy%d) && ([]<> x%dy%d)' % (
M - 1, 0, M - 1, N - 1, 0, N - 1)
print '------------------------------'
print 'hard_task: %s ||| soft_task: %s' % (str(hard_task), str(soft_task))
print '------------------------------'
# set planner
alpha = 10
robot_planner = ltl_planner(robot_model, hard_task, soft_task, alpha)
return robot_planner
def planner(letter, ts, act, task):
global header
global POSE
global c
global confirm
global object_name
global region
rospy.init_node('planner_%s' % letter)
print 'Agent %s: planner started!' % (letter)
###### publish to
activity_pub = rospy.Publisher('next_move_%s' % letter,
activity,
queue_size=10)
###### subscribe to
rospy.Subscriber('activity_done_%s' % letter, confirmation,
confirm_callback)
rospy.Subscriber('knowledge_%s' % letter, knowledge, knowledge_callback)
####### agent information
c = 0
k = 0
flag = 0
full_model = MotActModel(ts, act)
#### construct ltl_planner object
planner = ltl_planner(full_model, None, task)
####### initial plan synthesis
planner.optimal(10)
#######
while not rospy.is_shutdown():
next_activity = activity()
############### check for knowledge udpate
if object_name:
# konwledge detected
planner.update(object_name, region)
print 'Agent %s: object incorporated in map!' % (letter, )
planner.replan_simple()
object_name = None
############### send next move
next_move = planner.next_move
next_state = planner.next_state
############### implement next activity
if isinstance(next_move, str):
# next activity is action
next_activity.header = header
next_activity.type = next_move
next_activity.x = -0.76
next_activity.y = 0.30
print 'Agent %s: next action %s!' % (letter, next_activity.type)
while not ((confirm[0] == next_move) and
(confirm[1] > 0) and confirm[2] == header):
activity_pub.publish(next_activity)
rospy.sleep(0.06)
rospy.sleep(1)
confirm[1] = 0
header = header + 1
print 'Agent %s: action %s done!' % (letter, next_activity.type)
else:
print 'Agent %s: next waypoint (%.2f,%.2f,%.2f)!' % (
letter, next_move[0], next_move[1], next_move[2])
while not ((confirm[0] == 'goto') and
(confirm[1] > 0) and confirm[2] == header):
next_activity.type = 'goto'
next_activity.header = header
next_activity.x = next_move[0]
next_activity.y = next_move[1]
next_activity.psi = next_move[2]
activity_pub.publish(next_activity)
rospy.sleep(0.06)
rospy.sleep(1)
confirm[1] = 0
header = header + 1
print 'Agent %s: waypoint (%.2f,%.2f,%.2f) reached!' % (
letter, next_move[0], next_move[1], next_move[2])
planner.pose = [next_move[0], next_move[1]]
planner.find_next_move()
robot_motion = MotionFts(regions, ap, 'office' ) robot_motion.set_initial((15, -45, 5)) robot_motion.add_un_edges_by_ap(edges, unit_cost = 0.1) ############################## # action FTS ############# no action model action = dict() ############# with action robot_action = ActionModel(action) ############################## # complete robot model robot_model = MotActModel(robot_motion, robot_action) ############################## # specify tasks ########## only hard # robot 1 # hard_task = '<>(p1 && <> (p4 && <> p8)) && ([]<> p6) && ([]<> p9)' # robot 2 # hard_task = '([]<> (p1 && <> p2)) && ([]<> p4)' # robot 3 # hard_task = '([]<> (p3 && <> (p6 || p8)))' # robot 4 hard_task = '([]<> p7) && ([]<> p8) && ([]<> p5)'
'lb': (COST, 'b', set(['lb'])),
'ub': (COST, '1', set(['ub'])),
}
dep['lb'] = set([
'hb',
])
dep['ub'] = set([
'hb',
])
A1_action = ActionModel(A1_action_dict)
# A1 task
A1_hard_task = '<>(la && <>(ua && r2)) && <>(lb && <>(ub && r3)) && ([]<>r0)'
#A1_hard_task = '<> t1'
A1_soft_task = None
# A1 model, planner
A1_model = MotActModel(A1_motion, A1_action)
A1_planner = ltl_planner(A1_model, A1_hard_task, A1_soft_task)
#=================================
# A2 fts
A2_init_pose = [2.0, 2.0, 0.0]
A2_node_dict = {
(2.0, 2.0, 0.1): set(['r0']),
(0.5, 0.5, 0.2): set(['r1']),
(3.5, 0.5, 0.2): set(['r2']),
(0.5, 2.0, 0.2): set(['r3']),
(3.5, 2.0, 0.2): set(['r4']),
(0.5, 3.3, 0.2): set(['r5']),
(3.5, 3.3, 0.2): set(['r6']),
(2.0, 1.0, 0.3): set(['r7']),
(2.0, 3.0, 0.3): set(['r8']),
def compose_sys_fts(sys_models, add_data, symbols, buffer_size, com_rad=1):
#----------------------------
print 'compose_fts starts'
t0 = time.time()
#--------------------
nodes = set()
# construct nodes
N = len(sys_models)
N_f = int(floor(0.75 * N))
ind_nodes = []
for k in range(0, N):
regs = sys_models[k][0].nodes()
bufs = range(buffer_size[k] + 1)
new_s = []
for item in product(regs, bufs):
# (reg,d)
new_s.append(item)
ind_nodes.append(new_s)
#--------------------
comp_nodes = dict()
for items in product(*ind_nodes):
prop = set()
for k in range(0, N):
fts = sys_models[k][0]
reg = items[k][0]
label = fts.node[reg]['label']
prop.update(label)
comp_nodes[tuple(items)] = prop
comp_symbols = symbols
# print comp_nodes
#------------------------------
comp_motion = MotionFts(comp_nodes, comp_symbols, 'comp_FTS')
# build transitions
for f_n in comp_nodes:
# [(reg1,d1),(reg2,d2)...]
for t_n in comp_nodes:
if (f_n != t_n):
allowed = True
a_weight = 0
for i in range(0, N):
f_n_i = f_n[i][0]
t_n_i = t_n[i][0]
fts_i = sys_models[i][0]
if (t_n_i in fts_i.successors(f_n_i)):
label = fts_i.edge[f_n_i][t_n_i]['label']
f_d_i = f_n[i][1]
t_d_i = t_n[i][1]
if (label != 'goto') and (label != 'ul'):
extra_data = add_data[label]
if (t_d_i == f_d_i + extra_data):
e_weight = fts_i.edge[f_n_i][t_n_i]['weight']
if e_weight > a_weight:
a_weight = e_weight
else:
allowed = False
break
# data gather actions
elif (label == 'goto'):
one_ul = False
for j in range(N_f, N):
if t_n[j][0][1] == 'ul':
one_ul = True
break
if ((t_d_i == f_d_i)
or ((t_d_i != f_d_i) and (t_d_i == 0) and
(one_ul) and (i in range(0, N_f)))):
e_weight = fts_i.edge[f_n_i][t_n_i]['weight']
if e_weight > a_weight:
a_weight = e_weight
else:
allowed = False
break
elif (label == 'ul'):
t_reg_i = t_n_i[0]
all_empty = True
for j in range(0, N_f):
t_reg_j = t_n[j][0][0]
t_d_j = t_n[j][1]
f_d_j = f_n[j][1]
if distance(t_reg_i, t_reg_j) <= com_rad:
if t_d_j != 0:
all_empty = False
elif distance(t_reg_i, t_reg_j) > com_rad:
if t_d_j != f_d_j:
all_empty = False
if ((all_empty) and (t_d_i == 0)):
e_weight = fts_i.edge[f_n_i][t_n_i]['weight']
if e_weight > a_weight:
a_weight = e_weight
else:
allowed = False
break
else:
allowed = False
break
if allowed:
comp_motion.add_edge(f_n, t_n, weight=a_weight)
print 'Composed motion: nodes %d, edges %d' % (len(
comp_motion.nodes()), len(comp_motion.edges()))
# for e in comp_motion.edges():
#if (e[1][0][0][0] == e[1][1][0][0]) and (e[1][1][0][1] == 'ul'):
# if (e[0][1][0][1] == 'ul') and (e[0][0][1] == e[0][1][1] == 0):
# print e
init_node = []
for k in range(0, N):
for nd in sys_models[k][0].graph['initial']:
init_node.append((nd, 0))
comp_motion.graph['initial'] = set([tuple(init_node)])
#------------------------------
comp_action_dict = dict()
comp_action = ActionModel(comp_action_dict)
comp_fts = MotActModel(comp_motion, comp_action)
print '----------'
print 'comp_fts generated in %.2f' % (time.time() - t0)
print 'Composed fts: nodes %d, edges %d' % (len(
comp_fts.nodes()), len(comp_fts.edges()))
print '----------'
return comp_fts
def construct_sys_model_small(N):
#----------------------------
print 'Construct_agent_model_small starts'
t0 = time.time()
sys_models = []
#---------- load simplified roadmap
[c1_paths, c2_paths, c3_paths,
l_paths] = pickle.load(open('simp_ts.p', 'rb'))
#----------
# N c1 agents, N c2 agents, N c3 agents, N leaders
#----------
A_start_pose = [(5.666666666666667, 5.0),
(4.666666666666667, 4.333333333333333),
(6.5, 6.666666666666667), (5.666666666666667, 5.0),
(4.666666666666667, 4.333333333333333),
(6.5, 6.666666666666667)]
A_linear_speed = [0.45, 0.65, 1.35, 0.45, 0.65, 1.35] #m/s
A_angular_speed = [0.75, 0.9, 1.05, 0.75, 0.9, 1.05] #rad/s
COST = 1
#----------
add_data = {}
act_time = {}
symbols = []
l_regions = set()
#----------
#----------
#----------
# group ---**c1**---
for n in range(0, N):
r_name = 'a1%d' % n
r_init_pose = A_start_pose[n]
r_linear_speed = A_linear_speed[n]
r_angular_speed = A_angular_speed[n]
# motion fts
r_symbols = ['r%d0' % n, 'r%d1' % n, 'r%d2' % n, 'r%d3' % n]
symbols.append(r_symbols)
r_regs = {
r_init_pose: set([
r_symbols[0],
]),
(6.666666666666667, 9.5): set([
r_symbols[1],
]),
(1.0, 8.833333333333334): set([
r_symbols[2],
]),
(9.333333333333334, 9.0): set([
r_symbols[3],
]),
}
r_motion = MotionFts(r_regs, r_symbols, '%s_FTS' % r_name)
l_regions.update(set(r_regs.keys()))
for pair, route in c1_paths.iteritems():
if pair[0] in r_regs.keys():
if pair[1] in r_regs.keys():
r_motion.add_edge(pair[0],
pair[1],
weight=route[1] / r_linear_speed)
r_motion.set_initial(r_init_pose)
# action fts
add_data['g%d1' % n] = 2
act_time['g%d1' % n] = 2 * COST
symbols.append([
'g%d1' % n,
])
r_action_dict = {
'g%d1' % n: (act_time['g%d1' % n], '1', set([
'g%d1' % n,
])),
}
r_action = ActionModel(r_action_dict)
r_model = MotActModel(r_motion, r_action)
r_hard_task = '([] <> (r%d1 && g%d1)) && ([] <> (r%d2 && g%d1)) && ([] <> (r%d3 && g%d1))' % tuple(
(n, ) * 6)
r_soft_task = None
sys_models.append([r_model, r_hard_task, r_soft_task])
#----------
#----------
#----------
# group ---**c2**---
for n in range(0, N):
r_name = 'a2%d' % n
r_init_pose = A_start_pose[n]
r_linear_speed = A_linear_speed[n]
r_angular_speed = A_angular_speed[n]
# motion fts
r_symbols = ['r%d0' % n, 'r%d4' % n, 'r%d5' % n, 'r%d6' % n]
symbols.append(r_symbols)
r_regs = {
r_init_pose: set([
r_symbols[0],
]),
(9.666666666666666, 6.0): set([
r_symbols[1],
]),
(8.666666666666666, 1.1666666666666667): set([
r_symbols[2],
]),
(4.666666666666667, 0.5): set([
r_symbols[3],
]),
}
r_motion = MotionFts(r_regs, r_symbols, '%s_FTS' % r_name)
l_regions.update(set(r_regs.keys()))
for pair, route in c2_paths.iteritems():
if pair[0] in r_regs.keys():
if pair[1] in r_regs.keys():
r_motion.add_edge(pair[0],
pair[1],
weight=route[1] / r_linear_speed)
r_motion.set_initial(r_init_pose)
# action fts
add_data['g%d4' % n] = 2
act_time['g%d4' % n] = COST
symbols.append([
'g%d4' % n,
])
r_action_dict = {
'g%d4' % n: (act_time['g%d4' % n], '1', set([
'g%d4' % n,
])),
}
r_action = ActionModel(r_action_dict)
r_model = MotActModel(r_motion, r_action)
r_hard_task = '([] <> (r%d4 && g%d4)) && ([] <> (r%d6 && g%d4)) && ([] <> (r%d5 && g%d4))' % tuple(
(n, ) * 6)
r_soft_task = None
sys_models.append([r_model, r_hard_task, r_soft_task])
# #----------
# #----------
# #----------
# # group ---**c3**---
# for n in range(0, N):
# r_name = 'a3%d' %n
# r_init_pose = A_start_pose[n]
# r_linear_speed = A_linear_speed[n]
# r_angular_speed = A_angular_speed[n]
# # motion fts
# r_symbols = ['r%d0' %n, 'r%d7' %n, 'r%d8' %n, 'r%d9' %n]
# symbols.append(r_symbols)
# r_regs = {r_init_pose: set([r_symbols[0],]),
# (0.3333333333333333, 5.666666666666667): set([r_symbols[1],]),
# (1.0, 3.3333333333333335): set([r_symbols[2],]),
# (4.333333333333333, 2.3333333333333335): set([r_symbols[3],]),
# }
# r_motion = MotionFts(r_regs, r_symbols, '%s_FTS' %r_name)
# l_regions.update(set(r_regs.keys()))
# for pair, route in c3_paths.iteritems():
# if pair[0] in r_regs.keys():
# if pair[1] in r_regs.keys():
# r_motion.add_edge(pair[0], pair[1], weight = route[1]/r_linear_speed)
# r_motion.set_initial(r_init_pose)
# # action fts
# add_data['g%d6'%n] = 2
# act_time['g%d6'%n] = COST
# symbols.append(['g%d6'%n, ])
# r_action_dict = {
# 'g%d6'%n: (act_time['g%d6'%n], '1', set(['g%d6'%n,])),
# }
# r_action = ActionModel(r_action_dict)
# r_model = MotActModel(r_motion, r_action)
# r_hard_task = '([] <> (r%d7 && g%d6)) && ([] <> (r%d8 && g%d6)) && ([] <> (r%d9 && g%d6))' %tuple((n,)*6)
# r_soft_task = None
# sys_models.append([r_model, r_hard_task, r_soft_task])
#----------
#----------
#----------
# group ---**leader**---
for n in range(0, N):
r_name = 'l%d' % n
r_init_pose = A_start_pose[n]
r_linear_speed = A_linear_speed[n]
r_angular_speed = A_angular_speed[n]
# motion fts
r_regs = dict()
for nd in l_regions:
r_regs[nd] = set()
r_motion = MotionFts(r_regs, [], '%s_FTS' % r_name)
for pair, route in l_paths.iteritems():
if pair[0] in r_regs.keys():
if pair[1] in r_regs.keys():
r_motion.add_edge(pair[0],
pair[1],
weight=route[1] / r_linear_speed)
r_motion.set_initial(r_init_pose)
# action fts
add_data['ul'] = 0
act_time['ul'] = COST
symbols.append([
'ul',
])
r_action_dict = {
'ul': (act_time['ul'], '1', set([
'ul',
]))
}
r_action = ActionModel(r_action_dict)
r_model = MotActModel(r_motion, r_action)
r_hard_task = 'true'
r_soft_task = None
sys_models.append([r_model, r_hard_task, r_soft_task])
#--------------------
print 'Agent model construction done in %.2f' % (time.time() - t0)
print '%d source robots and %d relay robots' % (N, 3 * N)
return sys_models, add_data, symbols
def construct_sys_model(N):
#----------------------------
print 'Construct_agent_model starts'
t0 = time.time()
sys_models = []
#----------
[ws, tris, wps] = pickle.load(open('ws_model.p', 'rb'))
# --- construct roadmap as fts ---
roadmap = networkx.Graph()
# --- use only center point, to reduce the number of nodes
wpc = dict()
print 'wps number', len(wps)
for k, wp in enumerate(wps):
roadmap.add_node(wp[0])
wpc[wp[0]] = k
for f_n in roadmap.nodes():
for t_n in roadmap.nodes():
if f_n != t_n:
f_k = wpc[f_n]
t_k = wpc[t_n]
f_s = set(wps[f_k])
t_s = set(wps[t_k])
if f_s.intersection(t_s):
roadmap.add_edge(f_n, t_n, weight=distance(f_n, t_n))
# if complete wps are used
# for wp in wps:
# roadmap.add_edge(wp[0], wp[1], weight = distance(wp[0], wp[1]))
# roadmap.add_edge(wp[0], wp[2], weight = distance(wp[0], wp[2]))
# roadmap.add_edge(wp[0], wp[3], weight = distance(wp[0], wp[3]))
# roadmap.add_edge(wp[1], wp[2], weight = distance(wp[1], wp[2]))
# roadmap.add_edge(wp[1], wp[3], weight = distance(wp[1], wp[3]))
# roadmap.add_edge(wp[2], wp[3], weight = distance(wp[2], wp[3]))
#----------
# N c1 agents, N c2 agents, N c3 agents, N leaders
#----------
A_start_pose = [(5.666666666666667, 5.0),
(4.666666666666667, 4.333333333333333),
(6.5, 6.666666666666667), (5.666666666666667, 5.0),
(4.666666666666667, 4.333333333333333),
(6.5, 6.666666666666667)]
A_linear_speed = [0.45, 0.65, 1.35, 0.45, 0.65, 1.35] #m/s
A_angular_speed = [0.75, 0.9, 1.05, 0.75, 0.9, 1.05] #rad/s
COST = 1
#----------
add_data = {}
act_time = {}
symbols = []
#----------
#----------
#----------
# group ---**c1**---
for n in range(0, N):
r_name = 'a1%d' % n
r_init_pose = A_start_pose[n]
r_linear_speed = A_linear_speed[n]
r_angular_speed = A_angular_speed[n]
# motion fts
r_symbols = ['r%d0' % n, 'r%d1' % n, 'r%d2' % n, 'r%d3' % n]
symbols.append(r_symbols)
r_regs = {
r_init_pose: set([
r_symbols[0],
]),
(6.666666666666667, 9.5): set([
r_symbols[1],
]),
(1.0, 8.833333333333334): set([
r_symbols[2],
]),
(9.333333333333334, 9.0): set([
r_symbols[3],
]),
}
r_regions = {}
for nd in roadmap.nodes():
if nd in r_regs:
r_regions[nd] = r_regs[nd]
else:
r_regions[nd] = set()
r_motion = MotionFts(r_regions, r_symbols, '%s_FTS' % r_name)
r_motion.add_un_edges(roadmap.edges(), unit_cost=1 / r_linear_speed)
r_motion.set_initial(r_init_pose)
# action fts
add_data['g%d1' % n] = 1
add_data['g%d2' % n] = 2
add_data['g%d3' % n] = 2
act_time['g%d1' % n] = COST
act_time['g%d2' % n] = 2 * COST
act_time['g%d3' % n] = COST
symbols.append(['g%d1' % n, 'g%d2' % n, 'g%d3' % n])
r_action_dict = {
'g%d1' % n: (act_time['g%d1' % n], '1', set([
'g%d1' % n,
])),
'g%d2' % n: (act_time['g%d2' % n], '1', set([
'g%d2' % n,
])),
'g%d3' % n: (act_time['g%d3' % n], '1', set([
'g%d3' % n,
])),
}
r_action = ActionModel(r_action_dict)
r_model = MotActModel(r_motion, r_action)
r_hard_task = '([] <> (r%d1 && g%d1)) && ([] <> (r%d2 && g%d2)) && ([] <> (r%d3 && g%d3))' % tuple(
(n, ) * 6)
r_soft_task = None
sys_models.append([r_model, r_hard_task, r_soft_task])
#----------
#----------
#----------
# group ---**c2**---
for n in range(0, N):
r_name = 'a2%d' % n
r_init_pose = A_start_pose[n]
r_linear_speed = A_linear_speed[n]
r_angular_speed = A_angular_speed[n]
# motion fts
r_symbols = ['r%d0' % n, 'r%d4' % n, 'r%d5' % n, 'r%d6' % n]
symbols.append(r_symbols)
r_regs = {
r_init_pose: set([
r_symbols[0],
]),
(9.666666666666666, 6.0): set([
r_symbols[1],
]),
(8.666666666666666, 1.1666666666666667): set([
r_symbols[2],
]),
(4.666666666666667, 0.5): set([
r_symbols[3],
]),
}
r_regions = {}
for nd in roadmap.nodes():
if nd in r_regs:
r_regions[nd] = r_regs[nd]
else:
r_regions[nd] = set()
r_motion = MotionFts(r_regions, r_symbols, '%s_FTS' % r_name)
r_motion.add_un_edges(roadmap.edges(), unit_cost=1 / r_linear_speed)
r_motion.set_initial(r_init_pose)
# action fts
add_data['g%d4' % n] = 2
add_data['g%d5' % n] = 1
act_time['g%d4' % n] = COST
act_time['g%d5' % n] = COST
symbols.append(['g%d4' % n, 'g%d5' % n])
r_action_dict = {
'g%d4' % n: (act_time['g%d4' % n], '1', set([
'g%d4' % n,
])),
'g%d5' % n: (act_time['g%d5' % n], '1', set([
'g%d5' % n,
])),
}
r_action = ActionModel(r_action_dict)
r_model = MotActModel(r_motion, r_action)
r_hard_task = '([] <> (r%d4 && g%d4)) && ([] <> (r%d6 && g%d4)) && ([] <> (r%d5 && g%d5))' % tuple(
(n, ) * 6)
r_soft_task = None
sys_models.append([r_model, r_hard_task, r_soft_task])
#----------
#----------
#----------
# group ---**c3**---
for n in range(0, N):
r_name = 'a3%d' % n
r_init_pose = A_start_pose[n]
r_linear_speed = A_linear_speed[n]
r_angular_speed = A_angular_speed[n]
# motion fts
r_symbols = ['r%d0' % n, 'r%d7' % n, 'r%d8' % n, 'r%d9' % n]
symbols.append(r_symbols)
r_regs = {
r_init_pose: set([
r_symbols[0],
]),
(0.3333333333333333, 5.666666666666667): set([
r_symbols[1],
]),
(1.0, 3.3333333333333335): set([
r_symbols[2],
]),
(4.333333333333333, 2.3333333333333335): set([
r_symbols[3],
]),
}
r_regions = {}
for nd in roadmap.nodes():
if nd in r_regs:
r_regions[nd] = r_regs[nd]
else:
r_regions[nd] = set()
r_motion = MotionFts(r_regions, r_symbols, '%s_FTS' % r_name)
r_motion.add_un_edges(roadmap.edges(), unit_cost=1 / r_linear_speed)
r_motion.set_initial(r_init_pose)
# action fts
add_data['g%d6' % n] = 2
add_data['g%d7' % n] = 1
act_time['g%d6' % n] = COST
act_time['g%d7' % n] = 2 * COST
symbols.append(['g%d6' % n, 'g%d7' % n])
r_action_dict = {
'g%d6' % n: (act_time['g%d6' % n], '1', set([
'g%d6' % n,
])),
'g%d7' % n: (act_time['g%d7' % n], '1', set([
'g%d7' % n,
])),
}
r_action = ActionModel(r_action_dict)
r_model = MotActModel(r_motion, r_action)
r_hard_task = '([] <> (r%d7 && g%d6)) && ([] <> (r%d8 && g%d7)) && ([] <> (r%d9 && g%d6))' % tuple(
(n, ) * 6)
r_soft_task = None
sys_models.append([r_model, r_hard_task, r_soft_task])
#----------
#----------
#----------
# group ---**leader**---
for n in range(0, N):
r_name = 'l%d' % n
r_init_pose = A_start_pose[n]
r_linear_speed = A_linear_speed[n]
r_angular_speed = A_angular_speed[n]
# motion fts
r_regions = {}
for nd in roadmap.nodes():
r_regions[nd] = set()
r_motion = MotionFts(r_regions, [], '%s_FTS' % r_name)
r_motion.add_un_edges(roadmap.edges(), unit_cost=1 / r_linear_speed)
r_motion.set_initial(r_init_pose)
# action fts
act_time['ul'] = COST
symbols.append([
'ul',
])
r_action_dict = {
'ul': (act_time['ul'], '1', set([
'ul',
]))
}
r_action = ActionModel(r_action_dict)
r_model = MotActModel(r_motion, r_action)
r_hard_task = 'true'
r_soft_task = None
sys_models.append([r_model, r_hard_task, r_soft_task])
#--------------------
print 'Agent model construction done in %.2f' % (time.time() - t0)
print '%d source robots and %d relay robots' % (N, 3 * N)
return sys_models, add_data, symbols