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