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 planner(letter, ts, act, task):
global POSE
global c
global confirm
rospy.init_node('planner_%s' %letter)
print 'Agent %s: planner started!' %(letter)
###### publish to
activity_pub = rospy.Publisher('next_move_%s' %letter, activity)
###### subscribe to
rospy.Subscriber('cur_pose_%s' %letter, pose, pose_callback)
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, task, None)
####### initial plan synthesis
planner.optimal(10,'static')
#######
while not rospy.is_shutdown():
while not POSE:
rospy.sleep(0.1)
planner.cur_pose = POSE[0:2]
next_activity_bowser = activity()
next_activity = activity()
############### check for knowledge udpate
if object_name:
# konwledge detected
planner.update(object_name)
print 'Agent %s: object incorporated in map!' %(letter,)
planner.replan()
############### send next move
next_move = planner.next_move
############### implement next activity
if isinstance(next_move, str):
# next activity is action
next_activity.type = next_move
next_activity.x = 0
next_activity.y = 0
print 'Agent %s: next action %s!' %(letter, next_activity.type)
while not ((confirm[0]==next_move) and (confirm[1]>0)):
activity_pub.publish(next_activity)
rospy.sleep(0.5)
print 'Agent %s: action %s done!' %(letter, next_activity.type)
else:
print 'Agent %s: next waypoint (%d,%d)!' %(letter, next_move[0], next_move[1])
while not (check_dist(POSE[0:2], next_move)):
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.x = oriented_relative_pose[0]
next_activity.y = oriented_relative_pose[1]
activity_pub.publish(next_activity)
rospy.sleep(0.5)
print 'Agent %s: waypoint (%d,%d) reached!' %(letter, next_move[0], next_move[1])
planner.trace.append(planner.find_next_move())
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 hil_planner(sys_model, robot_name='turtlebot'):
global robot_pose, navi_control, tele_control, temp_task
robot_full_model, hard_task, soft_task = sys_model
robot_pose = [0, [0, 0, 0]]
navi_control = [0, 0]
tele_control = [0, 0]
mix_control = [0, 0]
temp_task = None
temp_task_s = False
temp_task_g = False
flag_task_incop = False
rospy.init_node('ltl_planner_%s' %robot_name)
print 'Robot %s: ltl_planner started!' %(robot_name)
###### publish to
#----------
#publish to
#----------
GoalPublisher = rospy.Publisher('move_base_simple/goal', PoseStamped, queue_size = 100)
MixPublisher = rospy.Publisher('cmd_vel_mux/input/mix', Twist, queue_size = 100)
#----------
#subscribe to
#----------
# position estimate from amcl
rospy.Subscriber('amcl_pose', PoseWithCovarianceStamped, PoseCallback)
# control command from amcl navigation
rospy.Subscriber('cmd_vel_mux/input/navi', Twist, NaviControlCallback)
# control command from tele operation
rospy.Subscriber('cmd_vel_mux/input/teleop', Twist, TeleControlCallback)
# temporary task
rospy.Subscriber('temp_task', task, TaskCallback)
####### robot information
initial_beta = 0
planner = ltl_planner(robot_full_model, hard_task, soft_task, initial_beta)
####### initial plan synthesis
planner.optimal()
print 'Original beta:', initial_beta
print 'Initial optimal plan', planner.run.suf_plan
#######
reach_bound = 0.5 # m
hi_bound = 0.1
hi_bool = False
hi_done = False
reach_new = False
#######
robot_path = []
reachable_prod_states = set(planner.product.graph['initial'])
pre_reach_ts = None
A_robot_pose = []
A_control = []
A_beta = []
#######
t0 = rospy.Time.now()
while not rospy.is_shutdown():
try:
t = rospy.Time.now()-t0
print '----------Time: %.2f----------' %t.to_sec()
A_robot_pose.append(list(robot_pose))
A_control.append([tele_control, navi_control, mix_control])
# robot past path update
reach_ts = planner.reach_ts_node(robot_pose[1], reach_bound)
if ((reach_ts) and (reach_ts != pre_reach_ts)):
print 'new region reached', reach_ts
robot_path.append(reach_ts)
reachable_prod_states = planner.update_reachable(reachable_prod_states, reach_ts)
pre_reach_ts = reach_ts
reach_new = True
else:
reach_new = False
#------------------------------
# mix control inputs
if norm2(tele_control, [0,0]) >= hi_bound:
print '--- Human inputs detected ---'
hi_bool = True
dist_to_trap = planner.prod_dist_to_trap(robot_pose[1], reachable_prod_states)
if dist_to_trap >=0:
print 'Distance to trap states in product: %.2f' %dist_to_trap
mix_control, gain = smooth_mix(tele_control, navi_control, dist_to_trap)
SendMix(MixPublisher, mix_control)
print 'mix_control: %s ||| navi_control: %s ||| tele_control: %s ||| gain: %.2f' %(mix_control, navi_control, tele_control, gain)
else:
print 'No trap states are close'
dist_to_trap = 1000
mix_control, gain = smooth_mix(tele_control, navi_control, dist_to_trap)
SendMix(MixPublisher, mix_control)
print 'mix_control: %s ||| navi_control: %s ||| tele_control: %s ||| gain: %.2f' %(mix_control, navi_control, tele_control, gain)
rospy.sleep(0.2)
else:
print 'No Human inputs. Autonomous controller used.'
mix_control = list(navi_control)
SendMix(MixPublisher, mix_control)
print 'robot_path:', robot_path
# print 'reachable_prod_states', reachable_prod_states
#------------------------------
# estimate human preference, i.e. beta
# and update discrete plan
if ((reach_new) and (planner.start_suffix())):
print 'robot_path:', robot_path
print 'reachable_prod_states', reachable_prod_states
if hi_bool:
print '------------------------------'
print '---------- In IRL mode now ----------'
est_beta_seq, match_score = planner.irl(robot_path, reachable_prod_states)
hi_bool = False
A_beta.append(est_beta_seq)
print '------------------------------'
print '--- New suffix execution---'
robot_path = [reach_ts]
reachable_prod_states = planner.intersect_accept(reachable_prod_states)
#------------------------------
# satisfy temporary task
if temp_task:
if not flag_task_incop:
planner.add_temp_task(temp_task)
flag_task_incop = True
reg_s = (temp_task[0], temp_task[1])
reg_g = (temp_task[2], temp_task[3])
if ((reach_ts) and (reach_ts[0] == reg_s)):
temp_task_s = True
print 'robot reaches pi_s in the temp task:%s' %str(reg_s)
if (temp_task_s) and ((reach_ts) and (reach_ts[0] == reg_g)):
temp_task_g = True
print 'robot reaches pi_g in the temp task:%s' %str(reg_g)
if (temp_task_s) and (temp_task_g):
print 'robot accomplished temporary task <>(%s && <> %s)' %(str(reg_s), str(reg_g))
temp_task = None
temp_task_s = False
temp_task_g = False
flag_task_incop = False
#------------------------------
# plan execution
current_goal = planner.next_move
# next move is action
if isinstance(current_goal, str):
print 'the robot next_move is an action, currently not implemented for %s' %robot_name
break
# next move is motion
if ((reach_ts) and (reach_ts[0] == current_goal)):
print('Goal %s reached by %s.' %(str(current_goal),str(robot_name)))
planner.find_next_move()
else:
SendGoal(GoalPublisher, current_goal, t)
print('Goal %s sent to %s.' %(str(current_goal),str(robot_name)))
rospy.sleep(0.5)
except rospy.ROSInterruptException:
pickle.dump([A_robot_pose, A_control, A_beta], open('data/exp_long_cord.p', 'wb'))
print 'exp_long_cord.p saved'
print A_robot_pose
pass
pickle.dump([A_robot_pose, A_control, A_beta], open('data/exp_long_cord.p', 'wb'))
print 'exp_long_cord.p saved'
print A_robot_pose
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()
def planner(letter, ts, act, task):
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, task, None)
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.type = next_move
next_activity.x = 0
next_activity.y = 0
print 'Agent %s: next action %s!' %(letter, next_activity.type)
while not ((confirm[0]==next_move) and (confirm[1]>0)):
activity_pub.publish(next_activity)
rospy.sleep(0.06)
rospy.sleep(1)
confirm[1] = 0
print 'Agent %s: action %s done!' %(letter, next_activity.type)
else:
print 'Agent %s: next waypoint (%.2f,%.2f)!' %(letter, next_move[0], next_move[1])
while not ((confirm[0]=='goto') and (confirm[1]>0)):
#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.x = oriented_relative_pose[0]
#next_activity.y = oriented_relative_pose[1]
next_activity.x = next_move[0]
next_activity.y = next_move[1]
activity_pub.publish(next_activity)
rospy.sleep(0.06)
rospy.sleep(1)
confirm[1] = 0
print 'Agent %s: waypoint (%.2f,%.2f) reached!' %(letter, next_move[0], next_move[1])
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
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