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 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_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(self, req): t = super(TestServer, self).create(req) task_utils.add_time_argument(t, rospy.Time()) if self.maximise_duration_delta > rospy.Duration(0): d = (t.end_before - t.start_after) - self.maximise_duration_delta task_utils.add_duration_argument(t, d) t.max_duration = d else: task_utils.add_duration_argument(t, t.max_duration) return t
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 create(self, req): task = super(StoreLogsServer, self).create(req) if task.start_node_id == "": task.start_node_id = "ChargingPoint" if task.end_node_id == "": task.end_node_id = task.start_node_id if task.max_duration.secs == 0: task.max_duration = task.end_before - task.start_after if task.priority == 0: task.priority = 5 # make sure we do this. task_utils.add_time_argument(task, task.end_before) return task
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 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 findcoffee(req): task.action = '/find_coffee' task.max_duration = rospy.Duration(500) 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() rospy.logwarn("Find action triggered") return std_srvs.srv.TriggerResponse(True,'findcoffee');
def create_wait_task(waypoint_name, duration=rospy.Duration(30)): patrol_task = Task(action='wait_action', start_node_id=waypoint_name, end_node_id=waypoint_name, max_duration=duration) task_utils.add_time_argument(patrol_task, rospy.Time()) task_utils.add_duration_argument(patrol_task, duration) return patrol_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) 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 = 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 = get_demand_task_service() demand_task(task) # Change the orientation of the camera 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 say("coffee not found at location "+ i) if coffee_found : coffee_place = i say("The coffee is at" + i) break 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)
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)