def planner(ts, init_pose, act, robot_task, robot_name='TIAGo'):
global robot_pose
robot_pose = [None, init_pose]
#rospy.init_node('ltl_planner_%s' %robot_name)
print 'Robot %s: ltl_planner started!' %(robot_name)
###### publish to
#----------
#publish to
#----------
InitialPosePublisher = rospy.Publisher('initialpose', PoseWithCovarianceStamped, queue_size = 100)
# for i in xrange(10):
# SendInitialPose(InitialPosePublisher, init_pose, rospy.Time.now())
# rospy.sleep(0.1)
# print('Initial pose set to %s.' %str(init_pose))
GoalPublisher = rospy.Publisher('move_base_simple/goal', PoseStamped, queue_size = 100)
ang_pub = rospy.Publisher('mobile_base_controller/cmd_vel', Twist, queue_size = 100)
ang = Twist()
#----------
#subscribe to
#----------
rospy.Subscriber('amcl_pose', PoseWithCovarianceStamped, PoseCallback)
####### robot information
full_model = MotActModel(ts, act)
planner = ltl_planner(full_model, robot_task, None)
####### initial plan synthesis
planner.optimal(5)
#######
reach_xy_bound = 0.5 # m
reach_yaw_bound = 0.1*PI # rad
t0 = rospy.Time.now()
while not rospy.is_shutdown():
try:
t = rospy.Time.now()-t0
print '----------Time: %.2f----------' %t.to_sec()
current_goal = planner.next_move
if isinstance(current_goal, str):
print 'the robot next_move is an action, currently not implemented for %s' %robot_name
break
print 'robot orientation %s' %str(robot_pose[1][2])
if norm2(robot_pose[1][0:2], current_goal[0:2]) > reach_xy_bound:
SendGoal(GoalPublisher, current_goal, t)
print('Goal %s sent to %s.' %(str(current_goal),str(robot_name)))
rospy.sleep(1)
else:
while abs(abs(robot_pose[1][2])-current_goal[2]) > reach_yaw_bound:
ang.angular.z = -5*(abs(robot_pose[1][2])-current_goal[2])
print 'diff = %s' %str(abs(robot_pose[1][2])-current_goal[2])
ang_pub.publish(ang)
ang.angular.z = 0
ang_pub.publish(ang)
print('Goal %s reached by %s.' %(str(current_goal),str(robot_name)))
planner.find_next_move()
if current_goal == planner.next_move:
break
except rospy.ROSInterruptException:
pass
def human_region_cb(self, req=rqt_simulation_msgs.srv.RegionRequest()):
region = req.region.data
print self.header, 'Received new human region: ', region
# Build hard task message
self.hard_task = self.removeRegion(region)
# Replan
self.full_model = MotActModel(self.robot_motion, self.robot_action)
self.planner = ltl_planner(self.full_model, self.hard_task,
self.soft_task)
####### initial plan synthesis
self.planner.optimal(10, 'static')
rsp = rqt_simulation_msgs.srv.RegionResponse()
return rsp
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)
planner = ltl_planner(full_model, None, task)
#planner = ltl_planner(full_model, task, None)
####### 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):
#relative_x = next_move[0]-POSE[0]
#relative_y = next_move[1]-POSE[1]
#relative_pose = [relative_x, relative_y]
#oriented_relative_pose = rotate_2d_vector(relative_pose, -POSE[2])
next_activity.type = 'goto'
next_activity.header = header
#next_activity.x = oriented_relative_pose[0]
#next_activity.y = oriented_relative_pose[1]
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()
def planner(ts, init_pose, act, robot_task, robot_name='TIAGo'):
global robot_pose
robot_pose = [None, init_pose]
#global pose_goal
rospy.init_node('ltl_planner_%s' % robot_name)
print 'Robot %s: ltl_planner started!' % (robot_name)
#----------
#publish to
#----------
GoalPublisher = actionlib.SimpleActionClient(
'move_base', move_base_msgs.msg.MoveBaseAction)
GoalPublisher.wait_for_server()
#----------
#subscribe to
#----------
rospy.Subscriber('amcl_pose', PoseWithCovarianceStamped, PoseCallback)
####### robot information
full_model = MotActModel(ts, act)
planner = ltl_planner(full_model, robot_task, None)
####### initial plan synthesis
planner.optimal(10)
#######
t0 = rospy.Time.now()
while not rospy.is_shutdown():
try:
t = rospy.Time.now() - t0
new_goal = planner.next_move
rospy.loginfo("NEW GOAL: " + str(new_goal))
if new_goal == 'pick':
tp = rospy.Time.now()
data = open(
"../pick_test/" + str(rospy.get_param('test_name')) +
".txt", "a")
data.write("Time = " + str((tp - t0).to_sec()) +
": Pick server starting\n")
data.close()
rospy.loginfo("Robot " + str(robot_name) +
" next action is action " + str(new_goal))
rospy.loginfo("Waiting for pick Service...")
# Pick object with Aruco marker
rospy.wait_for_service('/pick_gui')
rospy.loginfo("Service ready. Starting /pick_gui...")
pickclient = rospy.ServiceProxy('/pick_gui', Empty)
pickclient()
data = open(
"../pick_test/" + str(rospy.get_param('test_name')) +
".txt", "a")
data.write("Time = " + str((rospy.Time.now() - t0).to_sec()) +
": Pick server finished\n")
data.write("Pick duration: " +
str((rospy.Time.now() - tp).to_sec()) + "\n")
data.close()
rospy.loginfo("Pick server finished. Duration: " +
str((rospy.Time.now() - tp).to_sec()))
try:
aruco_detect = rospy.wait_for_message(
'/detected_aruco_pose', PoseStamped, timeout=0.5)
except rospy.ROSException:
return
#planner.find_next_move() #a effacer
else:
pose_goal = new_goal
# move_base action server travels till (x,y) coordinates
rospy.loginfo("Robot " + str(robot_name) + " moving to " +
str(pose_goal))
SendGoal(pose_goal, rospy.Time.now() - t0, GoalPublisher)
# the check_yaw function sets the orientation
rospy.loginfo("Current robot position : " + str(robot_pose[1]))
data = open(
"../pick_test/" + str(rospy.get_param('test_name')) +
".txt", "a")
if GoalPublisher.get_state() == 4:
rospy.logingo(
"Planning fail. Position not reached. Retrying...\n")
data.write("Planning fail. Position not reached.\n")
else:
rospy.loginfo("Goal " + str(pose_goal) + " reached")
data.write("Current robot position : " + str(robot_pose[1]) +
"\n")
data.close()
planner.find_next_move() #indent -1
if pose_goal == planner.next_move:
data = open(
"../pick_test/" + str(rospy.get_param('test_name')) +
".txt", "a")
data.write("Mission duration : " +
str((rospy.Time.now() - t0).to_sec()) + "\n")
data.close()
rospy.loginfo("Mission duration : " +
str((rospy.Time.now() - t0).to_sec()))
return
except rospy.ROSInterruptException:
data = open(
"../pick_test/" + str(rospy.get_param('test_name')) + ".txt",
"a")
data.write("Stopped by user\n")
data.close()
pass
def __init__(self):
self.node_name = "LTL Planner"
self.active = False
self.beta = 10
self.robot_pose = PoseWithCovarianceStamped()
self.hard_task = ''
self.soft_task = ''
self.temporary_task_ = temporaryTask()
self.robot_name = rospy.get_param('robot_name')
self.agent_type = rospy.get_param('agent_type')
scenario_file = rospy.get_param('scenario_file')
self.replan_timer = rospy.Time.now()
self.header = '[' + self.robot_name + '] '
self.last_current_pose = PoseStamped()
self.clear_costmap = rospy.ServiceProxy('move_base/clear_costmaps',
Empty)
robot_model = FTSLoader(scenario_file)
[
self.robot_motion, self.init_pose, self.robot_action,
self.robot_task
] = robot_model.robot_model
#-----------
# Publishers
#-----------
self.InitialPosePublisher = rospy.Publisher('initialpose',
PoseWithCovarianceStamped,
queue_size=100)
# Synthesised prefix plan Publisher
self.PrefixPlanPublisher = rospy.Publisher('prefix_plan',
PoseArray,
queue_size=1)
# Synthesised sufix plan Publisher
self.SufixPlanPublisher = rospy.Publisher('sufix_plan',
PoseArray,
queue_size=1)
# Goal pose for aerial vehicle
if self.agent_type == 'aerial':
self.GoalPublisher = rospy.Publisher('command/pose',
PoseStamped,
queue_size=1)
#------------
# Subscribers
#------------
if self.agent_type == 'ground':
#localization_topic = 'amcl_pose'
localization_topic = 'pose_gui'
elif self.agent_type == 'aerial':
localization_topic = 'ground_truth/pose_with_covariance'
self.sub_amcl_pose = rospy.Subscriber(localization_topic,
PoseWithCovarianceStamped,
self.PoseCallback)
# trigger start from GUI
self.sub_active_flag = rospy.Subscriber('/planner_active', Bool,
self.SetActiveCallback)
# initial position from GUI
self.sub_init_pose = rospy.Subscriber('init_pose', Pose,
self.GetInitPoseCallback)
# task from GUI
self.sub_soft_task = rospy.Subscriber('soft_task', String,
self.SoftTaskCallback)
self.sub_hard_task = rospy.Subscriber('hard_task', String,
self.HardTaskCallback)
self.sub_temp_task = rospy.Subscriber('temporary_task', TemporaryTask,
self.TemporaryTaskCallback)
# environment sense
self.sub_sense = rospy.Subscriber('/environment', Sense,
self.SenseCallback)
# clear costmap manually from GUI
self.sub_clear = rospy.Subscriber('clear_costmap', Bool,
self.ClearCallback)
#------------
# Services
#------------
rospy.Service(name='human_detected',
service_class=rqt_simulation_msgs.srv.Region,
handler=self.human_region_cb)
####### Wait 3 seconds to receive the initial position from the GUI
usleep = lambda x: time.sleep(x)
usleep(3)
self.robot_motion.set_initial(self.init_pose)
rospy.loginfo('%s : %s : The inital hard task is: %s' %
(self.node_name, self.robot_name, self.hard_task))
rospy.loginfo('%s : %s : The inital soft task is: %s' %
(self.node_name, self.robot_name, self.soft_task))
####### robot information
self.full_model = MotActModel(self.robot_motion, self.robot_action)
self.planner = ltl_planner(self.full_model, self.hard_task,
self.soft_task)
####### initial plan synthesis
self.planner.optimal(10, 'static')
###### Store initial plan regions
self.init_regions = self.getRegions(self.hard_task)
# Unsubscribe topics - cleanest way out of here ahahahah
self.sub_soft_task.unregister()
self.sub_hard_task.unregister()
self.sub_temp_task.unregister()
### Publish plan for GUI
prefix_msg = self.plan_msg_builder(self.planner.run.line,
rospy.Time.now())
self.PrefixPlanPublisher.publish(prefix_msg)
sufix_msg = self.plan_msg_builder(self.planner.run.loop,
rospy.Time.now())
self.SufixPlanPublisher.publish(sufix_msg)
### start up move_base
self.navigation = actionlib.SimpleActionClient("move_base",
MoveBaseAction)
self.navi_goal = GoalMsg = MoveBaseGoal()
edges = [(0, 1), (0, 2), (0, 4), (0, 9), (1, 4), (1, 2), (2, 4), (3, 11),
(5, 8), (5, 11), (5, 12), (6, 11), (7, 9), (7, 12), (8, 12), (8, 11),
(9, 12), (11, 12)]
edge_list = [(loc[e[0]], loc[e[1]]) for e in edges]
robot_motion.add_un_edges(edge_list, unit_cost=2)
##############################
# action FTS
############# no action model
action = dict()
robot_action = ActionModel(action)
robot_model = [robot_motion, init_pose, robot_action]
##############################
# complete robot model
robot_full_model = MotActModel(robot_motion, robot_action)
##############################
#specify soft and hard tasks
# case one
# hard_task = '(([]<> (r0 && <> (r8 && <> r7))) && ([]<> (r2 && <> r6)) && ([] !r5))'
# soft_task = '([]! c4)'
# case two
hard_task = '(([]<> r2) && ([]<> r3) && ([]<> r8))'
soft_task = '(([]<> r4) && ([]<> r6))'
sys_model = [robot_full_model, hard_task, soft_task]
def planner(ts, init_pose, act, robot_task, robot_name='TIAGo'):
global robot_pose
robot_pose = [None, init_pose]
global pose_goal
pose_goal = init_pose
print 'Robot %s: ltl_planner started!' % (robot_name)
#----------
#publish to
#----------
InitialPosePublisher = rospy.Publisher('initialpose',
PoseWithCovarianceStamped,
queue_size=100)
# for i in xrange(10):
# SendInitialPose(InitialPosePublisher, init_pose, rospy.Time.now())
# rospy.sleep(0.1)
# print('Initial pose set to %s.' %str(init_pose))
global GoalPublisher
GoalPublisher = actionlib.SimpleActionClient(
'move_base', move_base_msgs.msg.MoveBaseAction)
GoalPublisher.wait_for_server()
#----------
####### robot information
full_model = MotActModel(ts, act)
planner = ltl_planner(full_model, robot_task, None)
####### initial plan synthesis
planner.optimal(5)
#######
#subscribe to
#----------
rospy.Subscriber('amcl_pose', PoseWithCovarianceStamped, PoseCallback)
t0 = rospy.Time.now()
while not rospy.is_shutdown():
try:
t = rospy.Time.now() - t0
print '----------Time: %.2f----------' % t.to_sec()
new_goal = planner.next_move
print 'New current goal : %s' % str(new_goal)
if isinstance(new_goal, str):
print 'the robot next_move is an action, currently not implemented for %s' % robot_name
planner.find_next_move()
if pose_goal == planner.next_move:
print 'next move : %s\nAction break' % str(
planner.next_move)
break
else:
pose_goal = new_goal
print 'current yaw : %s' % str(abs(robot_pose[1][2]))
# move_base action server travels till (x,y) coordinates
SendGoal(pose_goal, t)
GoalPublisher.wait_for_result()
print '\\move_base Action server over\n-------------\nCurrent Goal : %s' % str(
pose_goal)
print 'Current position : %s' % str(robot_pose[1])
# check_yaw function set the orientation
check_yaw(pose_goal[2])
planner.find_next_move()
if pose_goal == planner.next_move:
print 'next move : %s\nMove break' % str(planner.next_move)
break
except rospy.ROSInterruptException:
pass
def planner(ts, init_pose, act, robot_task, robot_name='TIAGo'):
global robot_pose
robot_pose = [None, init_pose]
rospy.init_node('ltl_planner_%s' % robot_name)
print 'Robot %s: ltl_planner started!' % (robot_name)
###### publish to
#----------
#publish to
#----------
InitialPosePublisher = rospy.Publisher('initialpose',
PoseWithCovarianceStamped,
queue_size=100)
# for i in xrange(10):
# SendInitialPose(InitialPosePublisher, init_pose, rospy.Time.now())
# rospy.sleep(0.1)
# print('Initial pose set to %s.' %str(init_pose))
GoalPublisher = rospy.Publisher('move_base_simple/goal',
PoseStamped,
queue_size=100)
ang_pub = rospy.Publisher('mobile_base_controller/cmd_vel',
Twist,
queue_size=100)
ang = Twist()
#----------
#subscribe to
#----------
rospy.Subscriber('amcl_pose', PoseWithCovarianceStamped, PoseCallback)
####### robot information
full_model = MotActModel(ts, act)
planner = ltl_planner(full_model, robot_task, None)
####### initial plan synthesis
planner.optimal(5)
#######
reach_xy_bound = 0.5 # m
reach_yaw_bound = 0.1 * PI # rad
t0 = rospy.Time.now()
while not rospy.is_shutdown():
try:
t = rospy.Time.now() - t0
print '----------Time: %.2f----------' % t.to_sec()
next_move = planner.next_move
if isinstance(next_move, str):
print 'Robot %s next move is action %s' % (str(robot_name),
str(next_move))
client = SimpleActionClient('/play_motion', PlayMotionAction)
print 'Waiting for Action Server...'
client.wait_for_server()
goal = PlayMotionGoal()
goal.motion_name = next_move
goal.skip_planning = False
goal.priority = 0 # Optional
print 'Sending actionlib goal with motion: %s' % next_move
client.send_goal(goal)
print 'Waiting for result...'
action_ok = client.wait_for_result(rospy.Duration(30.0))
state = client.get_state()
if state >= 1:
print 'Successful execution of action %s' % next_move
planner.find_next_move()
else:
print 'Failed execution of action %s. Will retry' % next_move
else:
print 'Robot %s next move is motion to %s' % (str(robot_name),
str(next_move))
if ((norm2(robot_pose[1][0:2], next_move[0:2]) >
reach_xy_bound) or
(abs(robot_pose[1][2]) - next_move[2]) > reach_yaw_bound):
SendGoal(GoalPublisher, next_move, t)
print 'diff = %f' % (abs(robot_pose[1][2]) - next_move[2])
print('Goal %s sent to %s.' %
(str(next_move), str(robot_name)))
rospy.sleep(1)
else:
print('Goal %s reached by %s.' %
(str(next_move), str(robot_name)))
#ang.angular.z = 0
#ang_pub.publish(ang)
planner.find_next_move()
if next_move == planner.next_move:
break
except rospy.ROSInterruptException:
pass