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 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 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_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 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 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_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_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 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 __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)
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 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)
rospy.init_node("metric_map_task_client") # need message store to pass objects around msg_store = MessageStoreProxy() try: task = Task(start_node_id=sys.argv[1], action='ptu_pan_tilt_metric_map') task_utils.add_int_argument(task, '-160') task_utils.add_int_argument(task, '20') task_utils.add_int_argument(task, '160') task_utils.add_int_argument(task, '-30') task_utils.add_int_argument(task, '30') task_utils.add_int_argument(task, '30') task.max_duration = rospy.Duration(60 * 4) print task # now register this with the executor if len(sys.argv) > 2 and sys.argv[2] == "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' 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.loginfo("Done") add_task_srv = rospy.ServiceProxy(add_task_srv_name, AddTask)
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 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
add_tasks_srv_name = '/task_executor/add_tasks' set_exe_stat_srv_name = '/task_executor/set_execution_status' rospy.loginfo("Waiting for task_executor service...") rospy.wait_for_service(add_tasks_srv_name) rospy.wait_for_service(set_exe_stat_srv_name) rospy.loginfo("Done") add_tasks_srv = rospy.ServiceProxy(add_tasks_srv_name, AddTasks) set_execution_status = rospy.ServiceProxy(set_exe_stat_srv_name, SetExecutionStatus) return add_tasks_srv, set_execution_status if __name__ == '__main__': rospy.init_node("example_multi_add_client") # get services to call into execution framework add_tasks, set_execution_status = get_services() msg_store = MessageStoreProxy() task = Task() task.action = '/go_to_person_action' duration = 5 * 60 task.max_duration = rospy.Duration(duration) object_id = msg_store.insert(PoseStamped()) task_utils.add_object_id_argument(task, object_id, PoseStamped) task_utils.add_bool_argument(task, False) task_utils.add_float_argument(task, duration) add_tasks([task]) set_execution_status(True)