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
示例#2
0
 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
示例#6
0
    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
示例#7
0
    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
示例#8
0
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
示例#9
0
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)
示例#10
0
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
示例#11
0
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) 
示例#12
0
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)
示例#16
0
 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)