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 LaunchWidget(QDialog):
'''#TODO: comment
'''
# To be connected to PluginContainerWidget
sig_sysmsg = Signal(str)
def __init__(self, parent):
'''
@type parent: LaunchMain
'''
super(LaunchWidget, self).__init__()
self._parent = parent
self._config = None
#TODO: should be configurable from gui
self._port_roscore = 11311
self._run_id = None
self._rospack = rospkg.RosPack()
ui_file = os.path.join(self._rospack.get_path('rqt_launch'),
'resource', 'launch_widget.ui')
loadUi(ui_file, self)
# row=0 allows any number of rows to be added.
self._datamodel = QStandardItemModel(0, 1)
master_uri = rosenv.get_master_uri()
rospy.loginfo('LaunchWidget master_uri={}'.format(master_uri))
self._delegate = NodeDelegate(master_uri,
self._rospack)
self._treeview.setModel(self._datamodel)
self._treeview.setItemDelegate(self._delegate)
# NodeController used in controller class for launch operation.
self._node_controllers = []
self._pushbutton_start_all.clicked.connect(self._parent.start_all)
self._pushbutton_stop_all.clicked.connect(self._parent.stop_all)
# Bind package selection with .launch file selection.
self._combobox_pkg.currentIndexChanged[str].connect(
self._refresh_launchfiles)
# Bind a launch file selection with launch GUI generation.
self._combobox_launchfile_name.currentIndexChanged[str].connect(
self._load_launchfile_slot)
self._update_pkgs_contain_launchfiles()
# Used for removing previous nodes
self._num_nodes_previous = 0
def _load_launchfile_slot(self, launchfile_name):
# Not yet sure why, but everytime combobox.currentIndexChanged occurs,
# this method gets called twice with launchfile_name=='' in 1st call.
if launchfile_name == None or launchfile_name == '':
return
_config = None
rospy.logdebug('_load_launchfile_slot launchfile_name={}'.format(
launchfile_name))
try:
_config = self._create_launchconfig(launchfile_name,
self._port_roscore)
#TODO: folder_name_launchfile should be able to specify arbitrarily
# _create_launchconfig takes 3rd arg for it.
except IndexError as e:
msg = 'IndexError={} launchfile={}'.format(e.message,
launchfile_name)
rospy.logerr(msg)
self.sig_sysmsg.emit(msg)
return
except RLException as e:
msg = 'RLException={} launchfile={}'.format(e.message,
launchfile_name)
rospy.logerr(msg)
self.sig_sysmsg.emit(msg)
return
self._create_widgets_for_launchfile(_config)
def _create_launchconfig(self, launchfile_name, port_roscore=11311,
folder_name_launchfile='launch'):
'''
@rtype: ROSLaunchConfig
@raises RLException: raised by roslaunch.config.load_config_default.
'''
pkg_name = self._combobox_pkg.currentText()
try:
launchfile = os.path.join(self._rospack.get_path(pkg_name),
folder_name_launchfile, launchfile_name)
except IndexError as e:
raise RLException('IndexError: {}'.format(e.message))
try:
launch_config = roslaunch.config.load_config_default([launchfile],
port_roscore)
except rospkg.common.ResourceNotFound as e:
raise RLException('ResourceNotFound: {}'.format(e.message))
except RLException as e:
raise e
return launch_config
def _create_widgets_for_launchfile(self, config):
self._config = config
# Delete old nodes' GUIs.
self._node_controllers = []
# These lines seem to remove indexWidgets previously set on treeview.
# Per suggestion in API doc, we are not using reset(). Instead,
# using 2 methods below without any operation in between, which
# I suspect might be inproper.
# http://qt-project.org/doc/qt-4.8/qabstractitemmodel.html#reset
self._datamodel.beginResetModel()
self._datamodel.endResetModel()
# Need to store the num of nodes outside of the loop -- this will
# be used for removing excessive previous node rows.
order_xmlelement = 0
# Loop per xml element
for order_xmlelement, node in enumerate(self._config.nodes):
proxy = NodeProxy(self._run_id, self._config.master.uri, node)
# TODO: consider using QIcon.fromTheme()
status_label = StatusIndicator()
qindex_nodewidget = self._datamodel.index(order_xmlelement,
0, QModelIndex())
node_widget = self._delegate.create_node_widget(qindex_nodewidget,
proxy.config,
status_label)
#TODO: Ideally find a way so that we don't need this block.
#BEGIN If these lines are missing, widget won't be shown either.
std_item = QStandardItem(
#node_widget.get_node_name()
)
self._datamodel.setItem(order_xmlelement, 0, std_item)
#END If these lines are missing, widget won't be shown either.
self._treeview.setIndexWidget(qindex_nodewidget, node_widget)
node_controller = NodeController(proxy, node_widget)
self._node_controllers.append(node_controller)
node_widget.connect_start_stop_button( \
node_controller.start_stop_slot)
rospy.logdebug('loop #%d proxy.config.namespace=%s ' +
'proxy.config.name=%s',
order_xmlelement, proxy.config.namespace,
proxy.config.name)
self._num_nodes_previous = order_xmlelement
self._parent.set_node_controllers(self._node_controllers)
def _update_pkgs_contain_launchfiles(self):
'''
Inspired by rqt_msg.MessageWidget._update_pkgs_contain_launchfiles
'''
packages = sorted([pkg_tuple[0]
for pkg_tuple
in RqtRoscommUtil.iterate_packages('launch')])
self._package_list = packages
rospy.logdebug('pkgs={}'.format(self._package_list))
self._combobox_pkg.clear()
self._combobox_pkg.addItems(self._package_list)
self._combobox_pkg.setCurrentIndex(0)
def _refresh_launchfiles(self, package=None):
'''
Inspired by rqt_msg.MessageWidget._refresh_msgs
'''
if package is None or len(package) == 0:
return
self._launchfile_instances = [] # Launch[]
#TODO: RqtRoscommUtil.list_files's 2nd arg 'subdir' should NOT be
# hardcoded later.
_launch_instance_list = RqtRoscommUtil.list_files(package,
'launch')
rospy.logdebug('_refresh_launches package={} instance_list={}'.format(
package,
_launch_instance_list))
self._launchfile_instances = [x.split('/')[1]
for x in _launch_instance_list]
self._combobox_launchfile_name.clear()
self._combobox_launchfile_name.addItems(self._launchfile_instances)
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
class LaunchWidget(QDialog):
'''#TODO: comment
'''
# To be connected to PluginContainerWidget
sig_sysmsg = Signal(str)
def __init__(self, parent):
'''
@type parent: LaunchMain
'''
super(LaunchWidget, self).__init__()
self._parent = parent
self._config = None
#TODO: should be configurable from gui
self._port_roscore = 11311
self._run_id = None
self._rospack = rospkg.RosPack()
ui_file = os.path.join(self._rospack.get_path('rqt_launch'),
'resource', 'launch_widget.ui')
loadUi(ui_file, self)
# row=0 allows any number of rows to be added.
self._datamodel = QStandardItemModel(0, 1)
master_uri = rosenv.get_master_uri()
rospy.loginfo('LaunchWidget master_uri={}'.format(master_uri))
self._delegate = NodeDelegate(master_uri, self._rospack)
self._treeview.setModel(self._datamodel)
self._treeview.setItemDelegate(self._delegate)
# NodeController used in controller class for launch operation.
self._node_controllers = []
self._pushbutton_load_params.clicked.connect(self._parent.load_params)
self._pushbutton_start_all.clicked.connect(self._parent.start_all)
self._pushbutton_stop_all.clicked.connect(self._parent.stop_all)
# Bind package selection with .launch file selection.
self._combobox_pkg.currentIndexChanged[str].connect(
self._refresh_launchfiles)
# Bind a launch file selection with launch GUI generation.
self._combobox_launchfile_name.currentIndexChanged[str].connect(
self._load_launchfile_slot)
self._update_pkgs_contain_launchfiles()
# Used for removing previous nodes
self._num_nodes_previous = 0
def _load_launchfile_slot(self, launchfile_name):
# Not yet sure why, but everytime combobox.currentIndexChanged occurs,
# this method gets called twice with launchfile_name=='' in 1st call.
if launchfile_name == None or launchfile_name == '':
return
_config = None
rospy.logdebug(
'_load_launchfile_slot launchfile_name={}'.format(launchfile_name))
try:
_config = self._create_launchconfig(launchfile_name,
self._port_roscore)
#TODO: folder_name_launchfile should be able to specify arbitrarily
# _create_launchconfig takes 3rd arg for it.
except IndexError as e:
msg = 'IndexError={} launchfile={}'.format(e.message,
launchfile_name)
rospy.logerr(msg)
self.sig_sysmsg.emit(msg)
return
except RLException as e:
msg = 'RLException={} launchfile={}'.format(
e.message, launchfile_name)
rospy.logerr(msg)
self.sig_sysmsg.emit(msg)
return
self._create_widgets_for_launchfile(_config)
def _create_launchconfig(self,
launchfile_name,
port_roscore=11311,
folder_name_launchfile='launch'):
'''
@rtype: ROSLaunchConfig
@raises RLException: raised by roslaunch.config.load_config_default.
'''
pkg_name = self._combobox_pkg.currentText()
try:
launchfile = os.path.join(self._rospack.get_path(pkg_name),
folder_name_launchfile, launchfile_name)
except IndexError as e:
raise RLException('IndexError: {}'.format(e.message))
try:
launch_config = roslaunch.config.load_config_default([launchfile],
port_roscore)
except rospkg.common.ResourceNotFound as e:
raise RLException('ResourceNotFound: {}'.format(e.message))
except RLException as e:
raise e
return launch_config
def _create_widgets_for_launchfile(self, config):
self._config = config
# Delete old nodes' GUIs.
self._node_controllers = []
# These lines seem to remove indexWidgets previously set on treeview.
# Per suggestion in API doc, we are not using reset(). Instead,
# using 2 methods below without any operation in between, which
# I suspect might be inproper.
# http://qt-project.org/doc/qt-4.8/qabstractitemmodel.html#reset
self._datamodel.beginResetModel()
self._datamodel.endResetModel()
# Need to store the num of nodes outside of the loop -- this will
# be used for removing excessive previous node rows.
order_xmlelement = 0
# Loop per xml element
for order_xmlelement, node in enumerate(self._config.nodes):
proxy = NodeProxy(self._run_id, self._config.master.uri, node)
# TODO: consider using QIcon.fromTheme()
status_label = StatusIndicator()
qindex_nodewidget = self._datamodel.index(order_xmlelement, 0,
QModelIndex())
node_widget = self._delegate.create_node_widget(
qindex_nodewidget, proxy.config, status_label)
#TODO: Ideally find a way so that we don't need this block.
#BEGIN If these lines are missing, widget won't be shown either.
std_item = QStandardItem(
#node_widget.get_node_name()
)
self._datamodel.setItem(order_xmlelement, 0, std_item)
#END If these lines are missing, widget won't be shown either.
self._treeview.setIndexWidget(qindex_nodewidget, node_widget)
node_controller = NodeController(proxy, node_widget)
self._node_controllers.append(node_controller)
node_widget.connect_start_stop_button( \
node_controller.start_stop_slot)
rospy.logdebug(
'loop #%d proxy.config.namespace=%s ' + 'proxy.config.name=%s',
order_xmlelement, proxy.config.namespace, proxy.config.name)
self._num_nodes_previous = order_xmlelement
self._parent.set_node_controllers(self._node_controllers)
def _update_pkgs_contain_launchfiles(self):
'''
Inspired by rqt_msg.MessageWidget._update_pkgs_contain_launchfiles
'''
packages = sorted([
pkg_tuple[0]
for pkg_tuple in RqtRoscommUtil.iterate_packages('launch')
])
self._package_list = packages
rospy.logdebug('pkgs={}'.format(self._package_list))
self._combobox_pkg.clear()
self._combobox_pkg.addItems(self._package_list)
self._combobox_pkg.setCurrentIndex(0)
def _refresh_launchfiles(self, package=None):
'''
Inspired by rqt_msg.MessageWidget._refresh_msgs
'''
if package is None or len(package) == 0:
return
self._launchfile_instances = [] # Launch[]
#TODO: RqtRoscommUtil.list_files's 2nd arg 'subdir' should NOT be
# hardcoded later.
_launch_instance_list = RqtRoscommUtil.list_files(package, 'launch')
rospy.logdebug('_refresh_launches package={} instance_list={}'.format(
package, _launch_instance_list))
self._launchfile_instances = [
x.split('/')[1] for x in _launch_instance_list
]
self._combobox_launchfile_name.clear()
self._combobox_launchfile_name.addItems(self._launchfile_instances)
def load_parameters(self):
'''Loads all global parameters into roscore.'''
run_id = self._run_id if self._run_id is not None \
else roslaunch.rlutil.get_or_generate_uuid(None, True)
runner = roslaunch.ROSLaunchRunner(run_id, self._config)
runner._load_parameters()
msg = 'Loaded %d parameters to parameter server.' \
% len(self._config.params)
self.sig_sysmsg.emit(msg)
rospy.logdebug(msg)
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
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
class Conman(Plugin):
update_graph_sig = Signal(str)
def __init__(self, context):
super(Conman, self).__init__(context)
self._dotcode_sub = None
# Give QObjects reasonable names
self.setObjectName('Conman')
# 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._widget = QWidget()
# Get path to UI file which is a sibling of this file
# in this example the .ui and .py file are in the same folder
ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)), 'rqt_conman.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
self._widget.setObjectName('ConmanPluginUi')
# Show _widget.windowTitle on left-top of each plugin (when
# it's set in _widget). 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._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
# Add widget to the user interface
context.add_widget(self._widget)
palette = QPalette ()
palette.setColor(QPalette.Background, Qt.white)
self._widget.setPalette(palette)
#self._widget.subscribe_button.setCheckable(True)
self._widget.refresh_button.clicked[bool].connect(self._handle_refresh_clicked)
self._widget.commit_button.clicked[bool].connect(self._handle_commit_clicked)
#self._widget.xdot_widget.connect(
#self._widget.xdot_widget, SIGNAL('_update_graph'), self._widget.xdot_widget.set_dotcode)
self.update_graph_sig.connect(self._update_graph)
self.blocks = { }
self.groups = { }
self._ns = ""
self._actions_connected = False
self.enable_widgets(False)
self.new_dotcode_data = ''
self.update_timer = QTimer(self)
self.update_timer.setInterval(50)
self.update_timer.timeout.connect(self._update_widgets)
#self.update_timer.start()
self._get_blocks = None
self._set_blocks = None
self._blocks_model = QStandardItemModel(0,4)
self._blocks_model.setHeaderData(0, Qt.Horizontal, "")
self._blocks_model.setHeaderData(1, Qt.Horizontal, "Action")
self._blocks_model.setHeaderData(2, Qt.Horizontal, "State")
self._blocks_model.setHeaderData(3, Qt.Horizontal, "Block")
self._widget.blocks_table.setModel(self._blocks_model)
self._blocks_delegate = BlocksDelegate(self)
self._widget.blocks_table.setItemDelegate(self._blocks_delegate)
self._blocks_model.itemChanged.connect(self.block_changed)
self._groups_model = QStandardItemModel(0,4)
self._groups_model.setHeaderData(0, Qt.Horizontal, "")
self._groups_model.setHeaderData(1, Qt.Horizontal, "")
self._groups_model.setHeaderData(2, Qt.Horizontal, "")
self._groups_model.setHeaderData(3, Qt.Horizontal, "Group")
self._widget.groups_table.setModel(self._groups_model)
self._groups_delegate = GroupsDelegate(self)
self._widget.groups_table.setItemDelegate(self._groups_delegate)
def block_changed(self, item):
row = item.row()
name = self._blocks_model.item(row,3).text()
block = self.blocks[name]
checked = item.checkState() == Qt.Checked
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
# This will enable a setting button (gear icon) in each dock widget title bar
# Usually used to open a modal configuration dialog
def _get_result_cb(self, status, res):
rospy.loginfo("got result!")
self._blocks_model.setRowCount(0)
self._blocks_model.setRowCount(len(res.blocks))
for (i,block) in zip(range(len(res.blocks)),res.blocks):
# Store in dict
self.blocks[block.name] = block
cb = QStandardItem(True)
cb.setCheckable(True)
cb.setCheckState(Qt.Checked if block.state.value == conman_msgs.msg.TaskState.RUNNING else Qt.Unchecked)
cb.setTextAlignment(Qt.AlignHCenter)
cb.setTristate(True)
action = QStandardItem(True)
action.setText("")
state = QStandardItem(True)
state.setText(state_map[block.state.value])
name = QStandardItem(True)
name.setText(str(block.name))
self._blocks_model.setItem(i,0,cb)
self._blocks_model.setItem(i,1,action)
self._blocks_model.setItem(i,2,state)
self._blocks_model.setItem(i,3,name)
for (i,group) in zip(range(len(res.groups)),res.groups):
self.groups[group.name] = group
cb = QStandardItem(True)
cb.setCheckable(True)
cb.setCheckState(Qt.Checked)
cb.setTextAlignment(Qt.AlignHCenter)
cb.setEnabled(False)
name = QStandardItem(True)
name.setText(str(group.name))
self._groups_model.setItem(i,0,cb)
self._groups_model.setItem(i,3,name)
self._update_groups()
self._update_blocks()
self._widget.blocks_table.resizeColumnsToContents()
self._widget.blocks_table.horizontalHeader().setStretchLastSection(True)
self._widget.groups_table.resizeColumnsToContents()
self._widget.groups_table.horizontalHeader().setStretchLastSection(True)
def _update_blocks(self):
for (name, block) in self.blocks.items():
items = self._blocks_model.findItems(name, column=3)
if len(items) > 0:
item = items[0]
else:
continue
row = item.row()
checked = self._blocks_model.item(row,0).checkState() == Qt.Checked
if checked and block.state.value != conman_msgs.msg.TaskState.RUNNING:
self._blocks_model.item(row,1).setText("ENABLE")
elif not checked and block.state.value == conman_msgs.msg.TaskState.RUNNING:
self._blocks_model.item(row,1).setText("DISABLE")
else:
self._blocks_model.item(row,1).setText("")
self._update_groups()
def _enable_group(self, index, enable):
name = self._groups_model.item(index, 3).text()
group = self.groups[name]
for member in group.members:
items = self._blocks_model.findItems(member, column=3)
if len(items) > 0:
item = items[0]
else:
continue
row = item.row()
self._blocks_model.item(row,0).setCheckState(Qt.Checked if enable else Qt.Unchecked)
self._update_blocks()
def _update_groups(self):
for (name, group) in self.groups.items():
group_items = self._groups_model.findItems(name, column=3)
if len(group_items) > 0:
group_item = group_items[0]
else:
continue
group_row = group_item.row()
members_checked = []
for member in group.members:
items = self._blocks_model.findItems(member, column=3)
if len(items) > 0:
item = items[0]
else:
continue
row = item.row()
members_checked.append(self._blocks_model.item(row,0).checkState() == Qt.Checked)
if all(members_checked):
check = Qt.Checked
else:
check = Qt.Unchecked
self._groups_model.item(group_row,0).setCheckState(check)
def _query_blocks(self):
if self._get_blocks and self._actions_connected:
if self._get_blocks.simple_state == actionlib.SimpleGoalState.DONE:
rospy.loginfo("Getting blocks...")
goal = conman_msgs.msg.GetBlocksGoal()
goal.publish_flow_graph = self._widget.generate_graph_checkbox.isChecked()
self._get_blocks.send_goal(goal, done_cb=self._get_result_cb)
def enable_widgets(self, enable):
#self._widget.generate_graph_checkbox.setEnabled(enable)
self._widget.force_enable_checkbox.setEnabled(enable)
#self._widget.disable_unused_button.setEnabled(enable)
self._widget.xdot_widget.setEnabled(enable)
self._widget.blocks_table.setEnabled(enable)
self._widget.groups_table.setEnabled(enable)
self._widget.regenerate_graph_button.setEnabled(enable)
def _handle_refresh_clicked(self, checked):
ns = self._widget.namespace_input.text()
if len(ns) > 0:
if self._ns != ns:
self._actions_connected = False
self._ns = ns
self.enable_widgets(False)
self._dotcode_sub = rospy.Subscriber(
ns+'/dotcode',
std_msgs.msg.String,
self._dotcode_msg_cb)
self._get_blocks = actionlib.SimpleActionClient(
ns+'/get_blocks_action',
conman_msgs.msg.GetBlocksAction)
self._set_blocks = actionlib.SimpleActionClient(
ns+'/set_blocks_action',
conman_msgs.msg.SetBlocksAction)
rospy.loginfo("Creating action clients on namespace '%s'..." % ns)
if not self._get_blocks.wait_for_server(rospy.Duration(2)):
rospy.loginfo("Timed out waiting for %s." % self._get_blocks.action_client.ns)
return
if not self._set_blocks.wait_for_server(rospy.Duration(2)):
rospy.loginfo("Timed out waiting for %s." % self._set_blocks.action_client.ns)
return
rospy.loginfo("Action clients created.")
self._actions_connected = True
self.enable_widgets(True)
self._query_blocks()
def _handle_commit_clicked(self, checked):
if self._set_blocks and self._actions_connected:
if self._get_blocks.simple_state == actionlib.SimpleGoalState.DONE:
rospy.loginfo("Setting blocks...")
goal = conman_msgs.msg.SetBlocksGoal()
goal.diff = True
goal.force = True
for i in range(self._blocks_model.rowCount()):
name = self._blocks_model.item(i,3).text()
action = self._blocks_model.item(i,1).text()
if action == 'DISABLE':
goal.disable.append(name)
elif action == 'ENABLE':
goal.enable.append(name)
self._set_blocks.send_goal(goal, done_cb=self._get_set_result_cb)
def _get_set_result_cb(self, status, res):
self._query_blocks()
@Slot(str)
def _update_graph(self,dotcode):
self._widget.xdot_widget.set_dotcode(dotcode, center=False)
def _dotcode_msg_cb(self, msg):
#self.new_dotcode_data = msg.data
self.update_graph_sig.emit(msg.data)
def _update_widgets(self):
self._update_groups()
self._update_blocks()
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
