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
