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

Source Code for Module node_manager_fkie.master_view_proxy

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