Add calibration parameter client and generate grasp approach vectors from x, y axes

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'

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})
 

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<Eigen::Vector3d> getUnitApproachVectors(const double& alpha, const double& beta, const double& gamma);
 
 protected:
   /// Joint state publisher

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<Eigen::Vector3d> 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<Eigen::Vector3d> 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())