class TestRqtRosGraph(unittest.TestCase):
"""
:author: Isaac Saito
"""
def setUp(self):
unittest.TestCase.setUp(self)
self._model = QStandardItemModel()
node1 = QStandardItem('node1')
self._node1_1 = QStandardItem('node1_1')
self._node1_1_1 = QStandardItem('node1_1_1')
node1_1_2 = QStandardItem('node1_1_2')
node1_2 = QStandardItem('node1_2')
node1.appendRow(self._node1_1)
self._node1_1.appendRow(self._node1_1_1)
self._node1_1.appendRow(node1_1_2)
node1.appendRow(node1_2)
self._model.appendRow(node1)
#node_list = [node1, node1_1, self._node1_1_1, node1_1_2, node1_2]
#self._model.appendRow(node_list)
self._grn_node1_1_1 = '/node1/node1_1/node1_1_1'
self._len_lower_grn_node1_1 = 2
def tearDown(self):
unittest.TestCase.tearDown(self)
del self._model
def test_get_upper_grn(self):
self.assertEqual(RqtRosGraph.get_upper_grn(self._node1_1_1.index(), ''),
self._grn_node1_1_1)
def test_get_lower_grn_dfs(self):
self.assertEqual(len(RqtRosGraph.get_lower_grn_dfs(
self._node1_1.index(),
'')),
self._len_lower_grn_node1_1)
def test_get_full_grn(self):
self.assertEqual(RqtRosGraph.get_full_grn(self._node1_1_1.index()),
self._grn_node1_1_1)
class TestRqtRosGraph(unittest.TestCase):
"""
:author: Isaac Saito
"""
def setUp(self):
unittest.TestCase.setUp(self)
self._model = QStandardItemModel()
node1 = QStandardItem('node1')
self._node1_1 = QStandardItem('node1_1')
self._node1_1_1 = QStandardItem('node1_1_1')
node1_1_2 = QStandardItem('node1_1_2')
node1_2 = QStandardItem('node1_2')
node1.appendRow(self._node1_1)
self._node1_1.appendRow(self._node1_1_1)
self._node1_1.appendRow(node1_1_2)
node1.appendRow(node1_2)
self._model.appendRow(node1)
# node_list = [node1, node1_1, self._node1_1_1, node1_1_2, node1_2]
# self._model.appendRow(node_list)
self._grn_node1_1_1 = '/node1/node1_1/node1_1_1'
self._len_lower_grn_node1_1 = 2
def tearDown(self):
unittest.TestCase.tearDown(self)
del self._model
def test_get_upper_grn(self):
self.assertEqual(
RqtRosGraph.get_upper_grn(self._node1_1_1.index(), ''),
self._grn_node1_1_1)
def test_get_lower_grn_dfs(self):
self.assertEqual(
len(RqtRosGraph.get_lower_grn_dfs(self._node1_1.index(), '')),
self._len_lower_grn_node1_1)
def test_get_full_grn(self):
self.assertEqual(RqtRosGraph.get_full_grn(self._node1_1_1.index()),
self._grn_node1_1_1)
def _on_ctrl_info(self, index):
popup = self._popup_widget
ctrl = self._controllers[index.row()]
popup.ctrl_name.setText(ctrl.name)
popup.ctrl_type.setText(ctrl.type)
res_model = QStandardItemModel()
model_root = QStandardItem('Claimed Resources')
res_model.appendRow(model_root)
for hw_res in ctrl.claimed_resources:
hw_iface_item = QStandardItem(hw_res.hardware_interface)
model_root.appendRow(hw_iface_item)
for res in hw_res.resources:
res_item = QStandardItem(res)
hw_iface_item.appendRow(res_item)
popup.resource_tree.setModel(res_model)
popup.resource_tree.setItemDelegate(FontDelegate(popup.resource_tree))
popup.resource_tree.expandAll()
popup.move(QCursor.pos())
popup.show()
def _on_ctrl_info(self, index):
popup = self._popup_widget
ctrl = self._controllers[index.row()]
popup.ctrl_name.setText(ctrl.name)
popup.ctrl_type.setText(ctrl.type)
res_model = QStandardItemModel()
model_root = QStandardItem('Claimed Resources')
res_model.appendRow(model_root)
for hw_res in ctrl.claimed_resources:
hw_iface_item = QStandardItem(hw_res.hardware_interface)
model_root.appendRow(hw_iface_item)
for res in hw_res.resources:
res_item = QStandardItem(res)
hw_iface_item.appendRow(res_item)
popup.resource_tree.setModel(res_model)
popup.resource_tree.setItemDelegate(FontDelegate(popup.resource_tree))
popup.resource_tree.expandAll()
popup.move(QCursor.pos())
popup.show()
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 QtRappManager(Plugin):
_update_rapps_signal = Signal()
def __init__(self, context):
self._context = context
super(QtRappManager, self).__init__(context)
self._init_ui(context)
self._init_events()
self._init_variables()
self.spin()
def _init_ui(self, context):
self._widget = QWidget()
self.setObjectName('QtRappManger')
rospack = rospkg.RosPack()
ui_file = os.path.join(rospack.get_path('rocon_qt_app_manager'), 'ui',
'qt_rapp_manager.ui')
self._widget.setObjectName('QtRappManger')
loadUi(ui_file, self._widget, {})
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
context.add_widget(self._widget)
# Set view mode
self._widget.rapp_grid.setViewMode(QListView.IconMode)
#self._widget.rapp_grid.setViewMode(QListView.ListMode)
def _init_events(self):
#combo box change event
self._widget.namespace_cbox.currentIndexChanged.connect(
self._change_namespace)
# Rapp single click event
self._widget.rapp_grid.clicked.connect(self._rapp_single_click)
# Rapp double click event
self._widget.rapp_grid.doubleClicked.connect(self._rapp_double_click)
def _init_variables(self):
self.initialised = False
#init
self._qt_rapp_manager_info = QtRappManagerInfo(self._refresh_rapps)
self._update_rapps_signal.connect(self._update_rapp_list)
self._rapp_view_model = QStandardItemModel()
self._widget.rapp_grid.setModel(self._rapp_view_model)
self._widget.rapp_grid.setWrapping(True)
self._widget.rapp_grid.setIconSize(QSize(60, 60))
self._widget.rapp_grid.setSpacing(10)
self._selected_rapp = None
def spin(self):
self._get_appmanager_namespaces()
def _cleanup_rapps(self):
self._rapp_view_model.clear()
pass
def _get_appmanager_namespaces(self):
namespaces = self._qt_rapp_manager_info._get_namespaces()
for namespace in namespaces:
ns = namespace[:namespace.find('list_rapps')]
self._widget.namespace_cbox.addItem(ns)
def _exit(self):
pass
###################################################################
# Events
###################################################################
def _change_namespace(self, event):
self._qt_rapp_manager_info.select_rapp_manager(
self._widget.namespace_cbox.currentText())
def _refresh_rapps(self):
"""
Updates from qt_app_manager_info.
"""
self._update_rapps_signal.emit()
def _update_rapp_list(self):
"""
Rapp manager namespace event
"""
self._cleanup_rapps()
available_rapps = self._qt_rapp_manager_info.get_available_rapps()
running_rapps = self._qt_rapp_manager_info.get_running_rapps()
rapp_items = []
for r, v in available_rapps.items():
if r in running_rapps.keys():
item = QRappItem(v, running=True)
else:
item = QRappItem(v, running=False)
self._rapp_view_model.appendRow(item)
def _rapp_single_click(self, index):
qrapp = self._rapp_view_model.item(index.row())
rapp = qrapp.getRapp()
self._create_rapp_dialog(rapp)
def _create_rapp_dialog(self, rapp):
is_running = self._qt_rapp_manager_info.is_running_rapp(rapp)
self._selected_rapp = rapp
self._dialog = QtRappDialog(self._widget, rapp,
self._qt_rapp_manager_info.start_rapp,
self._qt_rapp_manager_info.stop_rapp,
is_running)
self._dialog.show()
def _rapp_double_click(self, item):
running_rapps = self._qt_rapp_manager_info.get_running_rapps()
if len(running_rapps) > 0:
names = [r['display_name'] for r in running_rapps.values()]
show_message(self._widget, "Error",
"Rapp %s are already running" % names)
else:
self._start_rapp()
def _start_rapp(self):
result = self._qt_rapp_manager_info.start_rapp(
self._selected_rapp['name'], [],
self._selected_rapp['public_parameters'])
show_message(self._widget, str(result.started), result.message)
self._selected_rapp = None
return result
def _stop_rapp(self):
result = self._qt_rapp_manager_info.stop_rapp()
show_message(self._widget, str(result.stopped), result.message)
self._selected_rapp = None
return result
########################################
# Legacy
########################################
def _select_rapp_tree_item(self, item):
if not item in self.rapps.keys():
print "HAS NO KEY"
else:
self.current_rapp = self.rapps[item]
self._widget.rapp_info_text.clear()
rapp_info = self.qt_rapp_manager_info._get_rapp_info(self.current_rapp)
self._widget.rapp_info_text.appendHtml(rapp_info)
self._update_rapp_parameter_layout(self.current_rapp)
self._update_implementation_tree(self.current_rapp)
def _update_rapp_parameter_layout(self, rapp):
parameters_layout = self._widget.rapp_parameter_layout
clear_layout(parameters_layout)
for param in rapp['public_parameters']:
one_param_layout = create_label_textedit_pair(
param.key, param.value)
parameters_layout.addLayout(one_param_layout)
def _update_implementation_tree(self, rapp):
self._widget.implementation_tree_widget.clear()
self.selected_impl = None
self.impls = {}
for impl in rapp['implementations']:
impl_item = QTreeWidgetItem(
self._widget.implementation_tree_widget)
impl_item.setText(0, impl)
self.impls[impl_item] = impl
def _select_implementation_tree_item(self, item):
if not item in self.impls.keys():
print "HAS NO KEY"
else:
self.selected_impl = self.impls[item]
def _get_public_parameters(self):
public_parameters = {}
parameters_layout = self._widget.rapp_parameter_layout
for i in reversed(range(parameters_layout.count())):
item = parameters_layout.itemAt(i)
key_label = item.itemAt(0).widget()
value_textbox = item.itemAt(1).widget()
public_parameters[key_label.text()] = str(
value_textbox.toPlainText())
return public_parameters
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 QtRappManager(Plugin):
_update_rapps_signal = Signal()
def __init__(self, context):
self._context = context
super(QtRappManager, self).__init__(context)
self._init_ui(context)
self._init_events()
self._init_variables()
self.spin()
def _init_ui(self, context):
self._widget = QWidget()
self.setObjectName('QtRappManger')
rospack = rospkg.RosPack()
ui_file = os.path.join(rospack.get_path('rocon_qt_app_manager'), 'ui',
'qt_rapp_manager.ui')
self._widget.setObjectName('QtRappManger')
loadUi(ui_file, self._widget, {})
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
context.add_widget(self._widget)
# Set view mode
self._widget.rapp_grid.setViewMode(QListView.IconMode)
# self._widget.rapp_grid.setViewMode(QListView.ListMode)
def _init_events(self):
# combo box change event
self._widget.namespace_cbox.currentIndexChanged.connect(
self._change_namespace)
# Rapp single click event
self._widget.rapp_grid.clicked.connect(self._rapp_single_click)
# Rapp double click event
self._widget.rapp_grid.doubleClicked.connect(self._rapp_double_click)
def _init_variables(self):
self.initialised = False
# init
self._qt_rapp_manager_info = QtRappManagerInfo(self._refresh_rapps)
self._update_rapps_signal.connect(self._update_rapp_list)
self._rapp_view_model = QStandardItemModel()
self._widget.rapp_grid.setModel(self._rapp_view_model)
self._widget.rapp_grid.setWrapping(True)
self._widget.rapp_grid.setIconSize(QSize(60, 60))
self._widget.rapp_grid.setSpacing(10)
self._selected_rapp = None
def spin(self):
self._get_appmanager_namespaces()
def _cleanup_rapps(self):
self._rapp_view_model.clear()
def _get_appmanager_namespaces(self):
namespaces = self._qt_rapp_manager_info._get_namespaces()
for namespace in namespaces:
ns = namespace[:namespace.find('list_rapps')]
self._widget.namespace_cbox.addItem(ns)
def _exit(self):
pass
###################################################################
# Events
###################################################################
def _change_namespace(self, event):
self._qt_rapp_manager_info.select_rapp_manager(
self._widget.namespace_cbox.currentText())
def _refresh_rapps(self):
"""
Updates from qt_app_manager_info.
"""
self._update_rapps_signal.emit()
def _update_rapp_list(self):
"""
Rapp manager namespace event
"""
self._cleanup_rapps()
available_rapps = self._qt_rapp_manager_info.get_available_rapps()
running_rapps = self._qt_rapp_manager_info.get_running_rapps()
for r, v in available_rapps.items():
if r in running_rapps.keys():
item = QRappItem(v, running=True)
else:
item = QRappItem(v, running=False)
self._rapp_view_model.appendRow(item)
def _rapp_single_click(self, index):
qrapp = self._rapp_view_model.item(index.row())
rapp = qrapp.getRapp()
self._create_rapp_dialog(rapp)
def _create_rapp_dialog(self, rapp):
is_running = self._qt_rapp_manager_info.is_running_rapp(rapp)
self._selected_rapp = rapp
self._dialog = QtRappDialog(self._widget, rapp,
self._qt_rapp_manager_info.start_rapp,
self._qt_rapp_manager_info.stop_rapp,
is_running)
self._dialog.show()
def _rapp_double_click(self, item):
running_rapps = self._qt_rapp_manager_info.get_running_rapps()
if len(running_rapps) > 0:
names = [r['display_name'] for r in running_rapps.values()]
show_message(self._widget, "Error",
"Rapp %s are already running" % names)
else:
self._start_rapp()
def _start_rapp(self):
result = self._qt_rapp_manager_info.start_rapp(
self._selected_rapp['name'], [],
self._selected_rapp['public_parameters'])
show_message(self._widget, str(result.started), result.message)
self._selected_rapp = None
return result
def _stop_rapp(self):
result = self._qt_rapp_manager_info.stop_rapp()
show_message(self._widget, str(result.stopped), result.message)
self._selected_rapp = None
return result
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 HandEyeCalibration(Plugin):
PLUGIN_TITLE = ' Intel OTC Robotics: Hand-Eye Calibration'
def __init__(self, context):
super(HandEyeCalibration, self).__init__(context)
self.context = context
self.node = context.node
self.widget = QWidget()
self.widget.setObjectName(self.PLUGIN_TITLE)
self.widget.setWindowTitle(self.PLUGIN_TITLE)
# Data
self.Tsamples = []
# Toolbar
_, path_pkg = get_resource('packages', 'handeye_dashboard')
print("{}".format(path_pkg))
self.snapshot_action = QAction(QIcon.fromTheme('camera-photo'),
'Take a snapshot', self.widget)
path = path_pkg + '/share/handeye_dashboard/images/capture.png'
self.calibrate_action = QAction(QIcon(QPixmap.fromImage(QImage(path))),
'Get the camera/robot transform',
self.widget)
self.clear_action = QAction(QIcon.fromTheme('edit-clear'),
'Clear the record data.', self.widget)
path = path_pkg + '/share/handeye_dashboard/images/UR5.png'
self.execut_action = QAction(QIcon(QPixmap.fromImage(QImage(path))),
'EStart the publishing the TF.',
self.widget)
self.toolbar = QToolBar()
self.toolbar.addAction(self.snapshot_action)
self.toolbar.addAction(self.calibrate_action)
self.toolbar.addAction(self.clear_action)
self.toolbar.addAction(self.execut_action)
# Toolbar0
self.l0 = QLabel(self.widget)
self.l0.setText("Camera-Mount-Type: ")
self.l0.setFixedWidth(150)
self.l0.setAlignment(QtCore.Qt.AlignRight | QtCore.Qt.AlignVCenter)
self.combobox = QComboBox(self.widget)
self.combobox.addItem('attached on robot')
self.combobox.addItem('fixed beside robot')
self.toolbar0 = QToolBar()
self.toolbar0.addWidget(self.l0)
self.toolbar0.addWidget(self.combobox)
# Toolbar1
self.l1 = QLabel(self.widget)
self.l1.setText("Camera-Frame: ")
self.l1.setFixedWidth(150)
self.l1.setAlignment(QtCore.Qt.AlignRight | QtCore.Qt.AlignVCenter)
self.camera_frame = QLineEdit(self.widget)
self.camera_frame.setText("camera_link")
self.toolbar1 = QToolBar()
self.toolbar1.addWidget(self.l1)
self.toolbar1.addWidget(self.camera_frame)
# Toolbar2
self.l2 = QLabel(self.widget)
self.l2.setText("Object-Frame: ")
self.l2.setFixedWidth(150)
self.l2.setAlignment(QtCore.Qt.AlignRight | QtCore.Qt.AlignVCenter)
self.object_frame = QLineEdit(self.widget)
self.object_frame.setText("calib_board")
self.toolbar2 = QToolBar()
self.toolbar2.addWidget(self.l2)
self.toolbar2.addWidget(self.object_frame)
# Toolbar3
self.l3 = QLabel(self.widget)
self.l3.setText("Robot-Base-Frame: ")
self.l3.setFixedWidth(150)
self.l3.setAlignment(QtCore.Qt.AlignRight | QtCore.Qt.AlignVCenter)
self.base_frame = QLineEdit(self.widget)
self.base_frame.setText("base")
self.toolbar3 = QToolBar()
self.toolbar3.addWidget(self.l3)
self.toolbar3.addWidget(self.base_frame)
# Toolbar4
self.l4 = QLabel(self.widget)
self.l4.setText("End-Effector-Frame: ")
self.l4.setFixedWidth(150)
self.l4.setAlignment(QtCore.Qt.AlignRight | QtCore.Qt.AlignVCenter)
self.endeffector_frame = QLineEdit(self.widget)
self.endeffector_frame.setText("tool0")
self.toolbar4 = QToolBar()
self.toolbar4.addWidget(self.l4)
self.toolbar4.addWidget(self.endeffector_frame)
# Toolbar5
self.l5 = QLabel(self.widget)
self.l5.setText("Sample-Number: ")
self.l5.setFixedWidth(150)
self.l5.setAlignment(QtCore.Qt.AlignRight | QtCore.Qt.AlignVCenter)
self.le5 = QLineEdit(self.widget)
self.le5.setValidator(QIntValidator())
self.le5.setText('10')
self.le5.setReadOnly(True)
self.toolbar5 = QToolBar()
self.toolbar5.addWidget(self.l5)
self.toolbar5.addWidget(self.le5)
# TreeView
self.treeview = QTreeView()
self.treeview.setAlternatingRowColors(True)
self.model = QStandardItemModel(self.treeview)
self.treeview.setModel(self.model)
self.treeview.setHeaderHidden(True)
# TextEdit
self.textedit = QTextEdit(self.widget)
self.textedit.setReadOnly(True)
# Layout
self.layout = QVBoxLayout()
self.layout.addWidget(self.toolbar0)
self.layout.addWidget(self.toolbar1)
self.layout.addWidget(self.toolbar2)
self.layout.addWidget(self.toolbar3)
self.layout.addWidget(self.toolbar4)
self.layout.addWidget(self.toolbar5)
self.layout.addWidget(self.toolbar)
self.layoutH = QHBoxLayout()
self.layoutH.addWidget(self.treeview)
self.layoutH.addWidget(self.textedit)
self.layout.addLayout(self.layoutH)
self.widget.setLayout(self.layout)
# Add the widget to the user interface
if context.serial_number() > 1:
self.widget.setWindowTitle(self.widget.windowTitle() +
(' (%d)' % context.serial_number()))
context.add_widget(self.widget)
# Make the connections
self.snapshot_action.triggered.connect(self.take_snapshot)
self.calibrate_action.triggered.connect(self.calibration)
self.clear_action.triggered.connect(self.clear)
self.execut_action.triggered.connect(self.execution)
# Package path
self.path_pkg = path_pkg
# Set up TF
self.cli = self.node.create_client(HandeyeTF, 'handeye_tf_service')
while not self.cli.wait_for_service(timeout_sec=1.0):
self.node.get_logger().info(
'service not available, waiting again...')
self.req = HandeyeTF.Request()
def clear(self):
# >>> Clear the recorded samples
self.textedit.append('Clearing the recorded data ...')
self.textedit.clear()
self.Tsamples = []
self.model.clear()
def get_tf_transform(self, frame_id, child_frame_id):
self.req.transform.header.frame_id = frame_id
self.req.transform.child_frame_id = child_frame_id
self.req.publish.data = False
future = self.cli.call_async(self.req)
rclpy.spin_until_future_complete(self.node, future)
transform = TransformStamped()
try:
result = future.result()
except Exception as e:
self.node.get_logger().info('Service call failed %r' % (e, ))
else:
transform = result.tf_lookup_result
return transform
def publish_tf_transform(self, transform_to_publish):
self.req.publish.data = True
self.req.transform = transform_to_publish
future = self.cli.call_async(self.req)
rclpy.spin_until_future_complete(self.node, future)
try:
future.result()
except Exception as e:
self.node.get_logger().info('Service call failed %r' % (e, ))
else:
self.node.get_logger().info(
'Send the camera-robot transform :\n\tfrom `{}` to `{}`.'.
format(self.req.transform.header.frame_id,
self.req.transform.child_frame_id))
def take_snapshot(self):
# >>> Take the snapshot
self.textedit.append('Taking snapshot ...')
# Get the transform from `tool0` to `base_link`
T = self.get_tf_transform(self.base_frame.text(),
self.endeffector_frame.text())
bTe = np.zeros((4, 4))
q = [
T.transform.rotation.w, T.transform.rotation.x,
T.transform.rotation.y, T.transform.rotation.z
]
bTe = br.quaternion.to_transform(q)
bTe[:3, 3] = np.array([
T.transform.translation.x, T.transform.translation.y,
T.transform.translation.z
])
self.textedit.append('Lookup transform: from `{}` to `{}`.'.format(
self.base_frame.text(), self.endeffector_frame.text()))
self.node.get_logger().info(bcolors.OKGREEN + 'bTe:' + bcolors.ENDC +
'\n{}'.format(bTe))
# Get the transform from `calib_board` to `camera_link`
T = self.get_tf_transform(self.camera_frame.text(),
self.object_frame.text())
cTo = np.zeros((4, 4))
q = [
T.transform.rotation.w, T.transform.rotation.x,
T.transform.rotation.y, T.transform.rotation.z
]
cTo = br.quaternion.to_transform(q)
cTo[:3, 3] = np.array([
T.transform.translation.x, T.transform.translation.y,
T.transform.translation.z
])
self.textedit.append('Lookup transform: from `{}` to `{}`.'.format(
self.camera_frame.text(), self.object_frame.text()))
self.node.get_logger().info(bcolors.OKGREEN + 'cTo:' + bcolors.ENDC +
'\n{}'.format(cTo))
parent = QStandardItem('Snapshot {}'.format(len(self.Tsamples)))
child_1 = QStandardItem('bTe:\n{}\n{}\n{}\n{}'.format(
bTe[0, :], bTe[1, :], bTe[2, :], bTe[3, :]))
child_2 = QStandardItem('cTo:\n{}\n{}\n{}\n{}'.format(
cTo[0, :], cTo[1, :], cTo[2, :], cTo[3, :]))
parent.appendRow(child_1)
parent.appendRow(child_2)
self.model.appendRow(parent)
self.Tsamples.append((bTe, cTo))
self.le5.setText(str(len(self.Tsamples)))
def calibration(self):
# >>> Compute the calibration
self.textedit.append('Making the calibration ...')
if len(self.Tsamples) == 0:
self.textedit.append(
'No transform recorded, please take snapshots.')
return
# save samples to `dataset.json` file
save_samples_to_file(self.Tsamples)
import handeye
if self.combobox.currentIndex() == 0:
solver_cri = handeye.calibrator.HandEyeCalibrator(setup='Moving')
if self.combobox.currentIndex() == 1:
solver_cri = handeye.calibrator.HandEyeCalibrator(setup='Fixed')
for sample in self.Tsamples:
solver_cri.add_sample(sample[0], sample[1])
try:
bTc = solver_cri.solve(method=handeye.solver.Daniilidis1999)
# save the calibration result to 'camera-robot.json' file
file_output = '/tmp/' + 'camera-robot.json'
with open(file_output, 'w') as f:
json.dump(bTc.tolist(), f)
except Exception:
self.textedit.append("Failed to solve the hand-eye calibration.")
def execution(self):
# >>> Publish the camera-robot transform
self.textedit.append('Publishing the camera TF ...')
file_input = '/tmp/' + 'camera-robot.json'
with open(file_input, 'r') as f:
datastore = json.load(f)
to_frame = self.camera_frame.text()
if self.combobox.currentIndex() == 0:
from_frame = self.endeffector_frame.text()
if self.combobox.currentIndex() == 1:
from_frame = self.base_frame.text()
bTc = np.array(datastore)
static_transformStamped = TransformStamped()
static_transformStamped.header.stamp = ROSClock().now().to_msg()
static_transformStamped.header.frame_id = from_frame
static_transformStamped.child_frame_id = to_frame
static_transformStamped.transform.translation.x = bTc[0, 3]
static_transformStamped.transform.translation.y = bTc[1, 3]
static_transformStamped.transform.translation.z = bTc[2, 3]
q = br.transform.to_quaternion(bTc)
static_transformStamped.transform.rotation.x = q[1]
static_transformStamped.transform.rotation.y = q[2]
static_transformStamped.transform.rotation.z = q[3]
static_transformStamped.transform.rotation.w = q[0]
self.publish_tf_transform(static_transformStamped)
output_string = "camera-robot pose:\n"
output_string += " Translation: [{}, {}, {}]\n".format(
bTc[0, 3], bTc[1, 3], bTc[2, 3])
output_string += " Rotation: in Quaternion [{}, {}, {}, {}]".format(
q[0], q[1], q[2], q[3])
file_path = '/tmp/' + 'camera-robot.txt'
with open(file_path, 'w') as f:
f.write(output_string)
def shutdown_plugin(self):
"""
Unregister subscribers when the plugin shutdown
"""
pass
def save_settings(self, plugin_settings, instance_settings):
# Nothing to be done here
pass
def restore_settings(self, plugin_settings, instance_settings):
# Nothing to be done here
pass
class OutputDialog(QDialog):
def __init__(self,
mode,
dotgraph,
dotcode_factory,
dotparser,
param_manager,
models_desc_file_path,
nodename='',
parent=None):
super(OutputDialog, self).__init__(parent)
if mode not in ['add', 'edit']:
raise Exception('Wrong mode for Ouput Dialog')
self.setWindowTitle("Add a output pipe")
self.setGeometry(300, 300, 450, 250)
# self._widget = QDialog()
ui_file = os.path.join(rospkg.RosPack().get_path('rqt_vino_plugin'),
'resource', 'add_output_dialog.ui')
loadUi(ui_file, self)
self.dotgraph = dotgraph
self.dotparser = dotparser
self.dotcode_factory = dotcode_factory
self.available_infers_list = param_manager.parse_inferlist_file(
models_desc_file_path)
self.connect_from_listmodel = QStandardItemModel()
self.output_type_combobox.currentTextChanged.connect(
self._update_display)
self.update_output_types()
if mode == 'add':
self.buttonBox.accepted.connect(self._create_node_in_graph)
elif mode == 'edit':
self.nodename = nodename
self.buttonBox.accepted.connect(self._edit_node_in_graph)
def update_output_types(self, outputlist_file=None):
self.output_type_combobox.clear()
self.output_type_combobox.addItem('ImageWindow')
self.output_type_combobox.addItem('Rviz')
self.output_type_combobox.addItem('Rostopic')
def get_infer_desc(self, infer_name):
for infer in self.available_infers_list:
if infer['infer_name'] == infer_name:
return infer
def _update_display(self, output_name):
self.output_name_display_lineEdit.setText(output_name)
self.update_connect_to_list(output_name)
def update_connect_to_list(self, output_name):
self.connect_from_listmodel.clear()
for node in self.dotgraph.get_node_list():
if node.get('nodetype') == 'input':
pass
elif node.get('nodetype') == 'output':
pass
elif node.get('nodetype') == 'infer':
if output_name in self.get_infer_desc(
node.get_name())['connect_to']:
pipeline_item = QStandardItem()
pipeline_item.setText(node.get_name())
pipeline_item.setCheckable(True)
pipeline_item.setCheckState(False)
self.connect_from_listmodel.appendRow(pipeline_item)
self.connect_from_listview.setModel(self.connect_from_listmodel)
def _create_node_in_graph(self):
output_node_name = str(self.output_type_combobox.currentText())
#Create Node
self.dotparser.parse_dotgraph_from_outputs(self.dotcode_factory,
self.dotgraph,
[output_node_name])
#Create connection from input node to infer node
for i in range(self.connect_from_listview.model().rowCount()):
node_name = self.connect_from_listmodel.item(i).text()
check_state = self.connect_from_listmodel.item(i).checkState()
if check_state:
connects = {node_name: [output_node_name]}
self.dotparser.parse_dotgraph_from_connects(
self.dotcode_factory, self.dotgraph, connects)
def _edit_node_in_graph(self):
self.dotcode_factory.del_node_from_graph(self.dotgraph, self.nodename)
self._create_node_in_graph()
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 VinoGraph(Plugin):
def __init__(self, context):
super(VinoGraph, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('VinoGraph')
# 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 should be in the "resource" folder of this package
ui_file = os.path.join(rospkg.RosPack().get_path('rqt_vino_plugin'),
'resource', 'rqt_vino_plugin.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget,
{'InteractiveGraphicsView': InteractiveGraphicsView})
# Give QObjects reasonable names
self._widget.setObjectName('VinoGraphUi')
# Show _widget.windowTitle on left-top of each plugin (when
# it's set in _widget). This is useful when you open multiple QListView
# 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()))
#A dict which stores pipeline name and dotgraph pair
self._dotgraphs = dict()
#which dotgraph currently drawing on the scence
self._current_dotcode = None
self._current_pipeline_name = ''
#Pydot
self.dotcode_factory = VinoPydotFactory()
self.dot_to_qt = DotToQtGenerator()
self.param_manager = ParamManagerWrapper()
#Binding scene canvas
self._scene = QGraphicsScene()
self._scene.setBackgroundBrush(Qt.white)
self._widget.graphics_view.setScene(self._scene)
self._widget.graphics_view.setClickNodeCallback(self._edit_node)
#QListview of pipelines
self._listmodel = QStandardItemModel()
self._widget.pipeline_name_listview.clicked.connect(
self._display_choosed_pipeline)
#self._widget.pipeline_name_listview.itemRenamed.connect(self._rename_pipeline)
#Load pipelines from yaml file
self._widget.load_pipeline_push_button.clicked.connect(
self._load_pipeline)
#Create a pipeline
self._widget.create_pipeline_push_button.clicked.connect(
self._create_pipeline)
#Add input to pipeline graph
self._widget.add_input_push_button.clicked.connect(self._add_input)
#Add infer
self._widget.add_inference_push_button.clicked.connect(self._add_infer)
self._widget.add_output_push_button.clicked.connect(self._add_output)
self._widget.save_pipeline_push_button.clicked.connect(
self._save_pipeline)
self.models_desc_file_path = os.path.join(
rospkg.RosPack().get_path('vino_param_lib'), 'param',
'models.yaml')
# Add widget to the user interface
context.add_widget(self._widget)
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 _refresh_rosgraph(self):
self._widget.pipeline_name_listview.setModel(self._listmodel)
self._widget.pipeline_name_listview.show()
self._redraw_graph_view()
# if not self.initialized:
# return
# self._update_graph_view(self._generate_dotcode())
def _rename_pipeline(self, item, col):
pass
def _display_choosed_pipeline(self, index):
pipeline_name = self._listmodel.data(index)
if pipeline_name in self._dotgraphs.iterkeys():
self._current_pipeline_name = pipeline_name
self._refresh_rosgraph()
def _load_pipeline(self):
self._dotgraphs.clear()
self._listmodel.clear()
file_path, _ = QFileDialog.getOpenFileName(
self._widget, "QFileDialog.getOpenFileName()", "", "*.yaml")
self._dotgraphs = generate_dotcode_from_yaml_file(
self.dotcode_factory, self.param_manager, str(file_path))
for pipeline_name, pipeline_dotgraph in self._dotgraphs.items():
pipeline_item = QStandardItem()
pipeline_item.setText(pipeline_name)
self._listmodel.appendRow(pipeline_item)
self._current_pipeline_name = self._dotgraphs.iterkeys().next()
self._refresh_rosgraph()
#self._display_choosed_pipeline(pipeline_item)
def _redraw_graph_view(self):
self._scene.clear()
if self._dotgraphs == None:
return
self._current_dotcode = self.dotcode_factory.create_dot(
self._dotgraphs[self._current_pipeline_name])
highlight_level = 2
(nodes, edges) = self.dot_to_qt.dotcode_to_qt_items(
self._current_dotcode,
highlight_level=highlight_level,
same_label_siblings=False,
scene=self._scene)
self._scene.setSceneRect(self._scene.itemsBoundingRect())
#if self._widget.auto_fit_graph_check_box.isChecked():
self._fit_in_view()
def _add_input(self):
dlg = InputDialog('add',
self._dotgraphs[self._current_pipeline_name],
self.dotcode_factory,
dotparser,
self.param_manager,
parent=self._widget)
dlg.exec_()
self._refresh_rosgraph()
def _add_infer(self):
if self._current_pipeline_name == '':
return
dlg = InferenceDialog('add',
self._dotgraphs[self._current_pipeline_name],
self.dotcode_factory,
dotparser,
self.param_manager,
self.models_desc_file_path,
parent=self._widget)
dlg.exec_()
self._refresh_rosgraph()
def _edit_node(self, nodename=''):
if self._current_pipeline_name == '':
return
selected_node = self._dotgraphs[self._current_pipeline_name].get_node(
nodename.encode('utf-8'))[0]
selected_node_type = selected_node.get('nodetype')
if selected_node_type == 'input':
self._edit_input(nodename)
elif selected_node_type == 'infer':
self._edit_infer(nodename)
elif selected_node_type == 'output':
self._edit_output(nodename)
def _edit_infer(self, nodename=''):
dlg = InferenceDialog('edit',
self._dotgraphs[self._current_pipeline_name],
self.dotcode_factory,
dotparser,
self.param_manager,
self.models_desc_file_path,
nodename=nodename,
parent=self._widget)
dlg.exec_()
self._refresh_rosgraph()
def _edit_input(self, nodename=''):
dlg = InputDialog('edit',
self._dotgraphs[self._current_pipeline_name],
self.dotcode_factory,
dotparser,
self.param_manager,
nodename=nodename,
parent=self._widget)
dlg.exec_()
self._refresh_rosgraph()
def _edit_output(self, nodename=''):
dlg = OutputDialog('edit',
self._dotgraphs[self._current_pipeline_name],
self.dotcode_factory,
dotparser,
self.param_manager,
models_desc_file_path=self.models_desc_file_path,
nodename=nodename,
parent=self._widget)
dlg.exec_()
self._refresh_rosgraph()
def _add_output(self):
dlg = OutputDialog('add',
self._dotgraphs[self._current_pipeline_name],
self.dotcode_factory,
dotparser,
self.param_manager,
models_desc_file_path=self.models_desc_file_path,
parent=self._widget)
dlg.exec_()
self._refresh_rosgraph()
def _fit_in_view(self):
self._widget.graphics_view.fitInView(self._scene.itemsBoundingRect(),
Qt.KeepAspectRatio)
def _create_pipeline(self):
pipeline_name, okPressed = QInputDialog.getText(
self._widget, "Create a new pipeline", "new pipeline name:",
QLineEdit.Normal, "")
if okPressed != True or pipeline_name == '':
#Empty name
return
pipeline_item = QStandardItem()
pipeline_item.setText(pipeline_name)
self._listmodel.appendRow(pipeline_item)
new_dotgraph = dotparser.generate_dotcode_from_empty(
self.dotcode_factory, pipeline_name)
self._dotgraphs.update(new_dotgraph)
self._current_pipeline_name = pipeline_name
self._refresh_rosgraph()
def _save_pipeline(self):
# print(self._dotgraphs )
self.dotcode_factory.parse_nodes(
self._dotgraphs[self._current_pipeline_name])
class InferenceDialog(QDialog):
def __init__(self, mode, dotgraph , dotcode_factory, dotparser , param_manager, models_desc_file_path, nodename='', parent = None):
super(InferenceDialog, self).__init__(parent)
if mode not in ['add','edit']:
raise Exception('wrong mode for InferenceDialog')
self.nodename = nodename
self.setWindowTitle("Dialog")
self.setGeometry(300,300,450,250)
# self._widget = QDialog()
ui_file = os.path.join(rospkg.RosPack().get_path('rqt_vino_plugin'), 'resource', 'add_inference_dialog.ui')
loadUi(ui_file, self)
self.dotgraph = dotgraph
self.dotparser = dotparser
self.dotcode_factory = dotcode_factory
self.available_infers_list = param_manager.parse_inferlist_file(models_desc_file_path)
# self.node_name_lineedit.textEdited.connect(self._edit_infer_name)
#self.node_infer_name_combobox.activated.connect(self.update_infer_names)
self.node_infer_combobox.currentTextChanged.connect(self._select_infer_type)
self.connect_from_listmodel = QStandardItemModel()#(self.connect_from_listview)
self.connect_to_listmodel = QStandardItemModel()#(self.connect_to_listview)
# self.connect_from_listmodel.itemChanged.connect(self._connect_from_changed)
#self.connect_to_listview.setModel(self.connect_to_listmodel)
self.update_infer_names()
self.update_connect_from_list()
if mode == 'add':
self.buttonBox.accepted.connect(self._create_node_in_graph)
elif mode == 'edit':
self.buttonBox.accepted.connect(self._edit_node_in_graph)
def update_infer_names(self):
self.node_infer_combobox.clear()
for infer in self.available_infers_list:
self.node_infer_combobox.addItem(infer['infer_name'])
def get_infer_desc(self,infer_name):
for infer in self.available_infers_list:
if infer['infer_name'] == infer_name:
return infer
def get_model_desc(self,infer_name,model_type):
for model_desc in self.get_infer_desc(infer_name)['available_models']:
if model_desc['name'] == model_type:
return model_desc
def update_connect_to_list(self, infer_name):
self.connect_from_listmodel.clear()
self.connect_to_listmodel.clear()
for node in self.dotgraph.get_node_list():
pipeline_item = QStandardItem()
pipeline_item.setText(node.get_name())
pipeline_item.setCheckable(True)
pipeline_item.setCheckState(False)
if node.get_name() == infer_name.encode('utf-8'):
continue
if node.get('nodetype') == 'input':
self.connect_from_listmodel.appendRow(pipeline_item)
# self.connect_from_listview.addItem(node.get_name())
elif node.get('nodetype') == 'output':
self.connect_to_listmodel.appendRow(pipeline_item)
# self.connect_to_listview.addItem(node.get_name())
elif node.get('nodetype') == 'infer':
if infer_name in self.get_infer_desc(infer_name)['connect_from']:
self.connect_from_listmodel.appendRow(pipeline_item)
elif infer_name in self.get_infer_desc(infer_name)['connect_to']:
self.connect_to_listmodel.appendRow(pipeline_item)
self.connect_from_listview.setModel(self.connect_from_listmodel)
self.connect_to_listview.setModel(self.connect_to_listmodel)
def update_connect_from_list(self):
pass
def update_model_type(self,infer_name):
self.node_model_combobox.clear()
# infer = self.get_infer_desc(infer_name)
for model in self.get_infer_desc(infer_name)['available_models']:
self.node_model_combobox.addItem(model['name'])
# self.node_model_combobox.addItem("MobileNetSSD")
# self.node_model_combobox.addItem("YoloV2")
def _select_infer_type(self,infer_name):
self.node_engine_combobox.clear()
infer_name = str(infer_name)
for infer in self.available_infers_list:
if infer['infer_name'] == infer_name:
for engine in infer['available_engine']:
self.node_engine_combobox.addItem(engine)
self.update_model_type(infer_name)
self.update_connect_to_list(infer_name)
def _edit_infer_name(self,infer_name):
self.node_name_display_lineEdit.setText(infer_name)
def _create_node_in_graph(self):
infer_node_name = str(self.node_infer_combobox.currentText())
print('ava_list',self.available_infers_list)
infer_node_engine = self.node_engine_combobox.currentText()
print(self.get_infer_desc(infer_node_name))
infer_node_model_type = self.node_model_combobox.currentText()
infer_node_label = self.get_model_desc(infer_node_name, infer_node_model_type)['label']
infer_node_model = self.get_model_desc(infer_node_name, infer_node_model_type)['model']
#Create Node
self.dotparser.parse_dotgraph_from_infers(self.dotcode_factory,self.dotgraph, [{'name': infer_node_name,
'engine':infer_node_engine,
'model':infer_node_model,
'label':infer_node_label,
'batch':1}])
#To-do Implement batch configure
#Create connection from input node to infer node
for i in range(self.connect_from_listview.model().rowCount()):
node_name = self.connect_from_listmodel.item(i).text()
check_state = self.connect_from_listmodel.item(i).checkState()
if check_state:
connects = { node_name: [infer_node_name] }
self.dotparser.parse_dotgraph_from_connects(self.dotcode_factory,self.dotgraph,connects)
#Create connection from infer node to output node
for i in range(self.connect_to_listview.model().rowCount()):
node_name = self.connect_to_listmodel.item(i).text()
check_state = self.connect_to_listmodel.item(i).checkState()
if check_state:
connects = { infer_node_name :[node_name] }
self.dotparser.parse_dotgraph_from_connects(self.dotcode_factory,self.dotgraph,connects)
def _edit_node_in_graph(self):
self.dotcode_factory.del_node_from_graph(self.dotgraph,self.nodename)
self._create_node_in_graph()
class InteractionsChooserUI():
def __init__(self,
rocon_master_uri='localhost',
host_name='localhost',
with_rqt=False):
self.with_rqt = with_rqt
self.widget = QWidget()
self.pairings_view_model = QStandardItemModel()
self.interactions_view_model = QStandardItemModel()
self.interactions_remocon = InteractionsRemocon(
rocon_master_uri, host_name)
self.interactions_remocon.connect(
[self.update_group_combobox, self.refresh_grids])
self.interactions_remocon.connect(
[self.update_pairings_group_combobox, self.refresh_grids])
self.default_group = "All"
rospack = rospkg.RosPack()
ui_file = os.path.join(rospack.get_path('rocon_remocon'), 'ui',
'interactions_chooser.ui')
loadUi(ui_file, self.widget, {})
# create a few directories for caching icons and ...
utils.setup_home_dirs()
self._init_ui()
self._init_events()
def shutdown(self):
self.interactions_remocon.shutdown()
@Slot()
def update_group_combobox(self):
"""
The underyling ros part of the remocon might get fresh data about the group list.
Connect to this slot to update the combobox in the ui.
"""
new_groups = copy.copy(
self.interactions_remocon.interactions_table.groups())
new_groups = [g for g in new_groups if g != "Hidden"]
# did the underlying groups change - if so, update the combobox
current_group = self.widget.interactions_group_combobox.currentText()
current_size = self.widget.interactions_group_combobox.count()
target_group = current_group if current_size != 1 else self.default_group
current_group_list = [
self.widget.interactions_group_combobox.itemText(i)
for i in range(self.widget.interactions_group_combobox.count())
]
if set(current_group_list) != set(['All'] + new_groups):
self.widget.interactions_group_combobox.clear()
self.widget.interactions_group_combobox.addItems(['All'] +
new_groups)
index = self.widget.interactions_group_combobox.findText(
target_group)
if index != -1:
self.widget.interactions_group_combobox.setCurrentIndex(index)
self.refresh_grids()
@Slot()
def update_pairings_group_combobox(self):
"""
The underyling ros part of the remocon might get fresh data about the group list.
Connect to this slot to update the combobox in the ui.
"""
new_groups = copy.copy(
self.interactions_remocon.pairings_table.groups())
# did the underlying groups change - if so, update the combobox
current_group = self.widget.pairings_group_combobox.currentText()
current_size = self.widget.pairings_group_combobox.count()
target_group = current_group if current_size != 1 else self.default_pairings_group
current_group_list = [
self.widget.pairings_group_combobox.itemText(i)
for i in range(self.widget.pairings_group_combobox.count())
]
if set(current_group_list) != set(['All'] + new_groups):
self.widget.pairings_group_combobox.clear()
self.widget.pairings_group_combobox.addItems(['All'] + new_groups)
index = self.widget.pairings_group_combobox.findText(target_group)
if index != -1:
self.widget.pairings_group_combobox.setCurrentIndex(index)
self.refresh_grids()
@Slot()
def refresh_grids(self):
"""
This just does a complete redraw of the interactions with the
currently selected role. It's a bit brute force doing this
every time the interactions' 'state' changes, but this suffices for now.
"""
self.pairings_view_model.clear()
self.interactions_view_model.clear()
active_pairing = copy.copy(self.interactions_remocon.active_pairing)
group = self.widget.pairings_group_combobox.currentText()
for p in self.interactions_remocon.pairings_table.sorted():
if group != "All" and p.group != group:
continue
is_running = False
enabled = False
if active_pairing is not None and p.name == active_pairing.name:
is_running = True
enabled = True
elif active_pairing is None:
enabled = True
# enabled = not p.requires_interaction
item = icon.QModelIconItem(p, enabled=enabled, running=is_running)
self.pairings_view_model.appendRow(item)
group = self.widget.interactions_group_combobox.currentText()
for i in self.interactions_remocon.interactions_table.sorted():
if group != "All" and i.group != group:
continue
if i.hidden:
continue
extra_tooltip_info = ""
if i.required_pairings:
extra_tooltip_info += " Requires "
for required_pairing in i.required_pairings:
extra_tooltip_info += "'" + required_pairing + "', "
extra_tooltip_info = extra_tooltip_info.rstrip(', ')
if not i.bringup_pairing:
extra_tooltip_info += " to be running"
extra_tooltip_info += "."
item = icon.QModelIconItem(
i,
enabled=self._is_interaction_enabled(i),
running=self._is_interaction_running(i),
extended_tooltip_info=extra_tooltip_info)
self.interactions_view_model.appendRow(item)
def _init_ui(self):
self.widget.pairings_grid.setViewMode(QListView.IconMode)
self.widget.pairings_grid.setModel(self.pairings_view_model)
self.widget.pairings_grid.setWordWrap(True)
self.widget.pairings_grid.setWrapping(True)
# really need to get away from listview, or subclass it if we want to control better how many lines of text show up
# self.widget.pairings_grid.setTextElideMode(Qt.ElideNone)
self.widget.pairings_grid.setIconSize(QSize(60, 60))
self.widget.pairings_grid.setSpacing(10)
self.widget.interactions_grid.setViewMode(QListView.IconMode)
self.widget.interactions_grid.setModel(self.interactions_view_model)
self.widget.interactions_grid.setWordWrap(True)
self.widget.interactions_grid.setWrapping(True)
self.widget.interactions_grid.setIconSize(QSize(60, 60))
self.widget.interactions_grid.setSpacing(10)
for ns in self.interactions_remocon.namespaces:
self.widget.namespace_checkbox.addItem(ns)
self.refresh_grids()
self.widget.pairings_group_combobox.addItems(
['All'] + self.interactions_remocon.pairings_table.groups())
self.widget.interactions_group_combobox.addItems(
['All'] + self.interactions_remocon.interactions_table.groups())
# TODO namespace checkbox to self.interactions_remocon.active_namespace
##############################################################################
# Private
##############################################################################
def _init_events(self):
self.widget.namespace_checkbox.currentIndexChanged.connect(
self._event_change_namespace)
self.widget.pairings_grid.clicked.connect(self._pairing_single_click)
self.widget.interactions_grid.clicked.connect(
self._interaction_single_click)
self.widget.button_stop_all_interactions.clicked.connect(
self.interactions_remocon.stop_all_interactions)
self.widget.pairings_group_combobox.currentIndexChanged.connect(
self.refresh_grids)
self.widget.interactions_group_combobox.currentIndexChanged.connect(
self.refresh_grids)
def _event_change_namespace(self):
rospy.logwarn(
"Remocon : changing interaction managers is currently not supported."
)
def _pairing_single_click(self, index):
pairing_item = self.pairings_view_model.item(index.row())
pairing = pairing_item.implementation
self._create_pairing_dialog(pairing)
def _create_pairing_dialog(self, pairing):
active_pairing = copy.copy(self.interactions_remocon.active_pairing)
if active_pairing is not None:
is_running = (active_pairing.name == pairing.name)
is_enabled = is_running
else:
is_enabled = True # not pairing.requires_interaction
is_running = False
self.selected_pairing = pairing
self.dialog = PairingDialog(self.widget, pairing,
self.interactions_remocon.start_pairing,
self.interactions_remocon.stop_pairing,
is_enabled, is_running)
self.dialog.show()
def _interaction_single_click(self, index):
interaction_item = self.interactions_view_model.item(index.row())
interaction = interaction_item.implementation
self._create_interaction_dialog(interaction)
def _create_interaction_dialog(self, interaction):
self.selected_interaction = interaction
self.dialog = InteractionDialog(
self.widget, interaction,
self.interactions_remocon.start_interaction,
self.interactions_remocon.stop_interaction,
self._is_interaction_enabled(interaction),
self._is_interaction_running(interaction),
self._is_interaction_permitted_new_launches(interaction))
self.dialog.show()
def _is_interaction_permitted_new_launches(self, interaction):
current_number_of_launches = len(
self.interactions_remocon.launched_interactions.get_launch_details(
interaction.hash))
if interaction.max == -1 or current_number_of_launches < interaction.max:
return True
else:
return False
def _is_interaction_enabled(self, interaction):
active_pairing = copy.copy(self.interactions_remocon.active_pairing)
enabled = True
if interaction.required_pairings:
if active_pairing and active_pairing.name not in interaction.required_pairings:
enabled = False
elif active_pairing is None:
if not interaction.bringup_pairing:
enabled = False
else:
available_required_pairings = [
name for name in interaction.required_pairings
if self.interactions_remocon.pairings_table.find(name)
is not None
]
if not available_required_pairings:
enabled = False
return enabled
def _is_interaction_running(self, interaction):
return True if self.interactions_remocon.launched_interactions.get_launch_details(
interaction.hash) else False
