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 3462db8..71c2e33 100644 --- a/grasp_utils/handeye_dashboard/src/handeye_dashboard/handeye_calibration.py +++ b/grasp_utils/handeye_dashboard/src/handeye_dashboard/handeye_calibration.py @@ -9,15 +9,17 @@ from handeye_tf_service.srv import HandeyeTF from ament_index_python.resources import get_resource +from rcl_interfaces.msg import Parameter, ParameterType, ParameterValue +from rcl_interfaces.srv import SetParameters + # Rqt widgets 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' @@ -87,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) @@ -199,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 ...') @@ -221,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() @@ -239,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: @@ -313,6 +351,18 @@ def calibration(self): self.textedit.append("Failed to solve the hand-eye calibration.") def execution(self): + # Set calibration state to success + req = SetParameters.Request() + + param = Parameter() + param.name = "calibration_state" + param.value.type = ParameterType.PARAMETER_INTEGER + param.value.integer_value = 4 + req.parameters.append(param) + + future = self.client_param.call_async(req) + rclpy.spin_until_future_complete(self.node, future) + # >>> Publish the camera-robot transform self.textedit.append('Publishing the camera TF ...') file_input = '/tmp/' + 'camera-robot.json' diff --git a/grasp_utils/robot_interface/CMakeLists.txt b/grasp_utils/robot_interface/CMakeLists.txt index ab72528..7a1295e 100644 --- a/grasp_utils/robot_interface/CMakeLists.txt +++ b/grasp_utils/robot_interface/CMakeLists.txt @@ -69,7 +69,7 @@ set(${PROJECT_NAME}_SOURCES src/control_base.cpp src/control_ur.cpp ) -add_library(${PROJECT_NAME} ${${PROJECT_NAME}_SOURCES}) +add_library(${PROJECT_NAME} SHARED ${${PROJECT_NAME}_SOURCES}) ament_target_dependencies(${PROJECT_NAME} rclcpp sensor_msgs geometry_msgs tf2_ros) target_link_libraries(${PROJECT_NAME} ${ur_modern_driver_LIBRARIES}) diff --git a/grasp_utils/robot_interface/include/robot_interface/control_base.hpp b/grasp_utils/robot_interface/include/robot_interface/control_base.hpp index e72b6fb..83867c1 100644 --- a/grasp_utils/robot_interface/include/robot_interface/control_base.hpp +++ b/grasp_utils/robot_interface/include/robot_interface/control_base.hpp @@ -300,12 +300,13 @@ class ArmControlBase: public rclcpp::Node void updateTFGoal(const geometry_msgs::msg::PoseStamped& pose_stamped); /** - * @brief This function is used to rotate a unit vector along z axis, i.e. (0, 0, 1) by the assigned rpy euler angles. + * @brief This function is used to rotate the three coordinate system axes by the assigned rpy euler angles. * @param alpha Rotation euler angle around X axis. * @param beta Rotation euler angle around Y axis. * @param gamma Rotation euler angle around Z axis. + * @return The rotated axes, dimension 3. */ - Eigen::Vector3d getUnitApproachVector(const double& alpha, const double& beta, const double& gamma); + std::vector getUnitApproachVectors(const double& alpha, const double& beta, const double& gamma); protected: /// Joint state publisher diff --git a/grasp_utils/robot_interface/src/control_base.cpp b/grasp_utils/robot_interface/src/control_base.cpp index 7233e3a..8415646 100644 --- a/grasp_utils/robot_interface/src/control_base.cpp +++ b/grasp_utils/robot_interface/src/control_base.cpp @@ -86,22 +86,30 @@ void ArmControlBase::toTcpPose(const Eigen::Isometry3d& pose, TcpPose& tcp_pose) tcp_pose.gamma = euler_angles[2]; } -Eigen::Vector3d ArmControlBase::getUnitApproachVector(const double& alpha, const double& beta, const double& gamma) +std::vector ArmControlBase::getUnitApproachVectors(const double& alpha, const double& beta, const double& gamma) { tf2::Quaternion q; q.setRPY(alpha, beta, gamma); tf2::Matrix3x3 r(q); - tf2::Vector3 approach_vector = r * tf2::Vector3(0, 0, 1); - approach_vector = approach_vector.normalize(); - return Eigen::Vector3d(approach_vector[0], approach_vector[1], approach_vector[2]); + // get approach_vectors along x, y, z axes + std::vector approach_vectors; + for (size_t i = 0; i < 3; i++) + { + tf2::Vector3 pre_rotation_vector(0, 0, 0); + pre_rotation_vector[i] = 1; + tf2::Vector3 post_rotation_vector = r * pre_rotation_vector; + post_rotation_vector = post_rotation_vector.normalize(); + approach_vectors.push_back(Eigen::Vector3d(post_rotation_vector[0], post_rotation_vector[1], post_rotation_vector[2])); + } + return approach_vectors; } bool ArmControlBase::pick(double x, double y, double z, double alpha, double beta, double gamma, double vel, double acc, double vel_scale, double approach) { - Eigen::Vector3d pre_grasp_origin = Eigen::Vector3d(x, y, z) - getUnitApproachVector(alpha, beta, gamma) * approach; + Eigen::Vector3d pre_grasp_origin = Eigen::Vector3d(x, y, z) - getUnitApproachVectors(alpha, beta, gamma)[2] * approach; Eigen::Isometry3d grasp, orientation, pre_grasp; orientation = Eigen::AngleAxisd(alpha, Eigen::Vector3d::UnitX()) @@ -146,7 +154,7 @@ bool ArmControlBase::place(double x, double y, double z, double alpha, double beta, double gamma, double vel, double acc, double vel_scale, double retract) { - Eigen::Vector3d pre_place_origin = Eigen::Vector3d(x, y, z) - getUnitApproachVector(alpha, beta, gamma) * retract; + Eigen::Vector3d pre_place_origin = Eigen::Vector3d(x, y, z) - getUnitApproachVectors(alpha, beta, gamma)[2] * retract; Eigen::Isometry3d place, orientation, pre_place; orientation = Eigen::AngleAxisd(alpha, Eigen::Vector3d::UnitX())