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 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 move_to(pose):
task = Task()
task.action = '/move_base'
xin = pose.pose.position.x
y = pose.pose.position.y
z = pose.pose.position.z
x = xin - 1
poseout = pose
poseout.pose.position.x = x
object_id = msg_store.insert(poseout)
task_utils.add_object_id_argument(task, object_id, PoseStamped)
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 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 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_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_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_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_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 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 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_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 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 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(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 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 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 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 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 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 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 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 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 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 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 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 __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 = '/speak' task.max_duration = rospy.Duration(15) #task.start_node_id = waypoints[i] #task.end_node_id = waypoints[i] task_utils.add_string_argument(task, 'Hello!') 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_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 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 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 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 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 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)
