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)