def create_wait_task(max_duration):
master_task = Task(action='wait_action',
start_node_id='ChargingPoint',
max_duration=max_duration)
task_utils.add_time_argument(master_task, rospy.Time())
task_utils.add_duration_argument(master_task, max_duration)
return master_task
def demand_charge(self, charge_duration):
charging_point = 'ChargingPoint'
charge_task = Task(action='wait_action',
start_node_id=charging_point,
end_node_id=charging_point,
max_duration=charge_duration)
task_utils.add_time_argument(charge_task, rospy.Time())
task_utils.add_duration_argument(charge_task, charge_duration)
self.demand_task(charge_task)
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 create(self, req):
t = super(TestServer, self).create(req)
task_utils.add_time_argument(t, rospy.Time())
if self.maximise_duration_delta > rospy.Duration(0):
d = (t.end_before - t.start_after) - self.maximise_duration_delta
task_utils.add_duration_argument(t, d)
t.max_duration = d
else:
task_utils.add_duration_argument(t, t.max_duration)
return t
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 create_wait_task(node, start, end, secs=rospy.Duration(5), prio=1):
wait_task = Task(
action='wait_action',
start_node_id=node,
end_node_id=node,
max_duration=secs,
start_after=start,
end_before=end,
priority=prio) #2280 passing time for 3 task, 2270 failing
task_utils.add_time_argument(wait_task,
rospy.Time()) # Lenka note: what is this???
task_utils.add_duration_argument(wait_task, secs)
return wait_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 create_datacentre_task(to_replicate, delete_after_move=True):
task = Task()
# no idea, let's say 2 hours for now -- this action server can't be preempted though, so this is cheating
task.max_duration = rospy.Duration(60 * 60 * 2)
task.action = 'move_mongodb_entries'
# add arg for collectionst o replication
collections = StringList(to_replicate)
msg_store = MessageStoreProxy()
object_id = msg_store.insert(collections)
task_utils.add_object_id_argument(task, object_id, StringList)
# move stuff over 24 hours old
task_utils.add_duration_argument(task, rospy.Duration(60 * 60 *24))
# and delete afterwards
task_utils.add_bool_argument(task, delete_after_move)
return task
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 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 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_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 findcoffee(req):
task.action = '/find_coffee'
task.max_duration = rospy.Duration(500)
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()
rospy.logwarn("Find action triggered")
return std_srvs.srv.TriggerResponse(True,'findcoffee');
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 create_wait_task(waypoint_name, duration=rospy.Duration(30)):
patrol_task = Task(action='wait_action', start_node_id=waypoint_name, end_node_id=waypoint_name, max_duration=duration)
task_utils.add_time_argument(patrol_task, rospy.Time())
task_utils.add_duration_argument(patrol_task, duration)
return patrol_task
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 demand_charge(self, charge_duration):
charging_point = 'ChargingPoint'
charge_task = Task(action='wait_action', start_node_id=charging_point, end_node_id=charging_point, max_duration=charge_duration)
task_utils.add_time_argument(charge_task, rospy.Time())
task_utils.add_duration_argument(charge_task, charge_duration)
self.demand_task(charge_task)
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)
# set_execution_status(False)
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 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)
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 = 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 = get_demand_task_service()
demand_task(task)
# Change the orientation of the camera
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
say("coffee not found at location "+ i)
if coffee_found :
coffee_place = i
say("The coffee is at" + i)
break
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)
