From 611aa7710c07ebc76c34a15cfa2ea4c73ca24c99 Mon Sep 17 00:00:00 2001 From: carebare47 Date: Tue, 13 Jun 2023 16:22:18 +0000 Subject: [PATCH 01/22] fix progress bar logic --- .../knuckle_position_calibration_action_server.py | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py b/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py index ba1f9b7..e84ede5 100755 --- a/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py +++ b/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py @@ -140,6 +140,9 @@ 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._number_of_checkpoints = 4 + self._progress_period = 1.0 / self._number_of_checkpoints self._action_server.start() def _update_current_knuckle_tf(self, hand: Hand): @@ -428,6 +431,7 @@ def _calibration(self, goal: CalibrateGoal): _result = CalibrateResult() sub = rospy.Subscriber("/tf", TFMessage, self._load_tf_callback, queue_size=10) + 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 +440,11 @@ 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 + math.floor(_feedback.progress * 100) != 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() From 7018f5b44ca2bbc9d606d95e464d3420c94e6ccf Mon Sep 17 00:00:00 2001 From: carebare47 Date: Tue, 13 Jun 2023 16:24:53 +0000 Subject: [PATCH 02/22] tf subscriber takes time to start up. Added a small sleep to make sure we actually have data points when we first call fit_sphere(...) which prevents three numpy RuntimeWarnings (we were sometimes calling np.std on an empty list) --- .../knuckle_position_calibration_action_server.py | 1 + 1 file changed, 1 insertion(+) diff --git a/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py b/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py index e84ede5..40d1042 100755 --- a/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py +++ b/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py @@ -431,6 +431,7 @@ 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(): From aa514217c279bf2fcbaf126e2866dbfd6001a18a Mon Sep 17 00:00:00 2001 From: carebare47 Date: Tue, 13 Jun 2023 16:27:18 +0000 Subject: [PATCH 03/22] add 'divisor' to tf callback, lets us only save every 'x' tf messages. Speeds up computation but potentially reduces accuracy. Needs testing --- ...ckle_position_calibration_action_server.py | 29 +++++++++++-------- 1 file changed, 17 insertions(+), 12 deletions(-) diff --git a/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py b/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py index 40d1042..0cc9589 100755 --- a/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py +++ b/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py @@ -143,6 +143,9 @@ def __init__(self, side: str = "right"): # How many times during calibration should we calculate the sphere fit and update quality % self._number_of_checkpoints = 4 self._progress_period = 1.0 / self._number_of_checkpoints + # Only save every 'x'th glove data point for calibration - trade-off between accuracy and computation time + self._tf_data_divisor = 5 + self._tf_data_counter = 0 self._action_server.start() def _update_current_knuckle_tf(self, hand: Hand): @@ -395,18 +398,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): """ From a6da3498d658427067264a58961a4644b13fc250 Mon Sep 17 00:00:00 2001 From: carebare47 Date: Tue, 13 Jun 2023 16:29:48 +0000 Subject: [PATCH 04/22] save results from previous sphere_fit and use as the initial guess for the next sphere_fit. Speeds up computation time --- .../knuckle_position_calibration_action_server.py | 15 +++++++++++++-- 1 file changed, 13 insertions(+), 2 deletions(-) diff --git a/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py b/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py index 0cc9589..aea2606 100755 --- a/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py +++ b/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py @@ -97,7 +97,8 @@ def __init__(self, _hand_prefix: str): self.finger_data = {} self.current_knuckle_tf = TransformStamped() self.pub = rospy.Publisher(f"/data_point_marker_{self.hand_prefix}", Marker, queue_size=1000) - + self.last_center_estimate = None + self.last_radius_estimate = None class SrGloveCalibration: SOURCE_TO_KNUCKLE_LIMITS = [[-0.1, -0.1, -0.1], [0.1, 0.1, 0.1]] @@ -495,13 +496,23 @@ 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 'numpy' in str(type(hand.last_center_estimate)) and 'numpy' in str(type(hand.last_radius_estimate)): + initial_guess = [] + initial_guess.extend(hand.last_center_estimate) + initial_guess.append(hand.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.last_center_estimate = center + hand.last_radius_estimate = radius + hand.finger_data[finger]['residual'] = residual hand.finger_data[finger]['length'].append(np.around(radius, 4)) From dec9b137e1e3cb1d9eca5756884927f6de5a611f Mon Sep 17 00:00:00 2001 From: carebare47 Date: Tue, 13 Jun 2023 16:43:58 +0000 Subject: [PATCH 05/22] lint --- .../knuckle_position_calibration_action_server.py | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py b/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py index aea2606..cac9e67 100755 --- a/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py +++ b/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py @@ -100,6 +100,7 @@ def __init__(self, _hand_prefix: str): self.last_center_estimate = None self.last_radius_estimate = None + 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] @@ -447,8 +448,10 @@ def _calibration(self, goal: CalibrateGoal): break _feedback.progress = ((rospy.Time.now().to_sec() - start)) / goal.time - if ((_feedback.progress - current_progress) > self._progress_period and - math.floor(_feedback.progress * 100) != 0): + if ( + (_feedback.progress - current_progress) > self._progress_period and + math.floor(_feedback.progress * 100) != 0 + ): self._get_knuckle_positions(hand) _feedback.quality = self.get_calibration_quality(hand) current_progress += self._progress_period From 814c95c124335d91b8e1af42b7e0d69a2d433f49 Mon Sep 17 00:00:00 2001 From: carebare47 Date: Tue, 13 Jun 2023 16:49:51 +0000 Subject: [PATCH 06/22] tidying --- .../knuckle_position_calibration_action_server.py | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py b/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py index cac9e67..6fab583 100755 --- a/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py +++ b/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py @@ -448,10 +448,7 @@ def _calibration(self, goal: CalibrateGoal): break _feedback.progress = ((rospy.Time.now().to_sec() - start)) / goal.time - if ( - (_feedback.progress - current_progress) > self._progress_period 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 From e024ce5e698dc34c45afaf6b52d7b393f645db19 Mon Sep 17 00:00:00 2001 From: carebare47 Date: Tue, 13 Jun 2023 16:53:59 +0000 Subject: [PATCH 07/22] removed unused import --- .../knuckle_position_calibration_action_server.py | 1 - 1 file changed, 1 deletion(-) diff --git a/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py b/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py index 6fab583..43d3265 100755 --- a/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py +++ b/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 From 5ed88bf809b0c36d4ae35b513c609dd97ca2a4b3 Mon Sep 17 00:00:00 2001 From: carebare47 Date: Wed, 14 Jun 2023 09:31:07 +0000 Subject: [PATCH 08/22] add data divisor as input parameter (also raise exception on not-an-int) --- launch/start.launch | 1 + .../knuckle_position_calibration_action_server.py | 13 ++++++++----- 2 files changed, 9 insertions(+), 5 deletions(-) diff --git a/launch/start.launch b/launch/start.launch index 1a639aa..c37fdcc 100644 --- a/launch/start.launch +++ b/launch/start.launch @@ -31,6 +31,7 @@ + $(arg boresight_calibration_file) diff --git a/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py b/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py index 43d3265..873ac3b 100755 --- a/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py +++ b/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py @@ -103,8 +103,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 @@ -142,10 +143,11 @@ def __init__(self, side: str = "right"): 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._number_of_checkpoints = 4 - self._progress_period = 1.0 / self._number_of_checkpoints + 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 = 5 + self._tf_data_divisor = tf_data_divisor self._tf_data_counter = 0 self._action_server.start() @@ -538,4 +540,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) From 17d5fd423924a71426c9591d8cc8542431de858c Mon Sep 17 00:00:00 2001 From: carebare47 Date: Wed, 14 Jun 2023 09:33:44 +0000 Subject: [PATCH 09/22] add data divisor as input parameter (also raise exception on not-an-int) --- .../knuckle_position_calibration_action_server.py | 1 + 1 file changed, 1 insertion(+) diff --git a/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py b/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py index 873ac3b..50a4100 100755 --- a/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py +++ b/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py @@ -146,6 +146,7 @@ def __init__(self, side: str = "right", tf_data_divisor: int = 5): 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 From 717d929cb1560ac162e6a72f4b585eca81152bff Mon Sep 17 00:00:00 2001 From: carebare47 Date: Wed, 14 Jun 2023 10:30:52 +0000 Subject: [PATCH 10/22] fix initial guess storage (was per hand, now per finger, now significantly faster), and fix parameter from launch file --- launch/start.launch | 3 ++- .../knuckle_position_calibration_action_server.py | 15 ++++++++------- 2 files changed, 10 insertions(+), 8 deletions(-) diff --git a/launch/start.launch b/launch/start.launch index c37fdcc..67f996f 100644 --- a/launch/start.launch +++ b/launch/start.launch @@ -31,7 +31,7 @@ - + $(arg boresight_calibration_file) @@ -46,6 +46,7 @@ + diff --git a/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py b/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py index 50a4100..311915f 100755 --- a/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py +++ b/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py @@ -96,8 +96,6 @@ def __init__(self, _hand_prefix: str): self.finger_data = {} self.current_knuckle_tf = TransformStamped() self.pub = rospy.Publisher(f"/data_point_marker_{self.hand_prefix}", Marker, queue_size=1000) - self.last_center_estimate = None - self.last_radius_estimate = None class SrGloveCalibration: @@ -346,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): @@ -499,10 +499,11 @@ def _get_knuckle_positions(self, hand: Hand, plot=False): sphere_fit = SphereFit(data=hand.finger_data[finger]['data'], plot=plot) initial_guess = None - if 'numpy' in str(type(hand.last_center_estimate)) and 'numpy' in str(type(hand.last_radius_estimate)): + 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.last_center_estimate) - initial_guess.append(hand.last_radius_estimate) + 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], @@ -512,8 +513,8 @@ def _get_knuckle_positions(self, hand: Hand, plot=False): if plot: sphere_fit.plot_data() - hand.last_center_estimate = center - hand.last_radius_estimate = radius + # 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)) From 3b99a89af94100e2b317f543b1823dd4fea32e78 Mon Sep 17 00:00:00 2001 From: carebare47 Date: Wed, 14 Jun 2023 10:39:32 +0000 Subject: [PATCH 11/22] actually apply the fix I thought I'd apply in the previous commit --- .../knuckle_position_calibration_action_server.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py b/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py index 311915f..ef0737d 100755 --- a/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py +++ b/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py @@ -500,7 +500,7 @@ def _get_knuckle_positions(self, hand: Hand, plot=False): 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): + 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']) @@ -513,8 +513,8 @@ def _get_knuckle_positions(self, hand: Hand, plot=False): 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]['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)) From 47c845d1e6e16c013370a75ec3f8781e19714221 Mon Sep 17 00:00:00 2001 From: carebare47 Date: Wed, 14 Jun 2023 16:47:05 +0000 Subject: [PATCH 12/22] turns out '_load_tf_callback' was missing ~80% of messages due to it's execution time being too long (the number of msgs recieved was system-dependant and made it impossible to compare accuracy of calibration fits between runs). Modified code to save all tf messsages and process them in get_knuckle_positions. Added code to load a bag file for testing/tuning/comparing accuracy of calibration fits. Now that _load_tf_callback is recieving all the messages, the average number of msgs per second has gone up from ~15 to ~100, drastically increasing the computation of sphere fit. But, now we can add code to downsample to a KNOWN number of 'datapoints per second', giving the same performance across systems and lettings us actually figure out how many datapoints per second we need for a reasonable calibration fit --- launch/start.launch | 2 - ...ckle_position_calibration_action_server.py | 75 ++++++++++++++----- 2 files changed, 57 insertions(+), 20 deletions(-) diff --git a/launch/start.launch b/launch/start.launch index 67f996f..1a639aa 100644 --- a/launch/start.launch +++ b/launch/start.launch @@ -31,7 +31,6 @@ - $(arg boresight_calibration_file) @@ -46,7 +45,6 @@ - diff --git a/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py b/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py index ef0737d..b6cd437 100755 --- a/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py +++ b/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py @@ -40,6 +40,7 @@ CalibrateResult, CalibrateGoal) from polhemus_ros_driver.srv import Publish, PublishRequest from polhemus_ros_driver.sphere_fit import SphereFit +import rosbag def calculate_distance(point1: Point, point2: Point): @@ -103,7 +104,7 @@ class SrGloveCalibration: FINGER_LENGTH_LIMITS = [0.03, 0.15] NUMBER_OF_CHECKPOINTS = 4 - def __init__(self, side: str = "right", tf_data_divisor: int = 5): + def __init__(self, side: str = "right", testing_bag_file_path: str = ''): """ Initializes the calibration action server and the interactive marker server. @param side: The hand(s) to calibrate - can be left, right or both @@ -142,12 +143,10 @@ def __init__(self, side: str = "right", tf_data_divisor: int = 5): 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._saved_tf_msgs = [] + self._testing_bag_file_path = testing_bag_file_path + self._bag_msgs_generator = None self._action_server.start() def _update_current_knuckle_tf(self, hand: Hand): @@ -402,7 +401,12 @@ def _create_control(quaternion: Quaternion, name: str): def _load_tf_callback(self, data): """ Callback for received TF data. """ - if self._tf_data_counter % self._tf_data_divisor == 0: + if 'polhemus_station_0' in data.transforms[0].child_frame_id: + self._saved_tf_msgs.append(data) + return + + def _process_tf_data(self): + for data in self._saved_tf_msgs: for individual_transform in data.transforms: for hand in self._hands.values(): for color_index, finger in enumerate(fingers): @@ -412,10 +416,24 @@ def _load_tf_callback(self, data): 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 + # @TODO: Re-enable this under certain conditions + # Maybe a checkbox in GUI that gets auto-ticked when ... + # 'open rviz calibration display' is clicked) + # If always enabled it takes the execution time of this method from ~25ms to ~750ms + if False: + data_point_marker = DataMarker(hand.polhemus_base_name, Point(*pos), + COLORS[color_index].value) + hand.pub.publish(data_point_marker) + + def _unpack_bag_to_tf_callback(self, event): + try: + topic_msg_t = next(self._bag_msgs_generator) + msg = topic_msg_t[1] + data = TFMessage() + data.transforms.extend(msg.transforms) + self._load_tf_callback(data) + except StopIteration: + pass def _calibration(self, goal: CalibrateGoal): """ @@ -434,13 +452,24 @@ def _calibration(self, goal: CalibrateGoal): self._reset_data(hand) self._remove_all_markers(hand) rospy.loginfo("Starting calibration..") + # Clear saved tf messages from previous calibration + self._saved_tf_msgs = [] start = rospy.Time.now().to_sec() _feedback = CalibrateFeedback() _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 + if self._testing_bag_file_path == '': + sub = rospy.Subscriber("/tf", TFMessage, self._load_tf_callback, queue_size=100) + rospy.sleep(0.5) # Ensure the tf subscriber has had time to start receiving messages + else: + self._bag_msgs_generator = (topic_msg_t for topic_msg_t in + rosbag.Bag(self._testing_bag_file_path).read_messages() + if topic_msg_t[0] == "/tf" + and topic_msg_t[1].transforms + and 'polhemus' in topic_msg_t[1].transforms[0].child_frame_id) + timer = rospy.Timer(rospy.Duration(0.001), self._unpack_bag_to_tf_callback) + current_progress = 0.0 while rospy.Time.now().to_sec() - start < goal.time: if self._action_server.is_preempt_requested(): @@ -448,7 +477,9 @@ def _calibration(self, goal: CalibrateGoal): self._action_server.set_preempted() _result.success = False break - + # Without this sleep, when testing with bag file, this loop consumes.. + # all the CPU assigned to this process and _unpack_bag_to_tf_callback never (rarely) gets called + rospy.sleep(0.001) _feedback.progress = ((rospy.Time.now().to_sec() - start)) / goal.time if (_feedback.progress - current_progress) > self._progress_period and _feedback.progress < 1.0: self._get_knuckle_positions(hand) @@ -456,7 +487,11 @@ def _calibration(self, goal: CalibrateGoal): current_progress += self._progress_period self._action_server.publish_feedback(_feedback) - sub.unregister() + if self._testing_bag_file_path == '': + sub.unregister() + else: + timer.shutdown() + self._get_knuckle_positions(hand) _feedback.quality = self.get_calibration_quality(hand) if not self._action_server.is_preempt_requested(): @@ -492,6 +527,7 @@ def _get_knuckle_positions(self, hand: Hand, plot=False): @param hand: Selected hand @param plot: If True, plots the data points and the fitted sphere """ + self._process_tf_data() for color_index, finger in enumerate(fingers): solution_marker = DataMarker(hand.polhemus_base_name, hand.finger_data[finger]['center'].pose.position, COLORS[color_index].value) @@ -521,7 +557,7 @@ def _get_knuckle_positions(self, hand: Hand, plot=False): center = np.around(center, 3) pose = Pose() - pose.position = Point(center[0], center[1], center[2]) + pose.position = Point(center[0], center[1], center[2]) pose.orientation = Quaternion(0, 0, 0, 1) self._marker_server.setPose(hand.finger_data[finger]['center'].name, pose) self._marker_server.applyChanges() @@ -535,6 +571,8 @@ def get_calibration_quality(hand: Hand): """ quality_list: List[float] = [] for finger in fingers: + if len(hand.finger_data[finger]['residual']) == 0: + print('no data recieved') quality_list.append(np.std(hand.finger_data[finger]['residual'])) return quality_list @@ -542,5 +580,6 @@ def get_calibration_quality(hand: Hand): if __name__ == "__main__": rospy.init_node('sr_knuckle_calibration') hand_side = rospy.get_param("~side", "both") - calibration_data_divisor = int(rospy.get_param("~calibration_data_divisor", 5)) - sr_glove_calibration = SrGloveCalibration(side=hand_side, tf_data_divisor=calibration_data_divisor) + testing_bag_path = '/home/user/bags/hugo/2022-11-01-13-50-09.orig.bag' + sr_glove_calibration = SrGloveCalibration(side=hand_side, + testing_bag_file_path=testing_bag_path) \ No newline at end of file From 2a8e44e4d27b1564313427158af90b2f629dc65f Mon Sep 17 00:00:00 2001 From: carebare47 Date: Thu, 15 Jun 2023 16:50:50 +0000 Subject: [PATCH 13/22] now saves all tf messages and processes outside the callback, downsampling to approximately a 'known' number of samples-per-second. Switched to MarkerArrays for faster marker publishing. Also general tidying --- launch/start.launch | 2 + ...ckle_position_calibration_action_server.py | 131 ++++++++++++------ 2 files changed, 92 insertions(+), 41 deletions(-) diff --git a/launch/start.launch b/launch/start.launch index 1a639aa..dd18774 100644 --- a/launch/start.launch +++ b/launch/start.launch @@ -31,6 +31,7 @@ + $(arg boresight_calibration_file) @@ -45,6 +46,7 @@ + diff --git a/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py b/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py index b6cd437..64c90c6 100755 --- a/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py +++ b/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py @@ -33,8 +33,9 @@ from std_msgs.msg import ColorRGBA from tf2_msgs.msg import TFMessage from tf2_ros import StaticTransformBroadcaster +from std_msgs.msg import Header from visualization_msgs.msg import (InteractiveMarker, - InteractiveMarkerControl, Marker) + InteractiveMarkerControl, Marker, MarkerArray) from polhemus_ros_driver.msg import (CalibrateAction, CalibrateFeedback, CalibrateResult, CalibrateGoal) @@ -73,10 +74,10 @@ class DataMarker(Marker): """ Class to store data about a marker to be published to rviz. """ _id = 0 - def __init__(self, frame_id: str, point: Point, color: ColorRGBA, size=0.002): + def __init__(self, frame_id: str, time_stamp: rospy.Time, point: Point, color: ColorRGBA, size=0.002): super().__init__() self.header.frame_id = frame_id - self.header.stamp = rospy.Time.now() + self.header.stamp = time_stamp self.type = self.POINTS # Markers need to have unique ids. With the line below it's ensured that # every new instance has a unique, incremental id @@ -96,7 +97,29 @@ def __init__(self, _hand_prefix: str): self.polhemus_base_name = f"polhemus_base_{self.polhemus_base_index}" self.finger_data = {} self.current_knuckle_tf = TransformStamped() - self.pub = rospy.Publisher(f"/data_point_marker_{self.hand_prefix}", Marker, queue_size=1000) + self._marker_array = MarkerArray() + self._pub = rospy.Publisher(f"/data_point_marker_{self.hand_prefix}", MarkerArray, queue_size=1000) + + def add_marker(self, frame_id: str, time_stamp: rospy.Time, point: Point, color: ColorRGBA): + data_point_marker = DataMarker(frame_id=frame_id, time_stamp=time_stamp, point=point, color=color) + self._marker_array.markers.append(data_point_marker) + + def get_number_of_markers(self): + return self._marker_array.markers.__len__() + + def publish_markers(self): + self._pub.publish(self._marker_array) + # Empty the marker array after publishing + self._marker_array = MarkerArray() + + def remove_published_markers(self): + self._marker_array = MarkerArray() + marker = Marker() + marker.header.frame_id = self.polhemus_base_name + marker.action = marker.DELETEALL + self._marker_array.markers.append(marker) + self._pub.publish(self._marker_array) + self._marker_array = MarkerArray() class SrGloveCalibration: @@ -104,10 +127,18 @@ class SrGloveCalibration: FINGER_LENGTH_LIMITS = [0.03, 0.15] NUMBER_OF_CHECKPOINTS = 4 - def __init__(self, side: str = "right", testing_bag_file_path: str = ''): + def __init__(self, side: str = "right", + desired_datapoints_per_sec: int = 20, + testing_bag_file_path: str = ''): """ Initializes the calibration action server and the interactive marker server. @param side: The hand(s) to calibrate - can be left, right or both + @param desired_datapoints_per_sec: The desired number of tf datapoints to save per second. Fewer datapoints + will result in a faster calibration process but + potentially a less accurate result + @param testing_bag_file_path: Passing a bag file path will cause this class to use data + from the bag file instead of live data + (useful for figuring out the ideal desired_datapoints_per_sec) """ # Interactive marker server for visualizing and interacting with the calibration process self._marker_server = InteractiveMarkerServer("knuckle_position_markers") @@ -143,10 +174,11 @@ def __init__(self, side: str = "right", testing_bag_file_path: str = ''): 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 - + # To store tf messages so we can process them outside the callback self._saved_tf_msgs = [] self._testing_bag_file_path = testing_bag_file_path self._bag_msgs_generator = None + self._desired_datapoints_per_sec = desired_datapoints_per_sec self._action_server.start() def _update_current_knuckle_tf(self, hand: Hand): @@ -399,14 +431,25 @@ def _create_control(quaternion: Quaternion, name: str): control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS return control - def _load_tf_callback(self, data): - """ Callback for received TF data. """ - if 'polhemus_station_0' in data.transforms[0].child_frame_id: + def _tf_callback(self, data): + """ + Callback for received TF data. + @param data: A TFMessage message from either the /tf topic or a bag + """ + if 'polhemus' in data.transforms[0].child_frame_id: self._saved_tf_msgs.append(data) return - def _process_tf_data(self): - for data in self._saved_tf_msgs: + def _process_tf_data(self, time_elapsed): + """ + Processes a subsample of the saved TF data and publishes markers of this subsample + @param time_elapsed: Time elapsed since the start of this calibration + """ + # Subsample the saved_tf_msgs (temporally) to approximately the desired number of datapoints per second + num_msgs = len(self._saved_tf_msgs) + desired_num_msgs = time_elapsed * self._desired_datapoints_per_sec + subsampled_tf_msgs = self._saved_tf_msgs[::int(round(num_msgs / desired_num_msgs))] + for data in subsampled_tf_msgs: for individual_transform in data.transforms: for hand in self._hands.values(): for color_index, finger in enumerate(fingers): @@ -416,22 +459,23 @@ def _process_tf_data(self): individual_transform.transform.translation.z] hand.finger_data[finger]['data'].append(pos) - # @TODO: Re-enable this under certain conditions - # Maybe a checkbox in GUI that gets auto-ticked when ... - # 'open rviz calibration display' is clicked) - # If always enabled it takes the execution time of this method from ~25ms to ~750ms - if False: - data_point_marker = DataMarker(hand.polhemus_base_name, Point(*pos), - COLORS[color_index].value) - hand.pub.publish(data_point_marker) - - def _unpack_bag_to_tf_callback(self, event): + hand.add_marker(frame_id=hand.polhemus_base_name, + time_stamp=individual_transform.header.stamp, + point=Point(*pos), + color=COLORS[color_index].value) + for hand in self._hands.values(): + hand.publish_markers() + + def _unpack_bag_msg_to_tf_callback(self, event): + """ + Grab the next message from the filtered generator and pass it to the tf_callback + """ try: topic_msg_t = next(self._bag_msgs_generator) msg = topic_msg_t[1] data = TFMessage() data.transforms.extend(msg.transforms) - self._load_tf_callback(data) + self._tf_callback(data) except StopIteration: pass @@ -460,15 +504,17 @@ def _calibration(self, goal: CalibrateGoal): _result = CalibrateResult() if self._testing_bag_file_path == '': - sub = rospy.Subscriber("/tf", TFMessage, self._load_tf_callback, queue_size=100) + # Use live /tf data (general use) + sub = rospy.Subscriber("/tf", TFMessage, self._tf_callback, queue_size=100) rospy.sleep(0.5) # Ensure the tf subscriber has had time to start receiving messages else: + # Use /tf data from a bag file (for testing) self._bag_msgs_generator = (topic_msg_t for topic_msg_t in rosbag.Bag(self._testing_bag_file_path).read_messages() if topic_msg_t[0] == "/tf" and topic_msg_t[1].transforms and 'polhemus' in topic_msg_t[1].transforms[0].child_frame_id) - timer = rospy.Timer(rospy.Duration(0.001), self._unpack_bag_to_tf_callback) + timer = rospy.Timer(rospy.Duration(0.001), self._unpack_bag_msg_to_tf_callback) current_progress = 0.0 while rospy.Time.now().to_sec() - start < goal.time: @@ -477,12 +523,13 @@ def _calibration(self, goal: CalibrateGoal): self._action_server.set_preempted() _result.success = False break - # Without this sleep, when testing with bag file, this loop consumes.. - # all the CPU assigned to this process and _unpack_bag_to_tf_callback never (rarely) gets called + # Without this sleep, when testing with bag file, this loop consumes.. + # all the CPU assigned to this process and _unpack_bag_msg_to_tf_callback never (rarely) gets called rospy.sleep(0.001) - _feedback.progress = ((rospy.Time.now().to_sec() - start)) / goal.time + time_elapsed = rospy.Time.now().to_sec() - start + _feedback.progress = time_elapsed / goal.time if (_feedback.progress - current_progress) > self._progress_period and _feedback.progress < 1.0: - self._get_knuckle_positions(hand) + self._get_knuckle_positions(hand, time_elapsed) _feedback.quality = self.get_calibration_quality(hand) current_progress += self._progress_period self._action_server.publish_feedback(_feedback) @@ -492,7 +539,7 @@ def _calibration(self, goal: CalibrateGoal): else: timer.shutdown() - self._get_knuckle_positions(hand) + self._get_knuckle_positions(hand, time_elapsed) _feedback.quality = self.get_calibration_quality(hand) if not self._action_server.is_preempt_requested(): _result.success = True @@ -516,22 +563,23 @@ def _remove_all_markers(hand: Hand): Removes markers previously displayed in Rviz @param hand: Selected hand """ - marker = Marker() - marker.header.frame_id = hand.polhemus_base_name - marker.action = marker.DELETEALL - hand.pub.publish(marker) + hand.remove_published_markers() - def _get_knuckle_positions(self, hand: Hand, plot=False): + def _get_knuckle_positions(self, hand: Hand, time_elapsed: float, plot=False): """ Updates the current solution for all fingers on the selected hand. @param hand: Selected hand + @param time_elapsed: Time elapsed since the start of this calibration @param plot: If True, plots the data points and the fitted sphere """ - self._process_tf_data() + self._process_tf_data(time_elapsed) for color_index, finger in enumerate(fingers): - solution_marker = DataMarker(hand.polhemus_base_name, hand.finger_data[finger]['center'].pose.position, - COLORS[color_index].value) - hand.pub.publish(solution_marker) + # Add and publish a solution marker + hand.add_marker(frame_id=hand.polhemus_base_name, + time_stamp=rospy.Time.now(), + point=hand.finger_data[finger]['center'].pose.position, + color=COLORS[color_index].value) + hand.publish_markers() sphere_fit = SphereFit(data=hand.finger_data[finger]['data'], plot=plot) initial_guess = None @@ -557,7 +605,7 @@ def _get_knuckle_positions(self, hand: Hand, plot=False): center = np.around(center, 3) pose = Pose() - pose.position = Point(center[0], center[1], center[2]) + pose.position = Point(center[0], center[1], center[2]) pose.orientation = Quaternion(0, 0, 0, 1) self._marker_server.setPose(hand.finger_data[finger]['center'].name, pose) self._marker_server.applyChanges() @@ -580,6 +628,7 @@ def get_calibration_quality(hand: Hand): if __name__ == "__main__": rospy.init_node('sr_knuckle_calibration') hand_side = rospy.get_param("~side", "both") - testing_bag_path = '/home/user/bags/hugo/2022-11-01-13-50-09.orig.bag' + desired_number_of_datapoints_per_sec = rospy.get_param("~desired_datapoints_per_sec", 20) sr_glove_calibration = SrGloveCalibration(side=hand_side, - testing_bag_file_path=testing_bag_path) \ No newline at end of file + desired_datapoints_per_sec=desired_number_of_datapoints_per_sec, + testing_bag_file_path='') From c8cb70620f955e19cb63d3f893aa0ca98f1b2c20 Mon Sep 17 00:00:00 2001 From: carebare47 Date: Thu, 15 Jun 2023 17:27:07 +0000 Subject: [PATCH 14/22] make sure is valid for large values of --- .../knuckle_position_calibration_action_server.py | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py b/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py index 64c90c6..2acb0ab 100755 --- a/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py +++ b/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py @@ -33,7 +33,6 @@ from std_msgs.msg import ColorRGBA from tf2_msgs.msg import TFMessage from tf2_ros import StaticTransformBroadcaster -from std_msgs.msg import Header from visualization_msgs.msg import (InteractiveMarker, InteractiveMarkerControl, Marker, MarkerArray) @@ -448,7 +447,7 @@ def _process_tf_data(self, time_elapsed): # Subsample the saved_tf_msgs (temporally) to approximately the desired number of datapoints per second num_msgs = len(self._saved_tf_msgs) desired_num_msgs = time_elapsed * self._desired_datapoints_per_sec - subsampled_tf_msgs = self._saved_tf_msgs[::int(round(num_msgs / desired_num_msgs))] + subsampled_tf_msgs = self._saved_tf_msgs[::int(round((num_msgs / desired_num_msgs)+0.5))] for data in subsampled_tf_msgs: for individual_transform in data.transforms: for hand in self._hands.values(): @@ -466,7 +465,7 @@ def _process_tf_data(self, time_elapsed): for hand in self._hands.values(): hand.publish_markers() - def _unpack_bag_msg_to_tf_callback(self, event): + def _unpack_bag_msg_to_tf_callback(self, _event=False): """ Grab the next message from the filtered generator and pass it to the tf_callback """ @@ -538,7 +537,6 @@ def _calibration(self, goal: CalibrateGoal): sub.unregister() else: timer.shutdown() - self._get_knuckle_positions(hand, time_elapsed) _feedback.quality = self.get_calibration_quality(hand) if not self._action_server.is_preempt_requested(): From fefb9b8accd6ee40615afd3625811a2f04a7fbd0 Mon Sep 17 00:00:00 2001 From: carebare47 Date: Thu, 15 Jun 2023 17:37:26 +0000 Subject: [PATCH 15/22] add comment to explain --- .../knuckle_position_calibration_action_server.py | 1 + 1 file changed, 1 insertion(+) diff --git a/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py b/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py index 2acb0ab..f3504a0 100755 --- a/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py +++ b/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py @@ -124,6 +124,7 @@ def remove_published_markers(self): 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] + # How many times throughout the calibration process should we calculate the current quality, publish markers etc.. NUMBER_OF_CHECKPOINTS = 4 def __init__(self, side: str = "right", From 595514327734a53e64e2e4da81fa5b06d4b6a1ed Mon Sep 17 00:00:00 2001 From: carebare47 Date: Fri, 16 Jun 2023 10:11:29 +0000 Subject: [PATCH 16/22] now set timer rate to desired_num_datapoints_per_sec for testing mode with a bag --- .../knuckle_position_calibration_action_server.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py b/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py index f3504a0..4ea6cff 100755 --- a/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py +++ b/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py @@ -514,7 +514,8 @@ def _calibration(self, goal: CalibrateGoal): if topic_msg_t[0] == "/tf" and topic_msg_t[1].transforms and 'polhemus' in topic_msg_t[1].transforms[0].child_frame_id) - timer = rospy.Timer(rospy.Duration(0.001), self._unpack_bag_msg_to_tf_callback) + timer = rospy.Timer(rospy.Duration(1/self._desired_datapoints_per_sec), + self._unpack_bag_msg_to_tf_callback) current_progress = 0.0 while rospy.Time.now().to_sec() - start < goal.time: From 69921100664d85bcc6d71af89627b598cbd4f5ae Mon Sep 17 00:00:00 2001 From: carebare47 Date: Mon, 26 Jun 2023 15:05:29 +0000 Subject: [PATCH 17/22] update default num datapoints per sec to 60 --- launch/start.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/launch/start.launch b/launch/start.launch index dd18774..ef45865 100644 --- a/launch/start.launch +++ b/launch/start.launch @@ -31,7 +31,7 @@ - + $(arg boresight_calibration_file) From daf4b72132a79fe28b5a6b92d5b455fd5be842a8 Mon Sep 17 00:00:00 2001 From: carebare47 Date: Mon, 26 Jun 2023 16:37:12 +0000 Subject: [PATCH 18/22] fix difference between bags and live tf messages, address PR comments --- ...ckle_position_calibration_action_server.py | 77 +++++++++++-------- 1 file changed, 46 insertions(+), 31 deletions(-) diff --git a/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py b/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py index 4ea6cff..37ba1a7 100755 --- a/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py +++ b/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py @@ -19,7 +19,7 @@ import os from enum import Enum -from typing import List, Dict +from typing import List, Dict, Optional import actionlib from dynamic_reconfigure import client, DynamicReconfigureCallbackException, DynamicReconfigureParameterException import numpy as np @@ -41,7 +41,7 @@ from polhemus_ros_driver.srv import Publish, PublishRequest from polhemus_ros_driver.sphere_fit import SphereFit import rosbag - +import time def calculate_distance(point1: Point, point2: Point): """ @@ -73,7 +73,7 @@ class DataMarker(Marker): """ Class to store data about a marker to be published to rviz. """ _id = 0 - def __init__(self, frame_id: str, time_stamp: rospy.Time, point: Point, color: ColorRGBA, size=0.002): + def __init__(self, frame_id: str, time_stamp: rospy.Time, point: Point, color: ColorRGBA, size=0.002) -> None: super().__init__() self.header.frame_id = frame_id self.header.stamp = time_stamp @@ -99,19 +99,19 @@ def __init__(self, _hand_prefix: str): self._marker_array = MarkerArray() self._pub = rospy.Publisher(f"/data_point_marker_{self.hand_prefix}", MarkerArray, queue_size=1000) - def add_marker(self, frame_id: str, time_stamp: rospy.Time, point: Point, color: ColorRGBA): + def add_marker(self, frame_id: str, time_stamp: rospy.Time, point: Point, color: ColorRGBA) -> None: data_point_marker = DataMarker(frame_id=frame_id, time_stamp=time_stamp, point=point, color=color) self._marker_array.markers.append(data_point_marker) - def get_number_of_markers(self): - return self._marker_array.markers.__len__() + def get_number_of_markers(self) -> int: + return len(self._marker_array.markers) - def publish_markers(self): + def publish_markers(self) -> None: self._pub.publish(self._marker_array) # Empty the marker array after publishing self._marker_array = MarkerArray() - def remove_published_markers(self): + def remove_published_markers(self) -> None: self._marker_array = MarkerArray() marker = Marker() marker.header.frame_id = self.polhemus_base_name @@ -129,7 +129,7 @@ class SrGloveCalibration: def __init__(self, side: str = "right", desired_datapoints_per_sec: int = 20, - testing_bag_file_path: str = ''): + testing_bag_file_path: Optional[str] = None) -> None: """ Initializes the calibration action server and the interactive marker server. @param side: The hand(s) to calibrate - can be left, right or both @@ -177,11 +177,16 @@ def __init__(self, side: str = "right", # To store tf messages so we can process them outside the callback self._saved_tf_msgs = [] self._testing_bag_file_path = testing_bag_file_path + if self._testing_bag_file_path is not None: + if not os.path.isfile(self._testing_bag_file_path): + rospy.logerr(f"Specified bag file {self._testing_bag_file_path} does not exist, exiting...") + exit(0) + self._glove_msg_period = self._get_average_bag_msg_period() self._bag_msgs_generator = None self._desired_datapoints_per_sec = desired_datapoints_per_sec self._action_server.start() - def _update_current_knuckle_tf(self, hand: Hand): + def _update_current_knuckle_tf(self, hand: Hand) -> None: """ Updates the current knuckle TF for a hand @param hand: The hand to update the TF for """ @@ -431,7 +436,7 @@ def _create_control(quaternion: Quaternion, name: str): control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS return control - def _tf_callback(self, data): + def _tf_callback(self, data) -> None: """ Callback for received TF data. @param data: A TFMessage message from either the /tf topic or a bag @@ -440,44 +445,54 @@ def _tf_callback(self, data): self._saved_tf_msgs.append(data) return - def _process_tf_data(self, time_elapsed): + def _process_tf_data(self, time_elapsed) -> None: """ Processes a subsample of the saved TF data and publishes markers of this subsample - @param time_elapsed: Time elapsed since the start of this calibration + @param time_elapsed: Time elapsed since the start of this calibration (seconds) """ # Subsample the saved_tf_msgs (temporally) to approximately the desired number of datapoints per second num_msgs = len(self._saved_tf_msgs) desired_num_msgs = time_elapsed * self._desired_datapoints_per_sec - subsampled_tf_msgs = self._saved_tf_msgs[::int(round((num_msgs / desired_num_msgs)+0.5))] + subsample_stride = int(round((num_msgs / desired_num_msgs)+0.51)) # Ensure that this is always '1' or more + subsampled_tf_msgs = self._saved_tf_msgs[::subsample_stride] for data in subsampled_tf_msgs: 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] + position = [individual_transform.transform.translation.x, + individual_transform.transform.translation.y, + individual_transform.transform.translation.z] - hand.finger_data[finger]['data'].append(pos) + hand.finger_data[finger]['data'].append(position) hand.add_marker(frame_id=hand.polhemus_base_name, time_stamp=individual_transform.header.stamp, - point=Point(*pos), + point=Point(*position), color=COLORS[color_index].value) for hand in self._hands.values(): hand.publish_markers() - def _unpack_bag_msg_to_tf_callback(self, _event=False): + def _unpack_bag_msg_to_tf_callback(self, _event=False) -> None: """ Grab the next message from the filtered generator and pass it to the tf_callback """ try: - topic_msg_t = next(self._bag_msgs_generator) - msg = topic_msg_t[1] + topic_msg_time = next(self._bag_msgs_generator) + msg = topic_msg_time[1] data = TFMessage() data.transforms.extend(msg.transforms) self._tf_callback(data) except StopIteration: pass + + def _get_average_bag_msg_period(self): + bag = rosbag.Bag(self._testing_bag_file_path) + polhemus_msgs = sorted((tm for m in bag if m.topic == '/tf' for tm in m.message.transforms), + key=lambda tfm: tfm.header.stamp.to_nsec()) + polhemus_msgs = filter(lambda m: m.header.frame_id == 'polhemus_base_0', polhemus_msgs) + polhemus_msgs = filter(lambda m: m.child_frame_id == 'polhemus_station_1', polhemus_msgs) + times = np.array([t.header.stamp for t in polhemus_msgs]) + return (times[1:] - times[:-1]).mean().to_sec() def _calibration(self, goal: CalibrateGoal): """ @@ -496,6 +511,7 @@ def _calibration(self, goal: CalibrateGoal): self._reset_data(hand) self._remove_all_markers(hand) rospy.loginfo("Starting calibration..") + # Clear saved tf messages from previous calibration self._saved_tf_msgs = [] @@ -503,7 +519,7 @@ def _calibration(self, goal: CalibrateGoal): _feedback = CalibrateFeedback() _result = CalibrateResult() - if self._testing_bag_file_path == '': + if self._testing_bag_file_path is None: # Use live /tf data (general use) sub = rospy.Subscriber("/tf", TFMessage, self._tf_callback, queue_size=100) rospy.sleep(0.5) # Ensure the tf subscriber has had time to start receiving messages @@ -514,7 +530,7 @@ def _calibration(self, goal: CalibrateGoal): if topic_msg_t[0] == "/tf" and topic_msg_t[1].transforms and 'polhemus' in topic_msg_t[1].transforms[0].child_frame_id) - timer = rospy.Timer(rospy.Duration(1/self._desired_datapoints_per_sec), + timer = rospy.Timer(rospy.Duration(self._glove_msg_period), self._unpack_bag_msg_to_tf_callback) current_progress = 0.0 @@ -530,16 +546,16 @@ def _calibration(self, goal: CalibrateGoal): time_elapsed = rospy.Time.now().to_sec() - start _feedback.progress = time_elapsed / goal.time if (_feedback.progress - current_progress) > self._progress_period and _feedback.progress < 1.0: - self._get_knuckle_positions(hand, time_elapsed) + self._compute_knuckle_positions(hand, time_elapsed) _feedback.quality = self.get_calibration_quality(hand) current_progress += self._progress_period self._action_server.publish_feedback(_feedback) - if self._testing_bag_file_path == '': + if self._testing_bag_file_path is None: sub.unregister() else: timer.shutdown() - self._get_knuckle_positions(hand, time_elapsed) + self._compute_knuckle_positions(hand, time_elapsed) _feedback.quality = self.get_calibration_quality(hand) if not self._action_server.is_preempt_requested(): _result.success = True @@ -565,7 +581,7 @@ def _remove_all_markers(hand: Hand): """ hand.remove_published_markers() - def _get_knuckle_positions(self, hand: Hand, time_elapsed: float, plot=False): + def _compute_knuckle_positions(self, hand: Hand, time_elapsed: float, plot=False): """ Updates the current solution for all fingers on the selected hand. @param hand: Selected hand @@ -620,7 +636,7 @@ def get_calibration_quality(hand: Hand): quality_list: List[float] = [] for finger in fingers: if len(hand.finger_data[finger]['residual']) == 0: - print('no data recieved') + rospy.logwarn(f'No data received for finger {finger}') quality_list.append(np.std(hand.finger_data[finger]['residual'])) return quality_list @@ -630,5 +646,4 @@ def get_calibration_quality(hand: Hand): hand_side = rospy.get_param("~side", "both") desired_number_of_datapoints_per_sec = rospy.get_param("~desired_datapoints_per_sec", 20) sr_glove_calibration = SrGloveCalibration(side=hand_side, - desired_datapoints_per_sec=desired_number_of_datapoints_per_sec, - testing_bag_file_path='') + desired_datapoints_per_sec=desired_number_of_datapoints_per_sec) From 368f107e3eb6854d6c8a6b1632d86ff350c849d4 Mon Sep 17 00:00:00 2001 From: carebare47 Date: Mon, 26 Jun 2023 16:51:01 +0000 Subject: [PATCH 19/22] oops --- .../knuckle_position_calibration_action_server.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py b/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py index 37ba1a7..ccb255d 100755 --- a/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py +++ b/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py @@ -637,7 +637,7 @@ def get_calibration_quality(hand: Hand): for finger in fingers: if len(hand.finger_data[finger]['residual']) == 0: rospy.logwarn(f'No data received for finger {finger}') - quality_list.append(np.std(hand.finger_data[finger]['residual'])) + quality_list.append(np.std(hand.finger_data[finger]['residual'])) return quality_list From c575a51fed623e290c729119a92516181630ab14 Mon Sep 17 00:00:00 2001 From: carebare47 Date: Mon, 26 Jun 2023 16:54:43 +0000 Subject: [PATCH 20/22] lint --- .../knuckle_position_calibration_action_server.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py b/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py index ccb255d..9796acf 100755 --- a/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py +++ b/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py @@ -41,7 +41,7 @@ from polhemus_ros_driver.srv import Publish, PublishRequest from polhemus_ros_driver.sphere_fit import SphereFit import rosbag -import time + def calculate_distance(point1: Point, point2: Point): """ @@ -484,7 +484,7 @@ def _unpack_bag_msg_to_tf_callback(self, _event=False) -> None: self._tf_callback(data) except StopIteration: pass - + def _get_average_bag_msg_period(self): bag = rosbag.Bag(self._testing_bag_file_path) polhemus_msgs = sorted((tm for m in bag if m.topic == '/tf' for tm in m.message.transforms), @@ -511,7 +511,7 @@ def _calibration(self, goal: CalibrateGoal): self._reset_data(hand) self._remove_all_markers(hand) rospy.loginfo("Starting calibration..") - + # Clear saved tf messages from previous calibration self._saved_tf_msgs = [] From 385df1a8e4e3223d2a16e9410964e56e4f87591e Mon Sep 17 00:00:00 2001 From: carebare47 Date: Thu, 31 Aug 2023 11:42:56 +0000 Subject: [PATCH 21/22] fixing cal score calc --- .../knuckle_position_calibration_action_server.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py b/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py index 14ac373..a521e23 100755 --- a/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py +++ b/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py @@ -638,7 +638,7 @@ def get_calibration_quality(hand: Hand): for finger in fingers: if len(hand.finger_data[finger]['residual']) == 0: rospy.logwarn(f'No data received for finger {finger}') - quality_list.append(np.std(hand.finger_data[finger]['residual'])) + quality_list.append(np.std(hand.finger_data[finger]['residual'])) return quality_list From 60b5fcf7e9850faf41ab415108a18be509de278c Mon Sep 17 00:00:00 2001 From: carebare47 Date: Thu, 31 Aug 2023 11:46:54 +0000 Subject: [PATCH 22/22] lint --- .../knuckle_position_calibration_action_server.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py b/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py index a521e23..c3ff556 100755 --- a/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py +++ b/src/polhemus_ros_driver/knuckle_position_calibration_action_server.py @@ -17,15 +17,16 @@ # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA import os +import sys from enum import Enum - from typing import List, Dict, Optional import actionlib -from dynamic_reconfigure import client, DynamicReconfigureCallbackException, DynamicReconfigureParameterException import numpy as np import rospkg import rospy +import rosbag import yaml +from dynamic_reconfigure import client, DynamicReconfigureCallbackException, DynamicReconfigureParameterException from geometry_msgs.msg import (Point, Pose, Quaternion, TransformStamped, Vector3) from interactive_markers.interactive_marker_server import \ @@ -40,7 +41,6 @@ CalibrateResult, CalibrateGoal) from polhemus_ros_driver.srv import Publish, PublishRequest from polhemus_ros_driver.sphere_fit import SphereFit -import rosbag def calculate_distance(point1: Point, point2: Point): @@ -180,7 +180,7 @@ def __init__(self, side: str = "right", if self._testing_bag_file_path is not None: if not os.path.isfile(self._testing_bag_file_path): rospy.logerr(f"Specified bag file {self._testing_bag_file_path} does not exist, exiting...") - exit(0) + sys.exit() self._glove_msg_period = self._get_average_bag_msg_period() self._bag_msgs_generator = None self._desired_datapoints_per_sec = desired_datapoints_per_sec