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 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(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 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 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 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 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(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 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 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 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)
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)
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