RS FW Report Updates

stretch_camera_testrig/.idea/.gitignore

@@ -0,0 +1,3 @@
+# Default ignored files
+/shelf/
+/workspace.xml

stretch_camera_testrig/.idea/inspectionProfiles/profiles_settings.xml

@@ -0,0 +1,6 @@
+<component name="InspectionProjectProfileManager">
+  <settings>
+    <option name="USE_PROJECT_PROFILE" value="false" />
+    <version value="1.0" />
+  </settings>
+</component>

stretch_camera_testrig/.idea/misc.xml

@@ -0,0 +1,4 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="ProjectRootManager" version="2" project-jdk-name="Python 3.6" project-jdk-type="Python SDK" />
+</project>

stretch_camera_testrig/.idea/modules.xml

@@ -0,0 +1,8 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="ProjectModuleManager">
+    <modules>
+      <module fileurl="file://$PROJECT_DIR$/.idea/stretch_camera_testrig.iml" filepath="$PROJECT_DIR$/.idea/stretch_camera_testrig.iml" />
+    </modules>
+  </component>
+</project>

stretch_camera_testrig/.idea/stretch_camera_testrig.iml

@@ -0,0 +1,8 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<module type="PYTHON_MODULE" version="4">
+  <component name="NewModuleRootManager">
+    <content url="file://$MODULE_DIR$" />
+    <orderEntry type="inheritedJdk" />
+    <orderEntry type="sourceFolder" forTests="false" />
+  </component>
+</module>

stretch_camera_testrig/.idea/vcs.xml

@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="VcsDirectoryMappings">
+    <mapping directory="$PROJECT_DIR$/.." vcs="Git" />
+  </component>
+</project>

stretch_camera_testrig/data/results/testrig_results_202203081051.yaml

@@ -0,0 +1,65 @@
+capture_id: '202203081051'
+lighting_condition:
+  brightness: null
+  temperature: null
+null_frames:
+  base_left_marker_pose: 600
+  base_right_marker_pose: 600
+  shoulder_marker_pose: 0
+  wrist_inside_marker_pose: 0
+  wrist_top_marker_pose: 0
+number_samples: 600
+performance_metrics:
+  angle_rotation_error:
+    base_left_marker_pose:
+      maximum: .nan
+      mean: .nan
+      median: .nan
+      rmse: .nan
+    base_right_marker_pose:
+      maximum: .nan
+      mean: .nan
+      median: .nan
+      rmse: .nan
+    shoulder_marker_pose:
+      maximum: 3.097955498135791
+      mean: 3.073899037378878
+      median: 3.0741315861747056
+      rmse: 3.0739089414710428
+    wrist_inside_marker_pose:
+      maximum: 3.1415761356878966
+      mean: 3.132097089689872
+      median: 3.1341885277231425
+      rmse: 3.1321072207088934
+    wrist_top_marker_pose:
+      maximum: 3.1415892941462182
+      mean: 3.130561913781429
+      median: 3.1323310066834207
+      rmse: 3.130571838773318
+  euclidean_error:
+    base_left_marker_pose:
+      maximum: .nan
+      mean: .nan
+      median: .nan
+      rmse: .nan
+    base_right_marker_pose:
+      maximum: .nan
+      mean: .nan
+      median: .nan
+      rmse: .nan
+    shoulder_marker_pose:
+      maximum: 0.382899354953261
+      mean: 0.3825723337040798
+      median: 0.3825632955437427
+      rmse: 0.38257234197690604
+    wrist_inside_marker_pose:
+      maximum: 0.3790289222347782
+      mean: 0.3788044075180077
+      median: 0.37881162813600683
+      rmse: 0.3788044182591934
+    wrist_top_marker_pose:
+      maximum: 0.3815277974107985
+      mean: 0.38131954873736296
+      median: 0.3813172708681125
+      rmse: 0.3813195561084727
+realsense_details: ''

stretch_camera_testrig/data/results/testrig_results_202203081401.yaml

