9 from app_manager.msg
import AppList
10 from app_manager.srv
import StartApp
11 from app_manager.srv
import StopApp
12 from std_srvs.srv
import Empty
14 from dialogflow_task_executive.msg
import DialogResponse
18 s1 = re.sub(
'(.)([A-Z][a-z]+)',
r'\1_\2', name)
19 return re.sub(
'([a-z0-9])([A-Z])',
r'\1_\2', s1).lower()
35 'started': on_started,
36 'stopped': on_stopped,
37 'installed': on_installed,
38 'uninstalled': on_uninstalled,
41 ns = rospy.get_param(
"robot/name",
"robot")
43 ns +
"/start_app", StartApp)
45 ns +
"/stop_app", StopApp)
48 self._srv_start_app.wait_for_service(timeout=timeout)
49 self._srv_stop_app.wait_for_service(timeout=timeout)
51 while not rospy.is_shutdown():
54 rospy.loginfo(
"Waiting for apps")
56 rospy.loginfo(
"AppManager initialized")
73 installed = all_apps - last_all
74 for name
in installed:
77 uninstalled = last_all - all_apps
78 for name
in uninstalled:
85 return map(
lambda a: a.name,
86 self._latest_msg.running_apps)
90 return map(
lambda a: a.name,
91 self._latest_msg.available_apps)
95 raise RuntimeError(
"{} is already running".format(name))
97 raise RuntimeError(
"{} is not available".format(name))
100 rospy.loginfo(
"{} successfully started".format(name))
103 raise RuntimeError(
"{0} failed to launch: {1} ({2})".format(
104 name, res.message, res.error_code))
108 raise RuntimeError(
"{} is not running".format(name))
111 rospy.loginfo(
"{} successfully stopped".format(name))
114 raise RuntimeError(
"{0} failed to stop: {1} ({2})".format(
115 name, res.message, res.error_code))
119 REMOVED =
'<REMOVED>' 126 def push(self, element, priority=None):
127 "Add a new element or update the priority of an existing element." 132 entry = [priority, element]
134 heapq.heappush(self.
heap, entry)
137 "Remove and return the highest priority element (1 is the highest)" 139 _, element = heapq.heappop(self.
heap)
140 if element
is not self.
REMOVED:
143 raise ValueError(
'Empty queue')
147 entry = self.mark_removed.pop(element)
151 return len(self.
heap)
160 raise StopIteration()
172 for key, app
in self.action_remappings.items():
173 if app
not in self.app_manager.available_apps:
174 rospy.logwarn(
"Action '{}' is not available".format(app))
178 "dialog_response", DialogResponse,
183 return len(self.app_manager.running_apps) == 0
186 if not msg.action
or msg.action.startswith(
'input.'):
187 rospy.loginfo(
"Action '{}' is ignored".format(msg.action))
194 rospy.loginfo(
"Stop action detected")
195 for app
in self.app_manager.running_apps:
197 self.app_manager.stop_app(app)
202 "Action {} is already executing" 203 .format(self.app_manager.running_apps))
208 if msg.action
in self.action_remappings.values():
214 for app_name
in self.app_manager.available_apps:
215 if action == app_name
or action == app_name.split(
'/')[1]:
218 if action
not in self.app_manager.available_apps:
219 rospy.logerr(
"Action '{}' is unknown".format(action))
222 params = json.loads(msg.parameters)
223 rospy.set_param(
"/action/parameters", params)
226 "Failed to parse parameters of action '{}'".format(msg.action))
229 "Starting '{}' with parameters '{}'" 230 .format(msg.action, msg.parameters))
231 self.app_manager.start_app(action)
234 rospy.loginfo(
"{} started".format(name))
236 stop_idle = rospy.ServiceProxy(
"/idle_behavior/disable", Empty)
237 stop_idle.wait_for_service(1)
239 except rospy.ROSException:
243 rospy.loginfo(
"{} stopped".format(name))
245 start_idle = rospy.ServiceProxy(
"/idle_behavior/enable", Empty)
246 start_idle.wait_for_service(1)
248 except rospy.ROSException:
251 rospy.delete_param(
"/action/parameters")
252 rospy.loginfo(
"Removed /action/parameters")
257 if __name__ ==
'__main__':
258 rospy.init_node(
"task_executive")
def app_stop_cb(self, name)
def start_app(self, name)
def app_start_cb(self, name)
def remove(self, element)
def push(self, element, priority=None)
def _list_apps_cb(self, msg)
def __init__(self, default_priority=5)
def __init__(self, on_started=None, on_stopped=None, on_installed=None, on_uninstalled=None, timeout=30)