diff --git a/grasp_utils/handeye_dashboard/images/green_circle_icon.png b/grasp_utils/handeye_dashboard/images/green_circle_icon.png new file mode 100644 index 0000000..eb19480 Binary files /dev/null and b/grasp_utils/handeye_dashboard/images/green_circle_icon.png differ diff --git a/grasp_utils/handeye_dashboard/images/red_circle_icon.png b/grasp_utils/handeye_dashboard/images/red_circle_icon.png new file mode 100644 index 0000000..94f628c Binary files /dev/null and b/grasp_utils/handeye_dashboard/images/red_circle_icon.png differ diff --git a/grasp_utils/handeye_dashboard/src/handeye_dashboard/handeye_calibration.py b/grasp_utils/handeye_dashboard/src/handeye_dashboard/handeye_calibration.py index 79e9943..71c2e33 100644 --- a/grasp_utils/handeye_dashboard/src/handeye_dashboard/handeye_calibration.py +++ b/grasp_utils/handeye_dashboard/src/handeye_dashboard/handeye_calibration.py @@ -16,11 +16,10 @@ from rqt_gui_py.plugin import Plugin from python_qt_binding import QtCore from python_qt_binding.QtGui import QIcon, QImage, QPixmap, QStandardItem, \ - QIntValidator, QStandardItemModel + QPen, QBrush, QPainter, QIntValidator, QStandardItemModel from python_qt_binding.QtWidgets import (QComboBox, QAction, QToolBar, QStatusBar, - QLineEdit, QWidget, QVBoxLayout, - QLabel, QTextEdit, QFrame, - QHBoxLayout, QTreeView) + QLineEdit, QWidget, QVBoxLayout, QLabel, + QTextEdit, QFrame, QHBoxLayout, QTreeView) class bcolors: HEADER = '\033[95m' @@ -74,11 +73,6 @@ def __init__(self, context): self.widget.setObjectName(self.PLUGIN_TITLE) self.widget.setWindowTitle(self.PLUGIN_TITLE) - # parameter - self.cli_param = self.node.create_client(SetParameters, '/grasp_modbus_server/set_parameters') - while not self.cli_param.wait_for_service(timeout_sec=1.0): - self.node.get_logger().info('service not available, waiting again...') - # Data self.Tsamples = [] @@ -95,12 +89,27 @@ def __init__(self, context): '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) + 'Start the publishing the TF.', self.widget) + self.path_red_icon = path_pkg + '/share/handeye_dashboard/images/red_circle_icon.png' + self.path_green_icon = path_pkg + '/share/handeye_dashboard/images/green_circle_icon.png' + self.parameter_action = QAction(QIcon(QPixmap.fromImage(QImage(self.path_red_icon))), + 'Connect to parameter service.', self.widget) + self.tf_action = QAction(QIcon(QPixmap.fromImage(QImage(self.path_red_icon))), + 'Connect to tf service.', 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) + self.parameter_label = QLabel(self.widget) + self.parameter_label.setText("Parameter service: ") + self.toolbar.addWidget(self.parameter_label) + self.toolbar.addAction(self.parameter_action) + self.tf_label = QLabel(self.widget) + self.tf_label.setText("tf service: ") + self.toolbar.addWidget(self.tf_label) + self.toolbar.addAction(self.tf_action) # Toolbar0 self.l0 = QLabel(self.widget) @@ -207,16 +216,37 @@ def __init__(self, context): self.calibrate_action.triggered.connect(self.calibration) self.clear_action.triggered.connect(self.clear) self.execut_action.triggered.connect(self.execution) + self.parameter_action.triggered.connect(self.parameter_connect) + self.tf_action.triggered.connect(self.tf_connect) # 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...') + # Set up TF service client + self.client_tf = self.node.create_client(HandeyeTF, 'handeye_tf_service') + self.tf_action.setIcon(QIcon(QPixmap.fromImage(QImage(self.path_red_icon))) \ + if not self.client_tf.wait_for_service(timeout_sec=0.5) \ + else QIcon(QPixmap.fromImage(QImage(self.path_green_icon)))) self.req = HandeyeTF.Request() + # Set up parameter service client + self.client_param = self.node.create_client(SetParameters, '/grasp_modbus_server/set_parameters') + self.parameter_action.setIcon(QIcon(QPixmap.fromImage(QImage(self.path_red_icon))) \ + if not self.client_param.wait_for_service(timeout_sec=0.5) \ + else QIcon(QPixmap.fromImage(QImage(self.path_green_icon)))) + + def tf_connect(self): + self.client_tf = self.node.create_client(HandeyeTF, 'handeye_tf_service') + self.tf_action.setIcon(QIcon(QPixmap.fromImage(QImage(self.path_red_icon))) \ + if not self.client_tf.wait_for_service(timeout_sec=0.5) \ + else QIcon(QPixmap.fromImage(QImage(self.path_green_icon)))) + + def parameter_connect(self): + self.client_param = self.node.create_client(SetParameters, '/grasp_modbus_server/set_parameters') + self.parameter_action.setIcon(QIcon(QPixmap.fromImage(QImage(self.path_red_icon))) \ + if not self.client_param.wait_for_service(timeout_sec=0.5) \ + else QIcon(QPixmap.fromImage(QImage(self.path_green_icon)))) + def clear(self): # >>> Clear the recorded samples self.textedit.append('Clearing the recorded data ...') @@ -229,7 +259,7 @@ def get_tf_transform(self, frame_id, child_frame_id): self.req.transform.child_frame_id = child_frame_id self.req.publish.data = False - future = self.cli.call_async(self.req) + future = self.client_tf.call_async(self.req) rclpy.spin_until_future_complete(self.node, future) transform = TransformStamped() @@ -247,7 +277,7 @@ 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) + future = self.client_tf.call_async(self.req) rclpy.spin_until_future_complete(self.node, future) try: @@ -330,7 +360,7 @@ def execution(self): param.value.integer_value = 4 req.parameters.append(param) - future = self.cli_param.call_async(req) + future = self.client_param.call_async(req) rclpy.spin_until_future_complete(self.node, future) # >>> Publish the camera-robot transform