@@ -0,0 +1,68 @@
+capture_id: '202203081401'
+lighting_condition:
+  brightness: null
+  temperature: null
+null_frames:
+  base_left_marker_pose: 600
+  base_right_marker_pose: 600
+  shoulder_marker_pose: 0
+  wrist_inside_marker_pose: 0
+  wrist_top_marker_pose: 0
+number_samples: 600
+performance_metrics:
+  angle_rotation_error:
+    base_left_marker_pose:
+      maximum: .nan
+      mean: .nan
+      median: .nan
+      rmse: .nan
+    base_right_marker_pose:
+      maximum: .nan
+      mean: .nan
+      median: .nan
+      rmse: .nan
+    shoulder_marker_pose:
+      maximum: 3.141584519643849
+      mean: 3.126448207110162
+      median: 3.1276875522310275
+      rmse: 3.1264639715464964
+    wrist_inside_marker_pose:
+      maximum: 3.141569730682565
+      mean: 3.1315863064453504
+      median: 3.1329069411440544
+      rmse: 3.131594936497842
+    wrist_top_marker_pose:
+      maximum: 3.141142764422492
+      mean: 3.1230911150059053
+      median: 3.1232765976473784
+      rmse: 3.1230998350351697
+  euclidean_error:
+    base_left_marker_pose:
+      maximum: .nan
+      mean: .nan
+      median: .nan
+      rmse: .nan
+    base_right_marker_pose:
+      maximum: .nan
+      mean: .nan
+      median: .nan
+      rmse: .nan
+    shoulder_marker_pose:
+      maximum: 0.46059878545645616
+      mean: 0.46029296570007244
+      median: 0.46030180067131776
+      rmse: 0.4602929833290485
+    wrist_inside_marker_pose:
+      maximum: 0.46751724743962775
+      mean: 0.4673098171295163
+      median: 0.4673148698265675
+      rmse: 0.4673098241295182
+    wrist_top_marker_pose:
+      maximum: 0.47689815540831476
+      mean: 0.4767455667689777
+      median: 0.4767559937236129
+      rmse: 0.4767455718786323
+realsense_details:
+  firmware: 05.13.00.50
+  serial: '134322074171'
+  usb: '3.2'

stretch_camera_testrig/nodes/collect_head_calibration_data.py

@@ -54,18 +54,16 @@ class CollectHeadCalibrationDataNode:
         if test_rig:
             self.number_of_testrig_samples = 40
 
-    def calibration_data_callback(self, marker_array, accel=None, joint_state=None):
+    def calibration_data_callback(self, marker_array, accel, joint_state):
         # Prepare and assign data for calibration to member
         # variables. The configuration of the robot's joints, the
         # D435i acceleration, and the 3D fiducial estimates made via
         # D435i imagery are expected to be synchronized for this
         # callback.
         with self.data_lock:
-            if not self.test_rig:
-                self.data_time = joint_state.header.stamp
-                self.joint_state = joint_state
-                self.acceleration = [accel.linear_acceleration.x, accel.linear_acceleration.y,
-                                     accel.linear_acceleration.z]
+            self.data_time = joint_state.header.stamp
+            self.joint_state = joint_state
+            self.acceleration = [accel.linear_acceleration.x, accel.linear_acceleration.y, accel.linear_acceleration.z]
 
             self.wrist_inside_marker_pose = None
             self.wrist_top_marker_pose = None
@@ -96,6 +94,43 @@ class CollectHeadCalibrationDataNode:
                 if marker.id == self.shoulder_marker_id:
                     self.shoulder_marker_pose = marker.pose
 
