def create_wait_task(node, secs=rospy.Duration(5)):
wait_task = Task(action='wait_action',start_node_id=node, end_node_id=node, max_duration=secs)
task_utils.add_time_argument(wait_task, rospy.Time())
task_utils.add_duration_argument(wait_task, secs)
wait_task.start_after = rospy.get_rostime() + rospy.Duration(2)
wait_task.end_before = rospy.get_rostime() + rospy.Duration(120)
return wait_task
def create_wait_task(node, secs=rospy.Duration(1), start_after=None, window_size=rospy.Duration(3600)):
if start_after is None:
start_after = rospy.get_rostime()
wait_task = Task(action='wait_action',start_node_id=node, end_node_id=node, max_duration=secs)
wait_task.start_after = start_after
wait_task.end_before = wait_task.start_after + window_size
task_utils.add_time_argument(wait_task, rospy.Time())
task_utils.add_duration_argument(wait_task, secs)
return wait_task
def bad_timings(self, task_status_fn=None):
waypoints = self.get_nodes()
# 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)
start_delay = rospy.Duration(3)
now = rospy.get_rostime()
# creates a wait task that waits for a lot longer than it should
wait_duration_secs = 100
wait_task = Task(action='wait_action',
start_node_id=random.choice(waypoints))
wait_task.max_duration = rospy.Duration(wait_duration_secs / 10)
wait_task.start_after = now + start_delay
wait_task.end_before = wait_task.start_after + wait_task.max_duration + wait_task.max_duration
task_utils.add_time_argument(wait_task, rospy.Time())
task_utils.add_duration_argument(wait_task,
rospy.Duration(wait_duration_secs))
rospy.Subscriber('task_executor/events', TaskEvent, self.task_status)
tasks = [wait_task]
try:
add_tasks_srv(tasks)
# Start the task executor running
set_execution_status(True)
wait_until = wait_task.end_before + rospy.Duration(60)
while now < wait_until and not rospy.is_shutdown():
rospy.sleep(1)
now = rospy.get_rostime()
if task_status_fn is not None:
task_status_fn(self.statuses)
except rospy.ServiceException, e:
print "Service call failed: %s" % e
def test_single_task_timing(self):
rospy.logwarn('test_single_task_timing')
msg_store = MessageStoreProxy()
# now wait for the distance service
time_srv_name = 'topological_navigation/travel_time_estimator'
rospy.wait_for_service(time_srv_name, timeout=10)
time_srv = rospy.ServiceProxy(time_srv_name, EstimateTravelTime)
checks = []
def checked(bool):
checks.append(bool)
task_checker = CheckTaskActionServer(result_fn=checked)
task_checker.start()
now = rospy.get_rostime()
delay = rospy.Duration(5)
current_node = self.current_node
task = Task()
task.action = 'check_task'
task.start_node_id = 'v_1'
# make sure action dur
travel_time = time_srv(current_node, task.start_node_id).travel_time
task.max_duration = rospy.Duration(travel_time.to_sec() / 4)
task.start_after = now + delay + travel_time
task.end_before = task.start_after + rospy.Duration(
travel_time.to_sec() / 2)
# add the task argument which is a reference to a copy of itself in the db
object_id = msg_store.insert(task)
task_utils.add_object_id_argument(task, object_id, Task)
client = actionlib.SimpleActionClient('check_task', TaskTestAction)
client.wait_for_server()
# get services to call into execution framework
add_tasks, set_execution_status = get_services()
add_tasks([task])
set_execution_status(True)
while len(checks) < 1 and not rospy.is_shutdown():
rospy.sleep(0.1)
for result in checks:
self.assertTrue(result)
def gowp(name):
task = Task()
task.action = '/wait_action'
max_wait_secs = 30
task.max_duration = rospy.Duration(max_wait_secs)
task_utils.add_time_argument(task, rospy.Time())
task_utils.add_duration_argument(task, rospy.Duration(10))
task.start_after = rospy.get_rostime() + rospy.Duration(10)
task.end_before = task.start_after + rospy.Duration(400)
task.start_node_id = name
task.end_node_id = task.start_node_id
set_execution_status = get_execution_status_service()
set_execution_status(True)
demand_task = get_demand_task_service()
demand_task(task)
log('Heading for %s - too miserable' % name)
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 wait_task_at_waypoint(wp): task = Task() task.action = '/wait_action' duration_time = 10 wait_time = 0 max_wait_seconds = 60*5 task.max_duration = rospy.Duration(max_wait_seconds) task_utils.add_time_argument(task, rospy.Time()) task_utils.add_duration_argument(task, rospy.Duration(duration_time)) task.start_after = rospy.get_rostime() + rospy.Duration(wait_time) task.end_before = task.start_after + rospy.Duration(task.max_duration.to_sec()) task.start_node_id = wp task.end_node_id = wp return task
def create_task(self, req):
factory_name = '/' + req.action + "_create"
start_after = rospy.Time.now() + rospy.Duration(secs=30)
rospy.loginfo(req)
end_before = start_after + rospy.Duration(secs=req.duration)
sa = "start_after: {secs: %d, nsecs: %d}" % \
(start_after.secs, start_after.nsecs)
eb = "end_before: {secs: %d, nsecs: %d}" % \
(end_before.secs, end_before.nsecs)
sn = "start_node_id: '%s'" % req.waypoint
en = "end_node_id: '%s'" % req.waypoint
yaml = "{%s, %s, %s, %s}" % (sa, eb, sn, en)
rospy.loginfo("calling with pre-populated yaml: %s" % yaml)
try:
factory = rospy.ServiceProxy(factory_name, CreateTask)
t = factory.call(yaml).task
rospy.loginfo("got the task back: %s" % str(t))
except Exception as e:
rospy.logwarn("Couldn't instantiate task from factory %s."
"error: %s."
"This is an error." % (factory_name, str(e)))
t = Task()
t.start_node_id = req.waypoint
t.end_node_id = req.waypoint
t.end_before = end_before
t.start_after = start_after
t.action = req.action
# use maximum duration of the one given here and the one returned from the constructor
t.max_duration.secs = max(t.max_duration.secs, req.duration)
t.max_duration.nsecs = 0
t.start_node_id = req.waypoint
t.end_node_id = req.waypoint
# allow to end this 60 seconds after the duration
# to give some slack for scheduling
#t.end_before = t.end_before + rospy.Duration(secs=60)
t.priority = self.demand_priority
return t
def wait_to(wpname):
task = Task()
task.action = '/wait_action'
max_wait_secs = 20
task.max_duration = rospy.Duration(max_wait_secs)
task_utils.add_time_argument(task, rospy.Time())
task_utils.add_duration_argument(task, rospy.Duration(10))
task.start_after = rospy.get_rostime() + rospy.Duration(10)
task.end_before = task.start_after + rospy.Duration(400)
rospy.logwarn(" HEADING TO "+str(wpname))
task.start_node_id = wpname
task.end_node_id = wpname
set_execution_status = get_execution_status_service()
set_execution_status(True)
demand_task = get_demand_task_service()
demand_task(task)
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)
if rospy.get_param('use_sim_time', False):
rospy.loginfo('Using sim time, waiting for time update')
rospy.wait_for_message('/clock', Clock)
# get services to call into execution framework
add_task, set_execution_status = get_services()
print 'Requesting wait at %s for %s seconds' % (sys.argv[1], sys.argv[2])
max_duration = rospy.Duration(int(sys.argv[2]))
wait_task = Task(action='wait_action',start_node_id=sys.argv[1], max_duration=rospy.Duration(10))
wait_task.start_after = rospy.get_rostime()
wait_task.end_before = wait_task.start_after + max_duration + max_duration + rospy.Duration(1200)
task_utils.add_time_argument(wait_task, rospy.Time())
task_utils.add_duration_argument(wait_task, max_duration)
# wait_task = Task(action='',start_node_id=sys.argv[1], max_duration=max_duration)
task_id = add_task([wait_task])
# Set the task executor running (if it isn't already)
set_execution_status(True)
# now let's stop execution while it's going on
# rospy.sleep(int(sys.argv[2])/2)
def execute(self, goal):
# TODO: CHECK IF IT IS COMPILING
now = rospy.get_rostime()
order = goal.order
rospy.loginfo("order is: %s" % order)
result = test_actionResult()
result.sequence.append(5)
self.say("I'm trying to find the coffee!")
# start doing something
coffee_found = False
list_of_waypoints = [
'WayPoint1', 'WayPoint2', 'WayPoint3', 'WayPoint4', 'WayPoint5'
]
for i in list_of_waypoints:
self.say("I'm going to " + i)
# ACTION : GO TO WAYPONT
# Move to the next location
task = Task()
task.action = '/wait_action'
max_wait_minutes = 160 * 160
task.max_duration = rospy.Duration(max_wait_minutes)
task.start_node_id = i
task.end_node_id = i
task_utils.add_time_argument(task, rospy.Time())
task_utils.add_duration_argument(task, rospy.Duration(10))
set_execution_status = self.get_execution_status_service()
set_execution_status(True)
task.start_after = rospy.get_rostime() + rospy.Duration(10)
task.end_before = task.start_after + rospy.Duration(
task.max_duration.to_sec() * 3)
# Execute task
demand_task = self.get_demand_task_service()
demand_task(task)
# Change the orientation of the camera
self.tilt_PTU(0, 40)
# obtaining from the topic a point cloud
msg = rospy.wait_for_message("/head_xtion/depth_registered/points",
sensor_msgs.msg.PointCloud2)
# calling the object recognition service
object_recog = rospy.ServiceProxy(
'/recognition_service/sv_recognition',
recognition_srv_definitions.srv.recognize)
object_recog.wait_for_service()
requestCoffee = recognition_srv_definitions.srv.recognizeRequest()
requestCoffee.cloud = msg
results = object_recog(requestCoffee)
results.ids
print results.ids
if "nescafe" in results.ids or "teabox" in results.ids:
coffee_found = True
#do stuff
#coffee_found = PSEUDOCODE_WHERE_I_SHOULD_CALL_THE_SERVICE
else:
self.say("coffee not found at location " + i)
if coffee_found:
coffee_place = i
self.say("The coffee is at" + i)
break
self.tilt_PTU(0, 0)
if coffee_found:
self.say("I found coffee i'm happy")
#twit that the coffee was found
# be happy
else:
#twit that someone stole the coffee
# be sad
self.say("Someone stole the coffee")
# end doing something
self.server.set_succeeded(result)
def execute(self, goal):
# TODO: CHECK IF IT IS COMPILING
now = rospy.get_rostime()
order = goal.order
rospy.loginfo("order is: %s"
% order)
result = test_actionResult()
result.sequence.append(5)
self.say("I'm trying to find the coffee!")
# start doing something
coffee_found = False
list_of_waypoints = ['WayPoint1','WayPoint2','WayPoint3','WayPoint4','WayPoint5']
for i in list_of_waypoints:
self.say("I'm going to " + i)
# ACTION : GO TO WAYPONT
# Move to the next location
task = Task()
task.action = '/wait_action'
max_wait_minutes = 160 * 160
task.max_duration = rospy.Duration(max_wait_minutes)
task.start_node_id = i
task.end_node_id = i
task_utils.add_time_argument(task, rospy.Time())
task_utils.add_duration_argument(task, rospy.Duration(10))
set_execution_status = self.get_execution_status_service()
set_execution_status(True)
task.start_after = rospy.get_rostime() + rospy.Duration(10)
task.end_before = task.start_after + rospy.Duration(task.max_duration.to_sec() * 3)
# Execute task
demand_task = self.get_demand_task_service()
demand_task(task)
# Change the orientation of the camera
self.tilt_PTU(0,40)
# obtaining from the topic a point cloud
msg = rospy.wait_for_message("/head_xtion/depth_registered/points", sensor_msgs.msg.PointCloud2)
# calling the object recognition service
object_recog = rospy.ServiceProxy('/recognition_service/sv_recognition', recognition_srv_definitions.srv.recognize)
object_recog.wait_for_service()
requestCoffee = recognition_srv_definitions.srv.recognizeRequest()
requestCoffee.cloud = msg
results = object_recog(requestCoffee)
results.ids
print results.ids
if "nescafe" in results.ids or "teabox" in results.ids :
coffee_found = True
#do stuff
#coffee_found = PSEUDOCODE_WHERE_I_SHOULD_CALL_THE_SERVICE
else :
self.say("coffee not found at location "+ i)
if coffee_found :
coffee_place = i
self.say("The coffee is at" + i)
break
self.tilt_PTU(0,0)
if coffee_found :
self.say("I found coffee i'm happy")
#twit that the coffee was found
# be happy
else :
#twit that someone stole the coffee
# be sad
self.say("Someone stole the coffee")
# end doing something
self.server.set_succeeded(result)
def task_from_gcal(self, gcal_event):
start = parser.parse(gcal_event['start']['dateTime'])
start_utc = start.astimezone(self.tz_utc)
end = parser.parse(gcal_event['end']['dateTime'])
end_utc = end.astimezone(self.tz_utc)
action_name = gcal_event['summary']
factory_name = '/' + action_name + "_create"
try:
factory = rospy.ServiceProxy(factory_name, CreateTask)
# if 'description' in gcal_event:
# t = factory.call(gcal_event['description']).task
# else:
start_after = rospy.Time.from_sec(timegm(start_utc.timetuple())) \
- self.time_offset
end_before = rospy.Time.from_sec(timegm(end_utc.timetuple())) \
- self.time_offset
sa = "start_after: {secs: %d, nsecs: %d}" % \
(start_after.secs, start_after.nsecs)
eb = "end_before: {secs: %d, nsecs: %d}" % \
(end_before.secs, end_before.nsecs)
sn = "start_node_id: '%s'" % gcal_event['location']
en = "end_node_id: '%s'" % gcal_event['location']
if gcal_event.has_key('description'):
ds = "description: '%s'" % gcal_event['description']
else:
ds = "description: "
yaml = "{%s, %s, %s, %s, %s}" % (sa, eb, sn, en, ds)
rospy.loginfo("calling with pre-populated yaml: %s" % yaml)
t = factory.call(yaml).task
rospy.loginfo("got the task back: %s" % str(t))
except Exception as e:
rospy.logwarn("Couldn't instantiate task from factory %s."
"error: %s."
"Using default constructor." %
(factory_name, str(e)))
t = Task()
t.action = gcal_event['summary']
t.start_after = rospy.Time.from_sec(
timegm(start_utc.timetuple())) \
- self.time_offset
t.end_before = rospy.Time.from_sec(timegm(end_utc.timetuple())) \
- self.time_offset
if 'location' in gcal_event:
t.start_node_id = gcal_event['location']
if len(t.end_node_id) == 0:
t.end_node_id = gcal_event['location']
if t.max_duration.secs == 0:
t.max_duration = (t.end_before - t.start_after) / 2
# if it's a time critical task, then the new
# scheduler requires the task to have the same end
# time as start time, to indicate time "criticalness".
# Opportunistically, in this case we assume the
# max duration to be the event length in calendar.
if self.time_critical:
t.max_duration = t.end_before - t.start_after
t.max_duration.secs = t.max_duration.secs / 2
t.end_before = t.start_after
return t
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
