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()
############################## # 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)' soft_task = None print 'robot_task: %s' %hard_task ############################## # set planner robot_planner = ltl_planner(robot_model, hard_task, soft_task) # synthesis start = time.time() robot_planner.optimal(10,'static') print 'full construction and synthesis done within %.2fs \n' %(time.time()-start)
print '===================='
c_buffer_size = [4,4,1,1]
buffer_size = []
for k in range(len(c_buffer_size)):
for n in range(N):
buffer_size.append(c_buffer_size[k])
com_rad = 1
t0 = time.time()
composed_fts = compose_sys_fts(sys_models, add_data, symbols, buffer_size, com_rad)
print 'Composed fts done in %.2f' %(time.time()-t0)
print '===================='
t0 = time.time()
composed_task = compose_sys_ltl(sys_models)
print 'Composed task done in %.2f' %(time.time()-t0)
print '===================='
t0 = time.time()
composed_planner = ltl_planner(composed_fts, composed_task, None)
print 'Composed planner initialized in %.2f' %(time.time()-t0)
print '===================='
t0 = time.time()
composed_planner.optimal(style='ready')
print 'Optimal plan found in %.2f' %(time.time()-t0)
print '===================='
print 'Everything done in %.2f' %(time.time()-T0)
'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 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()
A1_motion.add_full_edges(unit_cost = 1.0/SPEED[0])
# A1 action
A1_action_dict= {'la': (COST,'a',set([ 'la'])),
'ua': (COST,'1',set([ 'ua'])),
'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']),
}