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):
"""
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
_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
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
