00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 import roslib.names
00038 import rospy
00039
00040 import roslaunch.parent
00041 import roslaunch.pmon
00042
00043 from .app import AppDefinition, load_AppDefinition_by_name
00044 from .exceptions import LaunchException, AppException, InvalidAppException, NotFoundException
00045 from .master_sync import MasterSync
00046 from .msg import App, AppList, StatusCodes, AppStatus
00047 from .srv import StartApp, StopApp, ListApps, ListAppsResponse, StartAppResponse, StopAppResponse
00048
00049 class AppManager(object):
00050
00051 def __init__(self, robot_name, interface_master, app_list):
00052 self._robot_name = robot_name
00053 self._interface_master = interface_master
00054 self._app_list = app_list
00055 self._current_app = self._current_app_definition = None
00056
00057 rospy.loginfo("Starting app manager for %s"%self._robot_name)
00058
00059 self._app_interface = self.scoped_name('application')
00060
00061
00062 self._status_pub = rospy.Publisher(self.scoped_name('application/app_status'), AppStatus, latch=True)
00063 self._list_apps_pub = rospy.Publisher(self.scoped_name('app_list'), AppList, latch=True)
00064
00065 self._list_apps_srv = rospy.Service(self.scoped_name('list_apps'), ListApps, self.handle_list_apps)
00066 self._start_app_srv = rospy.Service(self.scoped_name('start_app'), StartApp, self.handle_start_app)
00067 self._stop_app_srv = rospy.Service(self.scoped_name('stop_app'), StopApp, self.handle_stop_app)
00068
00069 pub_names = [x.resolved_name for x in [self._list_apps_pub, self._status_pub]]
00070 service_names = [x.resolved_name for x in [self._list_apps_srv, self._start_app_srv, self._stop_app_srv]]
00071
00072 self._api_sync = MasterSync(self._interface_master,
00073 local_service_names=service_names,
00074 local_pub_names=pub_names)
00075
00076 self._launch = None
00077 self._interface_sync = None
00078
00079 roslaunch.pmon._init_signal_handlers()
00080
00081 self._app_list.update()
00082 self._list_apps_pub.publish([], self._app_list.get_app_list())
00083
00084 def shutdown(self):
00085 if self._api_sync:
00086 self._api_sync.stop()
00087 if self._launch:
00088 self._launch.shutdown()
00089 self._interface_sync.stop()
00090
00091 def _get_current_app(self):
00092 return self._current_app
00093
00094 def _set_current_app(self, app, app_definition):
00095 self._current_app = app
00096 self._current_app_definition = app_definition
00097
00098 if self._list_apps_pub:
00099 if app is not None:
00100 self._list_apps_pub.publish([app], self._app_list.get_app_list())
00101 else:
00102 self._list_apps_pub.publish([], self._app_list.get_app_list())
00103
00104 def scoped_name(self, name):
00105 return roslib.names.canonicalize_name('/%s/%s'%(self._robot_name, rospy.remap_name(name)))
00106
00107 def handle_list_apps(self, req):
00108 rospy.loginfo("Listing apps")
00109 current = self._current_app
00110 if current:
00111 running_apps = [current]
00112 else:
00113 running_apps = []
00114 self._app_list.update()
00115 rospy.loginfo("done listing apps")
00116 return ListAppsResponse(running_apps=running_apps, available_apps=self._app_list.get_app_list())
00117
00118 def handle_start_app(self, req):
00119 rospy.loginfo("start_app: %s"%(req.name))
00120 if self._current_app:
00121 self.stop_app(self._current_app_definition.name)
00122
00123
00124
00125
00126 appname = req.name
00127 rospy.loginfo("Loading app: %s"%(appname))
00128 try:
00129 app = load_AppDefinition_by_name(appname)
00130 except ValueError as e:
00131 return StartAppResponse(started=False, message=str(e), error_code=StatusCodes.BAD_REQUEST)
00132 except InvalidAppException as e:
00133 return StartAppResponse(started=False, message=str(e), error_code=StatusCodes.INTERNAL_ERROR)
00134 except NotFoundException as e:
00135 return StartAppResponse(started=False, message=str(e), error_code=StatusCodes.NOT_FOUND)
00136
00137 try:
00138 self._set_current_app(App(name=appname), app)
00139
00140 rospy.loginfo("Launching: %s"%(app.launch))
00141 self._status_pub.publish(AppStatus(AppStatus.INFO, 'launching %s'%(app.display_name)))
00142
00143
00144 self._launch = roslaunch.parent.ROSLaunchParent(rospy.get_param("/run_id"),
00145 [app.launch], is_core=False,
00146 process_listeners=())
00147 self._launch._load_config()
00148
00149
00150 for N in self._launch.config.nodes:
00151 for t in app.interface.published_topics.keys():
00152 N.remap_args.append((t, self._app_interface + '/' + t))
00153 for t in app.interface.subscribed_topics.keys():
00154 N.remap_args.append((t, self._app_interface + '/' + t))
00155 self._launch.start()
00156
00157 fp = [self._app_interface + '/' + x for x in app.interface.subscribed_topics.keys()]
00158 lp = [self._app_interface + '/' + x for x in app.interface.published_topics.keys()]
00159
00160 self._interface_sync = MasterSync(self._interface_master, foreign_pub_names=fp, local_pub_names=lp)
00161
00162 return StartAppResponse(started=True, message="app [%s] started"%(appname), namespace=self._app_interface)
00163
00164 except Exception as e:
00165 try:
00166
00167 self._stop_current()
00168 except:
00169 pass
00170 self._status_pub.publish(AppStatus(AppStatus.INFO, 'app start failed'))
00171 rospy.logerr("app start failed")
00172 return StartAppResponse(started=False, message="internal error [%s]"%(str(e)), error_code=StatusCodes.INTERNAL_ERROR)
00173
00174 def _stop_current(self):
00175 try:
00176 self._launch.shutdown()
00177 finally:
00178 self._launch = None
00179 try:
00180 self._interface_sync.stop()
00181 finally:
00182 self._interface_sync = None
00183
00184 def handle_stop_app(self, req):
00185 rospy.logerr("handle stop app: %s"%(req.name))
00186 return self.stop_app(req.name)
00187
00188 def stop_app(self, appname):
00189 resp = StopAppResponse(stopped=False)
00190 try:
00191 app = self._current_app_definition
00192
00193
00194 if app is not None and appname == '*':
00195 appname = app.name
00196
00197 if app is None or app.name != appname:
00198 rospy.loginfo("handle stop app: app [%s] is not running [x]"%(appname))
00199 resp.error_code = StatusCodes.NOT_RUNNING
00200 resp.message = "app %s is not running"%(appname)
00201 else:
00202 try:
00203 if self._launch:
00204 rospy.loginfo("handle stop app: stopping app [%s]"%(appname))
00205 self._status_pub.publish(AppStatus(AppStatus.INFO, 'stopping %s'%(app.display_name)))
00206 self._stop_current()
00207 rospy.loginfo("handle stop app: app [%s] stopped"%(appname))
00208 resp.stopped = True
00209 resp.message = "%s stopped"%(appname)
00210 else:
00211 rospy.loginfo("handle stop app: app [%s] is not running"%(appname))
00212 resp.message = "app [%s] is not running"%(appname)
00213 resp.error_code = StatusCodes.NOT_RUNNING
00214 finally:
00215 self._launch = None
00216 self._set_current_app(None, None)
00217
00218 except Exception as e:
00219 rospy.logerr("handle stop app: internal error %s"%(e))
00220 resp.error_code = StatusCodes.INTERNAL_ERROR
00221 resp.message = "internal error: %s"%(str(e))
00222
00223 return resp
