Package node_manager_fkie :: Module master_view_proxy
[frames] | no frames]

Source Code for Module node_manager_fkie.master_view_proxy

   1  # Software License Agreement (BSD License)
 
   2  #
 
   3  # Copyright (c) 2012, Fraunhofer FKIE/US, Alexander Tiderko
 
   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 Fraunhofer 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  from python_qt_binding import QtGui 
  34  from python_qt_binding import QtCore 
  35  from python_qt_binding import loadUi 
  36  
 
  37  import re 
  38  import os 
  39  import sys 
  40  import socket 
  41  import xmlrpclib 
  42  import threading 
  43  import time 
  44  import uuid 
  45  import getpass 
  46  
 
  47  from urlparse import urlparse 
  48  
 
  49  import rospy 
  50  import roslib 
  51  from rosgraph.names import is_legal_name 
  52  
 
  53  
 
  54  import node_manager_fkie as nm 
  55  from html_delegate import HTMLDelegate 
  56  from topic_list_model import TopicModel, TopicItem 
  57  from node_tree_model import NodeTreeModel, NodeItem, GroupItem, HostItem 
  58  from service_list_model import ServiceModel, ServiceItem 
  59  from parameter_list_model import ParameterModel, ParameterNameItem, ParameterValueItem 
  60  from default_cfg_handler import DefaultConfigHandler 
  61  from launch_config import LaunchConfig, LaunchConfigException 
  62  from master_discovery_fkie.master_info import NodeInfo  
  63  from parameter_dialog import ParameterDialog, MasterParameterDialog, ServiceDialog 
  64  from select_dialog import SelectDialog 
  65  from echo_dialog import EchoDialog 
  66  from parameter_handler import ParameterHandler 
  67  from detailed_msg_box import WarningMessageBox, DetailedError 
  68  from progress_queue import ProgressQueue, ProgressThread, InteractionNeededError 
  69  from common import masteruri_from_ros, get_packages, package_name 
  70  from launch_server_handler import LaunchServerHandler 
