F #he 948 improve calibration process

launch/start.launch

@@ -31,6 +31,7 @@
   <arg name="boresight_calibration_file_path" default="$(find polhemus_ros_driver)/config"/>
   <arg name="boresight_calibration_file_name" default="$(arg product_type)_calibration.yaml"/>
   <arg name="boresight_calibration_file" default="$(arg boresight_calibration_file_path)/$(arg boresight_calibration_file_name)"/>
+  <arg name="calibration_data_divisor" default="1"/>
 
   <node pkg="polhemus_ros_driver" type="polhemus_tf_broadcaster" name="$(arg node_name)" output="screen">
     <rosparam param="boresight_calibration_file" subst_value="True">$(arg boresight_calibration_file)</rosparam>
@@ -45,6 +46,7 @@
 
   <node name="sr_knuckle_calibration" pkg="polhemus_ros_driver" type="knuckle_position_calibration_action_server.py" output="screen">
     <param name="side" value="$(arg hands)" type="string"/>
+    <param name="calibration_data_divisor" value="$(arg calibration_data_divisor)" type="int"/>
   </node>
 
 </launch>

src/polhemus_ros_driver/knuckle_position_calibration_action_server.py

@@ -16,7 +16,6 @@
 #  along with this program; if not, write to the Free Software
 #  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA
 
-import math
 import os
 from enum import Enum
 
@@ -102,8 +101,9 @@ def __init__(self, _hand_prefix: str):
 class SrGloveCalibration:
     SOURCE_TO_KNUCKLE_LIMITS = [[-0.1, -0.1, -0.1], [0.1, 0.1, 0.1]]
     FINGER_LENGTH_LIMITS = [0.03, 0.15]
+    NUMBER_OF_CHECKPOINTS = 4
 
-    def __init__(self, side: str = "right"):
+    def __init__(self, side: str = "right", tf_data_divisor: int = 5):
         """
         Initializes the calibration action server and the interactive marker server.
         @param side: The hand(s) to calibrate - can be left, right or both
@@ -140,6 +140,14 @@ def __init__(self, side: str = "right"):
         # Action server allowing the GUI to trigger and monitor a calibration
         self._action_server = actionlib.SimpleActionServer("/calibration_action_server", CalibrateAction,
                                                            execute_cb=self._calibration, auto_start=False)
+        # How many times during calibration should we calculate the sphere fit and update quality %
+        self._progress_period = 1.0 / self.NUMBER_OF_CHECKPOINTS
+        if not isinstance(tf_data_divisor, int):
+            raise TypeError("tf_data_divisor must be an integer")
+
+        # Only save every 'x'th glove data point for calibration - trade-off between accuracy and computation time
+        self._tf_data_divisor = tf_data_divisor
+        self._tf_data_counter = 0
         self._action_server.start()
 
     def _update_current_knuckle_tf(self, hand: Hand):
@@ -336,6 +344,8 @@ def _initialize_finger_data(self, hand: Hand):
                 hand.finger_data[finger]['residual'] = 0
                 hand.finger_data[finger]['data'] = []
                 hand.finger_data[finger]['center'] = self._create_marker(hand, finger, COLORS[i].value)
+                hand.finger_data[finger]['last_center_estimate'] = None
+                hand.finger_data[finger]['last_radius_estimate'] = None
                 self._marker_server.insert(hand.finger_data[finger]['center'])
 
     def _create_marker(self, hand: Hand, finger: str, color):
@@ -392,18 +402,20 @@ def _create_control(quaternion: Quaternion, name: str):
 
     def _load_tf_callback(self, data):
         """ Callback for received TF data. """
-        for individual_transform in data.transforms:
-            for hand in self._hands.values():
-                for color_index, finger in enumerate(fingers):
-                    if individual_transform.child_frame_id == hand.finger_data[finger]['polhemus_tf_name']:
-                        pos = [individual_transform.transform.translation.x,
-                               individual_transform.transform.translation.y,
-                               individual_transform.transform.translation.z]
-
-                        hand.finger_data[finger]['data'].append(pos)
-                        data_point_marker = DataMarker(hand.polhemus_base_name, Point(*pos),
-                                                       COLORS[color_index].value)
-                        hand.pub.publish(data_point_marker)
+        if self._tf_data_counter % self._tf_data_divisor == 0:
+            for individual_transform in data.transforms:
+                for hand in self._hands.values():
+                    for color_index, finger in enumerate(fingers):
+                        if individual_transform.child_frame_id == hand.finger_data[finger]['polhemus_tf_name']:
+                            pos = [individual_transform.transform.translation.x,
+                                   individual_transform.transform.translation.y,
+                                   individual_transform.transform.translation.z]
+
+                            hand.finger_data[finger]['data'].append(pos)
+                            data_point_marker = DataMarker(hand.polhemus_base_name, Point(*pos),
+                                                           COLORS[color_index].value)
+                            hand.pub.publish(data_point_marker)
+        self._tf_data_counter += 1
 
     def _calibration(self, goal: CalibrateGoal):
         """
