def create_routine(self):
# Add task to routine where the duration
# to follow human is 300 seconds.
# Choose a waypoint which is the nearest one to the wait point
# which is set in conf/default.yaml
following_task = Task(start_node_id=self.waypoint,
max_duration=rospy.Duration(300),
action='simple_follow')
# The argument for the action, how long
# following action should be performed
task_utils.add_int_argument(following_task, 270)
rospy.loginfo("Creating strands human following routine...")
# Example to run human following the whole day several times
# today = (self.start, self.end)
# self.routine.repeat_every(patrol_routine, *today, times=5)
# Example to run human following every hour
# (can be several times)
self.routine.repeat_every_hour(following_task, hours=1, times=1)
# pass the routine tasks on to the runner which handles
# the daily instantiation of actual tasks
self.start_routine()
def create_har_observation_tasks(duration=rospy.Duration(30 * 60)):
topological_nodes = getNodes()
tables = getTables()
tablenodepairs = []
for table in tables:
table_position = [
table.pose.pose.position.x, table.pose.pose.position.y
]
distances = []
for topological_node in topological_nodes:
node_position = [
topological_node.pose.position.x,
topological_node.pose.position.y
]
distances.append(distance(table_position, node_position))
minindex = distances.index(min(distances))
tablenodepairs.append((table_position, topological_nodes[minindex]))
tasks = []
for pair in tablenodepairs:
task = Task(start_node_id=pair[1].name,
end_node_id=pair[1].name,
action='observe_har',
max_duration=duration)
task_utils.add_float_argument(task, pair[0][0])
task_utils.add_float_argument(task, pair[0][1])
task_utils.add_float_argument(task, 1.0)
tasks.append(task)
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_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_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 send_tasks(self):
rospy.loginfo("Sending next batch of patrol tasks")
patrol_tasks = [
Task(start_node_id=wp, end_node_id=wp) for wp in self.waypoints
]
self.add_tasks_srv(patrol_tasks)
def add_tasks(self, tasks_to_add):
#print self.bidder.available_tokens
#print self.bidder.currently_bid_tokens
for i in tasks_to_add:
wp1=i['origin']
wp2=i['goal']#+'"'
task=Task(action= i['action'],start_node_id=wp1,end_node_id=wp2,start_after=self.timeslot_start,end_before=self.timeslot_end,max_duration=rospy.Duration(2*60))
bid = i['tokens']
self.bidder.add_task_bid(task, bid)
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_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 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 demand_task(self, pose):
task = Task()
task.action = '/go_to_person_action'
duration = 5 * 60
task.max_duration = rospy.Duration(duration)
object_id = self.msg_store_tmp.insert(pose)
task_utils.add_object_id_argument(task, object_id, PoseStamped)
task_utils.add_bool_argument(task, True)
task_utils.add_float_argument(task, duration)
self.demand_task_srv(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_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_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 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 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 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
def add_task(self, timer_event):
print self.bidder.available_tokens
print self.bidder.currently_bid_tokens
task = Task(action='do_sweep',
start_node_id='CorpLocker6',
end_node_id='CorpLocker6',
start_after=rospy.get_rostime(),
end_before=rospy.get_rostime() + rospy.Duration(60 * 1),
max_duration=rospy.Duration(1 * 60))
bid = int(
ceil(0.1 * (self.bidder.available_tokens -
self.bidder.currently_bid_tokens)))
self.bidder.add_task_bid(task, bid)
def demand_task(self, req):
t = self.create_task(req)
rospy.loginfo('demand task %s' % t)
service_name = '/task_executor/demand_task'
try:
rospy.wait_for_service(service_name, timeout=10)
dt = rospy.ServiceProxy(service_name, ExecDemandTask)
rospy.loginfo(dt(t))
except Exception as e:
rospy.logerr("Couldn't demand task on scheduler. "
"error: %s." % str(e))
t = Task()
t.action = req.action
return DemandTaskResponse()
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 add_task(self, req):
t = self.create_task(req)
tasks = [t]
rospy.loginfo('add task %s to schedule now' % t)
service_name = '/task_executor/add_tasks'
try:
rospy.wait_for_service(service_name, timeout=10)
dt = rospy.ServiceProxy(service_name, AddTasks)
rospy.loginfo(dt(tasks))
except Exception as e:
rospy.logerr("Couldn't add task to scheduler. "
"error: %s." % str(e))
t = Task()
t.action = req.action
return DemandTaskResponse()
def create_tasks_for_get_edge_exploration(self, definitions, scores,
start_time, end_time):
res = []
for (edge, score) in zip(definitions, scores):
[start_wp, end_wp] = edge.split("_")
task = Task(action='(F ("' + start_wp + '" & (X "' + end_wp +
'")))',
start_after=start_time,
end_before=end_time,
max_duration=rospy.Duration(1))
res.append(
ExplorationTask(task_type="edge_exploration",
score=score,
task_def=task))
return res
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 create(self, req):
"""
implement the factory service. Overwrite to provide a
task specific factory method. Idea is that this function
return a fully instantiated task object with all parameters
set so that the task can be submitted to the scheduler without
requiring the routine (or other component that submits the task)
does not need to know about the internal parameters of the task
itself.
:param req.yaml: the yaml specification of the task. This can be
underspecified. All slots in the yaml are transferred into
task object for slots that exist in there.
"""
task = Task()
task.action = self.name
if len(req.yaml) > 0:
self._fill_slots(load(req.yaml), task)
return task
def cancel_task_ros_srv(self, req):
""" Cancel the speficially requested task """
self.service_lock.acquire()
cancelled = False
if self.is_in_active_tasks(req.task_id):
self.log_task_events(self.active_tasks, TaskEvent.CANCELLED_MANUALLY, rospy.get_rostime())
self.cancel_active_task()
cancelled = True
else:
cancelled = self.cancel_task(req.task_id)
if cancelled:
self.log_task_event(Task(task_id=req.task_id), TaskEvent.CANCELLED_MANUALLY, rospy.get_rostime())
self.service_lock.release()
return cancelled
def demand_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.logerr("Couldn't instantiate task from factory %s."
"error: %s."
"This is an error." % (factory_name, str(e)))
raise
# 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
tasks = [t]
rospy.loginfo('add task %s to schedule now' % t)
dt = rospy.ServiceProxy('/task_executor/add_tasks', AddTasks)
try:
rospy.loginfo(dt(tasks))
except Exception as e:
rospy.logerr("Couldn't add task to scheduler. "
"error: %s." % str(e))
t = Task()
t.action = req.action
return DemandTaskResponse()
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 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...")