71 72 73 74 -class LaunchArgsSelectionRequest(Exception):
75 ''' Request needed to set the args of a launchfile from another thread. 76 @param args: a dictionary with args and values 77 @type args: dict 78 @param error: an error description 79 @type error: str 80 '''
81 - def __init__(self, launchfile, args, error):
82 Exception.__init__(self) 83 self.launchfile = launchfile 84 self.args_dict = args 85 self.error = error
86
87 - def __str__(self):
88 return "LaunchArgsSelectionRequest for " + str(self.args_dict) + "::" + repr(self.error)
89
90 91 -class MasterViewProxy(QtGui.QWidget):
92 ''' 93 This class stores the informations about a ROS master and shows it on request. 94 ''' 95 96 updateHostRequest = QtCore.Signal(str) 97 host_description_updated = QtCore.Signal(str, str, str) 98 '''@ivar: the signal is emitted on description changes and contains the 99 ROS Master URI, host address and description a parameter.''' 100 101 capabilities_update_signal = QtCore.Signal(str, str, str, list) 102 '''@ivar: the signal is emitted if a description with capabilities is received 103 and has the ROS master URI, host address, the name of the default_cfg node and a list with 104 descriptions (L{default_cfg_fkie.Description}) as parameter.''' 105 remove_config_signal = QtCore.Signal(str) 106 '''@ivar: the signal is emitted if a default_cfg was removed''' 107 108 description_signal = QtCore.Signal(str, str) 109 '''@ivar: the signal is emitted to show a description (title, description)''' 110 111 request_xml_editor = QtCore.Signal(list, str) 112 '''@ivar: the signal to open a xml editor dialog (list with files, search text)''' 113 114 stop_nodes_signal = QtCore.Signal(str, list) 115 '''@ivar: the signal is emitted to stop on masteruri the nodes described in the list.''' 116 117 robot_icon_updated = QtCore.Signal(str, str) 118 '''@ivar: the signal is emitted, if the robot icon was changed by a configuration (masteruri, path)''' 119 120 loaded_config = QtCore.Signal(str, object) 121 '''@ivar: the signal is emitted, after a launchfile is successful loaded (launchfile, LaunchConfig)''' 122
123 - def __init__(self, masteruri, parent=None):
124 ''' 125 Creates a new master. 126 @param masteruri: the URI of the ROS master 127 @type masteruri: C{str} 128 ''' 129 QtGui.QWidget.__init__(self, parent) 130 self.setObjectName(' - '.join(['MasterViewProxy', masteruri])) 131 self.masteruri = masteruri 132 self.mastername = masteruri 133 try: 134 o = urlparse(self.masteruri) 135 self.mastername = o.hostname 136 except: 137 pass 138 self.__current_path = os.path.expanduser('~') 139 140 self._tmpObjects = [] 141 self.__master_state = None 142 self.__master_info = None 143 self.__force_update = False 144 self.__configs = dict() # [file name] = LaunchConfig 145 # self.rosconfigs = dict() # [launch file path] = LaunchConfig() 146 self.__in_question = [] # stores the changed files, until the user is interacted 147 # self.__uses_confgs = dict() # stores the decisions of the user for used configuration to start of node 148 '''@ivar: stored the question dialogs for changed files ''' 149 self._stop_ignores = ['rosout', rospy.get_name(), 'node_manager', 'master_discovery', 'master_sync', 'default_cfg', 'zeroconf'] 150 ''' @todo: detect the names of master_discovery and master_sync ndoes''' 151 152 self.__echo_topics_dialogs = dict() # [topic name] = EchoDialog 153 '''@ivar: stores the open EchoDialogs ''' 154 self.__last_info_type = None # {Node, Topic, Service} 155 self.__last_info_text = None 156 self.__use_sim_time = False 157 self.__current_user = nm.settings().default_user 158 self.__robot_icons = [] 159 self.__current_robot_icon = None 160 self.__current_parameter_robot_icon = '' 161 # store the running_nodes to update to duplicates after load a launch file 162 self.__running_nodes = dict() # dict (node name : masteruri) 163 self.default_cfg_handler = DefaultConfigHandler() 164 self.default_cfg_handler.node_list_signal.connect(self.on_default_cfg_nodes_retrieved) 165 self.default_cfg_handler.description_signal.connect(self.on_default_cfg_descr_retrieved) 166 self.default_cfg_handler.err_signal.connect(self.on_default_cfg_err) 167 168 self.__launch_servers = {} # uri : (pid, nodes) 169 self.launch_server_handler = LaunchServerHandler() 170 self.launch_server_handler.launch_server_signal.connect(self.on_launch_server_retrieved) 171 self.launch_server_handler.error_signal.connect(self.on_launch_server_err) 172 173 self.masterTab = QtGui.QWidget() 174 ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)), 'MasterTab.ui') 175 loadUi(ui_file, self.masterTab) 176 tabLayout = QtGui.QVBoxLayout(self) 177 tabLayout.setContentsMargins(0, 0, 0, 0) 178 tabLayout.addWidget(self.masterTab) 179 self._progress_queue = ProgressQueue(self.masterTab.progressFrame, self.masterTab.progressBar, self.masterTab.progressCancelButton) 180 181 # setup the node view 182 self.node_tree_model = NodeTreeModel(nm.nameres().address(self.masteruri), self.masteruri) 183 self.node_tree_model.hostInserted.connect(self.on_host_inserted) 184 self.masterTab.nodeTreeView.setModel(self.node_tree_model) 185 for i, (name, width) in enumerate(NodeTreeModel.header): 186 self.masterTab.nodeTreeView.setColumnWidth(i, width) 187 self.nodeNameDelegate = HTMLDelegate() 188 self.masterTab.nodeTreeView.setItemDelegateForColumn(0, self.nodeNameDelegate) 189 self.masterTab.nodeTreeView.collapsed.connect(self.on_node_collapsed) 190 self.masterTab.nodeTreeView.expanded.connect(self.on_node_expanded) 191 sm = self.masterTab.nodeTreeView.selectionModel() 192 sm.selectionChanged.connect(self.on_node_selection_changed) 193 self.masterTab.nodeTreeView.activated.connect(self.on_node_activated) 194 self.masterTab.nodeTreeView.clicked.connect(self.on_node_clicked) 195 # self.masterTab.nodeTreeView.setAcceptDrops(True) 196 # self.masterTab.nodeTreeWidget.setSortingEnabled(True) 197 198 # setup the topic view 199 self.topic_model = TopicModel() 200 self.topic_proxyModel = TopicsSortFilterProxyModel(self) 201 self.topic_proxyModel.setSourceModel(self.topic_model) 202 self.masterTab.topicsView.setModel(self.topic_proxyModel) 203 # self.masterTab.topicsView.setModel(self.topic_model) 204 for i, (name, width) in enumerate(TopicModel.header): 205 self.masterTab.topicsView.setColumnWidth(i, width) 206 self.topicNameDelegate = HTMLDelegate() 207 self.topicTypeDelegate = HTMLDelegate() 208 self.masterTab.topicsView.setItemDelegateForColumn(0, self.topicNameDelegate) 209 self.masterTab.topicsView.setItemDelegateForColumn(3, self.topicTypeDelegate) 210 sm = self.masterTab.topicsView.selectionModel() 211 sm.selectionChanged.connect(self.on_topic_selection_changed) 212 self.masterTab.topicsView.activated.connect(self.on_topic_activated) 213 self.masterTab.topicsView.clicked.connect(self.on_topic_clicked) 214 self.masterTab.topicsView.setSortingEnabled(True) 215 # self.topic_proxyModel.filterAcceptsRow = _filterTopicsAcceptsRow 216 217 # setup the service view 218 self.service_model = ServiceModel() 219 self.service_proxyModel = ServicesSortFilterProxyModel(self) 220 self.service_proxyModel.setSourceModel(self.service_model) 221 self.masterTab.servicesView.setModel(self.service_proxyModel) 222 # self.masterTab.servicesView.setModel(self.service_model) 223 for i, (name, width) in enumerate(ServiceModel.header): 224 self.masterTab.servicesView.setColumnWidth(i, width) 225 self.serviceNameDelegate = HTMLDelegate() 226 self.serviceTypeDelegate = HTMLDelegate() 227 self.masterTab.servicesView.setItemDelegateForColumn(0, self.serviceNameDelegate) 228 self.masterTab.servicesView.setItemDelegateForColumn(1, self.serviceTypeDelegate) 229 sm = self.masterTab.servicesView.selectionModel() 230 sm.selectionChanged.connect(self.on_service_selection_changed) 231 self.masterTab.servicesView.activated.connect(self.on_service_activated) 232 self.masterTab.servicesView.clicked.connect(self.on_service_clicked) 233 self.masterTab.servicesView.setSortingEnabled(True) 234 # self.service_proxyModel.filterAcceptsRow = _filterServiceAcceptsRow 235 236 # setup the parameter view 237 self.parameter_model = ParameterModel() 238 self.parameter_model.itemChanged.connect(self._on_parameter_item_changed) 239 self.parameter_proxyModel = ParameterSortFilterProxyModel(self) 240 self.parameter_proxyModel.setSourceModel(self.parameter_model) 241 self.masterTab.parameterView.setModel(self.parameter_proxyModel) 242 for i, (name, width) in enumerate(ParameterModel.header): 243 self.masterTab.parameterView.setColumnWidth(i, width) 244 self.parameterNameDelegate = HTMLDelegate() 245 self.masterTab.parameterView.setItemDelegateForColumn(0, self.parameterNameDelegate) 246 # self.parameterEditor = QtGui.QStyledItemDelegate() 247 # factory = QtGui.QItemEditorFactory.defaultFactory() 248 # self.parameterEditor.setItemEditorFactory(factory) 249 # self.masterTab.parameterView.setItemDelegateForColumn(1, self.parameterEditor) 250 sm = self.masterTab.parameterView.selectionModel() 251 sm.selectionChanged.connect(self.on_parameter_selection_changed) 252 self.masterTab.parameterView.setSortingEnabled(True) 253 254 # self.parameter_proxyModel.filterAcceptsRow = _filterParameterAcceptsRow 255 # self.masterTab.parameterView.activated.connect(self.on_service_activated) 256 257 # connect the buttons 258 self.masterTab.startButton.clicked.connect(self.on_start_clicked) 259 self.masterTab.stopButton.clicked.connect(self.on_stop_clicked) 260 # self.masterTab.stopContextButton.toggled.connect(self.on_stop_context_toggled) 261 self.masterTab.ioButton.clicked.connect(self.on_io_clicked) 262 self.masterTab.logButton.clicked.connect(self.on_log_clicked) 263 self.masterTab.logDeleteButton.clicked.connect(self.on_log_delete_clicked) 264 self.masterTab.dynamicConfigButton.clicked.connect(self.on_dynamic_config_clicked) 265 self.masterTab.editConfigButton.clicked.connect(self.on_edit_config_clicked) 266 self.masterTab.editRosParamButton.clicked.connect(self.on_edit_rosparam_clicked) 267 self.masterTab.saveButton.clicked.connect(self.on_save_clicked) 268 self.masterTab.closeCfgButton.clicked.connect(self.on_close_clicked) 269 270 self.masterTab.echoTopicButton.clicked.connect(self.on_topic_echo_clicked) 271 self.masterTab.hzTopicButton.clicked.connect(self.on_topic_hz_clicked) 272 self.masterTab.pubTopicButton.clicked.connect(self.on_topic_pub_clicked) 273 self.masterTab.pubStopTopicButton.clicked.connect(self.on_topic_pub_stop_clicked) 274 275 self.masterTab.callServiceButton.clicked.connect(self.on_service_call_clicked) 276 self.masterTab.topicFilterInput.textChanged.connect(self.on_topic_filter_changed) 277 self.masterTab.serviceFilterInput.textChanged.connect(self.on_service_filter_changed) 278 self.masterTab.parameterFilterInput.textChanged.connect(self.on_parameter_filter_changed) 279 self.masterTab.getParameterButton.clicked.connect(self.on_get_parameter_clicked) 280 self.masterTab.addParameterButton.clicked.connect(self.on_add_parameter_clicked) 281 self.masterTab.deleteParameterButton.clicked.connect(self.on_delete_parameter_clicked) 282 self.masterTab.saveParameterButton.clicked.connect(self.on_save_parameter_clicked) 283 284 #create a handler to request the parameter 285 self.parameterHandler = ParameterHandler() 286 self.parameterHandler.parameter_list_signal.connect(self._on_param_list) 287 self.parameterHandler.parameter_values_signal.connect(self._on_param_values) 288 self.parameterHandler.delivery_result_signal.connect(self._on_delivered_values) 289 #create a handler to request sim parameter 290 self.parameterHandler_sim = ParameterHandler() 291 # self.parameterHandler_sim.parameter_list_signal.connect(self._on_param_list) 292 self.parameterHandler_sim.parameter_values_signal.connect(self._on_sim_param_values) 293 # self.parameterHandler_sim.delivery_result_signal.connect(self._on_delivered_values) 294 295 # creates a start menu 296 # start_menu = QtGui.QMenu(self) 297 # self.forceStartNodesAct = QtGui.QAction("&Force start node", self, statusTip="Force the start of selected node", triggered=self.on_force_start_nodes) 298 # start_menu.addAction(self.forceStartNodesAct) 299 # self.startNodesAtHostAct = QtGui.QAction("&Start node on host(Loaded config needed)", self, statusTip="Start node on other host", triggered=self.on_start_nodes_at_host) 300 # start_menu.addAction(self.startNodesAtHostAct) 301 # self.masterTab.startButton.setMenu(start_menu) 302 303 # self._shortcut_start_node_force = QtGui.QShortcut(QtGui.QKeySequence(self.tr("Ctrl+Shift+P", "Force the start of selected node")), self) 304 # self._shortcut_start_node_force.activated.connect(self.on_force_start_nodes) 305 # self._shortcut_start_node_at_host = QtGui.QShortcut(QtGui.QKeySequence(self.tr("Ctrl++Shift+H", "Start node on other host")), self) 306 # self._shortcut_start_node_at_host.activated.connect(self.on_start_nodes_at_host) 307 308 # creates a stop menu 309 self._shortcut_kill_node = QtGui.QShortcut(QtGui.QKeySequence(self.tr("Ctrl+Backspace", "Kill selected node")), self) 310 self._shortcut_kill_node.activated.connect(self.on_kill_nodes) 311 self._shortcut_kill_node = QtGui.QShortcut(QtGui.QKeySequence(self.tr("Ctrl+Delete", "Removes the registration of selected nodes from ROS master")), self) 312 self._shortcut_kill_node.activated.connect(self.on_unregister_nodes) 313 # stop_menu = QtGui.QMenu(self) 314 # self.killNodesAct = QtGui.QAction("&Kill Node", self, shortcut=QtGui.QKeySequence(QtCore.Qt.CTRL + QtCore.Qt.Key_Backspace), statusTip="Kill selected node", triggered=self.on_kill_nodes) 315 # self.unregNodesAct = QtGui.QAction("&Unregister Nodes", self, shortcut=QtGui.QKeySequence(QtCore.Qt.CTRL + QtCore.Qt.Key_Delete), statusTip="Removes the registration of selected nodes from ROS master", triggered=self.on_unregister_nodes) 316 # stop_menu.addAction(self.killNodesAct) 317 # stop_menu.addAction(self.unregNodesAct)S 318 # self.masterTab.stopButton.setMenu(stop_menu) 319 320 # creates a screen menu 321 # screen_menu = QtGui.QMenu(self) 322 # self.killScreensAct = QtGui.QAction("&Kill Screen", self, shortcut=QtGui.QKeySequence(QtCore.Qt.SHIFT + QtCore.Qt.Key_Backspace), statusTip="Kill available screens", triggered=self.on_kill_screens) 323 # screen_menu.addAction(self.killScreensAct) 324 # self.showAllScreensAct = QtGui.QAction("&Show all available screens", self, shortcut=QtGui.QKeySequence(QtCore.Qt.SHIFT + QtCore.Qt.Key_S), statusTip="Shows all available screens", triggered=self.on_show_all_screens) 325 # screen_menu.addAction(self.showAllScreensAct) 326 # self.masterTab.ioButton.setMenu(screen_menu) 327 self.masterTab.ioButton.setEnabled(True) 328 self.masterTab.tabWidget.currentChanged.connect(self.on_tab_current_changed) 329 self._shortcut_screen_show_all = QtGui.QShortcut(QtGui.QKeySequence(self.tr("Shift+S", "Show all available screens")), self) 330 self._shortcut_screen_show_all.activated.connect(self.on_show_all_screens) 331 self._shortcut_screen_kill = QtGui.QShortcut(QtGui.QKeySequence(self.tr("Shift+Backspace", "Kill Screen")), self) 332 self._shortcut_screen_kill.activated.connect(self.on_kill_screens) 333 334 335 # create log menu 336 # self._shortcut_log1 = QtGui.QShortcut(QtGui.QKeySequence(self.tr("Alt+L", "Copy log path to clipboard")), self) 337 # self._shortcut_log1.activated.connect(self.on_log_path_copy) 338 # log_menu = QtGui.QMenu(self) 339 # self.logCopyPathAct = QtGui.QAction("&Copy log path to clipboard", self, statusTip="Copy log path to clipboard", triggered=self.on_log_path_copy) 340 # log_menu.addAction(self.logCopyPathAct) 341 # self.masterTab.logButton.setMenu(log_menu) 342 343 self.loaded_config.connect(self._apply_launch_config) 344 345 # set the shortcuts 346 self._shortcut1 = QtGui.QShortcut(QtGui.QKeySequence(self.tr("Alt+1", "Select first group")), self) 347 self._shortcut1.activated.connect(self.on_shortcut1_activated) 348 self._shortcut2 = QtGui.QShortcut(QtGui.QKeySequence(self.tr("Alt+2", "Select second group")), self) 349 self._shortcut2.activated.connect(self.on_shortcut2_activated) 350 self._shortcut3 = QtGui.QShortcut(QtGui.QKeySequence(self.tr("Alt+3", "Select third group")), self) 351 self._shortcut3.activated.connect(self.on_shortcut3_activated) 352 self._shortcut4 = QtGui.QShortcut(QtGui.QKeySequence(self.tr("Alt+4", "Select fourth group")), self) 353 self._shortcut4.activated.connect(self.on_shortcut4_activated) 354 self._shortcut5 = QtGui.QShortcut(QtGui.QKeySequence(self.tr("Alt+5", "Select fifth group")), self) 355 self._shortcut5.activated.connect(self.on_shortcut5_activated) 356 357 self._shortcut_collapse_all = QtGui.QShortcut(QtGui.QKeySequence(self.tr("Alt+C", "Collapse all groups")), self) 358 self._shortcut_collapse_all.activated.connect(self.on_shortcut_collapse_all) 359 self._shortcut_expand_all = QtGui.QShortcut(QtGui.QKeySequence(self.tr("Alt+E", "Expand all groups")), self) 360 self._shortcut_expand_all.activated.connect(self.masterTab.nodeTreeView.expandAll) 361 self._shortcut_run = QtGui.QShortcut(QtGui.QKeySequence(self.tr("Alt+R", "run selected nodes")), self) 362 self._shortcut_run.activated.connect(self.on_start_clicked) 363 self._shortcut_stop = QtGui.QShortcut(QtGui.QKeySequence(self.tr("Alt+S", "stop selected nodes")), self) 364 self._shortcut_stop.activated.connect(self.on_stop_clicked) 365 366 self._shortcut_copy = QtGui.QShortcut(QtGui.QKeySequence(self.tr("Ctrl+X", "copy selected alternative values to clipboard")), self) 367 self._shortcut_copy.activated.connect(self.on_copy_x_pressed)
368 369 # print "================ create", self.objectName() 370 # 371 # def __del__(self): 372 # print " Destroy mester view proxy", self.objectName(), " ..." 373 # for ps in self.__echo_topics_dialogs.values(): 374 # try: 375 # ps.terminate() 376 # except: 377 # pass 378 # print " ", self.objectName(), "destroyed" 379
380 - def stop(self):
381 print " Shutdown master", self.masteruri, "..." 382 self.default_cfg_handler.stop() 383 self.launch_server_handler.stop() 384 self._progress_queue.stop() 385 for ps in self.__echo_topics_dialogs.values(): 386 try: 387 ps.terminate() 388 except: 389 pass 390 print " Master", self.masteruri, " is down!"
391 392 @property
393 - def current_user(self):
394 return self.__current_user
395 396 @current_user.setter
397 - def current_user(self, user):
398 self.__current_user = user
399 400 @property
401 - def is_local(self):
402 return nm.is_local(nm.nameres().getHostname(self.masteruri))
403 404 @property
405 - def master_state(self):
406 return self.__master_state
407 408 @master_state.setter
409 - def master_state(self, master_state):
410 self.__master_state = master_state
411 412 @property
413 - def master_info(self):
414 return self.__master_info
415 416 @master_info.setter
417 - def master_info(self, master_info):
418 ''' 419 Sets the new master information. To determine whether a node is running the 420 PID and his URI are needed. The PID of remote nodes (host of the ROS master 421 and the node are different) will be not determine by discovering. Thus this 422 information must be obtain from other MasterInfo object and stored while 423 updating. 424 @param master_info: the mater information object 425 @type master_info: L{master_discovery_fkie.msg.MasterInfo} 426 ''' 427 try: 428 update_result = (set(), set(), set(), set(), set(), set(), set(), set(), set()) 429 if self.__master_info is None: 430 if (master_info.masteruri == self.masteruri): 431 self.__master_info = master_info 432 update_result[0].update(self.__master_info.node_names) 433 update_result[3].update(self.__master_info.topic_names) 434 update_result[6].update(self.__master_info.service_names) 435 else: 436 update_result = self.__master_info.updateInfo(master_info) 437 # print "MINFO", self.__master_info.listedState() 438 # we receive the master info from remove nodes first -> skip 439 if self.__master_info is None: 440 return 441 try: 442 # request the info of new remote nodes 443 hosts2update = set([nm.nameres().getHostname(self.__master_info.getNode(nodename).uri) for nodename in update_result[0]]) 444 hosts2update.update([nm.nameres().getHostname(self.__master_info.getService(nodename).uri) for nodename in update_result[6]]) 445 for host in hosts2update: 446 if host != nm.nameres().getHostname(self.masteruri): 447 self.updateHostRequest.emit(host) 448 except: 449 pass 450 # cputimes = os.times() 451 # cputime_init = cputimes[0] + cputimes[1] 452 # update nodes in the model 453 if update_result[0] or update_result[1] or update_result[2] or self.__force_update: 454 self.updateRunningNodesInModel(self.__master_info) 455 self.on_node_selection_changed(None, None) 456 # Updates the topic view based on the current master information. 457 if update_result[3] or update_result[4] or update_result[5] or self.__force_update: 458 self.topic_model.updateModelData(self.__master_info.topics, update_result[3], update_result[4], update_result[5]) 459 self.on_topic_selection_changed(None, None) 460 # Updates the service view based on the current master information. 461 if update_result[6] or update_result[7] or update_result[8] or self.__force_update: 462 self.service_model.updateModelData(self.__master_info.services, update_result[6], update_result[7], update_result[8]) 463 self.on_service_selection_changed(None, None) 464 # update the default configuration 465 self.updateDefaultConfigs(self.__master_info) 466 self.__force_update = False 467 # cputimes = os.times() 468 # cputime = cputimes[0] + cputimes[1] - cputime_init 469 # print " update on ", self.__master_info.mastername if not self.__master_info is None else self.__master_state.name, cputime 470 self.parameterHandler_sim.requestParameterValues(self.masteruri, ["/use_sim_time", "/robot_icon", "/roslaunch/uris"]) 471 except: 472 import traceback 473 print traceback.format_exc()
474 475 @property
476 - def use_sim_time(self):
477 return self.__use_sim_time
478
479 - def force_next_update(self):
480 self.__force_update = True
481
482 - def markNodesAsDuplicateOf(self, running_nodes):
483 ''' 484 Marks all nodes, which are not running and in a given list as a duplicates nodes. 485 @param running_nodes: The list with names of running nodes 486 @type running_nodes: C{[str]} 487 ''' 488 # store the running_nodes to update to duplicates after load a launch file 489 self.__running_nodes = running_nodes 490 self.node_tree_model.markNodesAsDuplicateOf(running_nodes, (not self.master_info is None and self.master_info.getNodeEndsWith('master_sync')))
491
492 - def getRunningNodesIfSync(self):
493 ''' 494 Returns the list with all running nodes, which are registered by this ROS 495 master. Also the nodes, which are physically running on remote hosts. 496 @return running_nodes: The list with names of running nodes 497 @rtype running_nodes: C{[str]} 498 ''' 499 if not self.master_info is None and self.master_info.getNodeEndsWith('master_sync'): 500 return self.master_info.node_names 501 return []
502
503 - def getRunningNodesIfLocal(self):
504 ''' 505 Returns the list with all running nodes, which are running (has process) on this host. 506 The nodes registered on this ROS master, but running on remote hosts are not 507 returned. 508 @return running_nodes: The dictionary with names of running nodes and their masteruri 509 @rtype running_nodes: C{dict(str:str)} 510 ''' 511 result = dict() 512 if not self.master_info is None: 513 for name, node in self.master_info.nodes.items(): 514 if node.isLocal: 515 result[node.name] = self.master_info.masteruri 516 return result
517 518
519 - def updateRunningNodesInModel(self, master_info):
520 ''' 521 Creates the dictionary with ExtendedNodeInfo objects and updates the nodes view. 522 @param master_info: the mater information object 523 @type master_info: L{master_discovery_fkie.msg.MasterInfo} 524 ''' 525 if not master_info is None: 526 self.node_tree_model.updateModelData(master_info.nodes) 527 self.updateButtons()
528
529 - def getNode(self, node_name):
530 ''' 531 @param node_name: The name of the node. 532 @type node_name: str 533 @return: The list the nodes with given name. 534 @rtype: [] 535 ''' 536 return self.node_tree_model.getNode(node_name)
537
538 - def updateButtons(self):
539 ''' 540 Updates the enable state of the buttons depending of the selection and 541 running state of the selected node. 542 ''' 543 selectedNodes = self.nodesFromIndexes(self.masterTab.nodeTreeView.selectionModel().selectedIndexes()) 544 has_running = False 545 has_stopped = False 546 has_invalid = False 547 for node in selectedNodes: 548 if not node.uri is None: 549 has_running = True 550 if node.pid is None: 551 has_invalid = True 552 else: 553 has_stopped = True 554 # self.masterTab.startButton.setEnabled(has_stopped or has_invalid) 555 # self.masterTab.stopButton.setEnabled(has_running or has_invalid) 556 self.masterTab.startButton.setEnabled(True) 557 self.masterTab.stopButton.setEnabled(True) 558 # self.masterTab.ioButton.setEnabled(has_running or has_stopped) 559 self.masterTab.logButton.setEnabled(True) 560 # self.masterTab.logButton.setEnabled(has_running or has_stopped) 561 self.masterTab.logDeleteButton.setEnabled(has_running or has_stopped) 562 # test for available dynamic reconfigure services 563 if not self.master_info is None: 564 dyn_cfg_available = False 565 dyncfgServices = [srv for srv_name, srv in self.master_info.services.items() if (srv_name.endswith('/set_parameters'))] 566 for n in selectedNodes: 567 for srv_name, srv in self.master_info.services.items(): 568 if (srv_name.endswith('/set_parameters')) and n.name in srv.serviceProvider: 569 dyn_cfg_available = True 570 break 571 self.masterTab.dynamicConfigButton.setEnabled(dyn_cfg_available) 572 # the configuration is only available, if only one node is selected 573 cfg_enable = False 574 if len(selectedNodes) >= 1: 575 cfg_enable = len(self._getCfgChoises(selectedNodes[0], True)) > 0 576 self.masterTab.editConfigButton.setEnabled(cfg_enable and len(selectedNodes) == 1) 577 # self.startNodesAtHostAct.setEnabled(cfg_enable) 578 self.masterTab.editRosParamButton.setEnabled(len(selectedNodes) == 1) 579 self.masterTab.saveButton.setEnabled(len(self.launchfiles) > 1) 580 # enable the close button only for local configurations 581 self.masterTab.closeCfgButton.setEnabled(len([path for path, cfg in self.__configs.items() if (isinstance(path, tuple) and path[2] == self.masteruri) or not isinstance(path, tuple)]) > 0)
582
583 - def hasLaunchfile(self, path):
584 ''' 585 @param path: the launch file 586 @type path: C{str} 587 @return: C{True} if the given launch file is open 588 @rtype: C{boolean} 589 ''' 590 return self.launchfiles.has_key(path)
591 592 @property
593 - def launchfiles(self):
594 ''' 595 Returns the copy of the dictionary with loaded launch files on this host 596 @rtype: C{dict(str(file) : L{LaunchConfig}, ...)} 597 ''' 598 result = dict() 599 for (c, cfg) in self.__configs.items(): 600 if not isinstance(c, tuple): 601 result[c] = cfg 602 return result
603 604 @launchfiles.setter
605 - def launchfiles(self, launchfile):
606 ''' 607 Loads the launch file. If this file is already loaded, it will be reloaded. 608 After successful load the node view will be updated. 609 @param launchfile: the launch file path 610 @type launchfile: C{str} 611 ''' 612 self._progress_queue.add2queue(str(uuid.uuid4()), 613 ''.join(['Loading ', os.path.basename(launchfile)]), 614 self._load_launchfile, 615 (launchfile,)) 616 self._progress_queue.start()
617
618 - def _load_launchfile(self, launchfile, argv_forced=[], pqid=None):
619 ''' 620 This method will be called in another thread. The configuration parameter 621 of the launch file will be requested using `LaunchArgsSelectionRequest` and 622 `InteractionNeededError`. After the file is successful loaded a 623 `loaded_config` signal will be emitted. 624 ''' 625 stored_argv = None 626 if self.__configs.has_key(launchfile): 627 # close the current loaded configuration with the same name 628 stored_argv = self.__configs[launchfile].argv 629 #load launch configuration 630 try: 631 # test for required args 632 launchConfig = LaunchConfig(launchfile, masteruri=self.masteruri) 633 loaded = False 634 # if the launch file currently open these args will be used 635 if stored_argv is None: 636 if argv_forced: 637 # if the parameter already requested `argv_forced` is filled: load! 638 loaded = launchConfig.load(argv_forced) 639 else: 640 # get the list with needed launch args 641 req_args = launchConfig.getArgs() 642 if req_args: 643 params = dict() 644 arg_dict = launchConfig.argvToDict(req_args) 645 for arg in arg_dict.keys(): 646 params[arg] = ('string', [arg_dict[arg]]) 647 # request the args: the dialog must run in the main thread of Qt 648 req = LaunchArgsSelectionRequest(launchfile, params, 'Needs input for args') 649 raise nm.InteractionNeededError(req, self._load_launchfile, (launchfile, )) 650 # load the launch file with args of currently open launch file 651 if not loaded or not stored_argv is None: 652 launchConfig.load(req_args if stored_argv is None else stored_argv) 653 # update the names of the hosts stored in the launch file 654 for name, machine in launchConfig.Roscfg.machines.items(): 655 if machine.name: 656 nm.nameres().addInfo(machine.name, machine.address, machine.address) 657 # do not load if the loadings process was canceled 658 if self._progress_queue.has_id(pqid): 659 self.loaded_config.emit(launchfile, launchConfig) 660 except InteractionNeededError: 661 raise 662 except Exception as e: 663 import os 664 err_text = ''.join([os.path.basename(launchfile),' loading failed!']) 665 err_details = ''.join([err_text, '\n\n', e.__class__.__name__, ": ", str(e)]) 666 rospy.logwarn("Loading launch file: %s", err_details) 667 raise DetailedError("Loading launch file", err_text, err_details) 668 # WarningMessageBox(QtGui.QMessageBox.Warning, "Loading launch file", err_text, err_details).exec_() 669 except: 670 import traceback 671 print traceback.print_exc()
672
673 - def _apply_launch_config(self, launchfile, launchConfig):
674 # QtCore.QCoreApplication.processEvents(QtCore.QEventLoop.ExcludeUserInputEvents) 675 stored_roscfg = None 676 if self.__configs.has_key(launchfile): 677 # close the current loaded configuration with the same name 678 self.removeConfigFromModel(launchfile) 679 stored_roscfg = self.__configs[launchfile].Roscfg 680 del self.__configs[launchfile] 681 try: 682 # add launch file object to the list 683 self.__configs[launchfile] = launchConfig 684 self.appendConfigToModel(launchfile, launchConfig.Roscfg) 685 self.masterTab.tabWidget.setCurrentIndex(0) 686 #get the descriptions of capabilities and hosts 687 try: 688 robot_descr = launchConfig.getRobotDescr() 689 capabilities = launchConfig.getCapabilitiesDesrc() 690 for (host, caps) in capabilities.items(): 691 if not host: 692 host = nm.nameres().mastername(self.masteruri) 693 host_addr = nm.nameres().address(host) 694 self.node_tree_model.addCapabilities(nm.nameres().masteruri(host), host_addr, launchfile, caps) 695 for (host, descr) in robot_descr.items(): 696 if not host: 697 host = nm.nameres().mastername(self.masteruri) 698 host_addr = nm.nameres().address(host) 699 tooltip = self.node_tree_model.updateHostDescription(nm.nameres().masteruri(host), host_addr, descr['type'], descr['name'], descr['description']) 700 self.host_description_updated.emit(self.masteruri, host_addr, tooltip) 701 except: 702 import traceback 703 print traceback.print_exc() 704 705 # by this call the name of the host will be updated if a new one is defined in the launch file 706 self.updateRunningNodesInModel(self.__master_info) 707 708 # detect files changes 709 if stored_roscfg and self.__configs[launchfile].Roscfg: 710 stored_values = [(name, str(p.value)) for name, p in stored_roscfg.params.items()] 711 new_values = [(name, str(p.value)) for name, p in self.__configs[launchfile].Roscfg.params.items()] 712 # detect changes parameter 713 paramset = set(name for name, value in (set(new_values) - set(stored_values))) 714 # detect new parameter 715 paramset |= (set(self.__configs[launchfile].Roscfg.params.keys()) - set(stored_roscfg.params.keys())) 716 # detect removed parameter 717 paramset |= (set(stored_roscfg.params.keys()) - set(self.__configs[launchfile].Roscfg.params.keys())) 718 # detect new nodes 719 stored_nodes = [roslib.names.ns_join(item.namespace, item.name) for item in stored_roscfg.nodes] 720 new_nodes = [roslib.names.ns_join(item.namespace, item.name) for item in self.__configs[launchfile].Roscfg.nodes] 721 nodes2start = set(new_nodes) - set(stored_nodes) 722 # determine the nodes of the changed parameter 723 for p in paramset: 724 for n in new_nodes: 725 if p.startswith(n): 726 nodes2start.add(n) 727 # filter out anonymous nodes 728 nodes2start = [n for n in nodes2start if not re.search(r"\d{3,6}_\d{10,}", n)] 729 # restart nodes 730 if nodes2start: 731 restart, ok = SelectDialog.getValue('Restart nodes?', "Select nodes to restart <b>@%s</b>:"%self.mastername, nodes2start, False, True, '', self) 732 self.start_nodes_by_name(restart, launchfile, True) 733 # set the robot_icon 734 if launchfile in self.__robot_icons: 735 self.__robot_icons.remove(launchfile) 736 self.__robot_icons.insert(0, launchfile) 737 self.markNodesAsDuplicateOf(self.__running_nodes) 738 # print "MASTER:", launchConfig.Roscfg.master 739 # print "NODES_CORE:", launchConfig.Roscfg.nodes_core 740 # for n in launchConfig.Roscfg.nodes: 741 # n.__slots__ = [] 742 # print "NODES:", pickle.dumps(launchConfig.Roscfg.nodes) 743 # 744 # print "ROSLAUNCH_FILES:", launchConfig.Roscfg.roslaunch_files 745 # # list of resolved node names. This is so that we can check for naming collisions 746 # print "RESOLVED_NAMES:", launchConfig.Roscfg.resolved_node_names 747 # 748 # print "TESTS:", launchConfig.Roscfg.tests 749 # print "MACHINES:", launchConfig.Roscfg.machines 750 # print "PARAMS:", launchConfig.Roscfg.params 751 # print "CLEAR_PARAMS:", launchConfig.Roscfg.clear_params 752 # print "EXECS:", launchConfig.Roscfg.executables 753 # 754 # # for tools like roswtf 755 # print "ERRORS:", launchConfig.Roscfg.config_errors 756 # 757 # print "M:", launchConfig.Roscfg.m 758 except Exception as e: 759 import os 760 err_text = ''.join([os.path.basename(launchfile),' loading failed!']) 761 err_details = ''.join([err_text, '\n\n', e.__class__.__name__, ": ", str(e)]) 762 rospy.logwarn("Loading launch file: %s", err_details) 763 WarningMessageBox(QtGui.QMessageBox.Warning, "Loading launch file", err_text, err_details).exec_() 764 except: 765 import traceback 766 print traceback.print_exc() 767 self.update_robot_icon(True)
768
769 - def reload_global_parameter_at_next_start(self, launchfile):
770 try: 771 self.__configs[launchfile].global_param_done.remove(self.masteruri) 772 self.on_node_selection_changed(None, None, True) 773 except: 774 pass
775
776 - def update_robot_icon(self, force=False):
777 ''' 778 Update the current robot icon. If the icon was changed a `robot_icon_updated` 779 signal will be emitted. 780 :return: the path to the current robot icon 781 :rtype: str 782 ''' 783 for l in self.__robot_icons: 784 try: 785 icon = self.__configs[l].get_robot_icon() 786 if icon: 787 if icon != self.__current_robot_icon or force: 788 self.__current_robot_icon = icon 789 self.robot_icon_updated.emit(self.masteruri, icon) 790 return icon 791 except: 792 pass 793 self.__current_robot_icon = self.__current_parameter_robot_icon 794 self.robot_icon_updated.emit(self.masteruri, str(self.__current_robot_icon)) 795 return self.__current_robot_icon
796
797 - def appendConfigToModel(self, launchfile, rosconfig):
798 ''' 799 Update the node view 800 @param launchfile: the launch file path 801 @type launchfile: C{str} 802 @param rosconfig: the configuration 803 @type rosconfig: L{LaunchConfig} 804 ''' 805 hosts = dict() # dict(addr : dict(node : [config]) ) 806 for n in rosconfig.nodes: 807 addr = nm.nameres().address(self.masteruri) 808 masteruri = self.masteruri 809 if n.machine_name and not n.machine_name == 'localhost': 810 if not rosconfig.machines.has_key(n.machine_name): 811 raise Exception(''.join(["ERROR: unknown machine [", n.machine_name,"]"])) 812 addr = rosconfig.machines[n.machine_name].address 813 masteruri = nm.nameres().masteruri(n.machine_name) 814 node = roslib.names.ns_join(n.namespace, n.name) 815 if not hosts.has_key((masteruri, addr)): 816 hosts[(masteruri, addr)] = dict() 817 hosts[(masteruri, addr)][node] = launchfile 818 # add the configurations for each host separately 819 for ((masteruri, addr), nodes) in hosts.items(): 820 self.node_tree_model.appendConfigNodes(masteruri, addr, nodes) 821 self.updateButtons()
822
823 - def removeConfigFromModel(self, launchfile):
824 ''' 825 Update the node view after removed configuration. 826 @param launchfile: the launch file path 827 @type launchfile: C{str} 828 ''' 829 if isinstance(launchfile, tuple): 830 self.remove_config_signal.emit(launchfile[0]) 831 self.node_tree_model.removeConfigNodes(launchfile) 832 self.updateButtons()
833
834 - def updateDefaultConfigs(self, master_info):
835 ''' 836 Updates the default configuration view based on the current master information. 837 @param master_info: the mater information object 838 @type master_info: L{master_discovery_fkie.msg.MasterInfo} 839 ''' 840 if self.__master_info is None: 841 return 842 default_cfgs = [] 843 for name in self.__master_info.service_names: 844 if name.endswith('list_nodes'): 845 srv = self.__master_info.getService(name) 846 default_cfgs.append((roslib.names.namespace(name).rstrip(roslib.names.SEP), srv.uri, srv.masteruri)) 847 # remove the node contained in default configuration form the view 848 removed = list(set([c for c in self.__configs.keys() if isinstance(c, tuple)]) - set(default_cfgs)) 849 if removed: 850 for r in removed: 851 # host = nm.nameres().address(r[1]) 852 self.node_tree_model.removeConfigNodes(r) 853 # service = self.__master_info.getService(roslib.names.ns_join(r[0], 'list_nodes')) 854 if r[2] == self.masteruri: 855 self.remove_config_signal.emit(r[0]) 856 del self.__configs[r] 857 if len(self.__configs) == 0: 858 address = nm.nameres().address(master_info.masteruri) 859 tooltip = self.node_tree_model.updateHostDescription(master_info.masteruri, address, '', '', '') 860 self.host_description_updated.emit(master_info.masteruri, address, tooltip) 861 # request the nodes of new default configurations 862 added = list(set(default_cfgs) - set(self.__configs.keys())) 863 for (name, uri, muri) in added: 864 self.default_cfg_handler.requestNodeList(uri, roslib.names.ns_join(name, 'list_nodes')) 865 #request the description 866 descr_service = self.__master_info.getService(roslib.names.ns_join(name, 'description')) 867 if not descr_service is None: 868 self.default_cfg_handler.requestDescriptionList(descr_service.uri, descr_service.name) 869 self.updateButtons()
870
871 - def on_default_cfg_nodes_retrieved(self, service_uri, config_name, nodes):
872 ''' 873 Handles the new list with nodes from default configuration service. 874 @param service_uri: the URI of the service provided the default configuration 875 @type service_uri: C{str} 876 @param config_name: the name of default configuration service 877 @type config_name: C{str} 878 @param nodes: the name of the nodes with name spaces 879 @type nodes: C{[str]} 880 ''' 881 # remove the current state 882 masteruri = self.masteruri 883 if not self.__master_info is None: 884 service = self.__master_info.getService(config_name) 885 if not service is None: 886 masteruri = service.masteruri 887 key = (roslib.names.namespace(config_name).rstrip(roslib.names.SEP), service_uri, masteruri) 888 # if self.__configs.has_key(key): 889 # self.node_tree_model.removeConfigNodes(key) 890 # add the new config 891 node_cfgs = dict() 892 for n in nodes: 893 node_cfgs[n] = key 894 host = nm.nameres().getHostname(service_uri) 895 host_addr = nm.nameres().address(host) 896 if host_addr is None: 897 host_addr = host 898 self.node_tree_model.appendConfigNodes(masteruri, host_addr, node_cfgs) 899 self.__configs[key] = nodes 900 self.updateButtons()
901
902 - def on_default_cfg_descr_retrieved(self, service_uri, config_name, items):
903 ''' 904 Handles the description list from default configuration service. 905 Emits a Qt signal L{host_description_updated} to notify about a new host 906 description and a Qt signal L{capabilities_update} to notify about a capabilities 907 update. 908 @param service_uri: the URI of the service provided the default configuration 909 @type service_uri: C{str} 910 @param config_name: the name of default configuration service 911 @type config_name: C{str} 912 @param items: list with descriptions 913 @type items: C{[L{default_cfg_fkie.Description}]} 914 ''' 915 if items: 916 masteruri = self.masteruri 917 if not self.__master_info is None: 918 service = self.__master_info.getService(config_name) 919 if not service is None: 920 masteruri = service.masteruri 921 key = (roslib.names.namespace(config_name).rstrip(roslib.names.SEP), service_uri, masteruri) 922 host = nm.nameres().getHostname(service_uri) 923 host_addr = nm.nameres().address(host) 924 #add capabilities 925 caps = dict() 926 for c in items[0].capabilities: 927 if not caps.has_key(c.namespace): 928 caps[c.namespace] = dict() 929 caps[c.namespace][c.name.decode(sys.getfilesystemencoding())] = { 'type' : c.type, 'images' : c.images, 'description' : c.description.replace("\\n ", "\n").decode(sys.getfilesystemencoding()), 'nodes' : list(c.nodes) } 930 if host_addr is None: 931 host_addr = nm.nameres().getHostname(key[1]) 932 self.node_tree_model.addCapabilities(masteruri, host_addr, key, caps) 933 # set host description 934 tooltip = self.node_tree_model.updateHostDescription(masteruri, host_addr, items[0].robot_type, items[0].robot_name.decode(sys.getfilesystemencoding()), items[0].robot_descr.decode(sys.getfilesystemencoding())) 935 self.host_description_updated.emit(masteruri, host_addr, tooltip) 936 self.capabilities_update_signal.emit(masteruri, host_addr, roslib.names.namespace(config_name).rstrip(roslib.names.SEP), items)
937
938 - def on_default_cfg_err(self, service_uri, service, msg):
939 ''' 940 Handles the error messages from default configuration service. 941 @param service_uri: the URI of the service provided the default configuration 942 @type service_uri: C{str} 943 @param service: the name of default configuration service 944 @type service: C{str} 945 @param msg: the error message 946 @type msg: C{str} 947 ''' 948 pass
949 # QtGui.QMessageBox.warning(self, 'Error while call %s'%service, 950 # str(msg), 951 # QtGui.QMessageBox.Ok) 952 953 @property
954 - def launch_servers(self):
955 return self.__launch_servers
956
957 - def has_launch_server(self):
958 ''' 959 Returns `True` if the there are roslaunch server, which have no `master` as 960 node or or have other nodes as `rosout-#` inside. 961 ''' 962 for uri, (pid, nodes) in self.__launch_servers.items(): 963 if not self._is_master_launch_server(nodes): 964 return True 965 return False
966
967 - def _is_master_launch_server(self, nodes):
968 if 'master' in nodes and len(nodes) < 3: 969 return True 970 return False
971
972 - def on_launch_server_retrieved(self, serveruri, pid, nodes):
973 ''' 974 Handles the info about roslaunch server. 975 Emits a Qt signal L{host_description_updated} to notify about a new host 976 description and a Qt signal L{capabilities_update} to notify about a capabilities 977 update. 978 @param serveruri: the URI of the roslaunch server 979 @type serveruri: C{str} 980 @param pid: the process id of the roslaunch server 981 @type pid: C{str} 982 @param nodes: list with nodes handled by the roslaunch server 983 @type nodes: C{[L{str}]} 984 ''' 985 self.__launch_servers[serveruri] = (pid, nodes)
986
987 - def on_launch_server_err(self, serveruri, msg):
988 ''' 989 Handles the error messages from launch server hanlder. 990 @param serveruri: the URI of the launch server 991 @type serveruri: C{str} 992 @param msg: the error message 993 @type msg: C{str} 994 ''' 995 try: 996 del self.__launch_servers[serveruri] 997 except: 998 pass
999
1001 ''' 1002 Kill all running launch server. The coresponding URIS are removed by master_monitor. 1003 ''' 1004 for lsuri, (pid, nodes) in self.__launch_servers.items(): 1005 try: 1006 if not self._is_master_launch_server(nodes): 1007 self._progress_queue.add2queue(str(uuid.uuid4()), 1008 ''.join(['kill roslaunch ', lsuri, '(', str(pid), ')']), 1009 nm.starter().kill, 1010 (nm.nameres().getHostname(lsuri), pid, False, self.current_user)) 1011 self.launch_server_handler.updateLaunchServerInfo(lsuri, delayed_exec=3.0) 1012 except Exception as e: 1013 rospy.logwarn("Error while kill roslaunch %s: %s", str(lsuri), str(e)) 1014 raise DetailedError("Kill error", 1015 ''.join(['Error while kill roslaunch ', lsuri]), 1016 str(e)) 1017 self._progress_queue.start()
1018 1019 1020 #%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 1021 #%%%%%%%%%%%%% Handling of the view activities %%%%%%%% 1022 #%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 1023
1024 - def on_node_activated(self, index):
1025 ''' 1026 Depending of the state of the node, it will be run or the screen output will 1027 be open. 1028 @param index: The index of the activated node 1029 @type index: L{PySide.QtCore.QModelIndex} 1030 ''' 1031 selectedNodes = self.nodesFromIndexes(self.masterTab.nodeTreeView.selectionModel().selectedIndexes(), False) 1032 if not selectedNodes: 1033 return 1034 has_running = False 1035 has_stopped = False 1036 has_invalid = False 1037 for node in selectedNodes: 1038 if not node.uri is None: 1039 has_running = True 1040 if node.pid is None: 1041 has_invalid = True 1042 else: 1043 has_stopped = True 1044 1045 if has_stopped: 1046 self.on_start_clicked() 1047 elif has_running and not has_invalid: 1048 self.on_io_clicked() 1049 else: 1050 self.on_log_clicked()
1051
1052 - def on_node_clicked(self, index):
1053 self.__last_info_type = 'Node' 1054 self.on_node_selection_changed(None, None, True)
1055 1056
1057 - def on_topic_activated(self, index):
1058 ''' 1059 @param index: The index of the activated topic 1060 @type index: L{PySide.QtCore.QModelIndex} 1061 ''' 1062 self.on_topic_echo_clicked()
1063
1064 - def on_topic_clicked(self, index):
1065 self.__last_info_type = 'Topic' 1066 self.on_topic_selection_changed(None, None, True)
1067
1068 - def on_service_activated(self, index):
1069 ''' 1070 @param index: The index of the activated service 1071 @type index: L{PySide.QtCore.QModelIndex} 1072 ''' 1073 self.on_service_call_clicked()
1074
1075 - def on_service_clicked(self, index):
1076 self.__last_info_type = 'Service' 1077 self.on_service_selection_changed(None, None, True)
1078
1079 - def on_host_inserted(self, item):
1080 if item.id == (self.masteruri, nm.nameres().getHostname(self.masteruri)): 1081 index = self.node_tree_model.indexFromItem(item) 1082 if index.isValid(): 1083 self.masterTab.nodeTreeView.expand(index)
1084 # self.masterTab.nodeTreeView.expandAll() 1085
1086 - def on_node_collapsed(self, index):
1087 if not index.parent ().isValid(): 1088 self.masterTab.nodeTreeView.selectionModel().clear()
1089
1090 - def on_node_expanded(self, index):
1091 pass
1092
1093 - def _create_html_list(self, title, items, type=None, name=''):
1094 ''' 1095 :param type: LAUNCH, TOPIC, NODE, SERVICE 1096 :type type: str 1097 ''' 1098 result = '' 1099 if items: 1100 result = '%s<b><u>%s</u></b>'%(result, title) 1101 if len(items) > 1: 1102 result = '%s <span style="color:gray;">[%d]</span>'%(result, len(items)) 1103 result = '%s<br><ul><span></span>'%result 1104 items.sort() 1105 for i in items: 1106 item = i 1107 # reduce the displayed name 1108 item_name = i 1109 if name: 1110 item_name = item_name.replace('%s%s'%(name, roslib.names.SEP), '~', 1) 1111 ns = roslib.names.namespace(name) 1112 if item_name.startswith(ns) and ns != roslib.names.SEP: 1113 item_name = item_name.replace(ns, '', 1) 1114 if type in ['NODE']: 1115 item = '<a href="node://%s%s">%s</a>'%(self.mastername, i, item_name) 1116 elif type in ['TOPIC_PUB', 'TOPIC_SUB']: 1117 # determine the count of publisher or subscriber 1118 count = None 1119 try: 1120 count = len(self.__master_info.getTopic(i).publisherNodes) if type == 'TOPIC_SUB' else len(self.__master_info.getTopic(i).subscriberNodes) 1121 except: 1122 pass 1123 item = '<a href="topic://%s">%s</a>'%(i, item_name) 1124 item += ' <a href="topicecho://%s%s"><span style="color:gray;"><i>echo</i></span></a>'%(self.mastername, i) 1125 # add the count 1126 if not count is None: 1127 item = '<span style="color:gray;">_%d_/ </span>%s'%(count, item) 1128 elif type == 'SERVICE': 1129 item = '<a href="service://%s%s">%s</a>'%(self.mastername, i, item_name) 1130 item += ' <a href="servicecall://%s%s"><span style="color:gray;"><i>call</i></span></a>'%(self.mastername, i) 1131 elif type == 'LAUNCH': 1132 item = '<a href="launch://%s">%s</a>'%(i, item_name) 1133 if i in self.__configs and self.masteruri in self.__configs[i].global_param_done: 1134 item += '%s<br><a href="reload_globals://%s"><font color="#339900">reload global parameter @next start</font></a>'%(item, i) 1135 result += '\n%s<br>'%(item) 1136 result += '</ul>' 1137 return result
1138
1139 - def on_tab_current_changed(self, index):
1140 if self.masterTab.tabWidget.tabText(index) == 'Topics': 1141 # select the topics of the selected node in the "Topic" view 1142 selections = self.masterTab.nodeTreeView.selectionModel().selectedIndexes() 1143 selectedNodes = self.nodesFromIndexes(selections) 1144 if len(selectedNodes) == 1: 1145 node = selectedNodes[0] 1146 selected_topics = self.topic_model.index_from_names(node.published, node.subscribed) 1147 for s in selected_topics: 1148 self.masterTab.topicsView.selectionModel().select(self.topic_proxyModel.mapFromSource(s), QtGui.QItemSelectionModel.Select) 1149 elif self.masterTab.tabWidget.tabText(index) == 'Services': 1150 # select the services of the selected node in the "Services" view 1151 selections = self.masterTab.nodeTreeView.selectionModel().selectedIndexes() 1152 selectedNodes = self.nodesFromIndexes(selections) 1153 if len(selectedNodes) == 1: 1154 node = selectedNodes[0] 1155 selected_services = self.service_model.index_from_names(node.services) 1156 for s in selected_services: 1157 self.masterTab.servicesView.selectionModel().select(self.service_proxyModel.mapFromSource(s), QtGui.QItemSelectionModel.Select)
1158 1159
1160 - def on_node_selection_changed(self, selected, deselected, force_emit=False, node_name=''):
1161 ''' 1162 updates the Buttons, create a description and emit L{description_signal} to 1163 show the description of host, group or node. 1164 ''' 1165 if node_name and not self.master_info is None: 1166 # get node by name 1167 selectedNodes = self.node_tree_model.getNode("%s"%node_name) 1168 if selectedNodes[0] is None: 1169 return 1170 selectedHosts = [] 1171 selections = [] 1172 else: 1173 # get node by selected items 1174 if self.masterTab.tabWidget.tabText(self.masterTab.tabWidget.currentIndex()) != 'Nodes': 1175 return 1176 selections = self.masterTab.nodeTreeView.selectionModel().selectedIndexes() 1177 selectedHosts = self.hostsFromIndexes(selections) 1178 selectedNodes = self.nodesFromIndexes(selections) 1179 self.masterTab.topicsView.selectionModel().clear() 1180 self.masterTab.servicesView.selectionModel().clear() 1181 name = '' 1182 text = '' 1183 # add host description, if only the one host is selected 1184 if len(selectedHosts) == 1 and len(selections) / 2 == 1: 1185 host = selectedHosts[0] 1186 name = '%s - Robot'%host.name 1187 text = host.generateDescription() 1188 text += '<br>' 1189 else: 1190 # add group description, if only the one group is selected 1191 selectedGroups = self.groupsFromIndexes(selections) 1192 if len(selectedGroups) == 1 and len(selections) / 2 == 1: 1193 group = selectedGroups[0] 1194 name = '%s - Group'%group.name 1195 text = group.generateDescription() 1196 text += '<br>' 1197 # add node description for one selected node 1198 if len(selectedNodes) == 1: 1199 node = selectedNodes[0] 1200 # create description for a node 1201 ns, sep, name = node.name.rpartition(rospy.names.SEP) 1202 text = '<font size="+1"><b><span style="color:gray;">%s%s</span><b>%s</b></font><br>'%(ns, sep, name) 1203 launches = [c for c in node.cfgs if not isinstance(c, tuple)] 1204 default_cfgs = [c[0] for c in node.cfgs if isinstance(c, tuple)] 1205 if launches or default_cfgs: 1206 text += '<a href="restart_node://%s">restart</a> - '%node.name 1207 text += '<a href="kill_node://%s">kill</a> - '%node.name 1208 text += '<a href="kill_screen://%s">kill screen</a><br>'%node.name 1209 if launches: 1210 text += '<a href="start_node_at_host://%s">start@host</a>'%node.name 1211 text += '<dl>' 1212 text += '<dt><b>URI</b>: %s</dt>'%node.node_info.uri 1213 text += '<dt><b>PID</b>: %s</dt>'%node.node_info.pid 1214 text += '<dt><b>ORG.MASTERURI</b>: %s</dt>'%node.node_info.masteruri 1215 if not is_legal_name(node.name): 1216 text += '<dt><font color="#CC0000"><b>This node has an illegal <node> name.<br><a href="http://ros.org/wiki/Names">http://ros.org/wiki/Names</a><br>This will likely cause problems with other ROS tools.</b></font></dt>' 1217 if node.is_ghost: 1218 if node.name.endswith('master_sync') or node.name.endswith('node_manager'): 1219 text += '<dt><font color="#FF9900"><b>This node is not synchronized by default. To get info about this node select the related host.</b></font></dt>' 1220 else: 1221 text += '<dt><font color="#FF9900"><b>The node is running on remote host, but is not synchronized, because of filter or errors while sync, see log of <i>master_sync</i></b></font></dt>' 1222 text += '<dt><font color="#FF9900"><i>Are you use the same ROS packages?</i></font></dt>' 1223 if node.has_running and node.node_info.pid is None and node.node_info.uri is None: 1224 text += '<dt><font color="#FF9900"><b>Where are nodes with the same name on remote hosts running. These will be terminated, if you run this node! (Only if master_sync is running or will be started somewhere!)</b></font></dt>' 1225 if not node.node_info.uri is None and node.node_info.masteruri != self.masteruri: 1226 text += '<dt><font color="#339900"><b>synchronized</b></font></dt>' 1227 if node.node_info.pid is None and not node.node_info.uri is None: 1228 if not node.node_info.isLocal: 1229 text += '<dt><font color="#FF9900"><b>remote nodes will not be ping, so they are always marked running</b></font>' 1230 else: 1231 text += '<dt><font color="#CC0000"><b>the node does not respond: </b></font>' 1232 text += '<a href="unregister_node://%s">unregister</a></dt>'%node.name 1233 text += '</dl>' 1234 text += self._create_html_list('Published Topics:', node.published, 'TOPIC_PUB', node.name) 1235 text += self._create_html_list('Subscribed Topics:', node.subscribed, 'TOPIC_SUB', node.name) 1236 text += self._create_html_list('Services:', node.services, 'SERVICE', node.name) 1237 # set loaunch file paths 1238 text += self._create_html_list('Loaded Launch Files:', launches, 'LAUNCH') 1239 text += self._create_html_list('Default Configurations:', default_cfgs, 'NODE') 1240 text += '<dt><a href="copy_log_path://%s">copy log path to clipboard</a></dt>'%node.name 1241 text = '<div>%s</div>'%text 1242 name = node.name 1243 elif len(selectedNodes) > 1: 1244 # stoppable_nodes = [sn for sn in selectedNodes if not sn.node_info.uri is None and not self._is_in_ignore_list(sn.name)] 1245 restartable_nodes = [sn for sn in selectedNodes if len(sn.cfgs) > 0 and not self._is_in_ignore_list(sn.name)] 1246 killable_nodes = [sn for sn in selectedNodes if not sn.node_info.pid is None and not self._is_in_ignore_list(sn.name)] 1247 unregisterble_nodes = [sn for sn in selectedNodes if sn.node_info.pid is None and not sn.node_info.uri is None and sn.node_info.isLocal and not self._is_in_ignore_list(sn.name)] 1248 # add description for multiple selected nodes 1249 if restartable_nodes or killable_nodes or unregisterble_nodes: 1250 text += '<b>Selected nodes:</b><br>' 1251 if restartable_nodes: 1252 text += '<a href="restart_node://all_selected_nodes">restart [%d]</a>'%len(restartable_nodes) 1253 if killable_nodes or unregisterble_nodes: 1254 text += ' - ' 1255 if killable_nodes: 1256 text += '<a href="kill_node://all_selected_nodes">kill [%d]</a> - '%len(killable_nodes) 1257 text += '<a href="kill_screen://all_selected_nodes">kill screen [%d]</a>'%len(killable_nodes) 1258 if unregisterble_nodes: 1259 text += ' - ' 1260 if unregisterble_nodes: 1261 text += '<a href="unregister_node://all_selected_nodes">unregister [%d]</a>'%len(unregisterble_nodes) 1262 if restartable_nodes: 1263 text += '<br><a href="start_node_at_host://all_selected_nodes">start@host [%d]</a>'%len(restartable_nodes) 1264 1265 if (self.__last_info_type == 'Node' and self.__last_info_text != text) or force_emit: 1266 self.__last_info_text = text 1267 self.description_signal.emit(name, text) 1268 self.updateButtons()
1269
1270 - def on_topic_selection_changed(self, selected, deselected, force_emit=False, topic_name=''):
1271 ''' 1272 updates the Buttons, create a description and emit L{description_signal} to 1273 show the description of selected topic 1274 ''' 1275 if topic_name and not self.master_info is None: 1276 selectedTopics = [self.master_info.getTopic("%s"%topic_name)] 1277 if selectedTopics[0] is None: 1278 return 1279 else: 1280 if self.masterTab.tabWidget.tabText(self.masterTab.tabWidget.currentIndex()) != 'Topics': 1281 return 1282 selectedTopics = self.topicsFromIndexes(self.masterTab.topicsView.selectionModel().selectedIndexes()) 1283 topics_selected = (len(selectedTopics) > 0) 1284 self.masterTab.echoTopicButton.setEnabled(topics_selected) 1285 self.masterTab.hzTopicButton.setEnabled(topics_selected) 1286 self.masterTab.pubStopTopicButton.setEnabled(topics_selected) 1287 if len(selectedTopics) == 1: 1288 topic = selectedTopics[0] 1289 ns, sep, name = topic.name.rpartition(rospy.names.SEP) 1290 text = '<font size="+1"><b><span style="color:gray;">%s%s</span><b>%s</b></font><br>'%(ns, sep, name) 1291 text += '<a href="topicecho://%s%s">echo</a> - '%(self.mastername, topic.name) 1292 text += '<a href="topichz://%s%s">hz</a>'%(self.mastername, topic.name) 1293 text += '<p>' 1294 text += self._create_html_list('Publisher:', topic.publisherNodes, 'NODE') 1295 text += self._create_html_list('Subscriber:', topic.subscriberNodes, 'NODE') 1296 text += '<b><u>Type:</u></b> %s'%self._href_from_msgtype(topic.type) 1297 text += '<dl>' 1298 try: 1299 mclass = roslib.message.get_message_class(topic.type) 1300 if not mclass is None: 1301 for f in mclass.__slots__: 1302 idx = mclass.__slots__.index(f) 1303 idtype = mclass._slot_types[idx] 1304 # base_type = roslib.msgs.base_msg_type(idtype) 1305 # primitive = "unknown" 1306 # if base_type in roslib.msgs.PRIMITIVE_TYPES: 1307 # primitive = "primitive" 1308 # else: 1309 # try: 1310 # primitive =roslib.message.get_message_class(base_type) 1311 ## primitive = "class", list_msg_class.__slots__ 1312 # except ValueError: 1313 # pass 1314 text += '%s: <span style="color:gray;">%s</span><br>'%(f, idtype) 1315 text += '<br>' 1316 except ValueError: 1317 pass 1318 text += '</dl>' 1319 info_text = '<div>%s</div>'%text 1320 if (self.__last_info_type == 'Topic' and self.__last_info_text != info_text) or force_emit: 1321 self.__last_info_text = info_text 1322 self.description_signal.emit(topic.name, info_text)
1323
1324 - def _href_from_msgtype(self, type):
1325 result = type 1326 if type: 1327 result = '<a href="http://ros.org/doc/api/%s.html">%s</a>'%(type.replace('/', '/html/msg/'), type) 1328 return result
1329
1330 - def on_service_selection_changed(self, selected, deselected, force_emit=False, service_name=''):
1331 ''' 1332 updates the Buttons, create a description and emit L{description_signal} to 1333 show the description of selected service 1334 ''' 1335 if service_name and not self.master_info is None: 1336 # get service by name 1337 selectedServices = [self.master_info.getService("%s"%service_name)] 1338 if selectedServices[0] is None: 1339 return 1340 else: 1341 # get service by selected items 1342 selectedServices = self.servicesFromIndexes(self.masterTab.servicesView.selectionModel().selectedIndexes()) 1343 self.masterTab.callServiceButton.setEnabled(len(selectedServices) > 0) 1344 if self.masterTab.tabWidget.tabText(self.masterTab.tabWidget.currentIndex()) != 'Services': 1345 return 1346 if len(selectedServices) == 1: 1347 service = selectedServices[0] 1348 ns, sep, name = service.name.rpartition(rospy.names.SEP) 1349 text = '<font size="+1"><b><span style="color:gray;">%s%s</span><b>%s</b></font><br>'%(ns, sep, name) 1350 text += '<a href="servicecall://%s%s">call</a>'%(self.mastername, service.name) 1351 text += '<dl>' 1352 text += '<dt><b>URI</b>: %s</dt>'%service.uri 1353 text += '<dt><b>ORG.MASTERURI</b>: %s</dt>'%service.masteruri 1354 text += self._create_html_list('Provider:', service.serviceProvider, 'NODE') 1355 if service.masteruri != self.masteruri: 1356 text += '<dt><font color="#339900"><b>synchronized</b></font></dt>' 1357 text += '</dl>' 1358 try: 1359 service_class = service.get_service_class(nm.is_local(nm.nameres().getHostname(service.uri))) 1360 text += '<h4>%s</h4>'%self._href_from_svrtype(service_class._type) 1361 text += '<b><u>Request:</u></b>' 1362 text += '<dl><dt>%s</dt></dl>'%service_class._request_class.__slots__ 1363 text += '<b><u>Response:</u></b>' 1364 text += '<dl><dt>%s</dt></dl>'%service_class._response_class.__slots__ 1365 except: 1366 text += '<h4><font color=red>Unknown service type</font></h4>' 1367 if service.isLocal: 1368 text += '<font color=red>Unable to communicate with service, is provider node running?</font>' 1369 else: 1370 text += '<font color=red>Try to refresh <b>all</b> hosts. Is provider node running?</font>' 1371 info_text = '<div>%s</div>'%text 1372 if (self.__last_info_type == 'Service' and self.__last_info_text != info_text) or force_emit: 1373 self.__last_info_text = info_text 1374 self.description_signal.emit(service.name, info_text)
1375
1376 - def _href_from_svrtype(self, type):
1377 result = type 1378 if type: 1379 result = '<a href="http://ros.org/doc/api/%s.html">%s</a>'%(type.replace('/', '/html/srv/'), type) 1380 return result
1381
1382 - def on_parameter_selection_changed(self, selected, deselected):
1383 selectedParameter = self.parameterFromIndexes(self.masterTab.parameterView.selectionModel().selectedIndexes()) 1384 self.masterTab.deleteParameterButton.setEnabled(len(selectedParameter) > 0) 1385 self.masterTab.saveParameterButton.setEnabled(len(selectedParameter) > 0)
1386
1387 - def hostsFromIndexes(self, indexes, recursive=True):
1388 result = [] 1389 for index in indexes: 1390 if index.column() == 0: 1391 item = self.node_tree_model.itemFromIndex(index) 1392 if not item is None: 1393 if isinstance(item, HostItem): 1394 result.append(item) 1395 return result
1396
1397 - def groupsFromIndexes(self, indexes, recursive=True):
1398 result = [] 1399 for index in indexes: 1400 if index.column() == 0 and index.parent().isValid(): 1401 item = self.node_tree_model.itemFromIndex(index) 1402 if not item is None: 1403 if isinstance(item, GroupItem): 1404 result.append(item) 1405 return result
1406
1407 - def nodesFromIndexes(self, indexes, recursive=True):
1408 result = [] 1409 for index in indexes: 1410 if index.column() == 0: 1411 item = self.node_tree_model.itemFromIndex(index) 1412 res = self._nodesFromItems(item, recursive) 1413 for r in res: 1414 if not r in result: 1415 result.append(r) 1416 return result
1417
1418 - def _nodesFromItems(self, item, recursive):
1419 result = [] 1420 if not item is None: 1421 if isinstance(item, (GroupItem, HostItem)): 1422 if recursive: 1423 for j in range(item.rowCount()): 1424 result[len(result):] = self._nodesFromItems(item.child(j), recursive) 1425 elif isinstance(item, NodeItem): 1426 if not item in result: 1427 result.append(item) 1428 return result
1429
1430 - def topicsFromIndexes(self, indexes):
1431 result = [] 1432 for index in indexes: 1433 model_index = self.topic_proxyModel.mapToSource(index) 1434 item = self.topic_model.itemFromIndex(model_index) 1435 if not item is None and isinstance(item, TopicItem): 1436 result.append(item.topic) 1437 return result
1438
1439 - def servicesFromIndexes(self, indexes):
1440 result = [] 1441 for index in indexes: 1442 model_index = self.service_proxyModel.mapToSource(index) 1443 item = self.service_model.itemFromIndex(model_index) 1444 if not item is None and isinstance(item, ServiceItem): 1445 result.append(item.service) 1446 return result
1447
1448 - def parameterFromIndexes(self, indexes):
1449 result = [] 1450 for index in indexes: 1451 model_index = self.parameter_proxyModel.mapToSource(index) 1452 item = self.parameter_model.itemFromIndex(model_index) 1453 if not item is None and isinstance(item, ParameterValueItem): 1454 result.append((item.name, item.value)) 1455 return result
1456 1457 1458 #%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 1459 #%%%%%%%%%%%%% Handling of the button activities %%%%%%%% 1460 #%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 1461
1462 - def on_start_clicked(self):
1463 ''' 1464 Starts the selected nodes. If for a node more then one configuration is 1465 available, the selection dialog will be show. 1466 ''' 1467 key_mod = QtGui.QApplication.keyboardModifiers() 1468 if (key_mod & QtCore.Qt.ShiftModifier or key_mod & QtCore.Qt.ControlModifier): 1469 self.masterTab.startButton.showMenu() 1470 else: 1471 cursor = self.cursor() 1472 self.masterTab.startButton.setEnabled(False) 1473 self.setCursor(QtCore.Qt.WaitCursor) 1474 try: 1475 selectedNodes = self.nodesFromIndexes(self.masterTab.nodeTreeView.selectionModel().selectedIndexes()) 1476 self.start_nodes(selectedNodes) 1477 finally: 1478 self.setCursor(cursor) 1479 self.masterTab.startButton.setEnabled(True)
1480
1481 - def start_node(self, node, force, config, force_host=None):
1482 1483 if node is None: 1484 raise DetailedError("Start error", 'None is not valid node name!') 1485 if node.pid is None or force: 1486 # start the node using launch configuration 1487 if config is None: 1488 raise DetailedError("Start error", 1489 'Error while start %s:\nNo configuration found!'%node.name) 1490 if isinstance(config, LaunchConfig): 1491 try: 1492 nm.starter().runNode(node.name, config, force_host, self.masteruri, user=self.current_user) 1493 except socket.error as se: 1494 rospy.logwarn("Error while start '%s': %s\n\n Start canceled!", node.name, str(se)) 1495 raise DetailedError("Start error", 1496 'Error while start %s\n\nStart canceled!'%node.name, 1497 '%s'%se) 1498 return False 1499 except nm.InteractionNeededError as ie: 1500 raise 1501 except (Exception, nm.StartException) as e: 1502 print type(e) 1503 import traceback 1504 print traceback.format_exc() 1505 rospy.logwarn("Error while start '%s': %s"%(node.name, e)) 1506 raise DetailedError("Start error", 'Error while start %s'%node.name, '%s'%e) 1507 elif isinstance(config, (str, unicode)): 1508 # start with default configuration 1509 from multimaster_msgs_fkie.srv import Task 1510 try: 1511 nm.starter().callService(self.master_info.getService(config).uri, config, Task, [node.name]) 1512 except (Exception, nm.StartException) as e: 1513 # socket_error = (str(e).find("timeout") or str(e).find("113")) 1514 rospy.logwarn("Error while call a service of node '%s': %s"%(node.name, e)) 1515 raise DetailedError("Service error", 1516 'Error while call a service of node %s [%s]'%(node.name, self.master_info.getService(config).uri), 1517 '%s'%e)
1518
1519 - def start_nodes(self, nodes, force=False, force_host=None):
1520 ''' 1521 Internal method to start a list with nodes 1522 @param nodes: the list with nodes to start 1523 @type nodes: C{[L{NodeItem}, ...]} 1524 @param force: force the start of the node, also if it is already started. 1525 @type force: C{bool} 1526 @param force_host: force the start of the node at specified host. 1527 @type force_host: C{str} 1528 ''' 1529 cfg_choices = dict() 1530 cfg_nodes = dict() 1531 for node in nodes: 1532 # do not start node, if it is in ingnore list and multiple nodes are selected 1533 if (node.pid is None or (not node.pid is None and force)) and not node.is_ghost: 1534 # test for duplicate nodes 1535 if node.uri is None and node.has_running: 1536 ret = QtGui.QMessageBox.question(self, 'Question', ''.join(['Some nodes, e.g. ', node.name, ' are already running on another host. If you start this node the other node will be terminated.\n Do you want proceed?']), QtGui.QMessageBox.Yes, QtGui.QMessageBox.No) 1537 if ret == QtGui.QMessageBox.No: 1538 return 1539 # determine the used configuration 1540 choices = self._getCfgChoises(node) 1541 ch_keys = choices.keys() 1542 if ch_keys: 1543 ch_keys.sort() 1544 choises_str = str(ch_keys) 1545 if not choises_str in cfg_choices.keys(): 1546 choice, ok = self._getUserCfgChoice(choices, node.name) 1547 if not choice is None: 1548 cfg_choices[choises_str] = choices[choice] 1549 cfg_nodes[node.name] = choices[choice] 1550 elif ok: 1551 res = WarningMessageBox(QtGui.QMessageBox.Warning, "Start error", 1552 ''.join(['Error while start ', node.name, ':\nNo configuration selected!'])).exec_() 1553 else: 1554 break 1555 else: 1556 cfg_nodes[node.name] = cfg_choices[choises_str] 1557 1558 # put into the queue and start 1559 for node in nodes: 1560 if node.name in cfg_nodes: 1561 self._progress_queue.add2queue(str(uuid.uuid4()), 1562 ''.join(['start ', node.node_info.name]), 1563 self.start_node, 1564 (node.node_info, force, cfg_nodes[node.node_info.name], force_host)) 1565 self._progress_queue.start()
1566
1567 - def start_nodes_by_name(self, nodes, cfg, force=False):
1568 ''' 1569 Start nodes given in a list by their names. 1570 @param nodes: a list with full node names 1571 @type nodes: C{[str]} 1572 ''' 1573 result = [] 1574 if not self.master_info is None: 1575 for n in nodes: 1576 node_info = NodeInfo(n, self.masteruri) 1577 node_item = NodeItem(node_info) 1578 node_item.addConfig(cfg) 1579 result.append(node_item) 1580 self.start_nodes(result, force)
1581
1582 - def on_force_start_nodes(self):
1583 ''' 1584 Starts the selected nodes (also if it already running). If for a node more then one configuration is 1585 available, the selection dialog will be show. 1586 ''' 1587 cursor = self.cursor() 1588 self.masterTab.startButton.setEnabled(False) 1589 self.setCursor(QtCore.Qt.WaitCursor) 1590 try: 1591 selectedNodes = self.nodesFromIndexes(self.masterTab.nodeTreeView.selectionModel().selectedIndexes()) 1592 self.stop_nodes(selectedNodes) 1593 self.start_nodes(selectedNodes, True) 1594 finally: 1595 self.setCursor(cursor) 1596 self.masterTab.startButton.setEnabled(True)
1597
1598 - def on_start_nodes_at_host(self):
1599 ''' 1600 Starts the selected nodes on an another host. 1601 ''' 1602 cursor = self.cursor() 1603 self.masterTab.startButton.setEnabled(False) 1604 params = {'Host' : ('string', 'localhost') } 1605 dia = ParameterDialog(params) 1606 dia.setFilterVisible(False) 1607 dia.setWindowTitle('Start node on...') 1608 dia.resize(350,120) 1609 dia.setFocusField('host') 1610 if dia.exec_(): 1611 try: 1612 params = dia.getKeywords() 1613 host = params['Host'] 1614 self.setCursor(QtCore.Qt.WaitCursor) 1615 try: 1616 selectedNodes = self.nodesFromIndexes(self.masterTab.nodeTreeView.selectionModel().selectedIndexes()) 1617 self.start_nodes(selectedNodes, True, host) 1618 finally: 1619 self.setCursor(cursor) 1620 except Exception, e: 1621 WarningMessageBox(QtGui.QMessageBox.Warning, "Start error", 1622 'Error while parse parameter', 1623 str(e)).exec_() 1624 self.masterTab.startButton.setEnabled(True)
1625
1626 - def _getDefaultCfgChoises(self, node):
1627 result = {} 1628 for c in node.cfgs: 1629 if isinstance(c, tuple): 1630 result[' '.join(['[default]', c[0]])] = roslib.names.ns_join(c[0], 'run') 1631 return result
1632
1633 - def _getCfgChoises(self, node, ignore_defaults=False):
1634 result = {} 1635 for c in node.cfgs: 1636 if c: 1637 if not isinstance(c, tuple): 1638 launch = self.launchfiles[c] 1639 result[''.join([str(launch.LaunchName), ' [', str(launch.PackageName), ']'])] = self.launchfiles[c] 1640 elif not ignore_defaults: 1641 result[' '.join(['[default]', c[0]])] = roslib.names.ns_join(c[0], 'run') 1642 return result
1643
1644 - def _getUserCfgChoice(self, choices, nodename):
1645 value = None 1646 ok = False 1647 # Open selection 1648 if len(choices) == 1: 1649 value = choices.keys()[0] 1650 ok = True 1651 elif len(choices) > 0: 1652 items, ok = SelectDialog.getValue('Configuration selection', 'Select configuration to launch <b>%s</b>'%nodename, choices.keys(), True) 1653 if items: 1654 value = items[0] 1655 return value, ok
1656
1657 - def on_stop_clicked(self):
1658 ''' 1659 Stops the selected and running nodes. If the node can't be stopped using his 1660 RPC interface, it will be unregistered from the ROS master using the masters 1661 RPC interface. 1662 ''' 1663 key_mod = QtGui.QApplication.keyboardModifiers() 1664 if (key_mod & QtCore.Qt.ShiftModifier or key_mod & QtCore.Qt.ControlModifier): 1665 self.masterTab.stopButton.showMenu() 1666 else: 1667 cursor = self.cursor() 1668 self.setCursor(QtCore.Qt.WaitCursor) 1669 try: 1670 selectedNodes = self.nodesFromIndexes(self.masterTab.nodeTreeView.selectionModel().selectedIndexes()) 1671 self.stop_nodes(selectedNodes) 1672 finally: 1673 self.setCursor(cursor)
1674
1675 - def stop_node(self, node, force=False):
1676 if not node is None and not node.uri is None and (not self._is_in_ignore_list(node.name) or force): 1677 try: 1678 rospy.loginfo("Stop node '%s'[%s]", str(node.name), str(node.uri)) 1679 #'print "STOP set timeout", node 1680 socket.setdefaulttimeout(10) 1681 #'print "STOP create xmlrpc", node 1682 p = xmlrpclib.ServerProxy(node.uri) 1683 #'print "STOP send stop", node 1684 p.shutdown(rospy.get_name(), ''.join(['[node manager] request from ', self.mastername])) 1685 #'print "STOP stop finished", node 1686 except Exception, e: 1687 # import traceback 1688 # formatted_lines = traceback.format_exc().splitlines() 1689 rospy.logwarn("Error while stop node '%s': %s", str(node.name), str(e)) 1690 if str(e).find(' 111') == 1: 1691 # self.masterTab.stopButton.setEnabled(False) 1692 raise DetailedError("Stop error", 1693 ''.join(['Error while stop node ', node.name]), 1694 str(e)) 1695 finally: 1696 socket.setdefaulttimeout(None) 1697 elif not node is None and node.is_ghost: 1698 #since for ghost nodes no info is available, emit a signal to handle the 1699 # stop message in other master_view_proxy 1700 self.stop_nodes_signal.emit(node.masteruri, [node.name]) 1701 return True
1702
1703 - def stop_nodes(self, nodes):
1704 ''' 1705 Internal method to stop a list with nodes 1706 @param nodes: the list with nodes to stop 1707 @type nodes: C{[L{master_discovery_fkie.NodeInfo}, ...]} 1708 ''' 1709 # put into the queue and start the que handling 1710 for node in nodes: 1711 self._progress_queue.add2queue(str(uuid.uuid4()), 1712 ''.join(['stop ', node.name]), 1713 self.stop_node, 1714 (node, (len(nodes)==1))) 1715 self._progress_queue.start()
1716
1717 - def stop_nodes_by_name(self, nodes):
1718 ''' 1719 Stop nodes given in a list by their names. 1720 @param nodes: a list with full node names 1721 @type nodes: C{[str]} 1722 ''' 1723 result = [] 1724 if not self.master_info is None: 1725 for n in nodes: 1726 node = self.master_info.getNode(n) 1727 if not node is None: 1728 result.append(node) 1729 self.stop_nodes(result)
1730
1731 - def kill_node(self, node, force=False):
1732 if not node is None and not node.uri is None and (not self._is_in_ignore_list(node.name) or force): 1733 pid = node.pid 1734 if pid is None: 1735 # try to get the process id of the node 1736 try: 1737 socket.setdefaulttimeout(10) 1738 rpc_node = xmlrpclib.ServerProxy(node.uri) 1739 code, msg, pid = rpc_node.getPid(rospy.get_name()) 1740 except: 1741 # self.masterTab.stopButton.setEnabled(False) 1742 pass 1743 finally: 1744 socket.setdefaulttimeout(None) 1745 # kill the node 1746 if not pid is None: 1747 try: 1748 self._progress_queue.add2queue(str(uuid.uuid4()), 1749 ''.join(['kill ', node.name, '(', str(pid), ')']), 1750 nm.starter().kill, 1751 (self.getHostFromNode(node), pid, False, self.current_user)) 1752 self._progress_queue.start() 1753 except Exception as e: 1754 rospy.logwarn("Error while kill the node %s: %s", str(node.name), str(e)) 1755 raise DetailedError("Kill error", 1756 ''.join(['Error while kill the node ', node.name]), 1757 str(e)) 1758 return True
1759 1760
1761 - def on_kill_nodes(self):
1762 selectedNodes = self.nodesFromIndexes(self.masterTab.nodeTreeView.selectionModel().selectedIndexes()) 1763 1764 # put into the queue and start the que handling 1765 for node in selectedNodes: 1766 self._progress_queue.add2queue(str(uuid.uuid4()), 1767 ''.join(['kill ', node.name]), 1768 self.kill_node, 1769 (node, (len(selectedNodes)==1))) 1770 self._progress_queue.start()
1771
1772 - def unregister_node(self, node, force=False):
1773 if not node is None and not node.uri is None and (not self._is_in_ignore_list(node.name) or force): 1774 # stop the node? 1775 # try: 1776 # p = xmlrpclib.ServerProxy(node.uri) 1777 # p.shutdown(rospy.get_name(), ''.join(['[node manager] request from ', self.hostname])) 1778 # except Exception, e: 1779 # rospy.logwarn("Error while stop node '%s': %s", str(node.name), str(e)) 1780 # self.masterTab.stopButton.setEnabled(False) 1781 # unregister all entries of the node from ROS master 1782 try: 1783 socket.setdefaulttimeout(10) 1784 master = xmlrpclib.ServerProxy(node.masteruri) 1785 master_multi = xmlrpclib.MultiCall(master) 1786 # master_multi.deleteParam(node.name, node.name) 1787 for p in node.published: 1788 rospy.loginfo("unregister publisher '%s' [%s] from ROS master: %s", p, node.name, node.masteruri) 1789 master_multi.unregisterPublisher(node.name, p, node.uri) 1790 for t in node.subscribed: 1791 rospy.loginfo("unregister subscriber '%s' [%s] from ROS master: %s", t, node.name, node.masteruri) 1792 master_multi.unregisterSubscriber(node.name, t, node.uri) 1793 if not self.master_state is None: 1794 for s in node.services: 1795 rospy.loginfo("unregister service '%s' [%s] from ROS master: %s", s, node.name, node.masteruri) 1796 service = self.master_info.getService(s) 1797 if not (service is None): 1798 master_multi.unregisterService(node.name, s, service.uri) 1799 r = master_multi() 1800 for code, msg, _ in r: 1801 if code != 1: 1802 rospy.logwarn("unregistration failed: %s", msg) 1803 except Exception, e: 1804 rospy.logwarn("Error while unregister node %s: %s", str(node.name), str(e)) 1805 raise DetailedError("Unregister error", 1806 ''.join(['Error while Unregister node ', node.name]), 1807 str(e)) 1808 finally: 1809 socket.setdefaulttimeout(None) 1810 return True
1811
1812 - def on_unregister_nodes(self):
1813 selectedNodes = self.nodesFromIndexes(self.masterTab.nodeTreeView.selectionModel().selectedIndexes()) 1814 # put into the queue and start the que handling 1815 for node in selectedNodes: 1816 self._progress_queue.add2queue(str(uuid.uuid4()), 1817 ''.join(['unregister node ', node.name]), 1818 self.unregister_node, 1819 (node, (len(selectedNodes)==1))) 1820 self._progress_queue.start()
1821
1822 - def on_stop_context_toggled(self, state):
1823 menu = QtGui.QMenu(self) 1824 self.killAct = QtGui.QAction("&Kill Node", self, shortcut=QtGui.QKeySequence.New, statusTip="Kill selected node", triggered=self.kill_nodes) 1825 self.unregAct = QtGui.QAction("&Unregister Nodes...", self, shortcut=QtGui.QKeySequence.Open, statusTip="Open an existing file", triggered=self.unreg_nodes) 1826 menu.addAction(self.killAct) 1827 menu.addAction(self.unregAct) 1828 menu.exec_(self.masterTab.stopContextButton.pos())
1829 1830
1831 - def getHostFromNode(self, node):
1832 ''' 1833 If the node is running the host the node URI will be returned. Otherwise 1834 tries to get the host from the launch configuration. If the configuration 1835 contains no machine assignment for this node the host of the ROS master URI 1836 will be used. 1837 @param node: 1838 @type node: L{master_discovery_fkie.NodeInfo} 1839 ''' 1840 if not node.uri is None: 1841 return nm.nameres().getHostname(node.uri) 1842 # try to get it from the configuration 1843 for c in node.cfgs: 1844 if not isinstance(c, tuple): 1845 launch_config = self.__configs[c] 1846 item = launch_config.getNode(node.name) 1847 if not item is None and item.machine_name and not item.machine_name == 'localhost': 1848 return launch_config.Roscfg.machines[item.machine_name].address 1849 # return the host of the assigned ROS master 1850 return nm.nameres().getHostname(node.masteruri)
1851
1852 - def on_io_clicked(self):
1853 ''' 1854 Shows IO of the selected nodes. 1855 ''' 1856 key_mod = QtGui.QApplication.keyboardModifiers() 1857 if (key_mod & QtCore.Qt.ShiftModifier or key_mod & QtCore.Qt.ControlModifier): 1858 self.masterTab.ioButton.showMenu() 1859 else: 1860 cursor = self.cursor() 1861 self.setCursor(QtCore.Qt.WaitCursor) 1862 try: 1863 selectedNodes = self.nodesFromIndexes(self.masterTab.nodeTreeView.selectionModel().selectedIndexes()) 1864 if selectedNodes: 1865 ret = True 1866 if len(selectedNodes) > 5: 1867 ret = QtGui.QMessageBox.question(self, "Show IO", "You are going to open the IO of "+ str(len(selectedNodes)) + " nodes at once\nContinue?", QtGui.QMessageBox.Ok, QtGui.QMessageBox.Cancel) 1868 ret = (ret == QtGui.QMessageBox.Ok) 1869 if ret: 1870 for node in selectedNodes: 1871 self._progress_queue.add2queue(str(uuid.uuid4()), 1872 ''.join(['show IO of ', node.name]), 1873 nm.screen().openScreen, 1874 (node.name, self.getHostFromNode(node), False, self.current_user)) 1875 self._progress_queue.start() 1876 else: 1877 self.on_show_all_screens() 1878 finally: 1879 self.setCursor(cursor)
1880
1881 - def on_kill_screens(self):
1882 ''' 1883 Kills selected screens, if some available. 1884 ''' 1885 cursor = self.cursor() 1886 self.setCursor(QtCore.Qt.WaitCursor) 1887 try: 1888 selectedNodes = self.nodesFromIndexes(self.masterTab.nodeTreeView.selectionModel().selectedIndexes()) 1889 for node in selectedNodes: 1890 self._progress_queue.add2queue(str(uuid.uuid4()), 1891 ''.join(['kill screen of ', node.name]), 1892 nm.screen().killScreens, 1893 (node.name, self.getHostFromNode(node), False, self.current_user)) 1894 self._progress_queue.start() 1895 finally: 1896 self.setCursor(cursor)
1897
1898 - def on_show_all_screens(self):
1899 ''' 1900 Shows all available screens. 1901 ''' 1902 cursor = self.cursor() 1903 self.setCursor(QtCore.Qt.WaitCursor) 1904 try: 1905 host = nm.nameres().getHostname(self.masteruri) 1906 sel_screen = [] 1907 try: 1908 screens = nm.screen().getActiveScreens(host, auto_pw_request=True, user=self.current_user) 1909 sel_screen, ok = SelectDialog.getValue('Open screen', '', screens, False, False, self) 1910 except Exception, e: 1911 rospy.logwarn("Error while get screen list: %s", str(e)) 1912 WarningMessageBox(QtGui.QMessageBox.Warning, "Screen list error", 1913 ''.join(['Error while get screen list from ', host]), 1914 str(e)).exec_() 1915 for screen in sel_screen: 1916 try: 1917 if not nm.screen().openScreenTerminal(host, screen, screen, self.current_user): 1918 # self.masterTab.ioButton.setEnabled(False) 1919 pass 1920 except Exception, e: 1921 rospy.logwarn("Error while show IO for %s: %s", str(screen), str(e)) 1922 WarningMessageBox(QtGui.QMessageBox.Warning, "Show IO error", 1923 ''.join(['Error while show IO ', screen, ' on ', host]), 1924 str(e)).exec_() 1925 finally: 1926 self.setCursor(cursor)
1927
1928 - def on_log_clicked(self):
1929 ''' 1930 Shows log files of the selected nodes. 1931 ''' 1932 try: 1933 key_mod = QtGui.QApplication.keyboardModifiers() 1934 if (key_mod & QtCore.Qt.ShiftModifier or key_mod & QtCore.Qt.ControlModifier): 1935 self.masterTab.logButton.showMenu() 1936 else: 1937 selectedNodes = self.nodesFromIndexes(self.masterTab.nodeTreeView.selectionModel().selectedIndexes()) 1938 ret = True 1939 if len(selectedNodes) > 5: 1940 ret = QtGui.QMessageBox.question(self, "Show Log", "You are going to open the logs of "+ str(len(selectedNodes)) + " nodes at once\nContinue?", QtGui.QMessageBox.Ok, QtGui.QMessageBox.Cancel) 1941 ret = (ret == QtGui.QMessageBox.Ok) 1942 if ret: 1943 for node in selectedNodes: 1944 self._progress_queue.add2queue(str(uuid.uuid4()), 1945 ''.join(['show log of ', node.name]), 1946 nm.starter().openLog, 1947 (node.name, self.getHostFromNode(node), self.current_user)) 1948 self._progress_queue.start() 1949 except Exception, e: 1950 import traceback 1951 print traceback.format_exc() 1952 rospy.logwarn("Error while show log: %s", str(e)) 1953 WarningMessageBox(QtGui.QMessageBox.Warning, "Show log error", 1954 'Error while show Log', 1955 str(e)).exec_()
1956
1957 - def on_log_path_copy(self):
1958 selectedNodes = self.nodesFromIndexes(self.masterTab.nodeTreeView.selectionModel().selectedIndexes()) 1959 nodenames = [] 1960 for n in selectedNodes: 1961 nodenames.append(n.name) 1962 try: 1963 host = nm.nameres().getHostname(self.masteruri) 1964 path_on_host = nm.starter().copylogPath2Clipboards(host, nodenames, True) 1965 QtGui.QApplication.clipboard().setText(''.join([getpass.getuser() if self.is_local else self.current_user, '@', host, ':', path_on_host])) 1966 except Exception as e: 1967 WarningMessageBox(QtGui.QMessageBox.Warning, "Get log path", 1968 'Error while get log path', 1969 str(e)).exec_()
1970 # self._progress_queue.add2queue(str(uuid.uuid4()), 1971 # 'Get log path', 1972 # nm.starter().copylogPath2Clipboards, 1973 # (nm.nameres().getHostname(self.masteruri), nodenames)) 1974 # self._progress_queue.start() 1975 1976 # def on_log_show_selected(self): 1977 # try: 1978 # nm.screen().LOG_PATH. 1979 # screens = nm.screen().getActiveScreens(host, auto_pw_request=True) 1980 # sel_screen, ok = SelectDialog.getValue('Open log', '', screens, False, self) 1981 # except Exception, e: 1982 # rospy.logwarn("Error while get screen list: %s", str(e)) 1983 # WarningMessageBox(QtGui.QMessageBox.Warning, "Screen list error", 1984 # ''.join(['Error while get screen list from ', host]), 1985 # str(e)).exec_() 1986
1987 - def on_log_delete_clicked(self):
1988 ''' 1989 Deletes log files of the selected nodes. 1990 ''' 1991 selectedNodes = self.nodesFromIndexes(self.masterTab.nodeTreeView.selectionModel().selectedIndexes()) 1992 for node in selectedNodes: 1993 self._progress_queue.add2queue(str(uuid.uuid4()), 1994 ''.join(['delete Log of ', node.name]), 1995 nm.starter().deleteLog, 1996 (node.name, self.getHostFromNode(node), False, self.current_user)) 1997 self._progress_queue.start()
1998
1999 - def on_dynamic_config_clicked(self):
2000 ''' 2001 Opens the dynamic configuration dialogs for selected nodes. 2002 ''' 2003 if not self.master_info is None: 2004 selectedNodes = self.nodesFromIndexes(self.masterTab.nodeTreeView.selectionModel().selectedIndexes()) 2005 for n in selectedNodes: 2006 try: 2007 nodes = sorted([srv_name[:-len('/set_parameters')] for srv_name, srv in self.master_info.services.items() if (srv_name.endswith('/set_parameters') and n.name in srv.serviceProvider)]) 2008 items = [] 2009 if len(nodes) == 1: 2010 items = nodes 2011 elif len(nodes) > 1: 2012 items, ok = SelectDialog.getValue('Dynamic configuration selection', '', [i for i in nodes]) 2013 if items is None: 2014 items = [] 2015 if len(items) > 3: 2016 ret = QtGui.QMessageBox.question(self, 'Start dynamic reconfigure','It will starts %s dynamic reconfigure nodes?\n\n Are you sure?'%str(len(items)), QtGui.QMessageBox.Yes, QtGui.QMessageBox.No) 2017 if ret != QtGui.QMessageBox.Yes: 2018 return 2019 for node in items: 2020 # self.masterTab.dynamicConfigButton.setEnabled(False) 2021 import os, subprocess 2022 env = dict(os.environ) 2023 env["ROS_MASTER_URI"] = str(self.master_info.masteruri) 2024 rospy.loginfo("Start dynamic reconfiguration for '%s'", str(node)) 2025 ps = subprocess.Popen(['rosrun', 'node_manager_fkie', 'dynamic_reconfigure', node, '__ns:=dynamic_reconfigure'], env=env) 2026 # wait for process to avoid 'defunct' processes 2027 thread = threading.Thread(target=ps.wait) 2028 thread.setDaemon(True) 2029 thread.start() 2030 except Exception, e: 2031 rospy.logwarn("Start dynamic reconfiguration for '%s' failed: %s", str(n.name), str(e)) 2032 WarningMessageBox(QtGui.QMessageBox.Warning, "Start dynamic reconfiguration error", 2033 ''.join(['Start dynamic reconfiguration for ', str(n.name), ' failed!']), 2034 str(e)).exec_()
2035
2036 - def on_edit_config_clicked(self):
2037 ''' 2038 ''' 2039 selectedNodes = self.nodesFromIndexes(self.masterTab.nodeTreeView.selectionModel().selectedIndexes()) 2040 for node in selectedNodes: 2041 choices = self._getCfgChoises(node, True) 2042 choice, ok = self._getUserCfgChoice(choices, node.name) 2043 config = choices[choice] if choices and choice else '' 2044 if ok and isinstance(config, LaunchConfig): 2045 # get the file, which include the node and the main configuration file 2046 node_cfg = config.getNode(node.name) 2047 files = [config.Filename] 2048 if not node_cfg.filename in files: 2049 files.append(node_cfg.filename) 2050 self.request_xml_editor.emit(files, ''.join(['name="', os.path.basename(node.name), '"']))
2051
2052 - def on_edit_rosparam_clicked(self):
2053 ''' 2054 ''' 2055 selectedNodes = self.nodesFromIndexes(self.masterTab.nodeTreeView.selectionModel().selectedIndexes()) 2056 for node in selectedNodes: 2057 # set the parameter in the ROS parameter server 2058 try: 2059 inputDia = MasterParameterDialog(node.masteruri if not node.masteruri is None else self.masteruri, ''.join([node.name, roslib.names.SEP]), parent=self) 2060 inputDia.setWindowTitle(' - '.join([os.path.basename(node.name), "parameter"])) 2061 if node.has_launch_cfgs(node.cfgs): 2062 inputDia.add_warning("The changes may not have any effect, because the launch file was also loaded as not 'default' and the parameter in the launch file will be reloaded on start of the ROS node.") 2063 inputDia.show() 2064 except: 2065 import traceback 2066 rospy.logwarn("Error on retrieve parameter for %s: %s", str(node.name), traceback.format_exc())
2067
2068 - def on_save_clicked(self):
2069 (fileName, filter) = QtGui.QFileDialog.getSaveFileName(self, 2070 "New launch file", 2071 self.__current_path, 2072 "Config files (*.launch);;All files (*)") 2073 if fileName: 2074 self.__current_path = os.path.dirname(fileName) 2075 try: 2076 (pkg, pkg_path) = package_name(os.path.dirname(fileName)) 2077 if pkg is None: 2078 ret = WarningMessageBox(QtGui.QMessageBox.Warning, "New File Error", 2079 'The new file is not in a ROS package', buttons=QtGui.QMessageBox.Ok|QtGui.QMessageBox.Cancel).exec_() 2080 if ret == QtGui.QMessageBox.Cancel: 2081 return 2082 with open(fileName, 'w+') as f: 2083 f.write("<launch>\n") 2084 for lfile in self.launchfiles.keys(): 2085 with open(lfile, 'r') as lf: 2086 f.write(re.sub('<\/?launch\ *\t*>', '', lf.read())) 2087 f.write("</launch>\n") 2088 except EnvironmentError as e: 2089 WarningMessageBox(QtGui.QMessageBox.Warning, "New File Error", 2090 'Error while create a new file', 2091 str(e)).exec_()
2092
2093 - def on_close_clicked(self):
2094 ''' 2095 Closes the open launch configurations. If more then one configuration is 2096 open a selection dialog will be open. 2097 ''' 2098 choices = dict() 2099 2100 for path, cfg in self.__configs.items(): 2101 if isinstance(path, tuple): 2102 if path[2] == self.masteruri: 2103 choices[''.join(['[', path[0], ']'])] = path 2104 else: 2105 choices[''.join([os.path.basename(path), ' [', str(package_name(os.path.dirname(path))[0]), ']'])] = path 2106 cfgs = [] 2107 2108 # if len(choices) > 1: 2109 cfgs, ok = SelectDialog.getValue('Close configurations', '', choices.keys(), False, False, self) 2110 # elif len(choices) == 1: 2111 # cfgs = choices.values()[0] 2112 2113 # close configurations 2114 for c in cfgs: 2115 self._close_cfg(choices[c]) 2116 self.updateButtons() 2117 self.update_robot_icon()
2118
2119 - def _close_cfg(self, cfg):
2120 try: 2121 self.removeConfigFromModel(cfg) 2122 if isinstance(cfg, tuple): 2123 if not self.master_info is None: 2124 # close default configuration: stop the default_cfg node 2125 node = self.master_info.getNode(cfg[0]) 2126 if not node is None: 2127 self.stop_nodes([node]) 2128 # else: 2129 # # remove machine entries 2130 # try: 2131 # for name, machine in self.__configs[cfg].Roscfg.machines.items(): 2132 # if machine.name: 2133 # nm.nameres().remove(name=machine.name, host=machine.address) 2134 # except: 2135 # import traceback 2136 # print traceback.format_exc() 2137 else: 2138 # remove from name resolution 2139 try: 2140 for name, machine in self.__configs[cfg].Roscfg.machines.items(): 2141 if machine.name: 2142 nm.nameres().removeInfo(machine.name, machine.address) 2143 except: 2144 pass 2145 del self.__configs[cfg] 2146 except: 2147 import traceback 2148 print traceback.format_exc() 2149 pass
2150
2151 - def on_topic_echo_clicked(self):
2152 ''' 2153 Shows the output of the topic in a terminal. 2154 ''' 2155 self._show_topic_output(False)
2156
2157 - def on_topic_hz_clicked(self):
2158 ''' 2159 Shows the hz of the topic in a terminal. 2160 ''' 2161 self._show_topic_output(True)
2162
2163 - def on_topic_pub_clicked(self):
2164 selectedTopics = self.topicsFromIndexes(self.masterTab.topicsView.selectionModel().selectedIndexes()) 2165 if len(selectedTopics) > 0: 2166 for topic in selectedTopics: 2167 if not self._start_publisher(topic.name, topic.type): 2168 break 2169 else: # create a new topic 2170 # fill the input fields 2171 # determine the list all available message types 2172 root_paths = [os.path.normpath(p) for p in os.getenv("ROS_PACKAGE_PATH").split(':')] 2173 packages = {} 2174 msg_types = [] 2175 for p in root_paths: 2176 ret = get_packages(p) 2177 packages = dict(ret.items() + packages.items()) 2178 for (p, direc) in packages.items(): 2179 import rosmsg 2180 for file in rosmsg._list_types('/'.join([direc, 'msg']), 'msg', rosmsg.MODE_MSG): 2181 msg_types.append("%s/%s"%(p, file)) 2182 msg_types.sort() 2183 fields = {'Type' : ('string', msg_types), 'Name' : ('string', [''])} 2184 2185 #create a dialog 2186 dia = ParameterDialog(fields, parent=self) 2187 dia.setWindowTitle('Publish to topic') 2188 dia.setFilterVisible(False) 2189 dia.resize(300, 95) 2190 dia.setFocusField('Name') 2191 if dia.exec_(): 2192 params = dia.getKeywords() 2193 try: 2194 if params['Name'] and params['Type']: 2195 try: 2196 self._start_publisher(params['Name'], params['Type']) 2197 except Exception, e: 2198 import traceback 2199 print traceback.format_exc() 2200 rospy.logwarn("Publish topic '%s' failed: %s", str(params['Name']), str(e)) 2201 WarningMessageBox(QtGui.QMessageBox.Warning, "Publish topic error", 2202 ''.join(['Publish topic ', params['Name'], ' failed!']), 2203 str(e)).exec_() 2204 else: 2205 WarningMessageBox(QtGui.QMessageBox.Warning, "Invalid name or type", 2206 ''.join(["Can't publish to topic '", params['Name'], "' with type '", params['Type'], "'!"])).exec_() 2207 except (KeyError, ValueError), e: 2208 WarningMessageBox(QtGui.QMessageBox.Warning, "Warning", 2209 'Error while add a parameter to the ROS parameter server', 2210 str(e)).exec_()
2211
2212 - def _start_publisher(self, topic_name, topic_type):
2213 try: 2214 topic_name = roslib.names.ns_join(roslib.names.SEP, topic_name) 2215 mclass = roslib.message.get_message_class(topic_type) 2216 if mclass is None: 2217 WarningMessageBox(QtGui.QMessageBox.Warning, "Publish error", 2218 'Error while publish to %s'%topic_name, 2219 ''.join(['invalid message type: ', topic_type,'.\nIf this is a valid message type, perhaps you need to run "rosmake"'])).exec_() 2220 return 2221 slots = mclass.__slots__ 2222 types = mclass._slot_types 2223 args = ServiceDialog._params_from_slots(slots, types) 2224 p = { '! Publish rate' : ('string', ['once', 'latch', '1']), topic_type : ('dict', args) } 2225 dia = ParameterDialog(p) 2226 dia.setWindowTitle(''.join(['Publish to ', topic_name])) 2227 dia.showLoadSaveButtons() 2228 dia.resize(450,300) 2229 dia.setFocusField('! Publish rate') 2230 2231 if dia.exec_(): 2232 params = dia.getKeywords() 2233 rate = params['! Publish rate'] 2234 opt_str = '' 2235 opt_name_suf = '__latch_' 2236 if rate == 'latch': 2237 opt_str = '' 2238 elif rate == 'once' or rate == '-1': 2239 opt_str = '--once' 2240 opt_name_suf = '__once_' 2241 else: 2242 try: 2243 i = int(rate) 2244 if i > 0: 2245 opt_str = ''.join(['-r ', rate]) 2246 opt_name_suf = ''.join(['__', rate, 'Hz_']) 2247 except: 2248 pass 2249 # remove empty lists 2250 topic_params = self._rem_empty_lists(params[topic_type]) 2251 # print params[topic_type] 2252 # print topic_params 2253 pub_cmd = ' '.join(['pub', topic_name, topic_type, '"', str(topic_params), '"', opt_str]) 2254 # nm.starter().runNodeWithoutConfig(nm.nameres().address(self.masteruri), 'rostopic', 'rostopic', ''.join(['rostopic_pub', topic_name, opt_name_suf, str(rospy.Time.now())]), args=[pub_cmd], masteruri=self.masteruri) 2255 self._progress_queue.add2queue(str(uuid.uuid4()), 2256 ''.join(['start publisher for ', topic_name]), 2257 nm.starter().runNodeWithoutConfig, 2258 (nm.nameres().address(self.masteruri), 'rostopic', 'rostopic', ''.join(['rostopic_pub', topic_name, opt_name_suf, str(rospy.Time.now())]), [pub_cmd], self.masteruri, False, self.current_user)) 2259 self._progress_queue.start() 2260 return True 2261 else: 2262 return False 2263 except Exception, e: 2264 rospy.logwarn("Publish topic '%s' failed: %s", str(topic_name), str(e)) 2265 WarningMessageBox(QtGui.QMessageBox.Warning, "Publish topic error", 2266 ''.join(['Publish topic ', topic_name, ' failed!']), 2267 str(e)).exec_() 2268 import traceback 2269 print traceback.format_exc() 2270 return False
2271
2272 - def _rem_empty_lists(self, param_dict):
2273 result = dict() 2274 for key, value in param_dict.iteritems(): 2275 if isinstance(value, dict): 2276 result[key] = self._rem_empty_lists(value) 2277 elif not (isinstance(value, list) and not value): 2278 result[key] = value 2279 return result
2280
2281 - def on_topic_pub_stop_clicked(self):
2282 selectedTopics = self.topicsFromIndexes(self.masterTab.topicsView.selectionModel().selectedIndexes()) 2283 if not self.master_info is None: 2284 nodes2stop = [] 2285 for topic in selectedTopics: 2286 topic_prefix = ''.join(['/rostopic_pub', topic.name, '_']) 2287 node_names = self.master_info.node_names 2288 for n in node_names: 2289 if n.startswith(topic_prefix): 2290 nodes2stop.append(n) 2291 self.stop_nodes_by_name(nodes2stop)
2292
2293 - def _show_topic_output(self, show_hz_only):
2294 ''' 2295 Shows the output of the topic in a terminal. 2296 ''' 2297 selectedTopics = self.topicsFromIndexes(self.masterTab.topicsView.selectionModel().selectedIndexes()) 2298 ret = True 2299 if len(selectedTopics) > 5: 2300 ret = QtGui.QMessageBox.question(self, "Show echo", "You are going to open the echo of "+ str(len(selectedTopics)) + " topics at once\nContinue?", QtGui.QMessageBox.Ok, QtGui.QMessageBox.Cancel) 2301 ret = (ret == QtGui.QMessageBox.Ok) 2302 if ret: 2303 for topic in selectedTopics: 2304 self._add_topic_output2queue(topic, show_hz_only)
2305
2306 - def show_topic_output(self, topic_name, show_hz_only):
2307 ''' 2308 Shows the topic output in a new window. 2309 ''' 2310 if not self.master_info is None: 2311 topic = self.master_info.getTopic("%s"%topic_name) 2312 if not topic is None: 2313 self._add_topic_output2queue(topic, show_hz_only) 2314 else: 2315 rospy.logwarn("topic not found: %s"%topic_name)
2316
2317 - def _add_topic_output2queue(self, topic, show_hz_only):
2318 try: 2319 # connect to topic on remote host 2320 import os, shlex, subprocess 2321 env = dict(os.environ) 2322 env["ROS_MASTER_URI"] = str(self.masteruri) 2323 cmd = ' '.join(['rosrun', 'node_manager_fkie', 'node_manager', '--echo', topic.name, topic.type, '--hz' if show_hz_only else '', ''.join(['__name:=echo_','hz_' if show_hz_only else '',str(nm.nameres().getHostname(self.masteruri)), topic.name])]) 2324 rospy.loginfo("Echo topic: %s", cmd) 2325 ps = subprocess.Popen(shlex.split(cmd), env=env, close_fds=True) 2326 self.__echo_topics_dialogs[topic.name] = ps 2327 # wait for process to avoid 'defunct' processes 2328 thread = threading.Thread(target=ps.wait) 2329 thread.setDaemon(True) 2330 thread.start() 2331 except Exception, e: 2332 rospy.logwarn("Echo topic '%s' failed: %s", str(topic.name), str(e)) 2333 WarningMessageBox(QtGui.QMessageBox.Warning, "Echo of topic error", 2334 ''.join(['Echo of topic ', topic.name, ' failed!']), 2335 str(e)).exec_()
2336
2337 - def _topic_dialog_closed(self, topic_name):
2338 if self.__echo_topics_dialogs.has_key(topic_name): 2339 del self.__echo_topics_dialogs[topic_name]
2340
2341 - def on_service_call_clicked(self):
2342 ''' 2343 calls a service. 2344 ''' 2345 selectedServices = self.servicesFromIndexes(self.masterTab.servicesView.selectionModel().selectedIndexes()) 2346 for service in selectedServices: 2347 param = ServiceDialog(service, self) 2348 param.show()
2349
2350 - def service_call(self, service_name):
2351 service = self.master_info.getService(str(service_name)) 2352 if not service is None: 2353 param = ServiceDialog(service, self) 2354 param.show()
2355
2356 - def on_topic_filter_changed(self, text):
2357 ''' 2358 Filter the displayed topics 2359 ''' 2360 self.topic_proxyModel.setFilterRegExp(QtCore.QRegExp(text, QtCore.Qt.CaseInsensitive, QtCore.QRegExp.Wildcard))
2361
2362 - def on_service_filter_changed(self, text):
2363 ''' 2364 Filter the displayed services 2365 ''' 2366 self.service_proxyModel.setFilterRegExp(QtCore.QRegExp(text, QtCore.Qt.CaseInsensitive, QtCore.QRegExp.Wildcard))
2367
2368 - def on_parameter_filter_changed(self, text):
2369 ''' 2370 Filter the displayed parameter 2371 ''' 2372 self.parameter_proxyModel.setFilterRegExp(QtCore.QRegExp(text, QtCore.Qt.CaseInsensitive, QtCore.QRegExp.Wildcard))
2373
2374 - def on_get_parameter_clicked(self):
2375 ''' 2376 Requests parameter list from the ROS parameter server. 2377 ''' 2378 self.parameterHandler.requestParameterList(self.masteruri)
2379
2380 - def on_add_parameter_clicked(self):
2381 ''' 2382 Adds a parameter to the ROS parameter server. 2383 ''' 2384 selectedParameter = self.parameterFromIndexes(self.masterTab.parameterView.selectionModel().selectedIndexes()) 2385 ns = '/' 2386 if selectedParameter: 2387 ns = roslib.names.namespace(selectedParameter[0][0]) 2388 fields = {'name' : ('string', ['', ns]), 'type' : ('string', ['string', 'int', 'float', 'bool', 'list']), 'value': ('string', '')} 2389 newparamDia = ParameterDialog(fields, parent=self) 2390 newparamDia.setWindowTitle('Add new parameter') 2391 newparamDia.setFilterVisible(False) 2392 newparamDia.resize(300, 140) 2393 if newparamDia.exec_(): 2394 params = newparamDia.getKeywords() 2395 try: 2396 if params['type'] == 'int': 2397 value = int(params['value']) 2398 elif params['type'] == 'float': 2399 value = float(params['value']) 2400 elif params['type'] == 'bool': 2401 value = bool(params['value'].lower() in ("yes", "true", "t", "1")) 2402 elif params['type'] == 'list': 2403 try: 2404 import yaml 2405 value = [yaml.load(params['value'])] 2406 # if there is no YAML, load() will return an 2407 # empty string. We want an empty dictionary instead 2408 # for our representation of empty. 2409 if value is None: 2410 value = [] 2411 except yaml.MarkedYAMLError, e: 2412 QtGui.QMessageBox.warning(self, self.tr("Warning"), "yaml error: %s"%str(e), QtGui.QMessageBox.Ok) 2413 return 2414 else: 2415 value = params['value'] 2416 self.parameterHandler.deliverParameter(self.masteruri, {params['name'] : value}) 2417 self.parameterHandler.requestParameterList(self.masteruri) 2418 except (KeyError, ValueError), e: 2419 WarningMessageBox(QtGui.QMessageBox.Warning, "Warning", 2420 'Error while add a parameter to the ROS parameter server', 2421 str(e)).exec_()
2422
2424 ''' 2425 Deletes the parameter from the ROS parameter server. 2426 ''' 2427 selectedParameter = self.parameterFromIndexes(self.masterTab.parameterView.selectionModel().selectedIndexes()) 2428 try: 2429 socket.setdefaulttimeout(15) 2430 name = rospy.get_name() 2431 master = xmlrpclib.ServerProxy(self.masteruri) 2432 master_multi = xmlrpclib.MultiCall(master) 2433 for (key, value) in selectedParameter: 2434 master_multi.deleteParam(name, key) 2435 r = master_multi() 2436 for code, msg, parameter in r: 2437 if code != 1: 2438 rospy.logwarn("Error on delete parameter '%s': %s", parameter, msg) 2439 except: 2440 import traceback 2441 rospy.logwarn("Error on delete parameter: %s", traceback.format_exc()) 2442 WarningMessageBox(QtGui.QMessageBox.Warning, "Warning", 2443 'Error while delete a parameter to the ROS parameter server', 2444 traceback.format_exc()).exec_() 2445 else: 2446 self.on_get_parameter_clicked() 2447 finally: 2448 socket.setdefaulttimeout(None)
2449
2450 - def on_save_parameter_clicked(self):
2451 ''' 2452 Stores selected parameter to a file. 2453 ''' 2454 selectedParameter = self.parameterFromIndexes(self.masterTab.parameterView.selectionModel().selectedIndexes()) 2455 if selectedParameter: 2456 (fileName, filter) = QtGui.QFileDialog.getSaveFileName(self, 2457 "Save parameter", 2458 self.__current_path, 2459 "YAML files (*.yaml);;All files (*)") 2460 if fileName: 2461 self.__current_path = os.path.dirname(fileName) 2462 try: 2463 with open(fileName, 'w+') as f: 2464 values = dict() 2465 #convert ROS namespaces of parameters to YAML namespaces 2466 for (key, value) in selectedParameter: 2467 keys = key.strip(rospy.names.SEP).split(rospy.names.SEP) 2468 curr_v = values 2469 for k in keys: 2470 if curr_v.has_key(k): 2471 curr_v = curr_v[k] 2472 elif k != keys[-1]: 2473 curr_v[k] = dict() 2474 curr_v = curr_v[k] 2475 else: 2476 curr_v[k] = value 2477 import yaml 2478 # print yaml.dump(values, default_flow_style=False) 2479 f.write(yaml.dump(values, default_flow_style=False)) 2480 except Exception as e: 2481 import traceback 2482 print traceback.format_exc() 2483 WarningMessageBox(QtGui.QMessageBox.Warning, "Save parameter Error", 2484 'Error while save parameter', 2485 str(e)).exec_()
2486
2487 - def _replaceDoubleSlash(self, liste):
2488 ''' 2489 used to avoid the adding of \\ for each \ in a string of a list 2490 ''' 2491 if liste and isinstance(liste, list): 2492 result = [] 2493 for l in liste: 2494 val = l 2495 if isinstance(l, (str, unicode)): 2496 val = l.replace("\\n", "\n") 2497 # result.append("".join([val])) 2498 elif isinstance(l, list): 2499 val = self._replaceDoubleSlash(l) 2500 result.append(val) 2501 return result 2502 return liste
2503
2504 - def _on_parameter_item_changed(self, item):
2505 ''' 2506 add changes to the ROS parameter server 2507 ''' 2508 if isinstance(item, ParameterValueItem): 2509 try: 2510 if isinstance(item.value, bool): 2511 value = bool(item.text().lower() in ("yes", "true", "t", "1")) 2512 elif isinstance(item.value, int): 2513 value = int(item.text()) 2514 elif isinstance(item.value, float): 2515 value = float(item.text()) 2516 elif isinstance(item.value, list): 2517 try: 2518 import yaml 2519 value = yaml.load(item.text()) 2520 # if there is no YAML, load() will return an 2521 # empty string. We want an empty dictionary instead 2522 # for our representation of empty. 2523 if value is None: 2524 value = [] 2525 value = self._replaceDoubleSlash(value) 2526 except yaml.MarkedYAMLError, e: 2527 QtGui.QMessageBox.warning(self, self.tr("Warning"), "yaml error: %s"%str(e), QtGui.QMessageBox.Ok) 2528 item.setText(unicode(item.value)) 2529 return 2530 else: 2531 value = item.text() 2532 self.parameterHandler.deliverParameter(self.masteruri, {item.name : value}) 2533 item.value = value 2534 except ValueError, e: 2535 WarningMessageBox(QtGui.QMessageBox.Warning, "Warning", 2536 'Error while add changes to the ROS parameter server', 2537 str(e)).exec_() 2538 item.setText(item.value)
2539
2540 - def _on_param_list(self, masteruri, code, msg, params):
2541 ''' 2542 @param masteruri: The URI of the ROS parameter server 2543 @type masteruri: C{str} 2544 @param code: The return code of the request. If not 1, the message is set and the list can be ignored. 2545 @type code: C{int} 2546 @param msg: The message of the result. 2547 @type msg: C{str} 2548 @param params: The list the parameter names. 2549 @type param: C{[str]} 2550 ''' 2551 if code == 1: 2552 params.sort() 2553 self.parameterHandler.requestParameterValues(masteruri, params)
2554
2555 - def _on_param_values(self, masteruri, code, msg, params):
2556 ''' 2557 @param masteruri: The URI of the ROS parameter server 2558 @type masteruri: C{str} 2559 @param code: The return code of the request. If not 1, the message is set and the list can be ignored. 2560 @type code: C{int} 2561 @param msg: The message of the result. 2562 @type msg: C{str} 2563 @param params: The dictionary the parameter names and request result. 2564 @type param: C{dict(paramName : (code, statusMessage, parameterValue))} 2565 ''' 2566 if code == 1: 2567 result = {} 2568 for p, (code_n, msg_n, val) in params.items(): 2569 if code_n == 1: 2570 result[p] = val 2571 else: 2572 result[p] = '' 2573 if p == '/use_sim_time': 2574 self.__use_sim_time = (code_n == 1 and val) 2575 self.parameter_model.updateModelData(result) 2576 else: 2577 rospy.logwarn("Error on retrieve parameter from %s: %s", str(masteruri), str(msg))
2578
2579 - def _on_delivered_values(self, masteruri, code, msg, params):
2580 ''' 2581 @param masteruri: The URI of the ROS parameter server 2582 @type masteruri: C{str} 2583 @param code: The return code of the request. If not 1, the message is set and the list can be ignored. 2584 @type code: C{int} 2585 @param msg: The message of the result. 2586 @type msg: C{str} 2587 @param params: The dictionary the parameter names and request result. 2588 @type param: C{dict(paramName : (code, statusMessage, parameterValue))} 2589 ''' 2590 errmsg = '' 2591 if code == 1: 2592 for p, (code_n, msg, val) in params.items(): 2593 if code_n != 1: 2594 errmsg = '\n'.join([errmsg, msg]) 2595 else: 2596 errmsg = msg if msg else 'Unknown error on set parameter' 2597 if errmsg: 2598 WarningMessageBox(QtGui.QMessageBox.Warning, "Warning", 2599 'Error while delivering parameter to the ROS parameter server', 2600 errmsg).exec_()
2601
2602 - def _on_sim_param_values(self, masteruri, code, msg, params):
2603 ''' 2604 @param masteruri: The URI of the ROS parameter server 2605 @type masteruri: C{str} 2606 @param code: The return code of the request. If not 1, the message is set and the list can be ignored. 2607 @type code: C{int} 2608 @param msg: The message of the result. 2609 @type msg: C{str} 2610 @param params: The dictionary the parameter names and request result. 2611 @type param: C{dict(paramName : (code, statusMessage, parameterValue))} 2612 ''' 2613 robot_icon_found = False 2614 if code == 1: 2615 result = {} 2616 for p, (code_n, msg_n, val) in params.items(): 2617 if p == '/use_sim_time': 2618 self.__use_sim_time = (code_n == 1 and val) 2619 elif p == '/robot_icon': 2620 robot_icon_found = True 2621 self.__current_parameter_robot_icon = val if code_n == 1 else '' 2622 self.update_robot_icon() 2623 elif p.startswith('/roslaunch/uris'): 2624 if code_n == 1: 2625 for key, value in val.items(): 2626 self.launch_server_handler.updateLaunchServerInfo(value) 2627 else: 2628 rospy.logwarn("Error on retrieve sim parameter value from %s: %s", str(masteruri), msg) 2629 if not robot_icon_found: 2630 self.__current_parameter_robot_icon = '' 2631 self.update_robot_icon()
2632
2633 - def _get_nm_masteruri(self):
2634 ''' 2635 Requests the ROS master URI from the ROS master through the RPC interface and 2636 returns it. The 'materuri' attribute will be set to the requested value. 2637 @return: ROS master URI 2638 @rtype: C{str} or C{None} 2639 ''' 2640 if not hasattr(self, '_nm_materuri') or self._nm_materuri is None: 2641 masteruri = masteruri_from_ros() 2642 master = xmlrpclib.ServerProxy(masteruri) 2643 code, message, self._nm_materuri = master.getUri(rospy.get_name()) 2644 return self._nm_materuri
2645 2646 2647 #%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2648 #%%%%%%%%%%%%% Shortcuts handling %%%%%%%% 2649 #%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2650
2651 - def select_host_block(self, index):
2652 ''' 2653 Selects all nodes of a host with given index 2654 @param index: the index of the host in the tree model 2655 @type index: C{int} 2656 ''' 2657 root = self.masterTab.nodeTreeView.model().index(index, 0); 2658 if not root.isValid(): 2659 return 2660 self.masterTab.nodeTreeView.expand(root) 2661 # firstChild = root.child(0, 0) 2662 last_row_index = len(self.node_tree_model.header)-1 2663 # lastChild = root.child(0, last_row_index) 2664 i = 0 2665 selection = QtGui.QItemSelection() 2666 while root.child(i, 0).isValid(): 2667 index = root.child(i, 0) 2668 item = self.node_tree_model.itemFromIndex(index) 2669 if not item is None and not self._is_in_ignore_list(item.name): 2670 selection.append(QtGui.QItemSelectionRange(index, root.child(i, last_row_index))) 2671 i = i + 1 2672 # selection = QtGui.QItemSelection(firstChild, lastChild) 2673 self.masterTab.nodeTreeView.selectionModel().select(selection, QtGui.QItemSelectionModel.ClearAndSelect)
2674
2675 - def _is_in_ignore_list(self, name):
2676 for i in self._stop_ignores: 2677 if name.endswith(i): 2678 return True 2679 return False
2680
2681 - def on_shortcut1_activated(self):
2682 self.select_host_block(0)
2683 - def on_shortcut2_activated(self):
2684 self.select_host_block(1)
2685 - def on_shortcut3_activated(self):
2686 self.select_host_block(2)
2687 - def on_shortcut4_activated(self):
2688 self.select_host_block(3)
2689 - def on_shortcut5_activated(self):
2690 self.select_host_block(4)
2691
2692 - def on_shortcut_collapse_all(self):
2693 self.masterTab.nodeTreeView.selectionModel().clearSelection() 2694 self.masterTab.nodeTreeView.collapseAll()
2695
2696 - def on_copy_x_pressed(self):
2697 result = '' 2698 if self.masterTab.nodeTreeView.hasFocus(): 2699 selectedNodes = self.nodesFromIndexes(self.masterTab.nodeTreeView.selectionModel().selectedIndexes()) 2700 for node in selectedNodes: 2701 try: 2702 result = ' '.join([result, str(node.pid)]) 2703 except Exception: 2704 pass 2705 elif self.masterTab.topicsView.hasFocus(): 2706 selectedTopics = self.topicsFromIndexes(self.masterTab.topicsView.selectionModel().selectedIndexes()) 2707 for topic in selectedTopics: 2708 try: 2709 result = ' '.join([result, topic.type]) 2710 except Exception: 2711 pass 2712 elif self.masterTab.servicesView.hasFocus(): 2713 selectedServices = self.servicesFromIndexes(self.masterTab.servicesView.selectionModel().selectedIndexes()) 2714 for service in selectedServices: 2715 try: 2716 result = ' '.join([result, service.type]) 2717 except Exception: 2718 pass 2719 elif self.masterTab.parameterView.hasFocus(): 2720 selectedParameter = self.parameterFromIndexes(self.masterTab.parameterView.selectionModel().selectedIndexes()) 2721 for (key, value) in selectedParameter: 2722 try: 2723 result = ' '.join([result, str(value)]) 2724 except Exception: 2725 pass 2726 QtGui.QApplication.clipboard().setText(result.strip())
2727
2728 2729 #%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2730 #%%%%%%%%%%%%% Filter handling %%%%%%%%%%% 2731 #%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2732 2733 -class TopicsSortFilterProxyModel(QtGui.QSortFilterProxyModel):
2734 - def filterAcceptsRow(self, sourceRow, sourceParent):
2735 ''' 2736 Perform filtering on columns 0 and 3 (Name, Type) 2737 ''' 2738 index0 = self.sourceModel().index(sourceRow, 0, sourceParent) 2739 index3 = self.sourceModel().index(sourceRow, 3, sourceParent) 2740 regex = self.filterRegExp() 2741 return (regex.indexIn(self.sourceModel().data(index0, TopicItem.NAME_ROLE)) != -1 2742 or regex.indexIn(self.sourceModel().data(index3)) != -1)
2743
2744 -class ServicesSortFilterProxyModel(QtGui.QSortFilterProxyModel):
2745 - def filterAcceptsRow(self, sourceRow, sourceParent):
2746 ''' 2747 Perform filtering on columns 0 and 1 (Name, Type) 2748 ''' 2749 index0 = self.sourceModel().index(sourceRow, 0, sourceParent) 2750 # index1 = self.sourceModel().index(sourceRow, 1, sourceParent) 2751 regex = self.filterRegExp() 2752 return (regex.indexIn(self.sourceModel().data(index0, ServiceItem.NAME_ROLE)) != -1 2753 or regex.indexIn(self.sourceModel().data(index0, ServiceItem.TYPE_ROLE)) != -1)
2754
2755 -class ParameterSortFilterProxyModel(QtGui.QSortFilterProxyModel):
2756 - def filterAcceptsRow(self, sourceRow, sourceParent):
2757 ''' 2758 Perform filtering on columns 0 and 1 (Name, Value) 2759 ''' 2760 index0 = self.sourceModel().index(sourceRow, 0, sourceParent) 2761 index1 = self.sourceModel().index(sourceRow, 1, sourceParent) 2762 index2 = self.sourceModel().index(sourceRow, 2, sourceParent) 2763 regex = self.filterRegExp() 2764 return (regex.indexIn(self.sourceModel().data(index0, ParameterNameItem.NAME_ROLE)) != -1 2765 or regex.indexIn(self.sourceModel().data(index2, ParameterValueItem.VALUE_ROLE)) != -1)
2766