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 loadUi 
  34  from python_qt_binding.QtCore import QRegExp, Qt, Signal, QRect 
  35  from python_qt_binding.QtGui import QImage, QKeySequence, QBrush, QPen 
  36  from rosgraph.names import is_legal_name 
  37  import getpass 
  38  import os 
  39  import re 
  40  import roslib 
  41  import rospy 
  42  import socket 
  43  import sys 
  44  import time 
  45  import traceback 
  46  import uuid 
  47  import xmlrpclib 
  48  
 
  49  from master_discovery_fkie.common import get_hostname 
  50  from master_discovery_fkie.master_info import NodeInfo 
  51  from node_manager_fkie.common import masteruri_from_ros, get_packages, package_name, resolve_paths, utf8 
  52  from node_manager_fkie.default_cfg_handler import DefaultConfigHandler 
  53  from node_manager_fkie.detailed_msg_box import MessageBox, DetailedError 
  54  from node_manager_fkie.html_delegate import HTMLDelegate 
  55  from node_manager_fkie.launch_config import LaunchConfig  # , LaunchConfigException 
  56  from node_manager_fkie.launch_server_handler import LaunchServerHandler 
  57  from node_manager_fkie.message_frame import MessageData, MessageFrame 
  58  from node_manager_fkie.node_tree_model import NodeTreeModel, NodeItem, GroupItem, HostItem, CellItem 
  59  from node_manager_fkie.parameter_dialog import ParameterDialog, MasterParameterDialog, ServiceDialog 
  60  from node_manager_fkie.parameter_handler import ParameterHandler 
  61  from node_manager_fkie.parameter_list_model import ParameterModel, ParameterNameItem, ParameterValueItem 
  62  from node_manager_fkie.progress_queue import ProgressQueue, InteractionNeededError  # , ProgressThread 
  63  from node_manager_fkie.select_dialog import SelectDialog 
  64  from node_manager_fkie.service_list_model import ServiceModel, ServiceItem, ServiceGroupItem 
  65  from node_manager_fkie.start_handler import AdvRunCfg 
  66  from node_manager_fkie.supervised_popen import SupervisedPopen 
  67  from node_manager_fkie.topic_list_model import TopicModel, TopicItem, TopicGroupItem 
  68  import node_manager_fkie as nm 
  69  try: 
  70      from python_qt_binding.QtGui import QAction, QFileDialog, QMenu, QShortcut, QWidget 
  71      from python_qt_binding.QtGui import QApplication, QVBoxLayout, QItemDelegate, QStyle 
  72  except: 
  73      from python_qt_binding.QtWidgets import QAction, QFileDialog, QMenu, QShortcut, QWidget 
  74      from python_qt_binding.QtWidgets import QApplication, QVBoxLayout, QItemDelegate, QStyle 
  75  
 
  76  
 
  77  try: 
  78      from python_qt_binding.QtGui import QItemSelection, QItemSelectionModel, QItemSelectionRange, QSortFilterProxyModel 
  79  except: 
  80      from python_qt_binding.QtCore import QItemSelection, QItemSelectionModel, QItemSelectionRange, QSortFilterProxyModel 
