app_manager.py
Go to the documentation of this file.
1 # Software License Agreement (BSD License)
2 #
3 # Copyright (c) 2011, Willow Garage, Inc.
4 # All rights reserved.
5 #
6 # Redistribution and use in source and binary forms, with or without
7 # modification, are permitted provided that the following conditions
8 # are met:
9 #
10 # * Redistributions of source code must retain the above copyright
11 # notice, this list of conditions and the following disclaimer.
12 # * Redistributions in binary form must reproduce the above
13 # copyright notice, this list of conditions and the following
14 # disclaimer in the documentation and/or other materials provided
15 # with the distribution.
16 # * Neither the name of Willow Garage, Inc. nor the names of its
17 # contributors may be used to endorse or promote products derived
18 # from this software without specific prior written permission.
19 #
20 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
23 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
24 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
25 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
26 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
27 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
28 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
29 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
30 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31 # POSSIBILITY OF SUCH DAMAGE.
32 #
33 # Revision $Id: app_manager.py 14948 2011-09-07 19:25:54Z pratkanis $
34 
35 # author: leibs
36 
37 import thread
38 import time
39 
40 import rosgraph.names
41 import rospy
42 
43 import roslaunch.parent
44 import roslaunch.pmon
45 
46 from std_srvs.srv import Empty, EmptyResponse
47 
48 from .app import AppDefinition, load_AppDefinition_by_name
49 from .exceptions import LaunchException, AppException, InvalidAppException, NotFoundException
50 from .master_sync import MasterSync
51 from .msg import App, AppList, StatusCodes, AppStatus, AppInstallationState, ExchangeApp
52 from .srv import StartApp, StopApp, ListApps, ListAppsResponse, StartAppResponse, StopAppResponse, InstallApp, UninstallApp, GetInstallationState, UninstallAppResponse, InstallAppResponse, GetInstallationStateResponse, GetAppDetails, GetAppDetailsResponse
53 
54 class AppManager(object):
55 
56  def __init__(self, robot_name, interface_master, app_list, exchange):
57  self._robot_name = robot_name
58  self._interface_master = interface_master
59  self._app_list = app_list
61  self._exchange = exchange
62 
63  rospy.loginfo("Starting app manager for %s"%self._robot_name)
64 
65  self._app_interface = self.scoped_name('application')
66 
67  # note: we publish into the application namespace
68  self._status_pub = rospy.Publisher(
69  self.scoped_name('application/app_status'), AppStatus,
70  latch=True, queue_size=1)
71  self._list_apps_pub = rospy.Publisher(
72  self.scoped_name('app_list'), AppList,
73  latch=True, queue_size=1)
74 
75  self._list_apps_srv = rospy.Service(self.scoped_name('list_apps'), ListApps, self.handle_list_apps)
76  self._start_app_srv = rospy.Service(self.scoped_name('start_app'), StartApp, self.handle_start_app)
77  self._stop_app_srv = rospy.Service(self.scoped_name('stop_app'), StopApp, self.handle_stop_app)
78  self._reload_app_list_srv = rospy.Service(self.scoped_name('reload_app_list'), Empty, self.handle_reload_app_list)
79  if (self._exchange):
80  self._exchange_list_apps_pub = rospy.Publisher(self.scoped_name('exchange_app_list'), AppInstallationState, latch=True)
81  self._list_exchange_apps_srv = rospy.Service(self.scoped_name('list_exchange_apps'), GetInstallationState, self.handle_list_exchange_apps)
82  self._get_app_details_srv = rospy.Service(self.scoped_name('get_app_details'), GetAppDetails, self.handle_get_app_details)
83  self._install_app_srv = rospy.Service(self.scoped_name('install_app'), InstallApp, self.handle_install_app)
84  self._uninstall_app_srv = rospy.Service(self.scoped_name('uninstall_app'), UninstallApp, self.handle_uninstall_app)
85 
86  pub_names = [x.resolved_name for x in [self._list_apps_pub, self._status_pub, self._exchange_list_apps_pub]]
87  service_names = [x.resolved_name for x in [self._list_apps_srv, self._start_app_srv, self._stop_app_srv, self._get_app_details_srv, self._list_exchange_apps_srv, self._install_app_srv, self._uninstall_app_srv]]
88  else:
89  pub_names = [x.resolved_name for x in [self._list_apps_pub, self._status_pub]]
90  service_names = [x.resolved_name for x in [self._list_apps_srv, self._start_app_srv, self._stop_app_srv]]
91 
92  self._api_sync = MasterSync(self._interface_master,
93  local_service_names=service_names,
94  local_pub_names=pub_names)
95 
96  self._launch = None
97  self._interface_sync = None
98 
99  roslaunch.pmon._init_signal_handlers()
100 
101  if (self._exchange):
102  self._exchange.update_local()
103 
104  self._app_list.update()
106  self.publish_list_apps()
107 
108  def shutdown(self):
109  if self._api_sync:
110  self._api_sync.stop()
111  if self._launch:
112  self._launch.shutdown()
113  self._interface_sync.stop()
114 
115  def _get_current_app(self):
116  return self._current_app
117 
118  def _set_current_app(self, app, app_definition):
119  self._current_app = app
120  self._current_app_definition = app_definition
121 
122  if self._list_apps_pub:
123  if app is not None:
124  self._list_apps_pub.publish([app], self._app_list.get_app_list())
125  else:
126  self._list_apps_pub.publish([], self._app_list.get_app_list())
127 
128  def scoped_name(self, name):
129  return rosgraph.names.canonicalize_name('/%s/%s'%(self._robot_name, rospy.remap_name(name)))
130 
131  def handle_get_app_details(self, req):
132  return GetAppDetailsResponse(app=self._exchange.get_app_details(req.name))
133 
135  if (self._exchange == None):
136  return None
137  if (req.remote_update):
138  print "UPDATE"
139  if (not self._exchange.update()):
140  return None
141  i_apps = self._exchange.get_installed_apps()
142  a_apps = self._exchange.get_available_apps()
143  return GetInstallationStateResponse(installed_apps=i_apps, available_apps=a_apps)
144 
145  def publish_list_apps(self):
146  if self._current_app:
147  self._list_apps_pub.publish([self._current_app], self._app_list.get_app_list())
148  else:
149  self._list_apps_pub.publish([], self._app_list.get_app_list())
150 
152  if (self._exchange == None):
153  return
154  i_apps = self._exchange.get_installed_apps()
155  a_apps = self._exchange.get_available_apps()
156  self._exchange_list_apps_pub.publish(i_apps, a_apps)
157 
158  def handle_install_app(self, req):
159  appname = req.name
160  if (self._exchange.install_app(appname)):
161  self._app_list.update()
162  self.publish_list_apps()
164  return InstallAppResponse(installed=True, message="app [%s] installed"%(appname))
165  else:
166  return InstallAppResponse(installed=False, message="app [%s] could not be installed"%(appname))
167 
168  def handle_uninstall_app(self, req):
169  appname = req.name
170  if (self._exchange.uninstall_app(appname)):
171  self._app_list.update()
172  self.publish_list_apps()
174  return UninstallAppResponse(uninstalled=True, message="app [%s] uninstalled"%(appname))
175  else:
176  return UninstallAppResponse(uninstalled=False, message="app [%s] could not be uninstalled"%(appname))
177 
178  def handle_list_apps(self, req):
179  rospy.loginfo("Listing apps")
180  current = self._current_app
181  if current:
182  running_apps = [current]
183  else:
184  running_apps = []
185  self._app_list.update()
186  rospy.loginfo("done listing apps")
187  return ListAppsResponse(running_apps=running_apps, available_apps=self._app_list.get_app_list())
188 
189  def handle_start_app(self, req):
190  rospy.loginfo("start_app: %s"%(req.name))
191  if self._current_app:
192  if self._current_app_definition.name == req.name:
193  return StartAppResponse(started=True, message="app [%s] already started"%(req.name), namespace=self._app_interface)
194  else:
195  self.stop_app(self._current_app_definition.name)
196  #return StartAppResponse(started=False, message="Please stop the running app before starting another app.", error_code=StatusCodes.MULTIAPP_NOT_SUPPORTED)
197 
198  # TODO: the app list has already loaded the App data. We should use that instead for consistency
199 
200  appname = req.name
201  rospy.loginfo("Loading app: %s"%(appname))
202  try:
203  app = load_AppDefinition_by_name(appname)
204  except ValueError as e:
205  return StartAppResponse(started=False, message=str(e), error_code=StatusCodes.BAD_REQUEST)
206  except InvalidAppException as e:
207  return StartAppResponse(started=False, message=str(e), error_code=StatusCodes.INTERNAL_ERROR)
208  except NotFoundException as e:
209  return StartAppResponse(started=False, message=str(e), error_code=StatusCodes.NOT_FOUND)
210 
211  try:
212  self._set_current_app(App(name=appname), app)
213 
214  rospy.loginfo("Launching: %s"%(app.launch))
215  self._status_pub.publish(AppStatus(AppStatus.INFO, 'launching %s'%(app.display_name)))
216 
217  #TODO:XXX This is a roslaunch-caller-like abomination. Should leverage a true roslaunch API when it exists.
218  self._launch = roslaunch.parent.ROSLaunchParent(rospy.get_param("/run_id"),
219  [app.launch], is_core=False,
220  process_listeners=())
221  self._launch._load_config()
222 
223  #TODO: convert to method
224  for N in self._launch.config.nodes:
225  for t in app.interface.published_topics.keys():
226  N.remap_args.append((t, self._app_interface + '/' + t))
227  for t in app.interface.subscribed_topics.keys():
228  N.remap_args.append((t, self._app_interface + '/' + t))
229  self._launch.start()
230 
231  fp = [self._app_interface + '/' + x for x in app.interface.subscribed_topics.keys()]
232  lp = [self._app_interface + '/' + x for x in app.interface.published_topics.keys()]
233 
234  self._interface_sync = MasterSync(self._interface_master, foreign_pub_names=fp, local_pub_names=lp)
235 
236  thread.start_new_thread(self.app_monitor,())
237 
238  return StartAppResponse(started=True, message="app [%s] started"%(appname), namespace=self._app_interface)
239 
240  except Exception as e:
241  try:
242  # attempt to kill any launched resources
243  self._stop_current()
244  except:
245  pass
246  self._status_pub.publish(AppStatus(AppStatus.INFO, 'app start failed'))
247  rospy.logerr("app start failed")
248  return StartAppResponse(started=False, message="internal error [%s]"%(str(e)), error_code=StatusCodes.INTERNAL_ERROR)
249 
250  def _stop_current(self):
251  try:
252  self._launch.shutdown()
253  finally:
254  self._launch = None
255  try:
256  self._interface_sync.stop()
257  finally:
258  self._interface_sync = None
259 
260  def handle_stop_app(self, req):
261  rospy.loginfo("handle stop app: %s"%(req.name))
262  return self.stop_app(req.name)
263 
264  def handle_reload_app_list(self, req=None):
265  try:
266  self._app_list.update()
267  self.publish_list_apps()
269  rospy.loginfo("app list is reloaded")
270  except Exception as e:
271  rospy.logerr("Failed to reload app list: %s" % e)
272  return EmptyResponse()
273 
274  def app_monitor(self):
275  while self._launch:
276  time.sleep(0.1)
277  launch = self._launch
278  if launch:
279  pm = launch.pm
280  if pm:
281  if pm.done:
282  time.sleep(1.0)
283  self.stop_app(self._current_app_definition.name)
284  break
285 
286 
287 
288  def stop_app(self, appname):
289  resp = StopAppResponse(stopped=False)
290  try:
291  app = self._current_app_definition
292 
293  # request to stop all apps.
294  if app is not None and appname == '*':
295  appname = app.name
296 
297  if app is None or app.name != appname:
298  rospy.loginfo("handle stop app: app [%s] is not running [x]"%(appname))
299  resp.error_code = StatusCodes.NOT_RUNNING
300  resp.message = "app %s is not running"%(appname)
301  else:
302  try:
303  if self._launch:
304  rospy.loginfo("handle stop app: stopping app [%s]"%(appname))
305  self._status_pub.publish(AppStatus(AppStatus.INFO, 'stopping %s'%(app.display_name)))
306  self._stop_current()
307  rospy.loginfo("handle stop app: app [%s] stopped"%(appname))
308  resp.stopped = True
309  resp.message = "%s stopped"%(appname)
310  else:
311  rospy.loginfo("handle stop app: app [%s] is not running"%(appname))
312  resp.message = "app [%s] is not running"%(appname)
313  resp.error_code = StatusCodes.NOT_RUNNING
314  finally:
315  self._launch = None
316  self._set_current_app(None, None)
317 
318  except Exception as e:
319  rospy.logerr("handle stop app: internal error %s"%(e))
320  resp.error_code = StatusCodes.INTERNAL_ERROR
321  resp.message = "internal error: %s"%(str(e))
322 
323  return resp
def handle_list_exchange_apps(self, req)
Definition: app_manager.py:134
def _set_current_app(self, app, app_definition)
Definition: app_manager.py:118
def __init__(self, robot_name, interface_master, app_list, exchange)
Definition: app_manager.py:56
def handle_reload_app_list(self, req=None)
Definition: app_manager.py:264
def load_AppDefinition_by_name(appname)
Definition: app.py:249


app_manager
Author(s): Jeremy Leibs, Ken Conley, Yuki Furuta
autogenerated on Tue Apr 2 2019 02:58:24