def create_wait_task(node, secs=rospy.Duration(5)): wait_task = Task(action='wait_action',start_node_id=node, end_node_id=node, max_duration=secs) task_utils.add_time_argument(wait_task, rospy.Time()) task_utils.add_duration_argument(wait_task, secs) wait_task.start_after = rospy.get_rostime() + rospy.Duration(2) wait_task.end_before = rospy.get_rostime() + rospy.Duration(120) return wait_task
def create_wait_task(node, secs=rospy.Duration(1), start_after=None, window_size=rospy.Duration(3600)): if start_after is None: start_after = rospy.get_rostime() wait_task = Task(action='wait_action',start_node_id=node, end_node_id=node, max_duration=secs) wait_task.start_after = start_after wait_task.end_before = wait_task.start_after + window_size task_utils.add_time_argument(wait_task, rospy.Time()) task_utils.add_duration_argument(wait_task, secs) return wait_task
def 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 wait_task_at_waypoint(wp): task = Task() task.action = '/wait_action' duration_time = 10 wait_time = 0 max_wait_seconds = 60*5 task.max_duration = rospy.Duration(max_wait_seconds) task_utils.add_time_argument(task, rospy.Time()) task_utils.add_duration_argument(task, rospy.Duration(duration_time)) task.start_after = rospy.get_rostime() + rospy.Duration(wait_time) task.end_before = task.start_after + rospy.Duration(task.max_duration.to_sec()) task.start_node_id = wp task.end_node_id = wp return task
def create_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.logwarn("Couldn't instantiate task from factory %s." "error: %s." "This is an error." % (factory_name, str(e))) t = Task() t.start_node_id = req.waypoint t.end_node_id = req.waypoint t.end_before = end_before t.start_after = start_after t.action = req.action # 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 return t
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)
if rospy.get_param('use_sim_time', False): rospy.loginfo('Using sim time, waiting for time update') rospy.wait_for_message('/clock', Clock) # get services to call into execution framework add_task, set_execution_status = get_services() print 'Requesting wait at %s for %s seconds' % (sys.argv[1], sys.argv[2]) max_duration = rospy.Duration(int(sys.argv[2])) wait_task = Task(action='wait_action',start_node_id=sys.argv[1], max_duration=rospy.Duration(10)) wait_task.start_after = rospy.get_rostime() wait_task.end_before = wait_task.start_after + max_duration + max_duration + rospy.Duration(1200) task_utils.add_time_argument(wait_task, rospy.Time()) task_utils.add_duration_argument(wait_task, max_duration) # wait_task = Task(action='',start_node_id=sys.argv[1], max_duration=max_duration) task_id = add_task([wait_task]) # Set the task executor running (if it isn't already) set_execution_status(True) # now let's stop execution while it's going on # rospy.sleep(int(sys.argv[2])/2) # set_execution_status(False)
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 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
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
return add_tasks_srv, set_execution_status if __name__ == '__main__': rospy.init_node("example_add_time_critical_client") # get services to call into execution framework add_task, set_execution_status = get_services() wp = 'WayPoint2' duration_secs = 30 wait_before = rospy.Duration(30) max_duration = rospy.Duration(duration_secs) wait_task = Task(action='wait_action', start_node_id=wp, max_duration=max_duration) wait_task.start_after = rospy.get_rostime() + wait_before wait_task.end_before = wait_task.start_after task_utils.add_time_argument(wait_task, rospy.Time()) task_utils.add_duration_argument(wait_task, max_duration) task_id = add_task([wait_task]) # Set the task executor running (if it isn't already) set_execution_status(True) # now let's stop execution while it's going on # rospy.sleep(int(sys.argv[2])/2) # set_execution_status(False)