+    def calibration_data_callback_testrig(self, marker_array, accel):
+        # Prepare and assign data for calibration to member
+        # variables. The configuration of the robot's joints, the
+        # D435i acceleration, and the 3D fiducial estimates made via
+        # D435i imagery are expected to be synchronized for this
+        # callback.
+        with self.data_lock:
+            self.wrist_inside_marker_pose = None
+            self.wrist_top_marker_pose = None
+            self.base_left_marker_pose = None
+            self.base_right_marker_pose = None
+            self.shoulder_marker_pose = None
+            self.acceleration = [accel.linear_acceleration.x, accel.linear_acceleration.y, accel.linear_acceleration.z]
+
+            self.marker_time = None
+            for marker in marker_array.markers:
+                # set marker_time to the earliest marker time
+                if self.marker_time is None:
+                    self.marker_time = marker.header.stamp
+                elif marker.header.stamp < self.marker_time:
+                    self.marker_time = marker.header.stamp
+
+                if marker.id == self.wrist_inside_marker_id:
+                    self.wrist_inside_marker_pose = marker.pose
+
+                if marker.id == self.wrist_top_marker_id:
+                    self.wrist_top_marker_pose = marker.pose
+
+                if marker.id == self.base_left_marker_id:
+                    self.base_left_marker_pose = marker.pose
+
+                if marker.id == self.base_right_marker_id:
+                    self.base_right_marker_pose = marker.pose
+
+                if marker.id == self.shoulder_marker_id:
+                    self.shoulder_marker_pose = marker.pose
+
     def move_to_pose(self, pose):
         # Prepare and send a goal pose to which the robot should move.
         joint_names = [key for key in pose]
@@ -802,11 +837,10 @@ class CollectHeadCalibrationDataNode:
             accel_subscriber = message_filters.Subscriber('/camera/accel/sample_corrected', Imu)
             aruco_subscriber = message_filters.Subscriber('/aruco/marker_array', MarkerArray)
             slop_time = 0.1
-            self.synchronizer = message_filters.ApproximateTimeSynchronizer([aruco_subscriber], 10, slop_time,
+            self.synchronizer = message_filters.ApproximateTimeSynchronizer([aruco_subscriber, accel_subscriber], 10,
+                                                                            slop_time,
                                                                             allow_headerless=True)
-            self.synchronizer.registerCallback(self.calibration_data_callback)
-
-            rate = rospy.Rate(self.rate)
+            self.synchronizer.registerCallback(self.calibration_data_callback_testrig)
             self.testrig_data_path = rospy.get_param('~testrig_data_directory')
             self.number_of_testrig_samples = rospy.get_param('~samples')
             self.testrig_data_collect(number_of_samples=self.number_of_testrig_samples)

stretch_camera_testrig/nodes/testrig_analyze_data.py

@@ -31,7 +31,7 @@ class TestRig_Analyze:
                           'wrist_top_marker_pose',
                           'shoulder_marker_pose']
 
-        self.realsense_fw = self.get_realsense_fw()
+        self.realsense_details = self.get_realsense_fw()
 
         self.data_dict = None
         self.euclidean_error_dict = None
@@ -54,7 +54,9 @@ class TestRig_Analyze:
         self.data_capture_date = capture_date
 
         self.test_results_dict = {'capture_id': self.data_capture_date,
-                                  'realsense_details': self.realsense_fw,
+                                  'camera_serial': self.realsense_details['serial'],
+                                  'firmware': self.realsense_details['firmware'],
+                                  'usb':self.realsense_details['usb'],
                                   'number_samples': None,
                                   'null_frames': {},
                                   'lighting_condition': {

stretch_camera_testrig/nodes/testrig_dashboard.py

@@ -117,7 +117,7 @@ class TestRig_dashboard():
         except KeyError:
             None
         info_txt = str(yaml.safe_dump(info_dict, allow_unicode=True, default_flow_style=False))
-        self.info_print = Label(self.window, text=info_txt, anchor="w", font=("Arial", 10), justify=LEFT)
+        self.info_print = Label(self.window, text=info_txt, anchor="w", font=("Arial", 11), justify=LEFT)
         self.info_print.place(x=x_pos, y=y_pos)
 
     def metrics_table_print(self, pos_x, pos_y, error_key):