81 82 83 -class LaunchArgsSelectionRequest(Exception):
84 ''' Request needed to set the args of a launchfile from another thread. 85 @param args: a dictionary with args and values 86 @type args: dict 87 @param error: an error description 88 @type error: str 89 ''' 90
91 - def __init__(self, launchfile, args, error):
92 Exception.__init__(self) 93 self.launchfile = launchfile 94 self.args_dict = args 95 self.error = error
96
97 - def __str__(self):
98 return "LaunchArgsSelectionRequest for " + utf8(self.args_dict) + "::" + repr(self.error)
99
100 101 -class MasterViewProxy(QWidget):
102 ''' 103 This class stores the informations about a ROS master and shows it on request. 104 ''' 105 106 updateHostRequest = Signal(str) 107 host_description_updated = Signal(str, str, str) 108 '''@ivar: the signal is emitted on description changes and contains the 109 ROS Master URI, host address and description a parameter.''' 110 111 capabilities_update_signal = Signal(str, str, str, list) 112 '''@ivar: the signal is emitted if a description with capabilities is received 113 and has the ROS master URI, host address, the name of the default_cfg node and a list with 114 descriptions (U{multimaster_msgs_fkie.srv.ListDescription<http://docs.ros.org/api/multimaster_msgs_fkie/html/srv/ListDescription.html>} Response) as parameter.''' 115 remove_config_signal = Signal(str) 116 '''@ivar: the signal is emitted if a default_cfg was removed''' 117 118 description_signal = Signal(str, str, bool) 119 '''@ivar: the signal is emitted to show a description (title, description)''' 120 121 request_xml_editor = Signal(list, str) 122 '''@ivar: the signal to open a xml editor dialog (list with files, search text)''' 123 124 stop_nodes_signal = Signal(str, list) 125 '''@ivar: the signal is emitted to stop on masteruri the nodes described in the list.''' 126 127 robot_icon_updated = Signal(str, str) 128 '''@ivar: the signal is emitted, if the robot icon was changed by a configuration (masteruri, path)''' 129 130 loaded_config = Signal(str, object) 131 '''@ivar: the signal is emitted, after a launchfile is successful loaded (launchfile, LaunchConfig)''' 132 133 save_profile_signal = Signal(str) 134 '''@ivar: the signal is emitted, to save profile. (masteruri) If masteruri is empty, save all masters else only for this master.''' 135 136 DIAGNOSTIC_LEVELS = {0: 'OK', 137 1: 'WARN', 138 2: 'ERROR', 139 3: 'STALE', 140 4: 'UNKNOWN', 141 5: 'UNKNOWN'} 142
143 - def __init__(self, masteruri, parent=None):
144 ''' 145 Creates a new master. 146 @param masteruri: the URI of the ROS master 147 @type masteruri: C{str} 148 ''' 149 QWidget.__init__(self, parent) 150 self.setObjectName(' - '.join(['MasterViewProxy', masteruri])) 151 self.masteruri = masteruri 152 self.mastername = masteruri 153 self.main_window = parent 154 try: 155 self.mastername = get_hostname(self.masteruri) 156 except: 157 pass 158 159 self._tmpObjects = [] 160 self.__master_state = None 161 self.__master_info = None 162 self.__force_update = False 163 self.__configs = dict() # [file name] : LaunchConfig or tuple(ROS node name, ROS service uri, ROS master URI) : ROS nodes 164 self.__online = False 165 self.__run_id = '' 166 # self.rosconfigs = dict() # [launch file path] = LaunchConfig() 167 self.__in_question = [] # stores the changed files, until the user is interacted 168 # self.__uses_confgs = dict() # stores the decisions of the user for used configuration to start of node 169 '''@ivar: stored the question dialogs for changed files ''' 170 self._stop_ignores = ['rosout', rospy.get_name(), 'node_manager', 'master_discovery', 'master_sync', 'default_cfg', 'zeroconf'] 171 ''' @todo: detect the names of master_discovery and master_sync ndoes''' 172 173 self.__echo_topics_dialogs = dict() # [topic name] = EchoDialog 174 '''@ivar: stores the open EchoDialogs ''' 175 self.__last_info_text = None 176 self.__use_sim_time = False 177 self.__current_user = nm.settings().host_user(self.mastername) 178 self.__robot_icons = [] 179 self.__current_robot_icon = None 180 self.__current_parameter_robot_icon = '' 181 self.__republish_params = {} # { topic : params, created by dialog} 182 self.__last_selection = 0 183 self._on_stop_kill_roscore = False 184 self._on_stop_poweroff = False 185 self._start_nodes_after_load_cfg = dict() 186 # store the running_nodes to update to duplicates after load a launch file 187 self.__running_nodes = dict() # dict (node name : masteruri) 188 self._nodelets = dict() # dict(launchfile: dict(nodelet manager: list(nodes)) 189 self.default_cfg_handler = DefaultConfigHandler() 190 self.default_cfg_handler.node_list_signal.connect(self.on_default_cfg_nodes_retrieved) 191 self.default_cfg_handler.description_signal.connect(self.on_default_cfg_descr_retrieved) 192 self.default_cfg_handler.err_signal.connect(self.on_default_cfg_err) 193 194 self.__launch_servers = {} # uri : (pid, nodes) 195 self.launch_server_handler = LaunchServerHandler() 196 self.launch_server_handler.launch_server_signal.connect(self.on_launch_server_retrieved) 197 self.launch_server_handler.error_signal.connect(self.on_launch_server_err) 198 199 self.masterTab = QWidget() 200 ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)), 'MasterTab.ui') 201 loadUi(ui_file, self.masterTab) 202 tabLayout = QVBoxLayout(self) 203 tabLayout.setContentsMargins(0, 0, 0, 0) 204 tabLayout.addWidget(self.masterTab) 205 self._progress_queue_prio = ProgressQueue(self.masterTab.progressPrioFrame, self.masterTab.progressPrioBar, self.masterTab.progressCancelPrioButton, 'Prio Master - %s' % self.mastername) 206 self._progress_queue = ProgressQueue(self.masterTab.progressFrame, self.masterTab.progressBar, self.masterTab.progressCancelButton, 'Master - %s' % self.mastername) 207 self._progress_queue_prio.no_screen_error_signal.connect(self._on_no_screen_error) 208 self._progress_queue.no_screen_error_signal.connect(self._on_no_screen_error) 209 210 # setup the node view 211 self.node_tree_model = NodeTreeModel(nm.nameres().address(self.masteruri), self.masteruri) 212 self.node_proxy_model = NodesSortFilterProxyModel(self) 213 self.node_proxy_model.setSourceModel(self.node_tree_model) 214 self.masterTab.nodeTreeView.setModel(self.node_proxy_model) 215 self.node_tree_model.hostInserted.connect(self.on_host_inserted) 216 for i, (_, width) in enumerate(NodeTreeModel.header): # _:=name 217 self.masterTab.nodeTreeView.setColumnWidth(i, width) 218 self.nodeNameDelegate = HTMLDelegate(check_for_ros_names=False, is_node=True) 219 self.masterTab.nodeTreeView.setItemDelegateForColumn(0, self.nodeNameDelegate) 220 self.node_delegate = IconsDelegate() 221 self.masterTab.nodeTreeView.setItemDelegateForColumn(1, self.node_delegate) 222 self.masterTab.nodeTreeView.collapsed.connect(self.on_node_collapsed) 223 self.masterTab.nodeTreeView.expanded.connect(self.on_node_expanded) 224 sm = self.masterTab.nodeTreeView.selectionModel() 225 sm.selectionChanged.connect(self.on_node_selection_changed) 226 self.masterTab.nodeTreeView.activated.connect(self.on_node_activated) 227 self.masterTab.nodeTreeView.clicked.connect(self.on_node_clicked) 228 # self.masterTab.nodeTreeView.setAcceptDrops(True) 229 # self.masterTab.nodeTreeWidget.setSortingEnabled(True) 230 231 # setup the topic view 232 self.topic_model = TopicModel() 233 self.topic_proxyModel = TopicsSortFilterProxyModel(self) 234 self.topic_proxyModel.setSourceModel(self.topic_model) 235 self.masterTab.topicsView.setModel(self.topic_proxyModel) 236 # self.masterTab.topicsView.setModel(self.topic_model) 237 self.masterTab.topicsView.expandAll() 238 self.masterTab.topicsView.sortByColumn(0, Qt.AscendingOrder) 239 for i, (_, width) in enumerate(TopicModel.header): # _:=name 240 self.masterTab.topicsView.setColumnWidth(i, width) 241 self.topicNameDelegate = HTMLDelegate(check_for_ros_names=False, is_node=True) 242 self.topicTypeDelegate = HTMLDelegate() 243 self.masterTab.topicsView.setItemDelegateForColumn(0, self.topicNameDelegate) 244 self.masterTab.topicsView.setItemDelegateForColumn(3, self.topicTypeDelegate) 245 sm = self.masterTab.topicsView.selectionModel() 246 sm.selectionChanged.connect(self.on_topic_selection_changed) 247 self.masterTab.topicsView.activated.connect(self.on_topic_activated) 248 self.masterTab.topicsView.clicked.connect(self.on_topic_clicked) 249 self.masterTab.topicsView.setSortingEnabled(True) 250 251 # setup the service view 252 self.service_model = ServiceModel() 253 self.service_proxyModel = ServicesSortFilterProxyModel(self) 254 self.service_proxyModel.setSourceModel(self.service_model) 255 self.masterTab.servicesView.setModel(self.service_proxyModel) 256 self.masterTab.servicesView.expandAll() 257 self.masterTab.servicesView.sortByColumn(0, Qt.AscendingOrder) 258 for i, (_, width) in enumerate(ServiceModel.header): # _:=name 259 self.masterTab.servicesView.setColumnWidth(i, width) 260 self.serviceNameDelegate = HTMLDelegate(check_for_ros_names=False, is_node=True) 261 self.serviceTypeDelegate = HTMLDelegate() 262 self.masterTab.servicesView.setItemDelegateForColumn(0, self.serviceNameDelegate) 263 self.masterTab.servicesView.setItemDelegateForColumn(1, self.serviceTypeDelegate) 264 sm = self.masterTab.servicesView.selectionModel() 265 sm.selectionChanged.connect(self.on_service_selection_changed) 266 self.masterTab.servicesView.activated.connect(self.on_service_activated) 267 self.masterTab.servicesView.clicked.connect(self.on_service_clicked) 268 self.masterTab.servicesView.setSortingEnabled(True) 269 270 # setup the parameter view 271 self.parameter_model = ParameterModel() 272 self.parameter_model.itemChanged.connect(self._on_parameter_item_changed) 273 self.parameter_proxyModel = ParameterSortFilterProxyModel(self) 274 self.parameter_proxyModel.setSourceModel(self.parameter_model) 275 self.masterTab.parameterView.setModel(self.parameter_proxyModel) 276 for i, (_, width) in enumerate(ParameterModel.header): # _:=name 277 self.masterTab.parameterView.setColumnWidth(i, width) 278 self.parameterNameDelegate = HTMLDelegate() 279 self.masterTab.parameterView.setItemDelegateForColumn(0, self.parameterNameDelegate) 280 sm = self.masterTab.parameterView.selectionModel() 281 sm.selectionChanged.connect(self.on_parameter_selection_changed) 282 self.masterTab.parameterView.setSortingEnabled(True) 283 284 # self.parameter_proxyModel.filterAcceptsRow = _filterParameterAcceptsRow 285 # self.masterTab.parameterView.activated.connect(self.on_service_activated) 286 287 # connect the buttons 288 self.masterTab.startButton.clicked.connect(self.on_start_clicked) 289 self.masterTab.stopButton.clicked.connect(self.on_stop_clicked) 290 # self.masterTab.stopContextButton.toggled.connect(self.on_stop_context_toggled) 291 self.masterTab.ioButton.clicked.connect(self.on_io_clicked) 292 self.masterTab.logButton.clicked.connect(self.on_log_clicked) 293 self.masterTab.logDeleteButton.clicked.connect(self.on_log_delete_clicked) 294 self.masterTab.dynamicConfigButton.clicked.connect(self.on_dynamic_config_clicked) 295 self.masterTab.editConfigButton.clicked.connect(self.on_edit_config_clicked) 296 self.masterTab.editRosParamButton.clicked.connect(self.on_edit_rosparam_clicked) 297 self.masterTab.saveButton.clicked.connect(self.on_save_clicked) 298 self.masterTab.closeCfgButton.clicked.connect(self.on_close_clicked) 299 300 self.masterTab.echoTopicButton.clicked.connect(self.on_topic_echo_clicked) 301 self.masterTab.hzTopicButton.clicked.connect(self.on_topic_hz_clicked) 302 self.masterTab.hzSshTopicButton.clicked.connect(self.on_topic_hz_ssh_clicked) 303 self.masterTab.pubTopicButton.clicked.connect(self.on_topic_pub_clicked) 304 self.masterTab.pubStopTopicButton.clicked.connect(self.on_topic_pub_stop_clicked) 305 306 self.masterTab.callServiceButton.clicked.connect(self.on_service_call_clicked) 307 self.masterTab.nodeFilterInput.textChanged.connect(self.on_node_filter_changed) 308 self.masterTab.topicFilterInput.textChanged.connect(self.on_topic_filter_changed) 309 self.masterTab.serviceFilterInput.textChanged.connect(self.on_service_filter_changed) 310 self.masterTab.parameterFilterInput.textChanged.connect(self.on_parameter_filter_changed) 311 self.masterTab.getParameterButton.clicked.connect(self.on_get_parameter_clicked) 312 self.masterTab.addParameterButton.clicked.connect(self.on_add_parameter_clicked) 313 self.masterTab.deleteParameterButton.clicked.connect(self.on_delete_parameter_clicked) 314 self.masterTab.saveParameterButton.clicked.connect(self.on_save_parameter_clicked) 315 316 # create a handler to request the parameter 317 self.parameterHandler = ParameterHandler() 318 self.parameterHandler.parameter_list_signal.connect(self._on_param_list) 319 self.parameterHandler.parameter_values_signal.connect(self._on_param_values) 320 self.parameterHandler.delivery_result_signal.connect(self._on_delivered_values) 321 # create a handler to request sim parameter 322 self.parameterHandler_sim = ParameterHandler() 323 # self.parameterHandler_sim.parameter_list_signal.connect(self._on_param_list) 324 self.parameterHandler_sim.parameter_values_signal.connect(self._on_sim_param_values) 325 # self.parameterHandler_sim.delivery_result_signal.connect(self._on_delivered_values) 326 327 self._shortcut_kill_node = QShortcut(QKeySequence(self.tr("Ctrl+Backspace", "Kill selected node")), self) 328 self._shortcut_kill_node.activated.connect(self.on_kill_nodes) 329 self._shortcut_kill_node = QShortcut(QKeySequence(self.tr("Ctrl+Delete", "Removes the registration of selected nodes from ROS master")), self) 330 self._shortcut_kill_node.activated.connect(self.on_unregister_nodes) 331 332 self.masterTab.ioButton.setEnabled(True) 333 self.masterTab.tabWidget.currentChanged.connect(self.on_tab_current_changed) 334 self._shortcut_screen_show_all = QShortcut(QKeySequence(self.tr("Shift+S", "Show all available screens")), self) 335 self._shortcut_screen_show_all.activated.connect(self.on_show_all_screens) 336 self._shortcut_screen_kill = QShortcut(QKeySequence(self.tr("Shift+Backspace", "Kill Screen")), self) 337 self._shortcut_screen_kill.activated.connect(self.on_kill_screens) 338 339 self.loaded_config.connect(self._apply_launch_config) 340 341 # set the shortcuts 342 self._shortcut1 = QShortcut(QKeySequence(self.tr("Alt+1", "Select first group")), self) 343 self._shortcut1.activated.connect(self.on_shortcut1_activated) 344 self._shortcut2 = QShortcut(QKeySequence(self.tr("Alt+2", "Select second group")), self) 345 self._shortcut2.activated.connect(self.on_shortcut2_activated) 346 self._shortcut3 = QShortcut(QKeySequence(self.tr("Alt+3", "Select third group")), self) 347 self._shortcut3.activated.connect(self.on_shortcut3_activated) 348 self._shortcut4 = QShortcut(QKeySequence(self.tr("Alt+4", "Select fourth group")), self) 349 self._shortcut4.activated.connect(self.on_shortcut4_activated) 350 self._shortcut5 = QShortcut(QKeySequence(self.tr("Alt+5", "Select fifth group")), self) 351 self._shortcut5.activated.connect(self.on_shortcut5_activated) 352 353 self._shortcut_collapse_all = QShortcut(QKeySequence(self.tr("Alt+C", "Collapse all groups")), self) 354 self._shortcut_collapse_all.activated.connect(self.on_shortcut_collapse_all) 355 self._shortcut_expand_all = QShortcut(QKeySequence(self.tr("Alt+E", "Expand all groups")), self) 356 self._shortcut_expand_all.activated.connect(self.masterTab.nodeTreeView.expandAll) 357 self._shortcut_run = QShortcut(QKeySequence(self.tr("Alt+R", "run selected nodes")), self) 358 self._shortcut_run.activated.connect(self.on_start_clicked) 359 self._shortcut_stop = QShortcut(QKeySequence(self.tr("Alt+S", "stop selected nodes")), self) 360 self._shortcut_stop.activated.connect(self.on_stop_clicked) 361 362 self.message_frame = MessageFrame() 363 self.masterTab.questionFrameLayout.addWidget(self.message_frame.frameui) 364 self.message_frame.accept_signal.connect(self._on_question_ok) 365 self.message_frame.cancel_signal.connect(self._on_question_cancel) 366 367 self.info_frame = MessageFrame(info=True) 368 self.masterTab.infoFrameLayout.addWidget(self.info_frame.frameui) 369 self.info_frame.accept_signal.connect(self._on_info_ok)
370 # self._shortcut_copy = QShortcut(QKeySequence(self.tr("Ctrl+C", "copy selected values to clipboard")), self) 371 # self._shortcut_copy.activated.connect(self.on_copy_c_pressed) 372 # self._shortcut_copy = QShortcut(QKeySequence(self.tr("Ctrl+X", "copy selected alternative values to clipboard")), self) 373 # self._shortcut_copy.activated.connect(self.on_copy_x_pressed) 374 375 # print "================ create", self.objectName() 376 # 377 # def __del__(self): 378 # print " Destroy mester view proxy", self.objectName(), " ..." 379 # print " ", self.objectName(), "destroyed" 380
381 - def stop(self):
382 print " Shutdown master", self.masteruri, "..." 383 self.default_cfg_handler.stop() 384 self.launch_server_handler.stop() 385 self._progress_queue_prio.stop() 386 self._progress_queue.stop() 387 if self._on_stop_kill_roscore: 388 self.killall_roscore() 389 for ps in self.__echo_topics_dialogs.values(): 390 try: 391 ps.terminate() 392 except: 393 pass 394 print " Master", self.masteruri, " is down!"
395 396 @property
397 - def current_user(self):
398 return self.__current_user
399 400 @current_user.setter
401 - def current_user(self, user):
402 self.__current_user = user 403 nm.settings().set_host_user(self.mastername, user)
404 405 @property
406 - def is_local(self):
407 return nm.is_local(get_hostname(self.masteruri))
408 409 @property
410 - def online(self):
411 ''' 412 The online meens that master is discovered and master_info was received. 413 ''' 414 return self.__online
415 416 @online.setter
417 - def online(self, state):
418 self.__online = state 419 self._start_queue(self._progress_queue) 420 self._start_queue(self._progress_queue_prio)
421 422 @property
423 - def master_state(self):
424 return self.__master_state
425 426 @master_state.setter
427 - def master_state(self, master_state):
428 self.__master_state = master_state
429 430 @property
431 - def master_info(self):
432 return self.__master_info
433 434 @master_info.setter
435 - def master_info(self, master_info):
436 ''' 437 Sets the new master information. To determine whether a node is running the 438 PID and his URI are needed. The PID of remote nodes (host of the ROS master 439 and the node are different) will be not determine by discovering. Thus this 440 information must be obtain from other MasterInfo object and stored while 441 updating. 442 @param master_info: the mater information object 443 @type master_info: U{master_discovery_fkie.msg.MasterInfo<http://docs.ros.org/kinetic/api/master_discovery_fkie/html/modules.html#module-master_discovery_fkie.master_info>} 444 ''' 445 try: 446 update_result = (set(), set(), set(), set(), set(), set(), set(), set(), set()) 447 if self.__master_info is None: 448 if (master_info.masteruri == self.masteruri): 449 self.__master_info = master_info 450 update_result[0].update(self.__master_info.node_names) 451 update_result[3].update(self.__master_info.topic_names) 452 update_result[6].update(self.__master_info.service_names) 453 else: 454 update_result = self.__master_info.updateInfo(master_info) 455 # print "MINFO", self.__master_info.listedState() 456 # we receive the master info from remove nodes first -> skip 457 if self.__master_info is None: 458 return 459 try: 460 if (master_info.masteruri == self.masteruri): 461 self.update_system_parameter() 462 self.online = True 463 # request the info of new remote nodes 464 hosts2update = set([get_hostname(self.__master_info.getNode(nodename).uri) for nodename in update_result[0]]) 465 hosts2update.update([get_hostname(self.__master_info.getService(nodename).uri) for nodename in update_result[6]]) 466 for host in hosts2update: 467 if host != get_hostname(self.masteruri): 468 self.updateHostRequest.emit(host) 469 except: 470 pass 471 # cputimes = os.times() 472 # cputime_init = cputimes[0] + cputimes[1] 473 # update nodes in the model 474 if update_result[0] or update_result[1] or update_result[2] or self.__force_update: 475 self.updateRunningNodesInModel(self.__master_info) 476 self.on_node_selection_changed(None, None) 477 # Updates the topic view based on the current master information. 478 if update_result[3] or update_result[4] or update_result[5] or self.__force_update: 479 self.topic_model.updateModelData(self.__master_info.topics, update_result[3], update_result[4], update_result[5]) 480 self.on_topic_selection_changed(None, None) 481 # Updates the service view based on the current master information. 482 if update_result[6] or update_result[7] or update_result[8] or self.__force_update: 483 self.service_model.updateModelData(self.__master_info.services, update_result[6], update_result[7], update_result[8]) 484 self.on_service_selection_changed(None, None) 485 # update the default configuration 486 self.updateDefaultConfigs(self.__master_info) 487 self.__force_update = False 488 # cputimes = os.times() 489 # cputime = cputimes[0] + cputimes[1] - cputime_init 490 # print " update on ", self.__master_info.mastername if not self.__master_info is None else self.__master_state.name, cputime 491 except: 492 print traceback.format_exc(1)
493
494 - def _start_queue(self, queue):
495 if self.online and self.master_info is not None and isinstance(queue, ProgressQueue): 496 queue.start()
497 498 @property
499 - def use_sim_time(self):
500 return self.__use_sim_time
501
502 - def in_process(self):
503 return self._progress_queue.count() > 0 or self._progress_queue_prio.count() > 0
504
505 - def force_next_update(self):
506 self.__force_update = True
507
508 - def update_system_parameter(self):
509 self.parameterHandler_sim.requestParameterValues(self.masteruri, ["/run_id", "/use_sim_time", "/robot_icon", "/roslaunch/uris"])
510
511 - def markNodesAsDuplicateOf(self, running_nodes):
512 ''' 513 Marks all nodes, which are not running and in a given list as a duplicates nodes. 514 @param running_nodes: The list with names of running nodes 515 @type running_nodes: C{[str]} 516 ''' 517 # store the running_nodes to update to duplicates after load a launch file 518 self.__running_nodes = running_nodes 519 self.node_tree_model.markNodesAsDuplicateOf(running_nodes, (self.master_info is not None and self.master_info.getNodeEndsWith('master_sync')))
520
521 - def getRunningNodesIfSync(self):
522 ''' 523 Returns the list with all running nodes, which are registered by this ROS 524 master. Also the nodes, which are physically running on remote hosts. 525 @return: The list with names of running nodes 526 @rtype: C{[str]} 527 ''' 528 if self.master_info is not None and self.master_info.getNodeEndsWith('master_sync'): 529 return self.master_info.node_names 530 return []
531
532 - def getRunningNodesIfLocal(self, remove_system_nodes=False):
533 ''' 534 Returns the list with all running nodes, which are running (has process) on this host. 535 The nodes registered on this ROS master, but running on remote hosts are not 536 returned. 537 @return: The dictionary with names of running nodes and their masteruri 538 @rtype: C{dict(str:str)} 539 ''' 540 result = dict() 541 if self.master_info is not None: 542 for _, node in self.master_info.nodes.items(): # _:=name 543 if node.isLocal: 544 if not remove_system_nodes or not self._is_in_ignore_list(node.name): 545 result[node.name] = self.master_info.masteruri 546 return result
547
548 - def updateRunningNodesInModel(self, master_info):
549 ''' 550 Creates the dictionary with ExtendedNodeInfo objects and updates the nodes view. 551 @param master_info: the mater information object 552 @type master_info: U{master_discovery_fkie.msg.MasterInfo<http://docs.ros.org/kinetic/api/master_discovery_fkie/html/modules.html#module-master_discovery_fkie.master_info>} 553 ''' 554 if master_info is not None: 555 self.node_tree_model.updateModelData(master_info.nodes) 556 self.updateButtons()
557
558 - def getNode(self, node_name):
559 ''' 560 @param node_name: The name of the node. 561 @type node_name: str 562 @return: The list the nodes with given name. 563 @rtype: [] 564 ''' 565 return self.node_tree_model.getNode("%s" % node_name, self.masteruri)
566
567 - def updateButtons(self, selected_nodes=None):
568 ''' 569 Updates the enable state of the buttons depending of the selection and 570 running state of the selected node. 571 ''' 572 selectedNodes = selected_nodes 573 if selectedNodes is None: 574 selectedNodes = self.nodesFromIndexes(self.masterTab.nodeTreeView.selectionModel().selectedIndexes()) 575 has_running = False 576 has_stopped = False 577 for node in selectedNodes: 578 if node.uri is not None: 579 has_running = True 580 else: 581 has_stopped = True 582 self.masterTab.startButton.setEnabled(True) 583 self.masterTab.stopButton.setEnabled(True) 584 # self.masterTab.ioButton.setEnabled(has_running or has_stopped) 585 self.masterTab.logButton.setEnabled(True) 586 # self.masterTab.logButton.setEnabled(has_running or has_stopped) 587 self.masterTab.logDeleteButton.setEnabled(has_running or has_stopped) 588 # test for available dynamic reconfigure services 589 if self.master_info is not None: 590 dyn_cfg_available = False 591 # dyncfgServices = [srv for srv_name, srv in self.master_info.services.items() if (srv_name.endswith('/set_parameters'))] 592 for n in selectedNodes: 593 for srv_name, srv in self.master_info.services.items(): 594 if (srv_name.endswith('/set_parameters')) and n.name in srv.serviceProvider: 595 dyn_cfg_available = True 596 break 597 self.masterTab.dynamicConfigButton.setEnabled(dyn_cfg_available) 598 # the configuration is only available, if only one node is selected 599 cfg_enable = False 600 if len(selectedNodes) >= 1: 601 cfg_enable = len(self._getCfgChoises(selectedNodes[0], True)) > 0 602 self.masterTab.editConfigButton.setEnabled(cfg_enable and len(selectedNodes) == 1) 603 # self.startNodesAtHostAct.setEnabled(cfg_enable) 604 self.masterTab.editRosParamButton.setEnabled(len(selectedNodes) == 1) 605 # self.masterTab.saveButton.setEnabled(len(self.launchfiles) > 1) 606 # enable the close button only for local configurations 607 self.masterTab.closeCfgButton.setEnabled(True)
608 # self.masterTab.closeCfgButton.setEnabled(len([path for path, _ in self.__configs.items() if (isinstance(path, tuple) and path[2] == self.masteruri) or not isinstance(path, tuple)]) > 0) #_:=cfg 609
610 - def hasLaunchfile(self, path):
611 ''' 612 @param path: the launch file 613 @type path: C{str} 614 @return: C{True} if the given launch file is open 615 @rtype: C{boolean} 616 ''' 617 return path in self.launchfiles
618 619 @property
620 - def default_cfgs(self):
621 ''' 622 Returns the copy of the dictionary with default configurations on this host 623 @rtype: C{[str(ROS node name)]} 624 ''' 625 result = [] 626 for (c, _) in self.__configs.items(): # _:=cfg 627 if isinstance(c, tuple): 628 result.append(c[0]) 629 return result
630 631 @property
632 - def launchfiles(self):
633 ''' 634 Returns the copy of the dictionary with loaded launch files on this host 635 @rtype: C{dict(str(file) : L{LaunchConfig}, ...)} 636 ''' 637 result = dict() 638 for (c, cfg) in self.__configs.items(): 639 if not isinstance(c, tuple): 640 result[c] = cfg 641 return result
642 643 @launchfiles.setter
644 - def launchfiles(self, launchfile):
645 ''' 646 Loads the launch file. If this file is already loaded, it will be reloaded. 647 After successful load the node view will be updated. 648 @param launchfile: the launch file path 649 @type launchfile: C{str} 650 ''' 651 lfile = launchfile 652 argv = [] 653 if isinstance(launchfile, tuple): 654 lfile, argv = launchfile 655 self._progress_queue_prio.add2queue(utf8(uuid.uuid4()), 656 'Loading %s' % os.path.basename(lfile), 657 self._load_launchfile, 658 (lfile, argv)) 659 self._start_queue(self._progress_queue_prio)
660
661 - def _load_launchfile(self, launchfile, argv_forced=[], pqid=None):
662 ''' 663 This method will be called in another thread. The configuration parameter 664 of the launch file will be requested using `LaunchArgsSelectionRequest` and 665 `InteractionNeededError`. After the file is successful loaded a 666 `loaded_config` signal will be emitted. 667 ''' 668 if argv_forced: 669 rospy.loginfo("LOAD launch: %s with args: %s" % (launchfile, argv_forced)) 670 else: 671 rospy.loginfo("LOAD launch: %s" % launchfile) 672 stored_argv = None 673 if launchfile in self.__configs: 674 # close the current loaded configuration with the same name 675 stored_argv = self.__configs[launchfile].argv 676 # load launch configuration 677 try: 678 # test for required args 679 launchConfig = LaunchConfig(launchfile, masteruri=self.masteruri) 680 loaded = False 681 # if the launch file currently open these args will be used 682 if stored_argv is None: 683 if argv_forced: 684 # if the parameter already requested `argv_forced` is filled: load! 685 loaded = launchConfig.load(argv_forced) 686 else: 687 # get the list with needed launch args 688 req_args = launchConfig.getArgs() 689 if req_args: 690 params = dict() 691 arg_dict = launchConfig.argvToDict(req_args) 692 for arg in arg_dict.keys(): 693 params[arg] = ('string', [arg_dict[arg]]) 694 # request the args: the dialog must run in the main thread of Qt 695 req = LaunchArgsSelectionRequest(launchfile, params, 'Needs input for args') 696 raise nm.InteractionNeededError(req, self._load_launchfile, (launchfile,)) 697 # load the launch file with args of currently open launch file 698 if not loaded or stored_argv is not None: 699 launchConfig.load(req_args if stored_argv is None else stored_argv) 700 # update the names of the hosts stored in the launch file 701 for _, machine in launchConfig.Roscfg.machines.items(): # _:=name 702 if machine.name: 703 nm.nameres().add_info(machine.name, machine.address) 704 # do not load if the loadings process was canceled 705 if self._progress_queue_prio.has_id(pqid): 706 self.loaded_config.emit(launchfile, launchConfig) 707 except InteractionNeededError: 708 raise 709 except Exception as e: 710 err_text = ''.join([os.path.basename(launchfile), ' loading failed!']) 711 err_details = ''.join([err_text, '\n\n', e.__class__.__name__, ": ", utf8(e)]) 712 rospy.logwarn("Loading launch file: %s", err_details) 713 raise DetailedError("Loading launch file", err_text, err_details) 714 # MessageBox.warning(self, "Loading launch file", err_text, err_details) 715 except: 716 print traceback.format_exc(3)
717
718 - def _apply_launch_config(self, launchfile, launchConfig):
719 stored_roscfg = None 720 expanded_items = None 721 if launchfile in self.__configs: 722 # store expanded items 723 expanded_items = self._get_expanded_groups() 724 # close the current loaded configuration with the same name 725 self.removeConfigFromModel(launchfile) 726 stored_roscfg = self.__configs[launchfile].Roscfg 727 del self.__configs[launchfile] 728 try: 729 # add launch file object to the list 730 self.__configs[launchfile] = launchConfig 731 self.appendConfigToModel(launchfile, launchConfig.Roscfg) 732 # self.masterTab.tabWidget.setCurrentIndex(0) 733 # get the descriptions of capabilities and hosts 734 try: 735 robot_descr = launchConfig.getRobotDescr() 736 capabilities = launchConfig.getCapabilitiesDesrc() 737 for (host, caps) in capabilities.items(): 738 if not host: 739 host = nm.nameres().mastername(self.masteruri) 740 host_addr = nm.nameres().address(host) 741 self.node_tree_model.addCapabilities(self.masteruri, host_addr, launchfile, caps) 742 for (host, descr) in robot_descr.items(): 743 if not host: 744 host = nm.nameres().mastername(self.masteruri) 745 host_addr = nm.nameres().address(host) 746 tooltip = self.node_tree_model.updateHostDescription(self.masteruri, host_addr, descr['type'], descr['name'], descr['description']) 747 self.host_description_updated.emit(self.masteruri, host_addr, tooltip) 748 except: 749 import traceback 750 print traceback.format_exc() 751 752 # by this call the name of the host will be updated if a new one is defined in the launch file 753 self.updateRunningNodesInModel(self.__master_info) 754 # detect files changes 755 if stored_roscfg and self.__configs[launchfile].Roscfg: 756 stored_values = [(name, utf8(p.value)) for name, p in stored_roscfg.params.items()] 757 new_values = [(name, utf8(p.value)) for name, p in self.__configs[launchfile].Roscfg.params.items()] 758 # detect changes parameter 759 paramset = set(name for name, _ in (set(new_values) - set(stored_values))) # _:=value 760 # detect new parameter 761 paramset |= (set(self.__configs[launchfile].Roscfg.params.keys()) - set(stored_roscfg.params.keys())) 762 # detect removed parameter 763 paramset |= (set(stored_roscfg.params.keys()) - set(self.__configs[launchfile].Roscfg.params.keys())) 764 # detect new nodes 765 stored_nodes = [roslib.names.ns_join(item.namespace, item.name) for item in stored_roscfg.nodes] 766 new_nodes = [roslib.names.ns_join(item.namespace, item.name) for item in self.__configs[launchfile].Roscfg.nodes] 767 nodes2start = set(new_nodes) - set(stored_nodes) 768 # determine the nodes of the changed parameter 769 for p in paramset: 770 for n in new_nodes: 771 if p.startswith(n): 772 nodes2start.add(n) 773 # detect changes in the arguments and remap 774 for n in stored_roscfg.nodes: 775 for new_n in self.__configs[launchfile].Roscfg.nodes: 776 if n.name == new_n.name and n.namespace == new_n.namespace: 777 if n.args != new_n.args or n.remap_args != new_n.remap_args: 778 nodes2start.add(roslib.names.ns_join(n.namespace, n.name)) 779 # filter out anonymous nodes 780 nodes2start = [n for n in nodes2start if not re.search(r"\d{3,6}_\d{10,}", n)] 781 # restart nodes 782 if nodes2start: 783 restart, ok = SelectDialog.getValue('Restart nodes?', "Select nodes to restart <b>@%s</b>:" % self.mastername, nodes2start, False, True, '', self) 784 if ok: 785 self.stop_nodes_by_name(restart) 786 self.start_nodes_by_name(restart, launchfile, True) 787 # set the robot_icon 788 if launchfile in self.__robot_icons: 789 self.__robot_icons.remove(launchfile) 790 self.__robot_icons.insert(0, launchfile) 791 self.markNodesAsDuplicateOf(self.__running_nodes) 792 # expand items to restore old view 793 if expanded_items is not None: 794 self._expand_groups(expanded_items) 795 # update nodelets 796 nodelets = {} 797 for n in launchConfig.Roscfg.nodes: 798 if n.package == 'nodelet' and n.type == 'nodelet': 799 args = n.args.split(' ') 800 if len(args) == 3 and args[0] == 'load': 801 nodelet_mngr = roslib.names.ns_join(n.namespace, args[2]) 802 if nodelet_mngr not in nodelets: 803 nodelets[nodelet_mngr] = [] 804 nodelets[nodelet_mngr].append(roslib.names.ns_join(n.namespace, n.name)) 805 for mngr, nlist in nodelets.iteritems(): 806 mngr_nodes = self.node_tree_model.getNode(mngr, self.masteruri) 807 for mn in mngr_nodes: 808 mn.nodelets = nlist 809 for nlet in nlist: 810 nlet_nodes = self.node_tree_model.getNode(nlet, self.masteruri) 811 for nn in nlet_nodes: 812 nn.nodelet_mngr = mngr 813 self._nodelets[launchfile] = nodelets 814 815 # print "MASTER:", launchConfig.Roscfg.master 816 # print "NODES_CORE:", launchConfig.Roscfg.nodes_core 817 # for n in launchConfig.Roscfg.nodes: 818 # n.__slots__ = [] 819 # print "NODES:", pickle.dumps(launchConfig.Roscfg.nodes) 820 # 821 # print "ROSLAUNCH_FILES:", launchConfig.Roscfg.roslaunch_files 822 # # list of resolved node names. This is so that we can check for naming collisions 823 # print "RESOLVED_NAMES:", launchConfig.Roscfg.resolved_node_names 824 # 825 # print "TESTS:", launchConfig.Roscfg.tests 826 # print "MACHINES:", launchConfig.Roscfg.machines 827 # print "PARAMS:", launchConfig.Roscfg.params 828 # print "CLEAR_PARAMS:", launchConfig.Roscfg.clear_params 829 # print "EXECS:", launchConfig.Roscfg.executables 830 # 831 # # for tools like roswtf 832 # print "ERRORS:", launchConfig.Roscfg.config_errors 833 # 834 # print "M:", launchConfig.Roscfg.m 835 if launchfile in self._start_nodes_after_load_cfg: 836 self.start_nodes_by_name(self._start_nodes_after_load_cfg[launchfile], launchfile, True) 837 del self._start_nodes_after_load_cfg[launchfile] 838 except Exception as e: 839 err_text = ''.join([os.path.basename(launchfile), ' loading failed!']) 840 err_details = ''.join([err_text, '\n\n', e.__class__.__name__, ": ", utf8(e)]) 841 rospy.logwarn("Loading launch file: %s", err_details) 842 MessageBox.warning(self, "Loading launch file", err_text, err_details) 843 import traceback 844 print traceback.format_exc(3) 845 except: 846 print traceback.format_exc(3) 847 self.update_robot_icon(True)
848
849 - def reload_global_parameter_at_next_start(self, launchfile):
850 try: 851 self.__configs[launchfile].global_param_done.remove(self.masteruri) 852 # self.on_node_selection_changed(None, None, True) 853 except Exception: 854 pass
855
856 - def question_reload_changed_file(self, changed, affected):
857 changed_res = "%s[%s]" % (os.path.basename(changed), utf8(package_name(os.path.dirname(changed))[0])) 858 self.message_frame.show_question(MessageFrame.TYPE_LAUNCH_FILE, 'Reload <b>%s</b>?<br>Changed files:' % os.path.basename(affected), MessageData(affected, [changed_res]))
859
860 - def question_transfer_changed_file(self, changed, affected):
861 self.message_frame.show_question(MessageFrame.TYPE_TRANSFER, 862 "Configuration file '%s' referenced by parameter in <b>%s</b> is changed.<br>Copy to remote host?" 863 "<br>Don\'t forget to restart the corresponding nodes!" % (changed, os.path.basename(affected)), MessageData(changed))
864
865 - def _get_nodelets(self, nodename, configname=''):
866 if configname and configname in self._nodelets: 867 if nodename in self._nodelets[configname]: 868 return self._nodelets[configname][nodename] 869 else: 870 for configname, mngrs in self._nodelets.iteritems(): 871 if nodename in mngrs: 872 return mngrs[nodename] 873 return []
874
875 - def _get_nodelet_manager(self, nodename, configname=''):
876 if configname and configname in self._nodelets: 877 for mngr, nodelets in self._nodelets[configname].iteritems(): 878 if nodename in nodelets: 879 return mngr 880 else: 881 for configname, mngrs in self._nodelets.iteritems(): 882 for mngr, nodelets in mngrs.iteritems(): 883 if nodename in nodelets: 884 return mngr 885 return None
886
887 - def _get_expanded_groups(self):
888 ''' 889 Returns a list of group names, which are expanded. 890 ''' 891 result = [] 892 try: 893 for r in range(self.masterTab.nodeTreeView.model().rowCount()): 894 index_host = self.masterTab.nodeTreeView.model().index(r, 0) 895 if index_host.isValid() and self.masterTab.nodeTreeView.isExpanded(index_host): 896 if self.masterTab.nodeTreeView.model().hasChildren(index_host): 897 for c in range(self.masterTab.nodeTreeView.model().rowCount(index_host)): 898 index_cap = self.masterTab.nodeTreeView.model().index(c, 0, index_host) 899 if index_cap.isValid() and self.masterTab.nodeTreeView.isExpanded(index_cap): 900 model_index = self.node_proxy_model.mapToSource(index_cap) 901 item = self.node_tree_model.itemFromIndex(model_index) 902 if isinstance(item, (GroupItem, HostItem)): 903 result.append(item.name) 904 except: 905 print traceback.format_exc(3) 906 return result
907
908 - def _expand_groups(self, groups=None):
909 ''' 910 Expands all groups, which are in the given list. If no list is given, 911 expands all groups of expanded hosts. 912 ''' 913 try: 914 for r in range(self.masterTab.nodeTreeView.model().rowCount()): 915 index_host = self.masterTab.nodeTreeView.model().index(r, 0) 916 if index_host.isValid() and self.masterTab.nodeTreeView.isExpanded(index_host): 917 if self.masterTab.nodeTreeView.model().hasChildren(index_host): 918 for c in range(self.masterTab.nodeTreeView.model().rowCount(index_host)): 919 index_cap = self.masterTab.nodeTreeView.model().index(c, 0, index_host) 920 if index_cap.isValid(): 921 model_index = self.node_proxy_model.mapToSource(index_cap) 922 item = self.node_tree_model.itemFromIndex(model_index) 923 if isinstance(item, (GroupItem, HostItem)): 924 if groups is None or item.name in groups: 925 self.masterTab.nodeTreeView.setExpanded(index_cap, True) 926 except: 927 print traceback.format_exc(3)
928
929 - def update_robot_icon(self, force=False):
930 ''' 931 Update the current robot icon. If the icon was changed a `robot_icon_updated` 932 signal will be emitted. 933 :return: the path to the current robot icon 934 :rtype: str 935 ''' 936 for l in self.__robot_icons: 937 try: 938 icon = self.__configs[l].get_robot_icon() 939 if icon: 940 if icon != self.__current_robot_icon or force: 941 self.__current_robot_icon = icon 942 self.robot_icon_updated.emit(self.masteruri, icon) 943 return icon 944 except: 945 pass 946 self.__current_robot_icon = self.__current_parameter_robot_icon 947 self.robot_icon_updated.emit(self.masteruri, utf8(self.__current_robot_icon)) 948 return self.__current_robot_icon
949
950 - def appendConfigToModel(self, launchfile, rosconfig):
951 ''' 952 Update the node view 953 @param launchfile: the launch file path 954 @type launchfile: C{str} 955 @param rosconfig: the configuration 956 @type rosconfig: L{LaunchConfig} 957 ''' 958 hosts = dict() # dict(addr : dict(node : [config]) ) 959 addr = nm.nameres().address(self.masteruri) 960 masteruri = self.masteruri 961 for n in rosconfig.nodes: 962 if n.machine_name and not n.machine_name == 'localhost': 963 if n.machine_name not in rosconfig.machines: 964 raise Exception(''.join(["ERROR: unknown machine [", n.machine_name, "]"])) 965 addr = rosconfig.machines[n.machine_name].address 966 masteruri = nm.nameres().masteruri(n.machine_name) 967 node = roslib.names.ns_join(n.namespace, n.name) 968 if (masteruri, addr) not in hosts: 969 hosts[(masteruri, addr)] = dict() 970 hosts[(masteruri, addr)][node] = launchfile 971 # add the configurations for each host separately 972 for ((masteruri, addr), nodes) in hosts.items(): 973 self.node_tree_model.appendConfigNodes(masteruri, addr, nodes) 974 self.updateButtons()
975
976 - def removeConfigFromModel(self, launchfile):
977 ''' 978 Update the node view after removed configuration. 979 @param launchfile: the launch file path 980 @type launchfile: C{str} 981 ''' 982 if isinstance(launchfile, tuple): 983 self.remove_config_signal.emit(launchfile[0]) 984 self.node_tree_model.removeConfigNodes(launchfile) 985 self.updateButtons()
986
987 - def updateDefaultConfigs(self, master_info):
988 ''' 989 Updates the default configuration view based on the current master information. 990 @param master_info: the mater information object 991 @type master_info: U{master_discovery_fkie.msg.MasterInfo<http://docs.ros.org/kinetic/api/master_discovery_fkie/html/modules.html#module-master_discovery_fkie.master_info>} 992 ''' 993 if self.__master_info is None: 994 return 995 default_cfgs = [] 996 for name in self.__master_info.service_names: 997 if name.endswith('list_nodes'): 998 srv = self.__master_info.getService(name) 999 default_cfgs.append((roslib.names.namespace(name).rstrip(roslib.names.SEP), srv.uri, srv.masteruri)) 1000 # remove the node contained in default configuration form the view 1001 removed = list(set([c for c in self.__configs.keys() if isinstance(c, tuple)]) - set(default_cfgs)) 1002 if removed: 1003 for r in removed: 1004 self.node_tree_model.removeConfigNodes(r) 1005 # service = self.__master_info.getService(roslib.names.ns_join(r[0], 'list_nodes')) 1006 if r[2] == self.masteruri: 1007 self.remove_config_signal.emit(r[0]) 1008 del self.__configs[r] 1009 if len(self.__configs) == 0: 1010 address = nm.nameres().address(master_info.masteruri) 1011 tooltip = self.node_tree_model.updateHostDescription(master_info.masteruri, address, '', '', '') 1012 self.host_description_updated.emit(master_info.masteruri, address, tooltip) 1013 # request the nodes of new default configurations 1014 added = list(set(default_cfgs) - set(self.__configs.keys())) 1015 for (name, uri, _) in added: # _:= masteruri 1016 self.default_cfg_handler.requestNodeList(uri, roslib.names.ns_join(name, 'list_nodes')) 1017 # request the description 1018 descr_service = self.__master_info.getService(roslib.names.ns_join(name, 'description')) 1019 if descr_service is not None: 1020 self.default_cfg_handler.requestDescriptionList(descr_service.uri, descr_service.name) 1021 self.updateButtons()
1022
1023 - def on_default_cfg_nodes_retrieved(self, service_uri, config_name, nodes):
1024 ''' 1025 Handles the new list with nodes from default configuration service. 1026 @param service_uri: the URI of the service provided the default configuration 1027 @type service_uri: C{str} 1028 @param config_name: the name of default configuration service 1029 @type config_name: C{str} 1030 @param nodes: the name of the nodes with name spaces 1031 @type nodes: C{[str]} 1032 ''' 1033 # remove the current state 1034 masteruri = self.masteruri 1035 if self.__master_info is not None: 1036 service = self.__master_info.getService(config_name) 1037 if service is not None: 1038 masteruri = service.masteruri 1039 cfg_name = roslib.names.namespace(config_name).rstrip(roslib.names.SEP) 1040 key = (cfg_name, service_uri, masteruri) 1041 # if self.__configs.has_key(key): 1042 # self.node_tree_model.removeConfigNodes(key) 1043 # add the new config 1044 node_cfgs = dict() 1045 for n in nodes: 1046 node_cfgs[n] = key 1047 host = get_hostname(service_uri) 1048 host_addr = nm.nameres().address(host) 1049 if host_addr is None: 1050 host_addr = host 1051 self.node_tree_model.appendConfigNodes(masteruri, host_addr, node_cfgs) 1052 self.__configs[key] = nodes 1053 # start nodes in the queue 1054 if cfg_name in self._start_nodes_after_load_cfg: 1055 self.start_nodes_by_name(self._start_nodes_after_load_cfg[cfg_name], roslib.names.ns_join(cfg_name, 'run'), True) 1056 del self._start_nodes_after_load_cfg[cfg_name] 1057 self.updateButtons()
1058
1059 - def on_default_cfg_descr_retrieved(self, service_uri, config_name, items):
1060 ''' 1061 Handles the description list from default configuration service. 1062 Emits a Qt signal L{host_description_updated} to notify about a new host 1063 description and a Qt signal L{capabilities_update_signal} to notify about a capabilities 1064 update. 1065 @param service_uri: the URI of the service provided the default configuration 1066 @type service_uri: C{str} 1067 @param config_name: the name of default configuration service 1068 @type config_name: C{str} 1069 @param items: list with descriptions 1070 @type items: C{[U{multimaster_msgs_fkie.srv.ListDescription<http://docs.ros.org/api/multimaster_msgs_fkie/html/srv/ListDescription.html>} Response]} 1071 ''' 1072 if items: 1073 masteruri = self.masteruri 1074 if self.__master_info is not None: 1075 service = self.__master_info.getService(config_name) 1076 if service is not None: 1077 masteruri = service.masteruri 1078 key = (roslib.names.namespace(config_name).rstrip(roslib.names.SEP), service_uri, masteruri) 1079 host = get_hostname(service_uri) 1080 host_addr = nm.nameres().address(host) 1081 # add capabilities 1082 caps = dict() 1083 for c in items[0].capabilities: 1084 if c.namespace not in caps: 1085 caps[c.namespace] = dict() 1086 caps[c.namespace][c.name.decode(sys.getfilesystemencoding())] = {'type': c.type, 'images': [resolve_paths(i) for i in c.images], 'description': resolve_paths(c.description.replace("\\n ", "\n").decode(sys.getfilesystemencoding())), 'nodes': list(c.nodes)} 1087 if host_addr is None: 1088 host_addr = get_hostname(key[1]) 1089 self.node_tree_model.addCapabilities(masteruri, host_addr, key, caps) 1090 # set host description 1091 tooltip = self.node_tree_model.updateHostDescription(masteruri, host_addr, items[0].robot_type, items[0].robot_name.decode(sys.getfilesystemencoding()), resolve_paths(items[0].robot_descr.decode(sys.getfilesystemencoding()))) 1092 self.host_description_updated.emit(masteruri, host_addr, tooltip) 1093 self.capabilities_update_signal.emit(masteruri, host_addr, roslib.names.namespace(config_name).rstrip(roslib.names.SEP), items)
1094
1095 - def on_default_cfg_err(self, service_uri, service, msg):
1096 ''' 1097 Handles the error messages from default configuration service. 1098 @param service_uri: the URI of the service provided the default configuration 1099 @type service_uri: C{str} 1100 @param service: the name of default configuration service 1101 @type service: C{str} 1102 @param msg: the error message 1103 @type msg: C{str} 1104 ''' 1105 pass
1106 # MessageBox.warning(self, 'Error while call %s'%service, 1107 # utf8(msg), 1108 # buttons=MessageBox.Ok) 1109 1110 @property
1111 - def launch_servers(self):
1112 return self.__launch_servers
1113
1114 - def has_launch_server(self):
1115 ''' 1116 Returns `True` if the there are roslaunch server, which have no `master` as 1117 node or or have other nodes as `rosout-#` inside. 1118 ''' 1119 for _, (_, nodes) in self.__launch_servers.items(): # _:= uri, pid 1120 if not self._is_master_launch_server(nodes): 1121 return True 1122 return False
1123
1124 - def _is_master_launch_server(self, nodes):
1125 if 'master' in nodes and len(nodes) < 3: 1126 return True 1127 return False
1128
1129 - def on_launch_server_retrieved(self, serveruri, pid, nodes):
1130 ''' 1131 Handles the info about roslaunch server. 1132 Emits a Qt signal L{host_description_updated} to notify about a new host 1133 description and a Qt signal L{capabilities_update_signal} to notify about a capabilities 1134 update. 1135 @param serveruri: the URI of the roslaunch server 1136 @type serveruri: C{str} 1137 @param pid: the process id of the roslaunch server 1138 @type pid: C{str} 1139 @param nodes: list with nodes handled by the roslaunch server 1140 @type nodes: C{[L{str}]} 1141 ''' 1142 self.__launch_servers[serveruri] = (pid, nodes)
1143
1144 - def on_launch_server_err(self, serveruri, msg):
1145 ''' 1146 Handles the error messages from launch server hanlder. 1147 @param serveruri: the URI of the launch server 1148 @type serveruri: C{str} 1149 @param msg: the error message 1150 @type msg: C{str} 1151 ''' 1152 try: 1153 del self.__launch_servers[serveruri] 1154 except: 1155 pass
1156
1158 ''' 1159 Kill all running launch server. The coresponding URIS are removed by master_monitor. 1160 ''' 1161 for lsuri, (pid, nodes) in self.__launch_servers.items(): 1162 try: 1163 if not self._is_master_launch_server(nodes): 1164 self._progress_queue.add2queue(utf8(uuid.uuid4()), 1165 ''.join(['kill roslaunch ', lsuri, '(', utf8(pid), ')']), 1166 nm.starter().kill, 1167 (get_hostname(lsuri), pid, False, self.current_user)) 1168 self.launch_server_handler.updateLaunchServerInfo(lsuri, delayed_exec=3.0) 1169 except Exception as e: 1170 rospy.logwarn("Error while kill roslaunch %s: %s", utf8(lsuri), utf8(e)) 1171 raise DetailedError("Kill error", 1172 ''.join(['Error while kill roslaunch ', lsuri]), 1173 utf8(e)) 1174 self._start_queue(self._progress_queue)
1175 1176 # %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 1177 # %%%%%%%%%%%%% Handling of the view activities %%%%%%%% 1178 # %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 1179
1180 - def on_node_activated(self, index):
1181 ''' 1182 Depending of the state of the node, it will be run or the screen output will 1183 be open. 1184 @param index: The index of the activated node 1185 @type index: U{QtCore.QModelIndex<https://srinikom.github.io/pyside-docs/PySide/QtCore/QModelIndex.html>} 1186 ''' 1187 selectedNodes = [] 1188 if index.column() == 0: 1189 selectedNodes = self.nodesFromIndexes(self.masterTab.nodeTreeView.selectionModel().selectedIndexes(), False) 1190 if not selectedNodes: 1191 return 1192 has_running = False 1193 has_stopped = False 1194 has_invalid = False 1195 for node in selectedNodes: 1196 if node.uri is not None: 1197 has_running = True 1198 if node.pid is None: 1199 has_invalid = True 1200 else: 1201 has_stopped = True 1202 if has_stopped: 1203 self.on_start_clicked() 1204 elif has_running and not has_invalid: 1205 self.on_io_clicked() 1206 else: 1207 self.on_log_clicked()
1208
1209 - def on_node_clicked(self, index):
1210 self.message_frame.hide_question([MessageFrame.TYPE_NODELET]) 1211 self.info_frame.hide_question([MessageFrame.TYPE_NOSCREEN]) 1212 if time.time() - self.__last_selection > 1.: 1213 self.on_node_selection_changed(None, None, True)
1214
1215 - def on_topic_activated(self, index):
1216 ''' 1217 @param index: The index of the activated topic 1218 @type index: U{QtCore.QModelIndex<https://srinikom.github.io/pyside-docs/PySide/QtCore/QModelIndex.html>} 1219 ''' 1220 model_index = self.topic_proxyModel.mapToSource(index) 1221 item = self.topic_model.itemFromIndex(model_index) 1222 if isinstance(item, TopicItem): 1223 self.on_topic_echo_clicked([item.topic])
1224
1225 - def on_topic_clicked(self, index):
1226 if time.time() - self.__last_selection > 1.: 1227 self.on_topic_selection_changed(None, None, True)
1228
1229 - def on_service_activated(self, index):
1230 ''' 1231 @param index: The index of the activated service 1232 @type index: U{QtCore.QModelIndex<https://srinikom.github.io/pyside-docs/PySide/QtCore/QModelIndex.html>} 1233 ''' 1234 model_index = self.service_proxyModel.mapToSource(index) 1235 item = self.service_model.itemFromIndex(model_index) 1236 if isinstance(item, ServiceItem): 1237 self.on_service_call_clicked([item.service])
1238
1239 - def on_service_clicked(self, index):
1240 if time.time() - self.__last_selection > 1.: 1241 self.on_service_selection_changed(None, None, True)
1242
1243 - def on_host_inserted(self, item):
1244 if item == (self.masteruri, nm.nameres().hostname(get_hostname(self.masteruri))): 1245 index = self.node_tree_model.indexFromItem(item) 1246 model_index = self.node_proxy_model.mapFromSource(index) 1247 if model_index.isValid(): 1248 self.masterTab.nodeTreeView.expand(model_index)
1249 # self.masterTab.nodeTreeView.expandAll() 1250
1251 - def on_node_collapsed(self, index):
1252 if not index.parent().isValid(): 1253 self.masterTab.nodeTreeView.selectionModel().clear()
1254
1255 - def on_node_expanded(self, index):
1256 pass
1257
1258 - def _create_html_list(self, title, items, list_type=None, name=''):
1259 ''' 1260 :param list_type: LAUNCH, TOPIC, NODE, SERVICE 1261 :type list_type: str 1262 ''' 1263 result = '' 1264 if items: 1265 result = '%s<b><u>%s</u></b>' % (result, title) 1266 if len(items) > 1: 1267 result = '%s <span style="color:gray;">[%d]</span>' % (result, len(items)) 1268 result = '%s<br><ul><span></span>' % result 1269 items.sort() 1270 for i in items: 1271 item = i 1272 # reduce the displayed name 1273 item_name = i 1274 if name: 1275 if item_name.startswith(name): 1276 item_name = item_name.replace('%s%s' % (name, roslib.names.SEP), '~', 1) 1277 ns = roslib.names.namespace(name) 1278 if item_name.startswith(ns) and ns != roslib.names.SEP: 1279 item_name = item_name.replace(ns, '', 1) 1280 if list_type in ['NODE']: 1281 item = '<a href="node://%s%s">%s</a>' % (self.mastername, i, item_name) 1282 elif list_type in ['TOPIC_PUB', 'TOPIC_SUB']: 1283 # determine the count of publisher or subscriber 1284 count = None 1285 try: 1286 tpc = self.__master_info.getTopic(i) 1287 if list_type == 'TOPIC_SUB': 1288 count = len(tpc.publisherNodes) 1289 if name not in tpc.subscriberNodes: 1290 count = None 1291 else: 1292 count = len(tpc.subscriberNodes) 1293 if name not in tpc.publisherNodes: 1294 count = None 1295 except: 1296 pass 1297 # add the count 1298 if count is not None: 1299 item = '<a href="topic://%s">%s</a>' % (i, item_name) 1300 item += ' <a href="topicecho://%s%s"><span style="color:gray;"><i>echo</i></span></a>' % (self.mastername, i) 1301 item = '<span style="color:gray;">_%d_/ </span>%s' % (count, item) 1302 else: 1303 item = '<a>%s</a>' % (item_name) 1304 item = '<span style="color:red;">!sync </span>%s' % (item) 1305 elif list_type == 'SERVICE': 1306 try: 1307 srv = self.__master_info.getService(i) 1308 if name in srv.serviceProvider: 1309 item = '<a href="service://%s%s">%s</a>' % (self.mastername, i, item_name) 1310 item += ' <a href="servicecall://%s%s"><span style="color:gray;"><i>call</i></span></a>' % (self.mastername, i) 1311 else: 1312 item = '<span style="color:red;">!sync </span>%s' % (item_name) 1313 except: 1314 item = '<span style="color:red;">!sync </span>%s' % (item_name) 1315 elif list_type == 'LAUNCH': 1316 item = '<a href="launch://%s">%s</a>' % (i, item_name) 1317 if i in self.__configs and self.masteruri in self.__configs[i].global_param_done: 1318 item += '%s<br><a href="reload-globals://%s"><font color="#339900">reload global parameter @next start</font></a>' % (item, i) 1319 result += '\n%s<br>' % (item) 1320 result += '</ul>' 1321 return result
1322
1323 - def on_tab_current_changed(self, index):
1324 tab_name = self.masterTab.tabWidget.currentWidget().objectName() 1325 if tab_name == 'tabTopics': 1326 # select the topics of the selected node in the "Topic" view 1327 selections = self.masterTab.nodeTreeView.selectionModel().selectedIndexes() 1328 selectedNodes = self.nodesFromIndexes(selections) 1329 if len(selectedNodes) == 1: 1330 node = selectedNodes[0] 1331 selected_topics = self.topic_model.index_from_names(node.published, node.subscribed) 1332 for s in selected_topics: 1333 self.masterTab.topicsView.selectionModel().select(self.topic_proxyModel.mapFromSource(s), QItemSelectionModel.Select) 1334 elif tab_name == 'tabServices': 1335 # select the services of the selected node in the "Services" view 1336 selections = self.masterTab.nodeTreeView.selectionModel().selectedIndexes() 1337 selectedNodes = self.nodesFromIndexes(selections) 1338 if len(selectedNodes) == 1: 1339 node = selectedNodes[0] 1340 selected_services = self.service_model.index_from_names(node.services) 1341 for s in selected_services: 1342 self.masterTab.servicesView.selectionModel().select(self.service_proxyModel.mapFromSource(s), QItemSelectionModel.Select)
1343
1344 - def _is_current_tab_name(self, tab_name):
1345 return (self.masterTab.tabWidget.currentWidget().objectName() == tab_name)
1346
1347 - def on_node_selection_changed(self, selected, deselected, force_emit=False, node_name=''):
1348 ''' 1349 updates the Buttons, create a description and emit L{description_signal} to 1350 show the description of host, group or node. 1351 ''' 1352 if selected is not None: 1353 # it is a workaround to avoid double updates a after click on an item 1354 self.__last_selection = time.time() 1355 selectedGroups = [] 1356 if node_name and self.master_info is not None: 1357 # get node by name 1358 selectedNodes = self.getNode(node_name) 1359 if not selectedNodes or selectedNodes[0] is None: 1360 if node_name: 1361 self.description_signal.emit(node_name, "<b>%s</b> not found" % node_name, True if selected or deselected or force_emit else False) 1362 return 1363 selectedHosts = [] 1364 selections = [] 1365 else: 1366 # get node by selected items 1367 if not self._is_current_tab_name('tabNodes'): 1368 return 1369 selections = self.masterTab.nodeTreeView.selectionModel().selectedIndexes() 1370 selectedHosts = self.hostsFromIndexes(selections) 1371 selectedNodes = self.nodesFromIndexes(selections) 1372 selectedGroups = self.groupsFromIndexes(selections) 1373 self.masterTab.topicsView.selectionModel().clear() 1374 self.masterTab.servicesView.selectionModel().clear() 1375 name = '' 1376 text = '' 1377 # add control buttons for more then one selected node 1378 if len(selectedNodes) > 1 or selectedGroups > 0: 1379 restartable_nodes = [sn for sn in selectedNodes if len(sn.cfgs) > 0 and not self._is_in_ignore_list(sn.name)] 1380 restartable_nodes_with_launchfiles = [sn for sn in selectedNodes if sn.has_launch_cfgs(sn.cfgs) > 0 and not self._is_in_ignore_list(sn.name)] 1381 killable_nodes = [sn for sn in selectedNodes if sn.node_info.pid is not None and not self._is_in_ignore_list(sn.name)] 1382 unregisterble_nodes = [sn for sn in selectedNodes if sn.node_info.pid is None and sn.node_info.uri is not None and sn.node_info.isLocal and not self._is_in_ignore_list(sn.name)] 1383 # add description for multiple selected nodes 1384 if restartable_nodes or killable_nodes or unregisterble_nodes: 1385 text += '<b>Selected nodes:</b><br>' 1386 if restartable_nodes: 1387 text += '<a href="restart-node://all_selected_nodes" title="Restart %s selected nodes"><img src=":icons/sekkyumu_restart_24.png" alt="restart">[%d]</a>' % (len(restartable_nodes), len(restartable_nodes)) 1388 text += '&nbsp;<a href="restart-node-g://all_selected_nodes" title="Reload global parameter and restart %s selected nodes"><img src=":icons/sekkyumu_restart_g_24.png" alt="restart">[%d]</a>' % (len(restartable_nodes), len(restartable_nodes)) 1389 if killable_nodes: 1390 # text += '&nbsp;<a href="kill-node://all_selected_nodes" title="Kill %s selected nodes"><img src=":icons/sekkyumu_kill_24.png" alt="kill">[%d]</a>' % (len(killable_nodes), len(killable_nodes)) 1391 text += '&nbsp;<a href="kill-screen://all_selected_nodes" title="Kill %s screens of selected nodes"><img src=":icons/sekkyumu_kill_screen_24.png" alt="killscreen">[%d]</a>' % (len(killable_nodes), len(killable_nodes)) 1392 if restartable_nodes_with_launchfiles: 1393 text += '&nbsp;<a href="start-node-at-host://all_selected_nodes" title="Start %s nodes at another host"><img src=":icons/sekkyumu_start_athost_24.png" alt="start@host">[%d]</a>' % (len(restartable_nodes_with_launchfiles), len(restartable_nodes_with_launchfiles)) 1394 text += '&nbsp;<a href="start-node-adv://all_selected_nodes" title="Start %s nodes with additional options, e.g. loglevel"><img src=":icons/sekkyumu_play_alt_24.png" alt="play alt">[%d]</a>' % (len(restartable_nodes_with_launchfiles), len(restartable_nodes_with_launchfiles)) 1395 if unregisterble_nodes: 1396 text += '<br><a href="unregister-node://all_selected_nodes">unregister [%d]</a>' % len(unregisterble_nodes) 1397 # add host description, if only the one host is selected 1398 if len(selectedHosts) == 1: # and len(selections) / 2 == 1: 1399 host = selectedHosts[0] 1400 name = '%s - Robot' % host.name 1401 text += host.generateDescription() 1402 text += '<br>' 1403 else: 1404 # add group description, if only the one group is selected 1405 if len(selectedGroups) == 1 and len(selections) / 2 == 1: 1406 group = selectedGroups[0] 1407 name = '%s - Group' % group.name 1408 text += group.generateDescription() 1409 text += '<br>' 1410 # add node description for one selected node 1411 if len(selectedHosts) != 1 and len(selectedNodes) == 1 and len(selectedGroups) == 0: 1412 node = selectedNodes[0] 1413 text = self.get_node_description(node_name, node) 1414 name = node.name 1415 if (self._is_current_tab_name('tabNodes') and self.__last_info_text != text) or force_emit: 1416 self.__last_info_text = text 1417 self.description_signal.emit(name, text, True if selected or deselected or force_emit else False) 1418 self.updateButtons(selectedNodes)
1419
1420 - def get_node_description(self, node_name, node=None):
1421 text = '' 1422 if node_name and node is None and self.master_info is not None: 1423 # get node by name 1424 selectedNodes = self.getNode(node_name) 1425 if len(selectedNodes) == 1: 1426 node = selectedNodes[0] 1427 # add node description for one selected node 1428 if node is not None: 1429 # create description for a node 1430 ns, sep, name = node.name.rpartition(rospy.names.SEP) 1431 text = '<font size="+1"><b><span style="color:gray;">%s%s</span><b>%s</b></font><br>' % (ns, sep, name) 1432 launches = [c for c in node.cfgs if not isinstance(c, tuple)] 1433 default_cfgs = [c[0] for c in node.cfgs if isinstance(c, tuple)] 1434 if launches or default_cfgs: 1435 text += '<a href="restart-node://%s" title="Restart node"><img src=":icons/sekkyumu_restart_24.png" alt="restart"></a>' % node.name # height="24" width="24" 1436 text += '&nbsp;<a href="restart-node-g://%s" title="Reload global parameter and restart node"><img src=":icons/sekkyumu_restart_g_24.png" alt="restart"></a>' % node.name # height="24" width="24" 1437 # text += '&nbsp; <a href="kill-node://%s" title="Kill node with pid %s"><img src=":icons/sekkyumu_kill_24.png" alt="kill"></a>' % (node.name, node.pid) 1438 text += '&nbsp; <a href="kill-screen://%s" title="Kill screen of the node"><img src=":icons/sekkyumu_kill_screen_24.png" alt="killscreen"></a>' % node.name 1439 if launches: 1440 text += '&nbsp; <a href="start-node-at-host://%s" title="Start node at another host"><img src=":icons/sekkyumu_start_athost_24.png" alt="start@host"></a>' % node.name 1441 # if node.node_info.pid is None or node.node_info.uri is None: 1442 text += '&nbsp; <a href="start-node-adv://%s" title="Start node with additional options, e.g. loglevel"><img src=":icons/sekkyumu_play_alt_24.png" alt="play alt"></a>' % node.name 1443 text += '&nbsp; <a href="copy-log-path://%s" title="copy log path to clipboard"><img src=":icons/crystal_clear_copy_log_path_24.png" alt="copy_log_path"></a>' % node.name 1444 text += '<dl>' 1445 text += '<dt><b>URI</b>: %s</dt>' % node.node_info.uri 1446 text += '<dt><b>PID</b>: %s</dt>' % node.node_info.pid 1447 text += '<dt><b>ORG.MASTERURI</b>: %s</dt>' % node.node_info.masteruri 1448 if not is_legal_name(node.name): 1449 text += '<dt><font color="#FF6600"><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>' 1450 if node.is_ghost: 1451 if node.name.endswith('master_sync') or node.name.endswith('node_manager'): 1452 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>' 1453 else: 1454 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>' 1455 text += '<dt><font color="#FF9900"><i>Are you use the same ROS packages?</i></font></dt>' 1456 if node.has_running and node.node_info.pid is None and node.node_info.uri is None: 1457 text += '<dt><font color="#FF9900"><b>There 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>' 1458 if node.node_info.uri is not None and node.node_info.masteruri != self.masteruri: 1459 text += '<dt><font color="#339900"><b>synchronized</b></font></dt>' 1460 if node.node_info.pid is None and node.node_info.uri is not None: 1461 if not node.node_info.isLocal: 1462 text += '<dt><font color="#FF9900"><b>remote nodes will not be ping, so they are always marked running</b></font>' 1463 else: 1464 text += '<dt><font color="#CC0000"><b>the node does not respond: </b></font>' 1465 text += '<a href="unregister-node://%s">unregister</a></dt>' % node.name 1466 if node.diagnostic_array and node.diagnostic_array[-1].level > 0: 1467 diag_status = node.diagnostic_array[-1] 1468 level_str = self.DIAGNOSTIC_LEVELS[diag_status.level] 1469 diag_color = '#FF6600' 1470 if diag_status.level == 2: 1471 diag_color = '#CC0000' 1472 elif diag_status.level == 3: 1473 diag_color = '#FFCC00' 1474 elif diag_status.level > 3: 1475 diag_color = '#0000CC' 1476 text += '<dt><font color="%s"><b>%s: %s</b></font></dt>' % (diag_color, level_str, node.diagnostic_array[-1].message) 1477 # if len(node.diagnostic_array) > 1: 1478 # text += '<dt><font color="#FF6600"><a href="view_diagnostics://%s">view recent %d items</a></font></dt>'%(node.name, len(node.diagnostic_array)) 1479 text += '</dl>' 1480 if node.nodelet_mngr: 1481 text += '<dt><b>Nodelet manager</b>: %s</dt>' % self._create_html_list('', [node.nodelet_mngr], 'NODE') 1482 if node.nodelets: 1483 text += '<dt>Manager for <b>%d</b> nodelets</dt>' % len(node.nodelets) 1484 if nm.settings().transpose_pub_sub_descr: 1485 text += self._create_html_list('Subscribed Topics:', node.subscribed, 'TOPIC_SUB', node.name) 1486 text += self._create_html_list('Published Topics:', node.published, 'TOPIC_PUB', node.name) 1487 else: 1488 text += self._create_html_list('Published Topics:', node.published, 'TOPIC_PUB', node.name) 1489 text += self._create_html_list('Subscribed Topics:', node.subscribed, 'TOPIC_SUB', node.name) 1490 text += self._create_html_list('Services:', node.services, 'SERVICE', node.name) 1491 # set loaunch file paths 1492 text += self._create_html_list('Loaded Launch Files:', launches, 'LAUNCH') 1493 text += self._create_html_list('Default Configurations:', default_cfgs, 'NODE') 1494 # text += '<dt><a href="copy-log-path://%s">copy log path to clipboard</a></dt>'%node.name 1495 return text
1496
1497 - def on_topic_selection_changed(self, selected, deselected, force_emit=False, topic_name=''):
1498 ''' 1499 updates the Buttons, create a description and emit L{description_signal} to 1500 show the description of selected topic 1501 ''' 1502 if selected is not None: 1503 # it is a workaround to avoid double updates a after click on an item 1504 self.__last_selection = time.time() 1505 selectedTopics = [] 1506 if topic_name and self.master_info is not None: 1507 selectedTopics = [self.master_info.getTopic("%s" % topic_name)] 1508 if len(selectedTopics) == 0: 1509 return 1510 else: 1511 if not self._is_current_tab_name('tabTopics'): 1512 return 1513 selectedTopics = self.topicsFromIndexes(self.masterTab.topicsView.selectionModel().selectedIndexes()) 1514 topics_selected = (len(selectedTopics) > 0) 1515 self.masterTab.echoTopicButton.setEnabled(topics_selected) 1516 self.masterTab.hzTopicButton.setEnabled(topics_selected) 1517 self.masterTab.hzSshTopicButton.setEnabled(topics_selected) 1518 self.masterTab.pubStopTopicButton.setEnabled(topics_selected) 1519 if len(selectedTopics) == 1: 1520 try: 1521 topic = selectedTopics[0] 1522 text = self.get_topic_description(topic_name, topic) 1523 info_text = '<div>%s</div>' % text 1524 if (self._is_current_tab_name('tabTopics') and self.__last_info_text != info_text) or force_emit: 1525 self.__last_info_text = info_text 1526 self.description_signal.emit(topic.name, info_text, True if selected or deselected or force_emit else False) 1527 except Exception as _: 1528 pass
1529
1530 - def get_topic_description(self, topic_name, topic=None):
1531 text = '' 1532 if topic is None: 1533 selectedTopics = [] 1534 if topic_name and self.master_info is not None: 1535 selectedTopics = [self.master_info.getTopic("%s" % topic_name)] 1536 else: 1537 selectedTopics = self.topicsFromIndexes(self.masterTab.topicsView.selectionModel().selectedIndexes()) 1538 if len(selectedTopics) == 1: 1539 topic = selectedTopics[0] 1540 if topic is not None: 1541 ns, sep, name = topic.name.rpartition(rospy.names.SEP) 1542 text = '<font size="+1"><b><span style="color:gray;">%s%s</span><b>%s</b></font><br>' % (ns, sep, name) 1543 text += '&nbsp;<a href="topicecho://%s%s" title="Show the content of the topic"><img src=":icons/sekkyumu_topic_echo_24.png" alt="echo"></a>' % (self.mastername, topic.name) 1544 text += '&nbsp;<a href="topichz://%s%s" title="Show only the receive rate of the topic.<br>All data is sent through the network"><img src=":icons/sekkyumu_topic_hz_24.png" alt="hz"></a>' % (self.mastername, topic.name) 1545 text += '&nbsp;<a href="topichzssh://%s%s" title="Show only the receive rate of the topic.<br>Uses an SSH connection to execute `rostopic hz` on remote host."><img src=":icons/sekkyumu_topic_echo_ssh_hz_24.png" alt="sshhz"></a>' % (self.mastername, topic.name) 1546 text += '&nbsp;<a href="topicpub://%s%s" title="Start a publisher for selected topic"><img src=":icons/sekkyumu_topic_pub_24.png" alt="pub"></a>' % (self.mastername, topic.name) 1547 if topic.name in self.__republish_params: 1548 text += '&nbsp;<a href="topicrepub://%s%s" title="Start a publisher with last parameters"><img src=":icons/sekkyumu_topic_repub_24.png" alt="repub"></a>' % (self.mastername, topic.name) 1549 topic_publisher = [] 1550 topic_prefix = '/rostopic_pub%s_' % topic.name 1551 node_names = self.master_info.node_names 1552 for n in node_names: 1553 if n.startswith(topic_prefix): 1554 topic_publisher.append(n) 1555 if topic_publisher: 1556 text += '&nbsp;<a href="topicstop://%s%s"><img src=":icons/sekkyumu_topic_pub_stop_24.png" alt="stop"> [%d]</a>' % (self.mastername, topic.name, len(topic_publisher)) 1557 text += '<p>' 1558 if nm.settings().transpose_pub_sub_descr: 1559 text += self._create_html_list('Subscriber:', topic.subscriberNodes, 'NODE') 1560 text += self._create_html_list('Publisher:', topic.publisherNodes, 'NODE') 1561 else: 1562 text += self._create_html_list('Publisher:', topic.publisherNodes, 'NODE') 1563 text += self._create_html_list('Subscriber:', topic.subscriberNodes, 'NODE') 1564 text += '<b><u>Type:</u></b> %s' % self._href_from_msgtype(topic.type) 1565 text += '<dl>' 1566 try: 1567 mclass = roslib.message.get_message_class(topic.type) 1568 if mclass is not None: 1569 for f in mclass.__slots__: 1570 idx = mclass.__slots__.index(f) 1571 idtype = mclass._slot_types[idx] 1572 # base_type = roslib.msgs.base_msg_type(idtype) 1573 # primitive = "unknown" 1574 # if base_type in roslib.msgs.PRIMITIVE_TYPES: 1575 # primitive = "primitive" 1576 # else: 1577 # try: 1578 # primitive =roslib.message.get_message_class(base_type) 1579 # # primitive = "class", list_msg_class.__slots__ 1580 # except ValueError: 1581 # pass 1582 text += '%s: <span style="color:gray;">%s</span><br>' % (f, idtype) 1583 text += '<br>' 1584 constants = {} 1585 for m in dir(mclass): 1586 if not m.startswith('_'): 1587 if type(getattr(mclass, m)) in [str, int, bool, float]: 1588 constants[m] = getattr(mclass, m) 1589 if constants: 1590 text += '<b><u>Constants:</u></b><br>' 1591 for n in sorted(constants.iterkeys()): 1592 text += '%s: <span style="color:gray;">%s</span><br>' % (n, constants[n]) 1593 except ValueError: 1594 pass 1595 text += '</dl>' 1596 return text
1597
1598 - def _href_from_msgtype(self, msg_type):
1599 result = msg_type 1600 if msg_type: 1601 result = '<a href="http://ros.org/doc/api/%s.html">%s</a>' % (msg_type.replace('/', '/html/msg/'), msg_type) 1602 return result
1603
1604 - def on_service_selection_changed(self, selected, deselected, force_emit=False, service_name=''):
1605 ''' 1606 updates the Buttons, create a description and emit L{description_signal} to 1607 show the description of selected service 1608 ''' 1609 if selected is not None: 1610 # it is a workaround to avoid double updates a after click on an item 1611 self.__last_selection = time.time() 1612 if service_name and self.master_info is not None: 1613 # get service by name 1614 selectedServices = [self.master_info.getService("%s" % service_name)] 1615 if selectedServices[0] is None: 1616 return 1617 else: 1618 # get service by selected items 1619 selectedServices = self.servicesFromIndexes(self.masterTab.servicesView.selectionModel().selectedIndexes()) 1620 self.masterTab.callServiceButton.setEnabled(len(selectedServices) > 0) 1621 if not self._is_current_tab_name('tabServices'): 1622 return 1623 if len(selectedServices) == 1: 1624 service = selectedServices[0] 1625 ns, sep, name = service.name.rpartition(rospy.names.SEP) 1626 text = '<font size="+1"><b><span style="color:gray;">%s%s</span><b>%s</b></font><br>' % (ns, sep, name) 1627 text += '<a href="servicecall://%s%s" title="call service"><img src=":icons/sekkyumu_call_service_24.png" alt="call"></a>' % (self.mastername, service.name) 1628 text += '<dl>' 1629 text += '<dt><b>URI</b>: %s</dt>' % service.uri 1630 text += '<dt><b>ORG.MASTERURI</b>: %s</dt>' % service.masteruri 1631 text += self._create_html_list('Provider:', service.serviceProvider, 'NODE') 1632 if service.masteruri != self.masteruri: 1633 text += '<dt><font color="#339900"><b>synchronized</b></font></dt>' 1634 text += '</dl>' 1635 try: 1636 service_class = service.get_service_class(nm.is_local(get_hostname(service.uri))) 1637 text += '<h4>%s</h4>' % self._href_from_svrtype(service_class._type) 1638 text += '<b><u>Request:</u></b>' 1639 text += '<dl><dt>%s</dt></dl>' % service_class._request_class.__slots__ 1640 text += '<b><u>Response:</u></b>' 1641 text += '<dl><dt>%s</dt></dl>' % service_class._response_class.__slots__ 1642 except: 1643 text += '<h4><font color=red>Unknown service type</font></h4>' 1644 if service.isLocal: 1645 text += '<font color=red>Unable to communicate with service, is provider node running?</font>' 1646 else: 1647 text += '<font color=red>Try to refresh <b>all</b> hosts. Is provider node running?</font>' 1648 info_text = '<div>%s</div>' % text 1649 self._is_current_tab_name('tabServices') 1650 if (self._is_current_tab_name('tabServices') and self.__last_info_text != info_text) or force_emit: 1651 self.__last_info_text = info_text 1652 self.description_signal.emit(service.name, info_text, True if selected or deselected or force_emit else False)
1653
1654 - def _href_from_svrtype(self, srv_type):
1655 result = srv_type 1656 if srv_type: 1657 result = '<a href="http://ros.org/doc/api/%s.html">%s</a>' % (srv_type.replace('/', '/html/srv/'), srv_type) 1658 return result
1659
1660 - def on_parameter_selection_changed(self, selected, deselected):
1661 selectedParameter = self.parameterFromIndexes(self.masterTab.parameterView.selectionModel().selectedIndexes()) 1662 self.masterTab.deleteParameterButton.setEnabled(len(selectedParameter) > 0) 1663 self.masterTab.saveParameterButton.setEnabled(len(selectedParameter) > 0)
1664
1665 - def hostsFromIndexes(self, indexes, recursive=True):
1666 result = [] 1667 for index in indexes: 1668 if index.column() == 0: 1669 model_index = self.node_proxy_model.mapToSource(index) 1670 item = self.node_tree_model.itemFromIndex(model_index) 1671 if item is not None: 1672 if isinstance(item, HostItem): 1673 result.append(item) 1674 return result
1675
1676 - def groupsFromIndexes(self, indexes, recursive=True):
1677 result = [] 1678 for index in indexes: 1679 if index.column() == 0 and index.parent().isValid(): 1680 model_index = self.node_proxy_model.mapToSource(index) 1681 item = self.node_tree_model.itemFromIndex(model_index) 1682 if item is not None: 1683 if isinstance(item, GroupItem): 1684 result.append(item) 1685 return result
1686
1687 - def nodesFromIndexes(self, indexes, recursive=True):
1688 result = [] 1689 for index in indexes: 1690 if index.column() == 0: 1691 model_index = self.node_proxy_model.mapToSource(index) 1692 item = self.node_tree_model.itemFromIndex(model_index) 1693 res = self._nodesFromItems(item, recursive) 1694 for r in res: 1695 if r not in result: 1696 result.append(r) 1697 return result
1698
1699 - def _nodesFromItems(self, item, recursive):
1700 result = [] 1701 if item is not None: 1702 if isinstance(item, (GroupItem, HostItem)): 1703 if recursive: 1704 for j in range(item.rowCount()): 1705 result[len(result):] = self._nodesFromItems(item.child(j), recursive) 1706 elif isinstance(item, NodeItem): 1707 if item not in result: 1708 result.append(item) 1709 return result
1710
1711 - def topicsFromIndexes(self, indexes):
1712 result = [] 1713 for index in indexes: 1714 model_index = self.topic_proxyModel.mapToSource(index) 1715 item = self.topic_model.itemFromIndex(model_index) 1716 if item is not None: 1717 if isinstance(item, TopicItem): 1718 result.append(item.topic) 1719 elif isinstance(item, TopicGroupItem): 1720 for titem in item.get_topic_items(): 1721 result.append(titem.topic) 1722 return result
1723
1724 - def servicesFromIndexes(self, indexes):
1725 result = [] 1726 for index in indexes: 1727 model_index = self.service_proxyModel.mapToSource(index) 1728 item = self.service_model.itemFromIndex(model_index) 1729 if item is not None: 1730 if isinstance(item, ServiceItem): 1731 result.append(item.service) 1732 elif isinstance(item, ServiceGroupItem): 1733 for sitem in item.get_service_items(): 1734 result.append(sitem.service) 1735 return result
1736
1737 - def parameterFromIndexes(self, indexes):
1738 result = [] 1739 for index in indexes: 1740 model_index = self.parameter_proxyModel.mapToSource(index) 1741 item = self.parameter_model.itemFromIndex(model_index) 1742 if item is not None and isinstance(item, ParameterValueItem): 1743 result.append((item.name, item.value)) 1744 return result
1745 1746 # %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 1747 # %%%%%%%%%%%%% Handling of the button activities %%%%%%%% 1748 # %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 1749
1750 - def on_start_clicked(self):
1751 ''' 1752 Starts the selected nodes. If for a node more then one configuration is 1753 available, the selection dialog will be show. 1754 ''' 1755 cursor = self.cursor() 1756 self.masterTab.startButton.setEnabled(False) 1757 self.setCursor(Qt.WaitCursor) 1758 try: 1759 selectedNodes = self.nodesFromIndexes(self.masterTab.nodeTreeView.selectionModel().selectedIndexes()) 1760 self.start_nodes(selectedNodes) 1761 finally: 1762 self.setCursor(cursor) 1763 self.masterTab.startButton.setEnabled(True)
1764
1765 - def on_start_alt_clicked(self):
1766 ''' 1767 Starts the selected nodes with additional options. 1768 ''' 1769 cursor = self.cursor() 1770 self.setCursor(Qt.WaitCursor) 1771 try: 1772 selectedNodes = self.nodesFromIndexes(self.masterTab.nodeTreeView.selectionModel().selectedIndexes()) 1773 self.start_nodes(selectedNodes, force=True, use_adv_cfg=True) 1774 finally: 1775 self.setCursor(cursor)
1776
1777 - def start_node(self, node, force, config, force_host=None, logging=None):
1778 1779 if node is None: 1780 raise DetailedError("Start error", 'None is not valid node name!') 1781 if node.pid is None or force: 1782 # start the node using launch configuration 1783 if config is None: 1784 raise DetailedError("Start error", 1785 'Error while start %s:\nNo configuration found!' % node.name) 1786 if isinstance(config, LaunchConfig): 1787 try: 1788 nm.starter().runNode(AdvRunCfg(node.name, config, force_host, self.masteruri, logging=logging, user=self.current_user)) 1789 except socket.error as se: 1790 rospy.logwarn("Error while start '%s': %s\n\n Start canceled!", node.name, utf8(se)) 1791 raise DetailedError("Start error", 1792 'Error while start %s\n\nStart canceled!' % node.name, 1793 '%s' % utf8(se)) 1794 return False 1795 except nm.InteractionNeededError as _: 1796 raise 1797 except (Exception, nm.StartException) as e: 1798 print type(e) 1799 print traceback.format_exc(3) 1800 rospy.logwarn("Error while start '%s': %s" % (node.name, utf8(e))) 1801 raise DetailedError("Start error", 'Error while start %s' % node.name, '%s' % utf8(e)) 1802 elif isinstance(config, (str, unicode)): 1803 # start with default configuration 1804 from multimaster_msgs_fkie.srv import Task 1805 try: 1806 nm.starter().callService(self.master_info.getService(config).uri, config, Task, [node.name]) 1807 except (Exception, nm.StartException) as e: 1808 rospy.logwarn("Error while call a service of node '%s': %s" % (node.name, utf8(e))) 1809 raise DetailedError("Service error", 1810 'Error while call a service of node %s [%s]' % (node.name, self.master_info.getService(config).uri), 1811 '%s' % utf8(e))
1812
1813 - def start_nodes(self, nodes, force=False, force_host=None, use_adv_cfg=False, check_nodelets=True):
1814 ''' 1815 Internal method to start a list with nodes 1816 @param nodes: the list with nodes to start 1817 @type nodes: C{[L{NodeItem}, ...]} 1818 @param force: force the start of the node, also if it is already started. 1819 @type force: C{bool} 1820 @param force_host: force the start of the node at specified host. 1821 @type force_host: C{str} 1822 ''' 1823 cfg_choices = dict() 1824 cfg_nodes = dict() 1825 has_launch_files = False 1826 for node in nodes: 1827 # do not start node, if it is in ingnore list and multiple nodes are selected 1828 if (node.pid is None or (node.pid is not None and force)) and not node.is_ghost: 1829 # test for duplicate nodes 1830 if node.uri is None and node.has_running: 1831 ret = MessageBox.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?']), buttons=MessageBox.Yes | MessageBox.No) 1832 if ret == MessageBox.No: 1833 return 1834 # determine the used configuration 1835 if node.next_start_cfg is not None: 1836 lcfg = node.next_start_cfg 1837 if node.next_start_cfg in self.launchfiles: 1838 lcfg = self.launchfiles[node.next_start_cfg] 1839 cfg_nodes[node.name] = lcfg 1840 node.launched_cfg = lcfg 1841 node.next_start_cfg = None 1842 else: 1843 choices = self._getCfgChoises(node) 1844 ch_keys = choices.keys() 1845 if ch_keys: 1846 ch_keys.sort() 1847 choises_str = utf8(ch_keys) 1848 if choises_str not in cfg_choices.keys(): 1849 choice, ok = self._getUserCfgChoice(choices, node.name) 1850 if choice is not None: 1851 cfg_choices[choises_str] = choices[choice] 1852 cfg_nodes[node.name] = choices[choice] 1853 node.launched_cfg = choices[choice] 1854 if isinstance(choices[choice], LaunchConfig): 1855 has_launch_files = True 1856 elif ok: 1857 MessageBox.warning(self, "Start error", 1858 'Error while start %s:\nNo configuration selected!' % node.name) 1859 else: 1860 break 1861 else: 1862 cfg_nodes[node.name] = cfg_choices[choises_str] 1863 node.launched_cfg = cfg_choices[choises_str] 1864 1865 # get the advanced configuration 1866 logging = None 1867 diag_canceled = False 1868 if use_adv_cfg and has_launch_files: 1869 log_params = {'Level': ('string', nm.settings().logging.get_alternatives('loglevel')), 1870 'Level (roscpp)': ('string', nm.settings().logging.get_alternatives('loglevel_roscpp')), 1871 'Level (super)': ('string', nm.settings().logging.get_alternatives('loglevel_superdebug')), 1872 'Format': ('string', nm.settings().logging.get_alternatives('console_format')) 1873 } 1874 params = {'Logging': ('dict', log_params)} 1875 dia = ParameterDialog(params) 1876 dia.setFilterVisible(False) 1877 dia.setWindowTitle('Start with parameters') 1878 dia.resize(480, 120) 1879 dia.setFocusField('Level') 1880 diag_canceled = not dia.exec_() 1881 if not diag_canceled: 1882 try: 1883 params = dia.getKeywords() 1884 nm.settings().logging.loglevel = params['Logging']['Level'] 1885 nm.settings().logging.loglevel_roscpp = params['Logging']['Level (roscpp)'] 1886 nm.settings().logging.loglevel_superdebug = params['Logging']['Level (super)'] 1887 nm.settings().logging.console_format = params['Logging']['Format'] 1888 nm.settings().store_logging() 1889 logging = nm.settings().logging 1890 except Exception, e: 1891 diag_canceled = True 1892 MessageBox.warning(self, "Get advanced start parameter", 1893 'Error while parse parameter', 1894 utf8(e)) 1895 if not diag_canceled: 1896 # check for nodelets 1897 if check_nodelets: 1898 self._check_for_nodelets(nodes) 1899 # put into the queue and start 1900 for node in nodes: 1901 if node.name in cfg_nodes: 1902 self._progress_queue.add2queue(utf8(uuid.uuid4()), 1903 ''.join(['start ', node.node_info.name]), 1904 self.start_node, 1905 (node.node_info, force, cfg_nodes[node.node_info.name], force_host, logging)) 1906 self._start_queue(self._progress_queue)
1907
1908 - def _check_for_nodelets(self, nodes):
1909 self._restart_nodelets = {} 1910 nodenames = [n.name for n in nodes] 1911 nodelet_mngr = '' 1912 nlmngr = '' 1913 for node in nodes: 1914 try: 1915 cfg_name = node.launched_cfg 1916 if isinstance(node.launched_cfg, LaunchConfig): 1917 cfg_name = node.launched_cfg.Filename 1918 nodelets = self._get_nodelets(node.name, cfg_name) 1919 if nodelets: 1920 nodelets = self._get_nodelets(node.name, cfg_name) 1921 r_nn = [] 1922 for nn in nodelets: 1923 if nn not in nodenames: 1924 r_nn.append(nn) 1925 if cfg_name not in self._restart_nodelets: 1926 self._restart_nodelets[cfg_name] = [] 1927 self._restart_nodelets[cfg_name].append(nn) 1928 if self._restart_nodelets: 1929 nlmngr = node.name 1930 else: 1931 nodelet_mngr = self._get_nodelet_manager(node.name, cfg_name) 1932 if nodelet_mngr: 1933 if nodelet_mngr not in nodenames: 1934 if cfg_name not in self._restart_nodelets: 1935 self._restart_nodelets[cfg_name] = [] 1936 self._restart_nodelets[cfg_name].append(nodelet_mngr) 1937 nodelets = self._get_nodelets(nodelet_mngr, cfg_name) 1938 r_nn = [] 1939 for nn in nodelets: 1940 if nn not in nodenames: 1941 r_nn.append(nn) 1942 self._restart_nodelets[cfg_name].append(nn) 1943 nodelet_mngr = nodelet_mngr 1944 except Exception as err: 1945 rospy.logwarn("Error while test for nodelets: %s" % utf8(err)) 1946 if nm.settings().check_for_nodelets_at_start: 1947 if nodelet_mngr and nodelet_mngr not in nodenames: 1948 self.message_frame.show_question(MessageFrame.TYPE_NODELET, "Nodelet manager '%s' not in current list. (Re)Start nodelet manager and all nodelets?" % nodelet_mngr, MessageData(self._restart_nodelets)) 1949 elif self._restart_nodelets: 1950 self.message_frame.show_question(MessageFrame.TYPE_NODELET, "Not all nodelets of manager '%s' are in the start list. (Re)Start these?" % nlmngr, MessageData(self._restart_nodelets))
1951
1952 - def start_nodes_by_name(self, nodes, cfg, force=False, check_nodelets=True):
1953 ''' 1954 Start nodes given in a list by their names. 1955 @param nodes: a list with full node names 1956 @type nodes: C{[str]} 1957 ''' 1958 result = [] 1959 if self.master_info is not None: 1960 for n in nodes: 1961 node_items = self.getNode(n) 1962 if node_items: 1963 node_item = node_items[0] 1964 # node_item.addConfig(cfg) 1965 if isinstance(cfg, tuple): 1966 node_item.next_start_cfg = cfg[0] 1967 else: 1968 node_item.next_start_cfg = cfg 1969 elif cfg: 1970 node_info = NodeInfo(n, self.masteruri) 1971 node_item = NodeItem(node_info) 1972 # node_item.addConfig(cfg) 1973 if isinstance(cfg, tuple): 1974 node_item.next_start_cfg = cfg[0] 1975 else: 1976 node_item.next_start_cfg = cfg 1977 if node_item is not None: 1978 result.append(node_item) 1979 self.start_nodes(result, force, check_nodelets=check_nodelets)
1980
1981 - def start_nodes_after_load_cfg(self, cfg_name, nodes, force=False):
1982 ''' 1983 Start nodes after the given configuration is loaded and applied to the model. 1984 :param cfg_name: the name of the cnofiguration 1985 :type cfg_name: str 1986 :param nodes: the list of node names 1987 :type nodes: list of strings 1988 ''' 1989 if cfg_name not in self._start_nodes_after_load_cfg: 1990 self._start_nodes_after_load_cfg[cfg_name] = set(nodes) 1991 else: 1992 self._start_nodes_after_load_cfg[cfg_name].update(set(nodes))
1993
1995 ''' 1996 Clears the list with nodes which should be startet after a launch file is loaded. 1997 ''' 1998 self._start_nodes_after_load_cfg = dict()
1999
2000 - def on_force_start_nodes(self, reset_global_param=False):
2001 ''' 2002 Starts the selected nodes (also if it already running). If for a node more then one configuration is 2003 available, the selection dialog will be show. 2004 ''' 2005 cursor = self.cursor() 2006 self.masterTab.startButton.setEnabled(False) 2007 self.setCursor(Qt.WaitCursor) 2008 try: 2009 selectedNodes = self.nodesFromIndexes(self.masterTab.nodeTreeView.selectionModel().selectedIndexes()) 2010 self.stop_nodes(selectedNodes) 2011 if reset_global_param: 2012 # reset config to load global parameter 2013 for node in selectedNodes: 2014 for cfg in node.cfgs: 2015 if cfg in self.launchfiles: 2016 self.reload_global_parameter_at_next_start(cfg) 2017 self.start_nodes(selectedNodes, True) 2018 finally: 2019 self.setCursor(cursor) 2020 self.masterTab.startButton.setEnabled(True)
2021
2022 - def on_start_nodes_at_host(self):
2023 ''' 2024 Starts the selected nodes on an another host. 2025 ''' 2026 cursor = self.cursor() 2027 self.masterTab.startButton.setEnabled(False) 2028 params = {'Host': ('string', 'localhost')} 2029 dia = ParameterDialog(params) 2030 dia.setFilterVisible(False) 2031 dia.setWindowTitle('Start node on...') 2032 dia.resize(350, 120) 2033 dia.setFocusField('host') 2034 if dia.exec_(): 2035 try: 2036 params = dia.getKeywords() 2037 host = params['Host'] 2038 self.setCursor(Qt.WaitCursor) 2039 try: 2040 selectedNodes = self.nodesFromIndexes(self.masterTab.nodeTreeView.selectionModel().selectedIndexes()) 2041 self.start_nodes(selectedNodes, True, host) 2042 finally: 2043 self.setCursor(cursor) 2044 except Exception, e: 2045 MessageBox.warning(self, "Start error", 2046 'Error while parse parameter', 2047 utf8(e)) 2048 self.masterTab.startButton.setEnabled(True)
2049
2050 - def _getDefaultCfgChoises(self, node):
2051 result = {} 2052 for c in node.cfgs: 2053 if isinstance(c, tuple): 2054 result[' '.join(['[default]', c[0]])] = roslib.names.ns_join(c[0], 'run') 2055 return result
2056
2057 - def _getCfgChoises(self, node, ignore_defaults=False):
2058 result = {} 2059 for c in node.cfgs: 2060 if c: 2061 if not isinstance(c, tuple): 2062 launch = self.launchfiles[c] 2063 result[''.join([utf8(launch.LaunchName), ' [', utf8(launch.PackageName), ']'])] = self.launchfiles[c] 2064 elif not ignore_defaults: 2065 result[' '.join(['[default]', c[0]])] = roslib.names.ns_join(c[0], 'run') 2066 return result
2067
2068 - def _getUserCfgChoice(self, choices, nodename):
2069 value = None 2070 ok = False 2071 # Open selection 2072 if len(choices) == 1: 2073 value = choices.keys()[0] 2074 ok = True 2075 elif len(choices) > 0: 2076 items, ok = SelectDialog.getValue('Configuration selection', 'Select configuration to launch <b>%s</b>' % nodename, choices.keys(), True) 2077 if items: 2078 value = items[0] 2079 return value, ok
2080
2081 - def on_stop_clicked(self):
2082 ''' 2083 Stops the selected and running nodes. If the node can't be stopped using his 2084 RPC interface, it will be unregistered from the ROS master using the masters 2085 RPC interface. 2086 ''' 2087 key_mod = QApplication.keyboardModifiers() 2088 if (key_mod & Qt.ShiftModifier or key_mod & Qt.ControlModifier): 2089 self.masterTab.stopButton.showMenu() 2090 else: 2091 cursor = self.cursor() 2092 self.setCursor(Qt.WaitCursor) 2093 try: 2094 selectedNodes = self.nodesFromIndexes(self.masterTab.nodeTreeView.selectionModel().selectedIndexes()) 2095 self.stop_nodes(selectedNodes) 2096 finally: 2097 self.setCursor(cursor)
2098
2099 - def stop_node(self, node, force=False):
2100 if node is not None and node.uri is not None and (not self._is_in_ignore_list(node.name) or force): 2101 try: 2102 rospy.loginfo("Stop node '%s'[%s]", utf8(node.name), utf8(node.uri)) 2103 nm.filewatcher().rem_binary(node.name) 2104 # 'print "STOP set timeout", node 2105 socket.setdefaulttimeout(10) 2106 # 'print "STOP create xmlrpc", node 2107 p = xmlrpclib.ServerProxy(node.uri) 2108 # 'print "STOP send stop", node 2109 p.shutdown(rospy.get_name(), ''.join(['[node manager] request from ', self.mastername])) 2110 # 'print "STOP stop finished", node 2111 except Exception, e: 2112 rospy.logwarn("Error while stop node '%s': %s", utf8(node.name), utf8(e)) 2113 if utf8(e).find(' 111') == 1: 2114 raise DetailedError("Stop error", 2115 ''.join(['Error while stop node ', node.name]), 2116 utf8(e)) 2117 finally: 2118 socket.setdefaulttimeout(None) 2119 elif isinstance(node, NodeItem) and node.is_ghost: 2120 # since for ghost nodes no info is available, emit a signal to handle the 2121 # stop message in other master_view_proxy 2122 self.stop_nodes_signal.emit(node.masteruri, [node.name]) 2123 return True
2124
2125 - def stop_nodes(self, nodes, force=False):
2126 ''' 2127 Internal method to stop a list with nodes 2128 @param nodes: the list with nodes to stop 2129 @type nodes: C{[U{master_discovery_fkie.NodeInfo<http://docs.ros.org/kinetic/api/master_discovery_fkie/html/modules.html#master_discovery_fkie.master_info.NodeInfo>}, ...]} 2130 ''' 2131 # put into the queue and start the que handling 2132 for node in nodes: 2133 self._progress_queue.add2queue(utf8(uuid.uuid4()), 2134 'stop %s' % node.name, 2135 self.