@@ -428,6 +440,8 @@ def _calibration(self, goal: CalibrateGoal):
         _result = CalibrateResult()
 
         sub = rospy.Subscriber("/tf", TFMessage, self._load_tf_callback, queue_size=10)
+        rospy.sleep(0.5)  # Ensure the tf subscriber has had time to start receiving messages
+        current_progress = 0.0
         while rospy.Time.now().to_sec() - start < goal.time:
             if self._action_server.is_preempt_requested():
                 rospy.loginfo("Calibration stopped.")
@@ -436,9 +450,10 @@ def _calibration(self, goal: CalibrateGoal):
                 break
 
             _feedback.progress = ((rospy.Time.now().to_sec() - start)) / goal.time
-            if math.floor(_feedback.progress * 100) % 25 == 0 and math.floor(_feedback.progress * 100) != 0:
+            if (_feedback.progress - current_progress) > self._progress_period and _feedback.progress < 1.0:
                 self._get_knuckle_positions(hand)
                 _feedback.quality = self.get_calibration_quality(hand)
+                current_progress += self._progress_period
             self._action_server.publish_feedback(_feedback)
 
         sub.unregister()
@@ -483,13 +498,24 @@ def _get_knuckle_positions(self, hand: Hand, plot=False):
             hand.pub.publish(solution_marker)
             sphere_fit = SphereFit(data=hand.finger_data[finger]['data'], plot=plot)
 
+            initial_guess = None
+            if (hand.finger_data[finger]['last_center_estimate'] is not None and
+               hand.finger_data[finger]['last_radius_estimate'] is not None):
+                initial_guess = []
+                initial_guess.extend(hand.finger_data[finger]['last_center_estimate'].tolist())
+                initial_guess.append(hand.finger_data[finger]['last_radius_estimate'])
+
             radius, center, residual = sphere_fit.fit_sphere(
                 SrGloveCalibration.SOURCE_TO_KNUCKLE_LIMITS[0], SrGloveCalibration.SOURCE_TO_KNUCKLE_LIMITS[1],
-                SrGloveCalibration.FINGER_LENGTH_LIMITS[0], SrGloveCalibration.FINGER_LENGTH_LIMITS[1])
+                SrGloveCalibration.FINGER_LENGTH_LIMITS[0], SrGloveCalibration.FINGER_LENGTH_LIMITS[1],
+                initial_guess=initial_guess)
 
             if plot:
                 sphere_fit.plot_data()
 
+            hand.finger_data[finger]['last_center_estimate'] = center
+            hand.finger_data[finger]['last_radius_estimate'] = radius
+
             hand.finger_data[finger]['residual'] = residual
             hand.finger_data[finger]['length'].append(np.around(radius, 4))
 
@@ -516,4 +542,5 @@ def get_calibration_quality(hand: Hand):
 if __name__ == "__main__":
     rospy.init_node('sr_knuckle_calibration')
     hand_side = rospy.get_param("~side", "both")
-    sr_glove_calibration = SrGloveCalibration(side=hand_side)
+    calibration_data_divisor = int(rospy.get_param("~calibration_data_divisor", 5))
+    sr_glove_calibration = SrGloveCalibration(side=hand_side, tf_data_divisor=calibration_data_divisor)