def to_QItemModel(self):
"""
Represent the DSDs debug data as QITemModel
"""
# Return cached result if available
if self.__cached_item_model is not None:
return self.__cached_item_model
# Return empty model when no dsd data was received yet
if self.__cached_msg is None:
return self.__empty_item_model()
# Construct a new item-model
model = QStandardItemModel()
for i in range(len(self.stack)):
elem, _ = self.stack[i]
elem_item = QStandardItem()
elem_item.setEditable(False)
if isinstance(elem, SequenceTreeElement):
elem_item.setText('Sequence: ' + ', '.join(
str(e) for e in elem.action_elements))
sequence = True
else:
elem_item.setText(str(elem))
sequence = False
self.__append_element_to_item(elem_item, elem.debug_data)
model.invisibleRootItem().appendRow(elem_item)
# Add a spacer if this is not the last item
if elem != self.stack[-1][0]:
spacer = QStandardItem()
spacer.setEditable(False)
model.invisibleRootItem().appendRow(spacer)
if sequence:
break
self.__cached_item_model = model
return self.__cached_item_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 NodeSelectorWidget(QWidget):
_COL_NAMES = ['Node']
# public signal
_sig_node_selected = Signal(str)
def __init__(self):
super(NodeSelectorWidget, self).__init__()
self.stretch = None
ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)),
'ui/node_selector.ui')
loadUi(ui_file, self)
# Setup treeview and models
self._std_model = QStandardItemModel()
self._node_selector_view.setModel(self._std_model)
self._rootitem = self._std_model.invisibleRootItem()
self._nodes_previous = None
# Calling this method updates the list of the node.
# Initially done only once.
self._update_nodetree()
# Setting slot for when user clickes on QTreeView.
selectionModel = self._node_selector_view.selectionModel()
selectionModel.selectionChanged.connect(self._selection_changed_slot)
#TODO(Isaac): Needs auto-update function enabled, once another function
# that updates node tree with maintaining collapse/expansion
# state. http://goo.gl/GuwYp can be a help.
# self.timer = QTimer()
# self.timer.timeout.connect(self._refresh_nodes)
# self.timer.start(5000) # 5sec interval is fast enough.
self._collapse_button.pressed.connect(
self._node_selector_view.collapseAll)
self._expand_button.pressed.connect(self._node_selector_view.expandAll)
def _selection_changed_slot(self, selected, deselected):
"""
Overriden from QItemSelectionModel.
:type new_item_select: QItemSelection
:type old_item_select: QItemSelection
"""
index_current = self._node_selector_view.selectionModel().currentIndex(
)
rospy.logdebug(
'_selection_changed_slot row=%d col=%d data=%s ' +
'data.parent=%s child(0, 0)=%s', index_current.row(),
index_current.column(), index_current.data(Qt.DisplayRole),
index_current.parent().data(Qt.DisplayRole),
index_current.child(0, 0).data(Qt.DisplayRole))
if not index_current.child(0, 0).data(Qt.DisplayRole) == None:
return # Meaning the tree has no nodes.
# get the text of the selected item
node_name_selected = self.get_full_grn_recur(index_current, '')
rospy.logdebug('_selection_changed_slot node_name_selected=%s',
node_name_selected)
self._sig_node_selected.emit(node_name_selected)
def get_full_grn_recur(self, model_index, str_grn):
"""
Create full path format of GRN (Graph Resource Names, see
http://www.ros.org/wiki/Names).
An example: /wide_stereo/left/image_color/compressed
Bulid GRN by recursively transcending parents & children of
QModelIndex instance.
Upon its very 1st call, the argument is the index where user clicks on on
the view object (here QTreeView is used but should work with other View).
str_grn can be 0-length string.
:type model_index: QModelIndex
:type str_grn: str
:param str_grn: This could be an incomplete or a complete GRN format.
"""
#TODO(Isaac) Consider moving this to rqt_py_common.
rospy.logdebug('get_full_grn_recur in str=%s', str_grn)
if model_index.data(Qt.DisplayRole) == None:
return str_grn
str_grn = '/' + str(model_index.data(Qt.DisplayRole)) + str_grn
rospy.logdebug('get_full_grn_recur out str=%s', str_grn)
return self.get_full_grn_recur(model_index.parent(), str_grn)
def _update_nodetree(self):
"""
"""
#TODO(Isaac): 11/25/2012 dynamic_reconfigure only returns params that
# are associated with nodes. In order to handle independent
# params, different approach needs taken.
try:
nodes = dynamic_reconfigure.find_reconfigure_services()
except rosservice.ROSServiceIOException as e:
rospy.logerr("Reconfigure GUI cannot connect to master.")
raise e # TODO Make sure 'raise' returns/finalizes this func.
if not nodes == self._nodes_previous:
paramname_prev = ''
paramitem_top_prev = None
i_debug = 0
for node_name_grn in nodes:
p = ParameterItem(node_name_grn)
p.set_param_name(node_name_grn)
names = p.get_param_names()
i_debug += 1
rospy.logdebug('_update_nodetree i=%d names=%s', i_debug,
names)
self._add_tree_node(p, self._rootitem, names)
def _add_tree_node(self, param_item_full, stditem_parent,
child_names_left):
"""
Evaluate current node and the previous node on the same depth.
If the name of both nodes is the same, current node instance is ignored.
If not, the current node gets added to the same parent node.
At the end, this function gets called recursively going down 1 level.
:type param_item_full: ParameterItem
:type stditem_parent: QStandardItem.
:type child_names_left: List of str
:param child_names_left: List of strings that is sorted in hierarchical
order of params.
"""
#TODO(Isaac): Consider moving to rqt_py_common.
name_curr = child_names_left.pop(0)
stditem_curr = ParameterItem(param_item_full.get_raw_param_name())
# item at the bottom is your most recent node.
row_index_parent = stditem_parent.rowCount() - 1
# Obtain and instantiate prev node in the same depth.
name_prev = ''
stditem_prev = None
if not stditem_parent.child(row_index_parent) == None:
stditem_prev = stditem_parent.child(row_index_parent)
name_prev = stditem_prev.text()
stditem = None
if name_prev != name_curr:
stditem_curr.setText(name_curr)
stditem_parent.appendRow(stditem_curr)
stditem = stditem_curr
else:
stditem = stditem_prev
rospy.logdebug(
'add_tree_node 1 name_curr=%s ' +
'\n\t\t\t\t\tname_prev=%s row_index_parent=%d', name_curr,
name_prev, row_index_parent)
if len(child_names_left) != 0:
#TODO: View & Model are closely bound here. Ideally isolate this 2.
# Maybe we should split into 2 classs, 1 handles view,
# the other does model.
self._add_tree_node(param_item_full, stditem, child_names_left)
def _refresh_nodes(self):
#TODO(Isaac) In the future, do NOT remove all nodes. Instead,
# remove only the ones that are gone. And add new ones too.
model = self._rootitem
if model.hasChildren():
row_count = model.rowCount()
model.removeRows(0, row_count)
rospy.logdebug("ParamWidget _refresh_nodes row_count=%s",
row_count)
self._update_nodetree()
def close_node(self):
rospy.logdebug(" in close_node")
class TreeviewWidgetSelectProve(QWidget):
def __init__(self):
super(TreeviewWidgetSelectProve, self).__init__()
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('rqt_prove'), 'resource',
#'treeview_non_custom.ui') # Works with pyqt.
'treeview_with_custom.ui') # No good.
#loadUi(ui_file, self)
loadUi(ui_file, self, {'CustomQTreeview': CustomQTreeview})
self._std_model = QStandardItemModel()
self._rootitem = self._std_model.invisibleRootItem()
item_r_1 = QStandardItem("r1")
self._rootitem.appendRow(item_r_1)
print('_rootitem index={}'.format(self._rootitem.index()))
self.treeview.setModel(self._std_model)
self.selectionModel = self.treeview.selectionModel()
self.selectionModel.selectionChanged.connect(
self._selection_changed_slot)
print('del/sel?\tde/sel index\tde/sel.row\tde/sel.dat\tparent\tinternal id')
self.show()
def _selection_changed_slot(self, selected, deselected):
"""
Receives args from signal QItemSelectionModel.selectionChanged.
:type selected: QItemSelection
:type deselected: QItemSelection
"""
self._test_sel_index(selected, deselected)
def _test_sel_index(self, selected, deselected):
src_model = self._std_model
index_current = None
index_deselected = None
index_parent_sel = None
index_parent_desel = None
curr_qstd_item = None
indexes = selected.indexes()
del_or_sel = 'Sel'
index_internalid = -1
if len(indexes) > 0:
index_internalid = selected.indexes()[0].internalId()
# # Trying many approaches to get the right qindex
# # Approach-1
index_current = selected.indexes()[0]
# # Approach-2
# index_current_from_indexes = selected.indexes()[0]
# index_current = src_model.index(index_current_from_indexes.row(),
# 0, QModelIndex())
# 0, index_current_from_indexes)
# index_current = self.selectionModel.currentIndex()
index_parent_sel = index_current.parent()
curr_qstd_item = src_model.itemFromIndex( # index_parent)
index_current)
elif len(deselected.indexes()) > 0:
index_internalid = deselected.indexes()[0].internalId()
del_or_sel = 'Desel'
index_deselected = self.selectionModel.currentIndex()
index_parent_desel = index_deselected.parent()
curr_qstd_item = src_model.itemFromIndex(index_deselected)
tabular_format = '{}\t{}\t{}\t{}\t{}\t{}'
if len(indexes) > 0:
print(tabular_format.format(
del_or_sel,
index_current,
index_current.row(),
index_current.data(Qt.DisplayRole).toString(),
#curr_qstd_item,
#curr_qstd_item.data(Qt.DisplayRole),
index_parent_sel,
#index_parent_sel.data(Qt.DisplayRole),
index_internalid))
elif len(deselected.indexes()) > 0:
print(tabular_format.format(
del_or_sel,
index_deselected,
index_deselected.row(),
index_deselected.data(Qt.DisplayRole).toString(),
#curr_qstd_item,
# curr_qstd_item.data(),
index_parent_desel,
# index_parent_desel.data(Qt.DisplayRole),
index_internalid))
self.selectionModel.select(index_deselected,
QItemSelectionModel.Deselect)
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 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
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')]
_params_monitored = ['/robot_description',
'/robot_description_semantic']
super(MoveitWidget, self).__init__()
self._parent = parent
self._plugin_context = plugin_context
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})
# Monitor node
self._node_qitems = {}
self._init_monitor_nodes(self._nodes_monitored)
#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._param_qitems = {}
self._init_monitor_parameters(_params_monitored)
def _init_monitor_nodes(self, nodes_monitored):
"""
@type params_monitored: str[]
"""
self._node_datamodel = QStandardItemModel(0, 2)
self._root_qitem = self._node_datamodel.invisibleRootItem()
self._view_nodes.setModel(self._node_datamodel)
self._node_monitor_thread = NodeMonitorThread(self, self.sig_node,
nodes_monitored)
self.sig_node.connect(self._monitor_nodes)
self._node_monitor_thread.start()
def _init_monitor_parameters(self, params_monitored):
"""
@type params_monitored: str[]
"""
self._param_datamodel = QStandardItemModel(0, 2)
self._root_qitem = self._param_datamodel.invisibleRootItem()
self._view_reconf.setModel(self._param_datamodel)
self._param_check_thread = ParamCheckThread(self, self.sig_param,
params_monitored)
self.sig_param.connect(self._monitor_parameters)
self._param_check_thread.start()
def _monitor_nodes(self, is_node_running, node_name):
"""
Slot
@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 _monitor_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 = 'Not found on Parameter Server'
if has_param:
_str_param_found = 'Found on Parameter Server'
qitem_param_status = QStandardItem(_str_param_found)
self._param_datamodel.setItem(qindex.row(), 1, qitem_param_status)
#.insertColumn([qitem_param_status])
def save_settings(self, plugin_settings, instance_settings):
# instance_settings.set_value('splitter', self._splitter.saveState())
pass
def restore_settings(self, plugin_settings, instance_settings):
# if instance_settings.contains('splitter'):
# self._splitter.restoreState(instance_settings.value('splitter'))
# else:
# self._splitter.setSizes([100, 100, 200])
pass
def shutdown(self):
# TODO: impl
pass
