def create_har_sweep_tasks(roi_db_name, roi_collection_name, roi_config):
topological_nodes = getNodes()
rois = getRegions(roi_db_name, roi_collection_name, roi_config)
roinodepairs = []
for roi in rois:
vertices = []
for apose in roi.posearray.poses:
vertices.append([apose.position.x, apose.position.y])
hull = ConvexHull(vertices)
cx = np.mean(hull.points[hull.vertices, 0])
cy = np.mean(hull.points[hull.vertices, 1])
roi_position = [cx, cy]
distances = []
for topological_node in topological_nodes:
node_position = [
topological_node.pose.position.x,
topological_node.pose.position.y
]
distances.append(distance(roi_position, node_position))
minindex = distances.index(min(distances))
roinodepairs.append((roi.id, topological_nodes[minindex]))
#print roinodepairs
tasks = dict()
for pair in roinodepairs:
task = Task(start_node_id=pair[1].name,
action='do_sweep',
max_duration=rospy.Duration(60 * 5))
#task.start_after = rospy.Time.now()
#task.end_before = task.start_after + rospy.Duration(task.start_after * 3)
task_utils.add_string_argument(task, 'medium')
tasks[pair[0]] = task
print tasks.keys()
return tasks
def create_master_task():
"""
Create an example of a task which we'll copy for other tasks later.
This is a good example of creating a task with a variety of arguments.
"""
# need message store to pass objects around
msg_store = MessageStoreProxy()
# get the pose of a named object
pose_name = "my favourite pose"
# get the pose if it's there
message, meta = msg_store.query_named(pose_name, Pose._type)
# if it's not there, add it in
if message == None:
message = Pose(Point(0, 1, 2), Quaternion(3, 4, 5, 6))
pose_id = msg_store.insert_named(pose_name, message)
else:
pose_id = meta["_id"]
master_task = Task(action='test_task')
task_utils.add_string_argument(master_task, 'hello world')
task_utils.add_object_id_argument(master_task, pose_id, Pose)
task_utils.add_int_argument(master_task, 24)
task_utils.add_float_argument(master_task, 63.678)
return master_task
def create_interaction_task(self, waypoint_name, max_duration=rospy.Duration(120)):
# Task: robot detects human and interacts
task = Task()
task.action = '/flexbe/execute_behavior'
task_utils.add_string_argument(task,'Tell Random Joke')
task.max_duration = max_duration
task.start_node_id = waypoint_name
task.end_node_id = waypoint_name
return task
def create_travel_task(self, waypoint_name, max_duration=rospy.Duration(30)):
# Task: robot travels to waypoint and then recognizes object there
#
task = Task()
task.action = '/flexbe/execute_behavior'
task_utils.add_string_argument(task,'Example Behavior')
task.max_duration = max_duration
task.start_node_id = waypoint_name
task.end_node_id = waypoint_name
return task
def create_travel_task(self, waypoint_name, max_duration=rospy.Duration(120)): # Task: robot travels to waypoint # task = Task() task.action = '/flexbe/execute_behavior' task_utils.add_string_argument(task,'Tell Random Joke') task.max_duration = max_duration task.start_node_id = waypoint_name task.end_node_id = waypoint_name print 'TravelAndDetectPeople, created waypoint task. Go to waypoint: ', waypoint_name return task
def execute(self, userdata):
rospy.loginfo("Entering observe mode...")
start = rospy.Time.now()
nav_goal = GotoNodeGoal()
nav_goal.target = userdata.waypoint
self.nav_client.send_goal(nav_goal)
self.nav_client.wait_for_result()
self._reset_minute_check(self.observe_duration)
rospy.sleep(0.1)
self._is_observing = True
end = rospy.Time.now()
duration_left = self.observe_duration - (end - start)
if duration_left > rospy.Duration(1, 0):
task = Task(
action="record_skeletons",
start_node_id=userdata.waypoint,
end_node_id=userdata.waypoint,
# start_after=rospy.Time.now(),
# end_before=rospy.Time.now()+self.observe_duration,
max_duration=duration_left
)
tu.add_duration_argument(task, duration_left)
tu.add_string_argument(task, userdata.roi)
tu.add_string_argument(task, self.soma_config)
rospy.sleep(1)
rospy.loginfo("Adding a task to task scheduler from %d to %d" % (start.secs, (start+self.observe_duration).secs))
self.add_tasks_srv(task)
# self.action_client.send_goal(
# skeletonGoal(duration_left, userdata.roi, self.soma_config)
# )
is_person_there = False
while (rospy.Time.now() - start) < duration_left and not rospy.is_shutdown():
if False not in self.minute_check and not is_person_there:
rospy.loginfo("Humans are constantly detected for %d minutes" % len(self.minute_check))
self._is_observing = False
is_person_there = True
self.ubd_srv()
# if self.action_client.get_state() in [
# actionlib.GoalStatus.SUCCEEDED, actionlib.GoalStatus.PREEMPTED, actionlib.GoalStatus.ABORTED
# ]:
# break
rospy.sleep(1)
# self.action_client.wait_for_result()
# if False in self.minute_check:
# Need to capture a pic
userdata.waypoint = ''
userdata.roi = ''
self._is_observing = False
return 'succeeded'
def tell_joke():
task = Task()
task.action = '/speak'
joke = random.choice(jokes)
# joke_text = "I will tell a joke." + joke + "Please smile or I will be sad."
joke_text = joke
task_utils.add_string_argument(task, joke_text)
max_wait_secs = 60
task.max_duration = rospy.Duration(max_wait_secs)
task.start_after = rospy.get_rostime() + rospy.Duration(10)
task.end_before = task.start_after + rospy.Duration(200)
return task
def create_tasks_for_fremen_grid_exp_srv(self, definitions, scores,
start_time, end_time):
res = []
do_learn = False
if date.today().isoweekday():
current_time = datetime.fromtimestamp(rospy.get_rostime().to_sec(),
tz=self.localtz).time()
if time_greater_than(current_time, self.start) and time_less_than(
current_time, self.end):
do_learn = True
n_learns = rospy.get_param("~n_learns_per_trigger", 1)
learn_thresh = get_x_best(deepcopy(scores), n_learns)
added_learns = 0
for (wp, score) in zip(definitions, scores):
if do_learn and score >= learn_thresh and added_learns < n_learns:
added_learns += 1
max_duration = rospy.Duration(
rospy.get_param("~object_learn_dur", 60 * 15))
task = Task(action='learn_object',
start_node_id=wp,
end_node_id=wp,
start_after=start_time,
end_before=end_time,
max_duration=max_duration)
tu.add_string_argument(task, wp)
res.append(
ExplorationTask(task_type="fremen_grid_exploration",
score=score,
task_def=task))
else:
task = Task(action='do_sweep',
start_node_id=wp,
end_node_id=wp,
start_after=start_time,
end_before=end_time,
max_duration=rospy.Duration(1 * 60))
tu.add_string_argument(task, 'medium')
res.append(
ExplorationTask(task_type="fremen_grid_exploration",
score=score,
task_def=task))
return res
def request_exploration(self, event):
self.budget_control.get_budget_alloc(self.region_wps.keys())
for (start, roi, budget) in self.budget_control.budget_alloc:
wp = self.region_wps[roi]
start_time = start - self.exploration_duration
end_time = start_time + self.exploration_duration + self.exploration_duration
duration = self.exploration_duration
task = Task(action="record_skeletons",
start_node_id=wp,
end_node_id=wp,
start_after=start_time,
end_before=end_time,
max_duration=duration)
task_utils.add_duration_argument(task, duration)
task_utils.add_string_argument(task, roi)
task_utils.add_string_argument(task, self.soma_config)
rospy.loginfo(
"Task to be requested: {wp:%s, roi:%s, start:%d, duration:%d, budget:%d"
% (wp, roi, start_time.secs, duration.secs, int(budget)))
self.budget_control.bidder.add_task_bid(task, int(budget))
rospy.loginfo("Finish adding tasks...")
def create_tasks_for_activity_exp_srv(self, definitions, scores,
start_time, end_time):
res = []
for (wp, score) in zip(definitions, scores):
duration = rospy.Duration(
rospy.get_param("~skeleton_action_dur", 10 * 60))
max_duration = duration + rospy.Duration(10)
task = Task(action='skeleton_action',
start_node_id=wp,
end_node_id=wp,
start_after=start_time,
end_before=end_time,
max_duration=max_duration)
tu.add_duration_argument(task, duration)
tu.add_string_argument(task, wp)
res.append(
ExplorationTask(task_type="activity_exploration",
score=score,
task_def=task))
return res
def __init__(self): set_execution_status = self.get_execution_status_service() set_execution_status(True) waypoints = ['WayPoint%d' % (i + 1) for i in range(4)] tasks = [] for i in range(len(waypoints)): task = Task() task.action = '/flexbe/execute_behavior' task.max_duration = rospy.Duration(15) task.start_node_id = waypoints[i] task.end_node_id = waypoints[i] task_utils.add_string_argument(task, 'Sweep Test Behavior') task.start_after = rospy.get_rostime() + rospy.Duration(10) task.end_before = task.start_after + rospy.Duration(task.max_duration.to_sec() * 8) tasks.append(task) add_tasks = self.get_add_tasks_service() add_tasks(tasks)
def __init__(self):
set_execution_status = self.get_execution_status_service()
set_execution_status(True)
waypoints = ['WayPoint%d' % (i + 1) for i in range(4)]
tasks = []
for i in range(len(waypoints)):
task = Task()
task.action = '/flexbe/execute_behavior'
task.max_duration = rospy.Duration(15)
task.start_node_id = waypoints[i]
task.end_node_id = waypoints[i]
task_utils.add_string_argument(task, 'Sweep Test Behavior')
task.start_after = rospy.get_rostime() + rospy.Duration(10)
task.end_before = task.start_after + rospy.Duration(
task.max_duration.to_sec() * 8)
tasks.append(task)
add_tasks = self.get_add_tasks_service()
add_tasks(tasks)
def create_task(self, req):
rospy.loginfo("Request to create task: %s" % req.identifier)
data = json.loads(req.identifier)
self.show_page_srv('nhm-moving.html')
self.speak.speak(self.move_speeches[randint(0, len(self.move_speeches)-1)])
task = Task()
action = self.buttonmappings[data['waypoint']]
waypoint = action['waypoint']
task.start_node_id = waypoint
task.action = 'execute_marathon_task'
task.max_duration = rospy.Duration(10*60)
task_utils.add_string_argument(task, data['action'])
task_utils.add_string_argument(task, action[data['action']]['page'])
task_utils.add_string_argument(task, action[data['action']]['text'])
task_utils.add_string_argument(task, self.tweets_action[randint(0, len(self.tweets_action)-1)])
self.td.demand_task(task)
return MapButtonServiceResponse()
def schedule_bellbot_task(self,
mode,
start_wp,
end_wp,
description,
start_time,
end_time,
on_demand=False):
try:
if on_demand:
task = Task(start_node_id="WayPoint4",
action=self.bellbot_as_name,
max_duration=self.task_duration)
else:
task = Task(start_node_id="WayPoint4",
action=self.bellbot_as_name,
max_duration=self.task_duration,
start_after=start_time,
end_before=end_time)
task_utils.add_int_argument(task, mode)
task_utils.add_string_argument(task, start_wp)
task_utils.add_string_argument(task, end_wp)
task_utils.add_string_argument(task, description)
print task
if on_demand:
print "Demanding task be run NOW."
add_task_srv_name = '/task_executor/demand_task'
else:
add_task_srv_name = '/task_executor/add_task'
set_exe_stat_srv_name = '/task_executor/set_execution_status'
print self.add_task_srv(task)
#Make sure the task executor is running
self.set_execution_status(True)
except rospy.ServiceException, e:
print "Service call failed: %s" % e
set_execution_status = rospy.ServiceProxy(set_exe_stat_srv_name,
SetExecutionStatus)
return add_tasks_srv, set_execution_status
if __name__ == '__main__':
rospy.init_node("demand_sweep")
if len(sys.argv) == 2:
waypoint = sys.argv[1]
print "Using waypoint: ", waypoint
else:
print "Usage: demand_sweep.py waypoint"
sys.exit(1)
# get services to call into execution framework
demand_task, set_execution_status = get_services()
# Set the task executor running (if it isn't already)
set_execution_status(True)
print 'set execution'
sweep_task = Task(action='do_sweep',
max_duration=rospy.Duration(60),
start_node_id=waypoint)
task_utils.add_string_argument(sweep_task, 'short')
resp = demand_task(sweep_task)
print 'demanded task as id: %s' % resp.task_id
rospy.loginfo('Success: %s' % resp.success)
rospy.loginfo('Wait: %s' % resp.remaining_execution_time)
def create_object_search_task(waypoint_name, duration=rospy.Duration(480)):
task = Task(action='search_object', max_duration=duration, start_node_id=waypoint_name)
task_utils.add_string_argument(task, waypoint_name)
return task
return add_tasks_srv#, set_execution_status
if __name__ == '__main__':
rospy.init_node("demand_activity_recognition_task")
if len(sys.argv)==3:
waypoint=sys.argv[1]
duration=rospy.Duration(int(sys.argv[2]))
print "Using waypoint: ", waypoint
#print "Duration: ", duration
else:
print "Usage: demand_activity_recognition_task.py waypoint_name duration (secs)"
sys.exit(1)
# get services to call into execution framework
# demand_task, set_execution_status = get_services()
# Set the task executor running (if it isn't already)
# set_execution_status(True)
# print 'set execution'
demand_task = get_services()
task= Task(action='recognise_action', max_duration=rospy.Duration(6000), start_node_id=waypoint)
task_utils.add_duration_argument(task, duration)
task_utils.add_string_argument(task, waypoint)
resp = demand_task(task)
print 'demanded task as id: %s' % resp.task_id
rospy.loginfo('Success: %s' % resp.success)
rospy.loginfo('Wait: %s' % resp.remaining_execution_time)
def add_task(self, *args, **kwargs):
# print "Availabe tokens: ", self.bidder.available_tokens
# print "Currently bid tokens: ", self.bidder.currently_bid_tokens
currentTime = rospy.Time.now()
midnight = self.getMidnightTime(currentTime.secs)
#print 'Current time: ', currentTime.secs, ' Midnight: ', midnight
if len(self.grids_schedule.timeInfo) == 0 or len(
self.objects_schedule.timeInfo) == 0:
rospy.loginfo("Bidder: waiting for both schedules.")
else:
if self.grids_schedule.timeInfo[
0] == midnight and self.objects_schedule.timeInfo[
0] == midnight:
rospy.loginfo("Received schedules for both explorations.")
self.slot_duration = self.grids_schedule.timeInfo[
2] - self.grids_schedule.timeInfo[1]
#rospy.loginfo("Time slot duration: %d", self.slot_duration)
#get current time slot:
currentSlot = self.getCurrentTimeSlot(self.slot_duration)
rospy.loginfo("Time slot: %d/%d", currentSlot, self.num_slots)
task_duration = rospy.Duration(secs=60 * 10)
if currentSlot is not self.last_bid:
if self.num_slots - currentSlot > 2:
rospy.loginfo("Adding task...")
#choose between full and mini:
if random.randint(0, 2) == 0:
#start_time = rospy.Time(secs = self.grids_schedule[currentSlot+1]['timeInfo'], nsecs = 0)
start_time = rospy.Time(
secs=self.grids_schedule.timeInfo[currentSlot +
1],
nsecs=0)
node_observation = self.grids_schedule.nodeID[
currentSlot + 1]
mode = self.grids_schedule.mode[currentSlot + 1]
else:
start_time = rospy.Time(
secs=self.objects_schedule.timeInfo[currentSlot
+ 1],
nsecs=0)
node_observation = self.objects_schedule.nodeID[
currentSlot + 1]
mode = self.objects_schedule.mode[currentSlot + 1]
#add task:
task = Task(
action='search_object',
start_node_id=node_observation,
end_node_id=node_observation,
start_after=start_time,
end_before=start_time + rospy.Duration(60 * 15),
max_duration=task_duration,
)
if mode == 'object_full':
rois = get_object_search_dfn_quasimodo(
node_observation)
else:
rois = get_object_search_dfn(node_observation)
if rois == None:
rospy.loginfo("waypoint not found in routine file")
else:
task_utils.add_string_argument(
task, node_observation)
task_utils.add_string_argument(task, rois[1])
task_utils.add_string_argument(task, rois[1])
task_utils.add_string_argument(task, mode)
bidRatio = self.num_slots - currentSlot
bid = int(
ceil((self.bidder.available_tokens -
self.bidder.currently_bid_tokens) /
bidRatio))
if bid > 0:
rospy.loginfo(
"Bidder: Bid Amount: %d -- Node: %s -- Start Time: %d -- Mode: %s -- Roi: %s -- Surface: %s",
bid, node_observation, start_time.secs,
mode, rois[1], rois[2])
self.bidder.add_task_bid(task, bid)
self.last_bid = currentSlot
else:
rospy.loginfo(
"Bidder: Bid value to low to add task!")
rospy.loginfo("Bidder: Current budget: %d",
self.bidder.available_tokens)
else:
rospy.loginfo(
"Last slot of the day, waiting for midnight!")
else:
rospy.loginfo(
"Bidder: Waiting for both schedules (3D changes and SOMA...)"
)
set_execution_status = rospy.ServiceProxy(set_exe_stat_srv_name, SetExecutionStatus)
return add_tasks_srv, set_execution_status
if __name__ == '__main__':
rospy.init_node("demand_sweep")
if len(sys.argv)==2:
waypoint=sys.argv[1]
print "Using waypoint: ", waypoint
else:
print "Usage: demand_sweep.py waypoint"
sys.exit(1)
# get services to call into execution framework
demand_task, set_execution_status = get_services()
# Set the task executor running (if it isn't already)
set_execution_status(True)
print 'set execution'
sweep_task = Task(action='do_sweep', max_duration=rospy.Duration(60), start_node_id=waypoint)
task_utils.add_string_argument(sweep_task, 'short')
resp = demand_task(sweep_task)
print 'demanded task as id: %s' % resp.task_id
rospy.loginfo('Success: %s' % resp.success)
rospy.loginfo('Wait: %s' % resp.remaining_execution_time)
rospy.init_node("ftp_upload_task_client")
# need message store to pass objects around
msg_store = MessageStoreProxy()
try:
task = Task(start_node_id=sys.argv[1], action='ftp_upload')
ftp_server =
username =
password =
remote_path =
remote_folder =
cachePath = expanduser("~") + '/.semanticMap/cache'
task_utils.add_string_argument(task, ftp_server) # FTP server address
task_utils.add_string_argument(task, username) # username
task_utils.add_string_argument(task, password) # password
task_utils.add_string_argument(task, remote_path) # remote path
task_utils.add_string_argument(task, remote_folder) # remote folder
task_utils.add_string_argument(task, cachePath) # local path
print task
# now register this with the executor
if len(sys.argv)>2 and sys.argv[2]=="demand":
print "Demanding task be run NOW."
add_task_srv_name = '/task_executor/demand_task'
else:
add_task_srv_name = '/task_executor/add_task'
set_exe_stat_srv_name = '/task_executor/set_execution_status'
rospy.wait_for_service(demand_task_srv_name)
# rospy.wait_for_service(set_exe_stat_srv_name)
rospy.loginfo("cpm Done")
add_tasks_srv = rospy.ServiceProxy(demand_task_srv_name, DemandTask)
# set_execution_status = rospy.ServiceProxy(set_exe_stat_srv_name, SetExecutionStatus)
return add_tasks_srv #, set_execution_status
if __name__ == '__main__':
rospy.init_node("demand_cpm_task")
if len(sys.argv) == 2:
duration = rospy.Duration(int(sys.argv[1]))
print "Duration: ", duration
else:
print "Usage: demand_activity_recognition_task.py waypoint_name duration (secs)"
sys.exit(1)
# Set the task exec
demand_task = get_services()
task = Task(action='cpm_action',
max_duration=rospy.Duration(28800),
start_node_id=waypoint)
task_utils.add_duration_argument(task, duration)
task_utils.add_string_argument(task, waypoint)
resp = demand_task(task)
print 'demanded task as id: %s' % resp.task_id
rospy.loginfo('Success: %s' % resp.success)
rospy.loginfo('Wait: %s' % resp.remaining_execution_time)
def run_test(self,
task_descriptions_fn,
test_tasks=20,
time_critical_tasks=0,
time_diffs_fn=None,
pause_delay=rospy.Duration(10),
pause_count=0,
demanded_tasks=0):
waypoints = self.get_nodes()
action_types = 5
action_sleep = rospy.Duration.from_sec(2)
action_prefix = 'test_task_'
msg_store = MessageStoreProxy()
task_descriptions = []
# list comprehension seemed to use the same result from randrange
start_delay = rospy.Duration(8)
now = rospy.get_rostime()
task_start = now + start_delay
task_window_size = action_sleep + action_sleep + rospy.Duration(100000)
for n in range(test_tasks):
string = 'oh what a lovely number %s is' % n
pose = Pose(Point(n, 1, 2), Quaternion(3, 4, 5, 6))
task_descriptions += [[
random.choice(waypoints),
action_prefix + str(randrange(action_types)), string, pose, n,
n + 0.1, task_start, task_start + task_window_size
]]
task_start += action_sleep
assert test_tasks == len(task_descriptions)
time_critical_step = rospy.Duration(600)
time_critical_start = now + start_delay + rospy.Duration(1200)
for n in range(test_tasks, test_tasks + time_critical_tasks):
string = 'Please run me at %s is' % time_critical_start.to_sec()
pose = Pose(Point(n, 1, 2), Quaternion(3, 4, 5, 6))
task_descriptions += [[
random.choice(waypoints),
action_prefix + str(randrange(action_types)), string, pose, n,
time_critical_start.to_sec(), time_critical_start,
time_critical_start
]]
time_critical_start += time_critical_step
to_demand = []
# demand_start = now + start_delay + rospy.Duration(120)
demand_start = now + start_delay + rospy.Duration(0)
demand_step = rospy.Duration(60)
for n in range(test_tasks + time_critical_tasks,
test_tasks + time_critical_tasks + demanded_tasks):
string = 'Demand me at %s' % demand_start.to_sec()
pose = Pose(Point(n, 1, 2), Quaternion(3, 4, 5, 6))
to_demand += [[
random.choice(waypoints),
action_prefix + str(randrange(action_types)), string, pose, n,
demand_start.to_sec(), demand_start, demand_start
]]
demand_start += demand_step
task_tester = TaskTester(action_types, action_prefix,
task_descriptions + to_demand, action_sleep,
self)
# get task services
add_task_srv_name = 'task_executor/add_tasks'
demand_task_srv_name = 'task_executor/demand_task'
set_exe_stat_srv_name = 'task_executor/set_execution_status'
rospy.loginfo("Waiting for task_executor service...")
rospy.wait_for_service(add_task_srv_name)
rospy.wait_for_service(set_exe_stat_srv_name)
rospy.wait_for_service(demand_task_srv_name)
rospy.loginfo("Done")
add_tasks_srv = rospy.ServiceProxy(add_task_srv_name, AddTasks)
demand_task_srv = rospy.ServiceProxy(demand_task_srv_name, DemandTask)
set_execution_status = rospy.ServiceProxy(set_exe_stat_srv_name,
SetExecutionStatus)
try:
tasks = []
for task_description in task_descriptions:
# create the task from the description
task = Task(start_node_id=task_description[0],
action=task_description[1])
# add some dummy arguments
task_utils.add_string_argument(task, task_description[2])
task_utils.add_object_id_argument(
task, msg_store.insert(task_description[3]), Pose)
task_utils.add_int_argument(task, task_description[4])
task_utils.add_float_argument(task, task_description[5])
task.start_after = task_description[6]
task.end_before = task_description[7]
task.max_duration = action_sleep + action_sleep
tasks.append(task)
# print task
tasks_to_demand = []
for task_description in to_demand:
# create the task from the description
task = Task(start_node_id=task_description[0],
action=task_description[1])
# add some dummy arguments
task_utils.add_string_argument(task, task_description[2])
task_utils.add_object_id_argument(
task, msg_store.insert(task_description[3]), Pose)
task_utils.add_int_argument(task, task_description[4])
task_utils.add_float_argument(task, task_description[5])
task.start_after = task_description[6]
task.end_before = task_description[7]
task.max_duration = action_sleep + action_sleep
tasks_to_demand.append(task)
# print task
add_tasks_srv(tasks)
# Start the task executor running
set_execution_status(True)
max_wait_duration = start_delay
if time_critical_tasks > 0:
max_wait_duration += time_critical_start - now
if demanded_tasks > 0:
max_wait_duration += demand_start - now
max_travel = rospy.Duration(90)
for n in range(test_tasks):
max_wait_duration += (action_sleep + max_travel)
for pause in range(pause_count):
rospy.sleep(pause_delay)
print 'pause %s/%s' % (pause + 1, pause_count)
set_execution_status(False)
for i in range(10):
print 10 - i
rospy.sleep(1)
set_execution_status(True)
for demand in tasks_to_demand:
while now < demand.start_after and not rospy.is_shutdown():
rospy.sleep(1)
# print 'wait'
now = rospy.get_rostime()
print 'demanding'
print demand_task_srv(demand)
task_tester.wait_for_completion(max_wait_duration)
with task_tester.state_lock:
if task_descriptions_fn is not None:
task_descriptions_fn(task_tester.task_descriptions)
if time_critical_tasks > 0 and time_diffs_fn is not None:
time_diffs_fn(task_tester.time_diffs)
except rospy.ServiceException, e:
print "Service call failed: %s" % e
