class ModelLogger: """ Convenience class for logging data. """ def __init__(self): """ Initializes the ModelLogger. """ self.__log_model = QStandardItemModel(0, 4, None) self.__log_model.setHorizontalHeaderLabels( ["type", "date", "location", "message"]) def log(self, type, date, location, message): """ Adds a log entry to the log_model. :param type: the type of the log_entry :type type: str :param date: the time of the log_entry :type date: Time :param location: the location, were the fault/info/... occured :type location: str :param message: the message of the log_entry :type message: str """ self.__log_model.insertRow(0) self.__log_model.setData(self.__log_model.index(0, 0), str(type)) self.__log_model.setData( self.__log_model.index(0, 1), time.strftime("%d.%m-%H:%M:%S", time.localtime(int(str(date)) / 1000000000))) self.__log_model.setData(self.__log_model.index(0, 2), str(location)) self.__log_model.setData(self.__log_model.index(0, 3), str(message)) def get_representation(self): """ Returns the log as a QStandartItemModel :returns: the log-model :rtype: QStandartItemModel """ return self.__log_model
class ModelLogger: """ Convenience class for logging data. """ def __init__(self): """ Initializes the ModelLogger. """ self.__log_model = QStandardItemModel(0, 4, None) self.__log_model.setHorizontalHeaderLabels(["type", "date", "location", "message"]) def log(self, type, date, location, message): """ Adds a log entry to the log_model. :param type: the type of the log_entry :type type: str :param date: the time of the log_entry :type date: Time :param location: the location, were the fault/info/... occured :type location: str :param message: the message of the log_entry :type message: str """ self.__log_model.insertRow(0) self.__log_model.setData(self.__log_model.index(0, 0), str(type)) self.__log_model.setData(self.__log_model.index(0, 1), time.strftime("%d.%m-%H:%M:%S", time.localtime(int(str(date)) / 1000000000))) self.__log_model.setData(self.__log_model.index(0, 2), str(location)) self.__log_model.setData(self.__log_model.index(0, 3), str(message)) def get_representation(self): """ Returns the log as a QStandartItemModel :returns: the log-model :rtype: QStandartItemModel """ return self.__log_model
class MoveitWidget(QWidget): """#TODO: comment """ # To be connected to PluginContainerWidget sig_sysmsg = None sig_param = Signal(bool, str) # param name emitted sig_node = Signal(bool, str) # param name emitted _SPLITTER_H = 'splitter_horizontal' def __init__(self, parent, plugin_context): """ @type parent: MoveitMain """ self._nodes_monitored = ['/move_group'] self._selected_topics = [('/pointcloud', 'sensor_msgs/PointCloud'), ('/pointcloud2', 'sensor_msgs/PointCloud2'), ('/image', 'sensor_msgs/Image'), ('/camera_info', 'sensor_msgs/CameraInfo')] self._params_monitored = [ '/robot_description', '/robot_description_semantic' ] super(MoveitWidget, self).__init__() self._parent = parent self._plugin_context = plugin_context self._refresh_rate = 5 # With default value self._rospack = rospkg.RosPack() ui_file = os.path.join(self._rospack.get_path('rqt_moveit'), 'resource', 'moveit_top.ui') loadUi(ui_file, self, {'TopicWidget': TopicWidget}) # Custom widget classes don't show in QSplitter when they instantiated # in .ui file and not explicitly added to QSplitter like this. Thus # this is a workaround. self._splitter.addWidget(self._widget_topic) self._spinbox_refreshrate.valueChanged.connect( self._update_refreshrate) # Show default ref rate on QSpinbox self._spinbox_refreshrate.setValue(self._refresh_rate) # Monitor node self._is_checking_nodes = True self._node_qitems = {} self._node_monitor_thread = self._init_monitor_nodes( self._nodes_monitored) self._node_monitor_thread.start() #TODO: connect sys msg for nodes. # topic to show # Delegate GUI functionality to rqt_topic.TopicWidget. self._widget_topic.set_selected_topics(self._selected_topics) self._widget_topic.set_topic_specifier(TopicWidget.SELECT_BY_MSGTYPE) self._widget_topic.start() # To connect signal in a widget to PluginContainerWidget. #TODO: In this way, Signal from only one instance is hooked. # Not a good design at all. self.sig_sysmsg = self._widget_topic.sig_sysmsg # Init monitoring parameters. self._is_checking_params = True self._param_qitems = {} _col_names_paramtable = ['Param name', 'Found on Parameter Server?'] self._param_check_thread = self._init_monitor_parameters( self._params_monitored, _col_names_paramtable) self._param_check_thread.start() def _init_monitor_nodes(self, nodes_monitored): """ @type params_monitored: str[] @rtype: Thread """ self._node_datamodel = QStandardItemModel(0, 2) self._root_qitem = self._node_datamodel.invisibleRootItem() self._view_nodes.setModel(self._node_datamodel) node_monitor_thread = Thread(target=self._check_nodes_alive, args=(self.sig_node, self._nodes_monitored)) # self._node_monitor_thread = NodeMonitorThread(self, self.sig_node, # nodes_monitored) self.sig_node.connect(self._update_output_nodes) return node_monitor_thread def _check_nodes_alive(self, signal, nodes_monitored): """ Working as a callback of Thread class, this method keeps looping to watch if the nodes whose names are passed exist and emits signal per each node. Notice that what MoveitWidget._check_nodes_alive & MoveitWidget._check_params_alive do is very similar, but since both of them are supposed to be passed to Thread class, there might not be a way to generalize these 2. @param signal: Signal(bool, str) @type nodes_monitored: str[] """ while self._is_checking_nodes: rosnode_dynamically_loaded = __import__('rosnode') #from rosnode import rosnode_ping for nodename in nodes_monitored: #TODO: rosnode_ping prints when the node is not found. # Currently I have no idea how to capture that from here. try: #is_node_running = rosnode_ping(nodename, 1) is_node_running = rosnode_dynamically_loaded.rosnode_ping( nodename, 1) except rosnode_dynamically_loaded.ROSNodeIOException as e: #TODO: Needs to be indicated on GUI # (eg. PluginContainerWidget) rospy.logerr(e.message) is_node_running = False signal.emit(is_node_running, nodename) rospy.logdebug('_update_output_nodes') del rosnode_dynamically_loaded time.sleep(self._refresh_rate) def _init_monitor_parameters(self, params_monitored, _col_names_paramtable=None): """ @type params_monitored: str[] """ self._params_monitored = params_monitored self._param_datamodel = QStandardItemModel(0, 2) self._root_qitem = self._param_datamodel.invisibleRootItem() self._view_params.setModel(self._param_datamodel) # Names of header on the QTableView. if not _col_names_paramtable: _col_names_paramtable = [ 'Param name', 'Found on Parameter Server?' ] self._param_datamodel.setHorizontalHeaderLabels(_col_names_paramtable) self.sig_param.connect(self._update_output_parameters) param_check_thread = Thread(target=self._check_params_alive, args=(self.sig_param, self._params_monitored)) return param_check_thread def _update_output_nodes(self, is_node_running, node_name): """ Slot for signals that tell nodes existence. @type is_node_running: bool @type node_name: str """ #TODO: What this method does is exactly the same with # monitor_parameters. Unify them. rospy.logdebug('is_node_running={} par_name={}'.format( is_node_running, node_name)) node_name = str(node_name) node_qitem = None if not node_name in self._node_qitems: node_qitem = QStandardItem(node_name) self._node_qitems[node_name] = node_qitem self._node_datamodel.appendRow(node_qitem) else: # qsitem with the node name already exists. node_qitem = self._node_qitems[str(node_name)] qindex = self._node_datamodel.indexFromItem(node_qitem) _str_node_running = 'Not running' if is_node_running: _str_node_running = 'Running' qitem_node_status = QStandardItem(_str_node_running) self._node_datamodel.setItem(qindex.row(), 1, qitem_node_status) def _check_params_alive(self, signal, params_monitored): """ Working as a callback of Thread class, this method keeps looping to watch if the params whose names are passed exist and emits signal per each node. Notice that what MoveitWidget._check_nodes_alive & MoveitWidget._check_params_alive do is very similar, but since both of them are supposed to be passed to Thread class, there might not be a way to generalize these 2. @type signal: Signal(bool, str) @param_name signal: emitting a name of the parameter that's found. @type params_monitored: str[] """ while self._is_checking_params: # self._is_checking_params only turns to false when the plugin # shuts down. has_param = False for param_name in params_monitored: is_rosmaster_running = RqtRoscommUtil.is_roscore_running() try: if is_rosmaster_running: # Only if rosmaster is running, check if the parameter # exists or not. has_param = rospy.has_param(param_name) except rospy.exceptions.ROSException as e: self.sig_sysmsg.emit( 'Exception upon rospy.has_param {}'.format(e.message)) signal.emit(has_param, param_name) rospy.loginfo('has_param {}, check_param_alive: {}'.format( has_param, param_name)) time.sleep(self._refresh_rate) def _update_output_parameters(self, has_param, param_name): """ Slot @type has_param: bool @type param_name: str """ rospy.logdebug('has_param={} par_name={}'.format( has_param, param_name)) param_name = str(param_name) param_qitem = None if not param_name in self._param_qitems: param_qitem = QStandardItem(param_name) self._param_qitems[param_name] = param_qitem self._param_datamodel.appendRow(param_qitem) else: # qsitem with the param name already exists. param_qitem = self._param_qitems[str(param_name)] qindex = self._param_datamodel.indexFromItem(param_qitem) _str_param_found = 'No' if has_param: _str_param_found = 'Yes' qitem_param_status = QStandardItem(_str_param_found) self._param_datamodel.setItem(qindex.row(), 1, qitem_param_status) self._view_params.resizeColumnsToContents() def _update_refreshrate(self, refresh_rate): """ Slot @type refresh_rate: int """ self._refresh_rate = refresh_rate def save_settings(self, plugin_settings, instance_settings): instance_settings.set_value(self._SPLITTER_H, self._splitter.saveState()) def restore_settings(self, plugin_settings, instance_settings): if instance_settings.contains(self._SPLITTER_H): self._splitter.restoreState( instance_settings.value(self._SPLITTER_H)) else: self._splitter.setSizes([100, 100, 200]) pass def shutdown(self): """ Overridden. Close threads. @raise RuntimeError: """ try: #self._node_monitor_thread.join() # No effect self._is_checking_nodes = False self._node_monitor_thread = None #self._param_check_thread.join() self._is_checking_params = False self._param_check_thread = None except RuntimeError as e: rospy.logerr(e) raise e
class MoveitWidget(QWidget): """#TODO: comment """ # To be connected to PluginContainerWidget sig_sysmsg = None sig_param = Signal(bool, str) # param name emitted sig_node = Signal(bool, str) # param name emitted _SPLITTER_H = 'splitter_horizontal' def __init__(self, parent, plugin_context): """ @type parent: MoveitMain """ self._nodes_monitored = ['/move_group'] self._selected_topics = [('/pointcloud', 'sensor_msgs/PointCloud'), ('/pointcloud2', 'sensor_msgs/PointCloud2'), ('/image', 'sensor_msgs/Image'), ('/camera_info', 'sensor_msgs/CameraInfo')] self._params_monitored = ['/robot_description', '/robot_description_semantic'] super(MoveitWidget, self).__init__() self._parent = parent self._plugin_context = plugin_context self._refresh_rate = 5 # With default value self._rospack = rospkg.RosPack() ui_file = os.path.join(self._rospack.get_path('rqt_moveit'), 'resource', 'moveit_top.ui') loadUi(ui_file, self, {'TopicWidget': TopicWidget}) # Custom widget classes don't show in QSplitter when they instantiated # in .ui file and not explicitly added to QSplitter like this. Thus # this is a workaround. self._splitter.addWidget(self._widget_topic) self._spinbox_refreshrate.valueChanged.connect( self._update_refreshrate) # Show default ref rate on QSpinbox self._spinbox_refreshrate.setValue(self._refresh_rate) # Monitor node self._is_checking_nodes = True self._node_qitems = {} self._node_monitor_thread = self._init_monitor_nodes( self._nodes_monitored) self._node_monitor_thread.start() #TODO: connect sys msg for nodes. # topic to show # Delegate GUI functionality to rqt_topic.TopicWidget. self._widget_topic.set_selected_topics(self._selected_topics) self._widget_topic.set_topic_specifier(TopicWidget.SELECT_BY_MSGTYPE) self._widget_topic.start() # To connect signal in a widget to PluginContainerWidget. #TODO: In this way, Signal from only one instance is hooked. # Not a good design at all. self.sig_sysmsg = self._widget_topic.sig_sysmsg # Init monitoring parameters. self._is_checking_params = True self._param_qitems = {} _col_names_paramtable = ['Param name', 'Found on Parameter Server?'] self._param_check_thread = self._init_monitor_parameters( self._params_monitored, _col_names_paramtable) self._param_check_thread.start() def _init_monitor_nodes(self, nodes_monitored): """ @type params_monitored: str[] @rtype: Thread """ self._node_datamodel = QStandardItemModel(0, 2) self._root_qitem = self._node_datamodel.invisibleRootItem() self._view_nodes.setModel(self._node_datamodel) node_monitor_thread = Thread(target=self._check_nodes_alive, args=(self.sig_node, self._nodes_monitored)) # self._node_monitor_thread = NodeMonitorThread(self, self.sig_node, # nodes_monitored) self.sig_node.connect(self._update_output_nodes) return node_monitor_thread def _check_nodes_alive(self, signal, nodes_monitored): """ Working as a callback of Thread class, this method keeps looping to watch if the nodes whose names are passed exist and emits signal per each node. Notice that what MoveitWidget._check_nodes_alive & MoveitWidget._check_params_alive do is very similar, but since both of them are supposed to be passed to Thread class, there might not be a way to generalize these 2. @param signal: Signal(bool, str) @type nodes_monitored: str[] """ while self._is_checking_nodes: rosnode_dynamically_loaded = __import__('rosnode') #from rosnode import rosnode_ping for nodename in nodes_monitored: #TODO: rosnode_ping prints when the node is not found. # Currently I have no idea how to capture that from here. try: #is_node_running = rosnode_ping(nodename, 1) is_node_running = rosnode_dynamically_loaded.rosnode_ping( nodename, 1) except rosnode_dynamically_loaded.ROSNodeIOException as e: #TODO: Needs to be indicated on GUI # (eg. PluginContainerWidget) rospy.logerr(e.message) is_node_running = False signal.emit(is_node_running, nodename) rospy.logdebug('_update_output_nodes') del rosnode_dynamically_loaded time.sleep(self._refresh_rate) def _init_monitor_parameters(self, params_monitored, _col_names_paramtable=None): """ @type params_monitored: str[] """ self._params_monitored = params_monitored self._param_datamodel = QStandardItemModel(0, 2) self._root_qitem = self._param_datamodel.invisibleRootItem() self._view_params.setModel(self._param_datamodel) # Names of header on the QTableView. if not _col_names_paramtable: _col_names_paramtable = ['Param name', 'Found on Parameter Server?'] self._param_datamodel.setHorizontalHeaderLabels(_col_names_paramtable) self.sig_param.connect(self._update_output_parameters) param_check_thread = Thread(target=self._check_params_alive, args=(self.sig_param, self._params_monitored)) return param_check_thread def _update_output_nodes(self, is_node_running, node_name): """ Slot for signals that tell nodes existence. @type is_node_running: bool @type node_name: str """ #TODO: What this method does is exactly the same with # monitor_parameters. Unify them. rospy.logdebug('is_node_running={} par_name={}'.format(is_node_running, node_name)) node_name = str(node_name) node_qitem = None if not node_name in self._node_qitems: node_qitem = QStandardItem(node_name) self._node_qitems[node_name] = node_qitem self._node_datamodel.appendRow(node_qitem) else: # qsitem with the node name already exists. node_qitem = self._node_qitems[str(node_name)] qindex = self._node_datamodel.indexFromItem(node_qitem) _str_node_running = 'Not running' if is_node_running: _str_node_running = 'Running' qitem_node_status = QStandardItem(_str_node_running) self._node_datamodel.setItem(qindex.row(), 1, qitem_node_status) def _check_params_alive(self, signal, params_monitored): """ Working as a callback of Thread class, this method keeps looping to watch if the params whose names are passed exist and emits signal per each node. Notice that what MoveitWidget._check_nodes_alive & MoveitWidget._check_params_alive do is very similar, but since both of them are supposed to be passed to Thread class, there might not be a way to generalize these 2. @type signal: Signal(bool, str) @param_name signal: emitting a name of the parameter that's found. @type params_monitored: str[] """ while self._is_checking_params: # self._is_checking_params only turns to false when the plugin # shuts down. has_param = False for param_name in params_monitored: is_rosmaster_running = RqtRoscommUtil.is_roscore_running() try: if is_rosmaster_running: # Only if rosmaster is running, check if the parameter # exists or not. has_param = rospy.has_param(param_name) except rospy.exceptions.ROSException as e: self.sig_sysmsg.emit( 'Exception upon rospy.has_param {}'.format(e.message)) signal.emit(has_param, param_name) rospy.loginfo('has_param {}, check_param_alive: {}'.format( has_param, param_name)) time.sleep(self._refresh_rate) def _update_output_parameters(self, has_param, param_name): """ Slot @type has_param: bool @type param_name: str """ rospy.logdebug('has_param={} par_name={}'.format(has_param, param_name)) param_name = str(param_name) param_qitem = None if not param_name in self._param_qitems: param_qitem = QStandardItem(param_name) self._param_qitems[param_name] = param_qitem self._param_datamodel.appendRow(param_qitem) else: # qsitem with the param name already exists. param_qitem = self._param_qitems[str(param_name)] qindex = self._param_datamodel.indexFromItem(param_qitem) _str_param_found = 'No' if has_param: _str_param_found = 'Yes' qitem_param_status = QStandardItem(_str_param_found) self._param_datamodel.setItem(qindex.row(), 1, qitem_param_status) self._view_params.resizeColumnsToContents() def _update_refreshrate(self, refresh_rate): """ Slot @type refresh_rate: int """ self._refresh_rate = refresh_rate def save_settings(self, plugin_settings, instance_settings): instance_settings.set_value(self._SPLITTER_H, self._splitter.saveState()) def restore_settings(self, plugin_settings, instance_settings): if instance_settings.contains(self._SPLITTER_H): self._splitter.restoreState(instance_settings.value( self._SPLITTER_H)) else: self._splitter.setSizes([100, 100, 200]) pass def shutdown(self): """ Overridden. Close threads. @raise RuntimeError: """ try: #self._node_monitor_thread.join() # No effect self._is_checking_nodes = False self._node_monitor_thread = None #self._param_check_thread.join() self._is_checking_params = False self._param_check_thread = None except RuntimeError as e: rospy.logerr(e) raise e
class RqtSrVisualServoing(Plugin): def __init__(self, context): super(RqtSrVisualServoing, self).__init__(context) # Give QObjects reasonable names self.setObjectName('RqtSrVisualServoing') # # Process standalone plugin command-line arguments # from argparse import ArgumentParser # parser = ArgumentParser() # # Add argument(s) to the parser. # parser.add_argument("-q", "--quiet", action="store_true", # dest="quiet", # help="Put plugin in silent mode") # args, unknowns = parser.parse_known_args(context.argv()) # if not args.quiet: # print 'arguments: ', args # print 'unknowns: ', unknowns # Create QWidget self.ui = QWidget() # Get path to UI file which is in our packages resource directory rp = rospkg.RosPack() self.ui_file = os.path.join(rp.get_path(PKG), 'resource', 'gui.ui') # Extend the widget with all attributes and children from UI file loadUi(self.ui_file, self.ui) # Give QObjects reasonable names self.ui.setObjectName('RqtSrVisualServoingUi') # Show ui.windowTitle on left-top of each plugin (when it's set in ui). # This is useful when you open multiple plugins at once. Also if you # open multiple instances of your plugin at once, these lines add # number to make it easy to tell from pane to pane. if context.serial_number() > 1: self.ui.setWindowTitle(self.ui.windowTitle() + (' (%d)' % context.serial_number())) # Wire up the buttons self.ui.startBtn.clicked.connect( self.start_clicked ) self.ui.stopBtn.clicked.connect( self.stop_clicked ) # Annoyingly setting the icon theme in designer only works in designer, # we have to set it again here for it to work at run time self.ui.startBtn.setIcon(QIcon.fromTheme('media-playback-start')) self.ui.stopBtn.setIcon(QIcon.fromTheme('media-playback-stop')) # Add widget to the user interface context.add_widget(self.ui) # Setup a model for the feedback and link to the view. self.feedback_model = QStandardItemModel(0,2) self.feedback_model.setHorizontalHeaderLabels(['Name','Value']) self.ui.feedbackView.setModel(self.feedback_model) self.ui.connect( self.ui, SIGNAL('feedback(QString)'), self.update_feedback ) # ROS setup self.last_feedback = None self.client = actionlib.SimpleActionClient('visual_servo', VisualServoingAction) msg = "" if self.client.wait_for_server(rospy.Duration(2.0)): msg = "Found action server, servoing appears to be running" rospy.loginfo(msg) else: msg = "Can't find action server, servoing not running" rospy.logerr(msg) self.ui.statusValue.setText(msg) def shutdown_plugin(self): # TODO unregister all publishers here pass def save_settings(self, plugin_settings, instance_settings): # TODO save intrinsic configuration, usually using: # instance_settings.set_value(k, v) pass def restore_settings(self, plugin_settings, instance_settings): # TODO restore intrinsic configuration, usually using: # v = instance_settings.value(k) pass #def trigger_configuration(self): # Comment in to signal that the plugin has a way to configure it # Usually used to open a configuration dialog def start_clicked(self): goal = VisualServoingActionGoal() self.client.send_goal(goal, feedback_cb = self._feedback_cb) self.ui.statusValue.setText("Starting") def stop_clicked(self): self.client.cancel_all_goals() self.ui.statusValue.setText("Stopped") self.feedback_model.clear() def _feedback_cb(self, feedback): # We can't update the UI in this thread so stash the data and emit a # signal that can be traped by the main thread and update the ui. self.last_feedback = feedback self.ui.emit( SIGNAL('feedback(QString)'), "" ) def update_feedback(self, data): """Listen for feedback signals and update the interface.""" fb = self.last_feedback self.ui.statusValue.setText(str(self.client.get_goal_status_text())) # Update the feedback model, which triggers the view to update m = self.feedback_model m.setHorizontalHeaderLabels(['Name','Value']) m.setItem(0,0,QStandardItem('distance')) m.setItem(0,1,QStandardItem(str(fb.distance))) m.setItem(1,0,QStandardItem('object_pose.position.x')) m.setItem(1,1,QStandardItem(str(fb.object_pose.position.x))) m.setItem(2,0,QStandardItem('object_pose.position.y')) m.setItem(2,1,QStandardItem(str(fb.object_pose.position.y))) m.setItem(3,0,QStandardItem('object_pose.position.z')) m.setItem(3,1,QStandardItem(str(fb.object_pose.position.z))) m.setItem(4,0,QStandardItem('object_pose.orientation.x')) m.setItem(4,1,QStandardItem(str(fb.object_pose.orientation.x))) m.setItem(5,0,QStandardItem('object_pose.orientation.y')) m.setItem(5,1,QStandardItem(str(fb.object_pose.orientation.y))) m.setItem(6,0,QStandardItem('object_pose.orientation.z')) m.setItem(6,1,QStandardItem(str(fb.object_pose.orientation.z))) m.setItem(7,0,QStandardItem('object_pose.orientation.w')) m.setItem(7,1,QStandardItem(str(fb.object_pose.orientation.w))) m.setItem(8,0,QStandardItem('grasp_pose.position.x')) m.setItem(8,1,QStandardItem(str(fb.grasp_pose.position.x))) m.setItem(9,0,QStandardItem('grasp_pose.position.y')) m.setItem(9,1,QStandardItem(str(fb.grasp_pose.position.y))) m.setItem(10,0,QStandardItem('grasp_pose.position.z')) m.setItem(10,1,QStandardItem(str(fb.grasp_pose.position.z))) m.setItem(11,0,QStandardItem('grasp_pose.orientation.x')) m.setItem(11,1,QStandardItem(str(fb.grasp_pose.orientation.x))) m.setItem(12,0,QStandardItem('grasp_pose.orientation.y')) m.setItem(12,1,QStandardItem(str(fb.grasp_pose.orientation.y))) m.setItem(13,0,QStandardItem('grasp_pose.orientation.z')) m.setItem(13,1,QStandardItem(str(fb.grasp_pose.orientation.z))) m.setItem(14,0,QStandardItem('grasp_pose.orientation.w')) m.setItem(14,1,QStandardItem(str(fb.grasp_pose.orientation.w))) self.ui.feedbackView.resizeColumnsToContents()
class MoveitWidget(QWidget): """ This Widget provides an overview about the presence of different parts of a running moveIt instance. """ # To be connected to PluginContainerWidget sig_sysmsg = Signal(str) sig_param = Signal(bool, str) # param name emitted sig_node = Signal(bool, str) # node name emitted sig_topic = Signal(list) # topic name emitted _SPLITTER_H = 'splitter_horizontal' def __init__(self, parent, plugin_context): """ @type parent: MoveitMain """ self._ros_master = xmlrpclib.ServerProxy(os.environ['ROS_MASTER_URI']) self._stop_event = threading.Event() self._nodes_monitored = ['/move_group'] self._selected_topics = [('/pointcloud', 'sensor_msgs/PointCloud'), ('/pointcloud2', 'sensor_msgs/PointCloud2'), ('/image', 'sensor_msgs/Image'), ('/camera_info', 'sensor_msgs/CameraInfo')] self._params_monitored = [ '/robot_description', '/robot_description_semantic' ] super(MoveitWidget, self).__init__() self._parent = parent self._plugin_context = plugin_context self._refresh_rate = 5 # With default value self._rospack = rospkg.RosPack() ui_file = os.path.join(self._rospack.get_path('rqt_moveit'), 'resource', 'moveit_top.ui') loadUi(ui_file, self, {'TopicWidget': TopicWidget}) # Custom widget classes don't show in QSplitter when they instantiated # in .ui file and not explicitly added to QSplitter like this. Thus # this is a workaround. self._splitter.addWidget(self._widget_topic) self._spinbox_refreshrate.valueChanged.connect( self._update_refreshrate) # Show default ref rate on QSpinbox self._spinbox_refreshrate.setValue(self._refresh_rate) # Monitor node self._node_qitems = {} self._node_monitor_thread = self._init_monitor_nodes() self._node_monitor_thread.start() # topic to show self._registered_topics = None self._topic_monitor_thread = self._init_monitor_topics() self._topic_monitor_thread.start() # Init monitoring parameters. self._param_qitems = {} _col_names_paramtable = ['Param name', 'Found on Parameter Server?'] self._param_check_thread = self._init_monitor_parameters( _col_names_paramtable) self._param_check_thread.start() def _init_monitor_nodes(self): """ @rtype: Thread """ self._node_datamodel = QStandardItemModel(0, 2) self._root_qitem = self._node_datamodel.invisibleRootItem() self._view_nodes.setModel(self._node_datamodel) node_monitor_thread = threading.Thread(target=self._check_nodes_alive, args=(self.sig_node, self._nodes_monitored, self._stop_event)) self.sig_node.connect(self._update_output_nodes) return node_monitor_thread def _check_nodes_alive(self, signal, nodes_monitored, stop_event): """ Working as a callback of Thread class, this method keeps looping to watch if the nodes whose names are passed exist and emits signal per each node. Notice that what MoveitWidget._check_nodes_alive & MoveitWidget._check_params_alive do is very similar, but since both of them are supposed to be passed to Thread class, there might not be a way to generalize these 2. @param signal: Signal(bool, str) @type nodes_monitored: str[] @type stop_event: Event() """ rosnode_dynamically_loaded = __import__('rosnode') while True: for nodename in nodes_monitored: try: registered_nodes = rosnode_dynamically_loaded.get_node_names( ) is_node_running = nodename in registered_nodes except rosnode_dynamically_loaded.ROSNodeIOException as e: # TODO: Needs to be indicated on GUI # (eg. PluginContainerWidget) rospy.logerr(e.message) is_node_running = False signal.emit(is_node_running, nodename) rospy.logdebug('_update_output_nodes') if stop_event.wait(self._refresh_rate): del rosnode_dynamically_loaded return def _init_monitor_topics(self): """ @rtype: Thread """ topic_monitor_thread = threading.Thread( target=self._check_topics_alive, args=(self.sig_topic, self._selected_topics, self._stop_event)) self.sig_topic.connect(self._update_output_topics) return topic_monitor_thread def _check_topics_alive(self, signal, topics_monitored, stop_event): """ Working as a callback of Thread class, this method keeps looping to watch if the topics whose names are passed exist and emits signal per each node. @param signal: Signal() @type topics_monitored: str[] @type stop_event: Event() """ while True: code, msg, val = self._ros_master.getPublishedTopics( '/rqt_moveit_update_script', "") if code == 1: published_topics = dict(val) else: rospy.logerr("Communication with rosmaster failed") registered_topics = [] for topic in topics_monitored: if topic[0] in published_topics and topic[ 1] == published_topics.get(topic[0]): registered_topics.append( (topic[0], published_topics.get(topic[0]))) signal.emit(list(registered_topics)) rospy.logdebug('_update_output_topics') if stop_event.wait(self._refresh_rate): return def _init_monitor_parameters(self, _col_names_paramtable=None): """ @rtype: Thread """ self._param_datamodel = QStandardItemModel(0, 2) self._root_qitem = self._param_datamodel.invisibleRootItem() self._view_params.setModel(self._param_datamodel) # Names of header on the QTableView. if not _col_names_paramtable: _col_names_paramtable = [ 'Param name', 'Found on Parameter Server?' ] self._param_datamodel.setHorizontalHeaderLabels(_col_names_paramtable) self.sig_param.connect(self._update_output_parameters) param_check_thread = threading.Thread(target=self._check_params_alive, args=(self.sig_param, self._params_monitored, self._stop_event)) return param_check_thread def _update_output_nodes(self, is_node_running, node_name): """ Slot for signals that tell nodes existence. @type is_node_running: bool @type node_name: str """ # TODO: What this method does is exactly the same with # monitor_parameters. Unify them. rospy.logdebug('is_node_running={} par_name={}'.format( is_node_running, node_name)) node_name = str(node_name) node_qitem = None if node_name not in self._node_qitems: node_qitem = QStandardItem(node_name) self._node_qitems[node_name] = node_qitem self._node_datamodel.appendRow(node_qitem) else: # qsitem with the node name already exists. node_qitem = self._node_qitems[str(node_name)] qindex = self._node_datamodel.indexFromItem(node_qitem) _str_node_running = 'Not running' if is_node_running: _str_node_running = 'Running' qitem_node_status = QStandardItem(_str_node_running) self._node_datamodel.setItem(qindex.row(), 1, qitem_node_status) def _update_output_topics(self, registered_topics): """ Slot for signals that tell topic's existence. @type registered_topics: list """ # This branch will cause that once a selected topic was found the topic view will # never be empty again. if len(registered_topics) > 0: if self._registered_topics is None: self._widget_topic.set_selected_topics(registered_topics) self._widget_topic.set_topic_specifier( TopicWidget.SELECT_BY_NAME) self._widget_topic.start() elif self._registered_topics is not None and set( self._registered_topics) != set(registered_topics): self._widget_topic.set_selected_topics(registered_topics) self._registered_topics = registered_topics def _check_params_alive(self, signal, params_monitored, stop_event): """ Working as a callback of Thread class, this method keeps looping to watch if the params whose names are passed exist and emits signal per each node. Notice that what MoveitWidget._check_nodes_alive & MoveitWidget._check_params_alive do is very similar, but since both of them are supposed to be passed to Thread class, there might not be a way to generalize these 2. @type signal: Signal(bool, str) @param_name signal: emitting a name of the parameter that's found. @type params_monitored: str[] @type stop_event: Event() """ while True: has_param = False for param_name in params_monitored: is_rosmaster_running = RqtRoscommUtil.is_roscore_running() try: if is_rosmaster_running: # Only if rosmaster is running, check if the parameter # exists or not. has_param = rospy.has_param(param_name) except rospy.exceptions.ROSException as e: rospy.logerr('Exception upon rospy.has_param {}'.format( e.message)) self.sig_sysmsg.emit( 'Exception upon rospy.has_param {}'.format(e.message)) signal.emit(has_param, param_name) rospy.logdebug('has_param {}, check_param_alive: {}'.format( has_param, param_name)) if stop_event.wait(self._refresh_rate): return def _update_output_parameters(self, has_param, param_name): """ Slot @type has_param: bool @type param_name: str """ rospy.logdebug('has_param={} par_name={}'.format( has_param, param_name)) param_name = str(param_name) param_qitem = None if param_name not in self._param_qitems: param_qitem = QStandardItem(param_name) self._param_qitems[param_name] = param_qitem self._param_datamodel.appendRow(param_qitem) else: # qsitem with the param name already exists. param_qitem = self._param_qitems[str(param_name)] qindex = self._param_datamodel.indexFromItem(param_qitem) _str_param_found = 'No' if has_param: _str_param_found = 'Yes' qitem_param_status = QStandardItem(_str_param_found) self._param_datamodel.setItem(qindex.row(), 1, qitem_param_status) self._view_params.resizeColumnsToContents() def _update_refreshrate(self, refresh_rate): """ Slot @type refresh_rate: int """ self._refresh_rate = refresh_rate def save_settings(self, plugin_settings, instance_settings): instance_settings.set_value(self._SPLITTER_H, self._splitter.saveState()) def restore_settings(self, plugin_settings, instance_settings): if instance_settings.contains(self._SPLITTER_H): self._splitter.restoreState( instance_settings.value(self._SPLITTER_H)) else: self._splitter.setSizes([100, 100, 200]) pass def shutdown(self): """ Overridden. Close threads. @raise RuntimeError: """ try: self._stop_event.set() self._node_monitor_thread.join() self._param_check_thread.join() self._topic_monitor_thread.join() self._node_monitor_thread = None self._param_check_thread = None self._topic_monitor_thread = None except RuntimeError as e: rospy.logerr(e) raise e
class ChatbotGUI(Plugin): def __init__(self, context): super(ChatbotGUI, self).__init__(context) self.setObjectName('ChatbotGUI') from argparse import ArgumentParser parser = ArgumentParser() parser.add_argument("-q", "--quiet", action="store_true", dest="quiet", help="Put plugin in silent mode") args, unknowns = parser.parse_known_args(context.argv()) if not args.quiet: print 'arguments: ', args print 'unknowns: ', unknowns self._widget = QWidget() ui_file = os.path.join(rospkg.RosPack().get_path('chatbot_gui'), 'resource', 'ChatbotGUI.ui') loadUi(ui_file, self._widget, {'NavViewWidget': NavViewWidget}) self._widget.setObjectName('ChatbotGUIUi') if context.serial_number() > 1: self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number())) context.add_widget(self._widget) self._widget.pickDone.clicked[bool].connect(self._handle_pick_clicked) self._widget.placeDone.clicked[bool].connect( self._handle_place_clicked) self._widget.textSay.keyPressEvent = self._handle_custom_keypress #ros stuff self.pubPick = rospy.Publisher('picker', String, queue_size=10) self.pubPlace = rospy.Publisher('placer', String, queue_size=10) self.pubRecog = rospy.Publisher('recognition/raw_result', String, queue_size=10) self.subRecog = rospy.Subscriber("recognition/raw_result", String, self._recog_callback) self.subLog = rospy.Subscriber("chatbot_gui/log", String, self._log_callback) self.subStatus = rospy.Subscriber("chatbot_gui/status", String, self._status_callback) self.model = QStandardItemModel() self.model.setColumnCount(2) headerNames = [] headerNames.append("Timestamp") headerNames.append("Message") self.model.setHorizontalHeaderLabels(headerNames) self._widget.tableLog.setModel(self.model) self._widget.tableLog.horizontalHeader().setStretchLastSection(True) self._widget.tableLog.setColumnWidth(0, 170) self._message_limit = 20000 self._message_count = 0 def _add_log(self, data): timestamp = QStandardItem( QDateTime.currentDateTime().toString('d.M.yyyy hh:mm:ss.zzz')) timestamp.setTextAlignment(Qt.AlignRight) timestamp.setEditable(False) message = QStandardItem(data) message.setEditable(False) row = [] row.append(timestamp) row.append(message) self._message_count = self._message_count + 1 self.model.insertRow(0, row) if self._message_count > self._message_limit: self.model.removeRow(self.model.rowCount() - 1) self._message_count = self._message_count - 1 def _handle_pick_clicked(self): self.pubPick.publish("[pick done]") def _handle_place_clicked(self): self.pubPlace.publish("[place done]") def _handle_custom_keypress(self, event): if event.key() == Qt.Key_Return or event.key() == Qt.Key_Enter: self.pubRecog.publish(self._widget.textSay.toPlainText()) self._widget.textSay.setPlainText("") else: QPlainTextEdit.keyPressEvent(self._widget.textSay, event) def _recog_callback(self, data): self._add_log("heard: '" + data.data + "'") def _log_callback(self, data): self._add_log(data.data) def _status_callback(self, data): self._widget.labelStatus.setText(data.data) def shutdown_plugin(self): print("shutdown_plugin: clearing ros publishers.") self.subRecog.unregister() self.subStatus.unregister() self.subLog.unregister() self.pubPick.unregister() self.pubPlace.unregister() self.pubRecog.unregister() pass def save_settings(self, plugin_settings, instance_settings): # TODO save intrinsic configuration, usually using: # instance_settings.set_value(k, v) pass def restore_settings(self, plugin_settings, instance_settings): # TODO restore intrinsic configuration, usually using: # v = instance_settings.value(k) pass