Test Rig Analyze Class completed, Dashboard GUI display class completed

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_202202171855.yaml

@@ -0,0 +1,65 @@
+capture_id: '202202171855'
+lighting_condition:
+  brightness: null
+  temperature: null
+null_frames:
+  base_left_marker_pose: 0
+  base_right_marker_pose: 0
+  shoulder_marker_pose: 327
+  wrist_inside_marker_pose: 0
+  wrist_top_marker_pose: 0
+number_samples: 600
+performance_metrics:
+  angle_rotation_error:
+    base_left_marker_pose:
+      maximum: 3.130310772767618
+      mean: 3.1123105605302857
+      median: 3.1121101802740316
+      rmse: 3.1123183433291164
+    base_right_marker_pose:
+      maximum: 3.141490159715747
+      mean: 3.126268687591132
+      median: 3.1270113289070975
+      rmse: 3.1262809684459114
+    shoulder_marker_pose:
+      maximum: 3.1351679171566076
+      mean: 3.112359136964902
+      median: 3.1132823943622094
+      rmse: 3.112370862795674
+    wrist_inside_marker_pose:
+      maximum: 3.1351074913380303
+      mean: 3.091203628929577
+      median: 3.0897949788692305
+      rmse: 3.09122966284615
+    wrist_top_marker_pose:
+      maximum: 3.1412404426476863
+      mean: 3.1094737772610457
+      median: 3.1103044636557806
+      rmse: 3.1095094548879647
+  euclidean_error:
+    base_left_marker_pose:
+      maximum: 0.5824225026965391
+      mean: 0.5820963567556977
+      median: 0.5820901959143456
+      rmse: 0.5820963644996675
+    base_right_marker_pose:
+      maximum: 0.5793795286072739
+      mean: 0.578940614355908
+      median: 0.5789297846748012
+      rmse: 0.5789406328075614
+    shoulder_marker_pose:
+      maximum: 0.5814463353933174
+      mean: 0.5812017213971998
+      median: 0.5812040960870808
+      rmse: 0.5812017272530052
+    wrist_inside_marker_pose:
+      maximum: 0.5788094647317441
+      mean: 0.5784990979155296
+      median: 0.5784922563334053
+      rmse: 0.5784991059700069
+    wrist_top_marker_pose:
+      maximum: 0.5806082586912994
+      mean: 0.5802254663635843
+      median: 0.5802384545676887
+      rmse: 0.5802254782763624
+realsense_firmware: 05.13.00.50

stretch_camera_testrig/nodes/testrig_analyze_data.py

@@ -1,6 +1,7 @@
 #!/usr/bin/env python
 import argparse
 import os
+import pprint
 import sys
 import yaml
 import numpy as np
@@ -11,15 +12,19 @@ from subprocess import Popen, PIPE
 import time
 
 parser = argparse.ArgumentParser(description='Analyzes the recent Collected Test Rig Data')
-parser.add_argument("--observation",help="Shows the computed error stats from the most recent collected data using the Testrig.", action="store_true")
-parser.add_argument('-f', metavar='data_file', type=str, help='File path to specific sample data collected using Test Rig (Default:Most Recent Sample Data)')
+parser.add_argument("--observation", help="Shows the computed error stats from the most recent collected data using "
+                                          "the Testrig.", action="store_true")
+parser.add_argument('-f', metavar='data_file', type=str, help='File path to specific sample data collected using Test '
+                                                              'Rig (Default:Most Recent Sample Data)')
 args = parser.parse_args()
+data_file = None
 
 
-class TestRig_analyze:
-    def __init__(self,data_filename=None):
+class TestRig_Analyze:
+    def __init__(self, data_filename=None):
         self.data_directory = os.path.expanduser('~/catkin_ws/src/stretch_ros/stretch_camera_testrig/data')
-        self.nominal_poses_filename = os.path.expanduser('~/catkin_ws/src/stretch_ros/stretch_camera_testrig/config/testrig_marker_info.yaml')
+        self.nominal_poses_filename = os.path.expanduser(
+            '~/catkin_ws/src/stretch_ros/stretch_camera_testrig/config/testrig_marker_info.yaml')
         self.data_keys = ['base_left_marker_pose',
                           'base_right_marker_pose',
                           'wrist_inside_marker_pose',
@@ -34,7 +39,7 @@ class TestRig_analyze:
 
         self.data_filename = None
         self.data_capture_date = None
-        self.number_samples = None
+        self.number_of_samples = None
 
         if data_filename:
             self.data_filename = data_filename
@@ -43,20 +48,68 @@ class TestRig_analyze:
             files.sort()
             if files[-1].startswith("testrig_collected_data_"):
                 self.data_filename = self.data_directory + '/' + files[-1]
-        
+
         capture_date = self.data_filename.split('_')[-1]
         capture_date = capture_date.split('.')[0]
         self.data_capture_date = capture_date
 
+        self.test_results_dict = {'capture_id': self.data_capture_date,
+                                  'realsense_firmware': self.realsense_fw,
+                                  'number_samples': None,
+                                  'null_frames': {},
+                                  'lighting_condition': {
+                                      'temperature': None,
+                                      'brightness': None,
+                                  },
+                                  'performance_metrics': {}
+                                  }
+
+    def get_performance_metric(self, error_data_dict):
+        performance_metrics = {}
+        for key in self.data_keys:
+            performance_metric = {}
+            performance_metric['mean'] = float(np.mean(self.remove_null_vals(error_data_dict[key])))
+            performance_metric['maximum'] = float(np.max(self.remove_null_vals(error_data_dict[key])))
+            performance_metric['median'] = float(np.median(self.remove_null_vals(error_data_dict[key])))
+            performance_metric['rmse'] = float(np.sqrt(np.mean(self.remove_null_vals(error_data_dict[key]) ** 2)))
+
+            performance_metrics[key] = performance_metric
+        return performance_metrics
+
+    def remove_null_vals(self, arr):
+        arr = np.array(arr, dtype=np.float)
+        return arr[~np.isnan(arr)]
+
+    def populate_performance_metrics(self):
+        self.test_results_dict['performance_metrics']['angle_rotation_error'] = self.get_performance_metric(
+            self.angle_rotation_error_dict)
+        self.test_results_dict['performance_metrics']['euclidean_error'] = self.get_performance_metric(
+            self.euclidean_error_dict)
+        self.test_results_dict['number_samples'] = self.number_of_samples
+        self.test_results_dict['null_frames'] = self.get_null_frames_count()
+        pp = pprint.PrettyPrinter(indent=4)
+        pp.pprint(self.test_results_dict)
+
+    def get_null_frames_count(self):
+        null_counts = {}
+        for key in self.data_keys:
+            nulls = 0
+            for x in self.data_dict[key]:
+                if x is None:
+                    nulls = nulls+1
+            null_counts[key] = nulls
+        return null_counts
+
     def get_realsense_fw(self):
-        fw_details = Popen("rs-fw-update -l | grep -i 'firmware'", shell=True, bufsize=64, stdin=PIPE, stdout=PIPE, close_fds=True).stdout.read()
+        fw_details = Popen("rs-fw-update -l | grep -i 'firmware'", shell=True, bufsize=64, stdin=PIPE, stdout=PIPE,
+                           close_fds=True).stdout.read()
         fw_details = fw_details.split(',')[3]
         fw_version = fw_details.split(' ')[-1]
         return fw_version
 
-    def testrg_data_parse(self,filename):
+    def testrg_data_parse(self, filename):
         # Parses the collected data in yaml
-        # returns  disctionary  with list of 4x4 Transformation Matrix
+        # returns  dictionary  with list of 4x4 Transformation Matrix
         print('Test Rig Data File : {0}'.format(filename))
         data_dict = {}
         try:
@@ -72,7 +125,7 @@ class TestRig_analyze:
             for observation in data_list:
                 for key in self.data_keys:
                     marker_pose = observation['camera_measurements'][key]
-                    if marker_pose!=None:
+                    if marker_pose != None:
                         data_dict[key].append(np.array(marker_pose))
                     else:
                         data_dict[key].append(None)
@@ -80,52 +133,52 @@ class TestRig_analyze:
         except IOError:
             print('[Error]:Unable to open Testrig Data file: {0}'.format(filename))
 
-    def get_euclidean_errors(self,data_dict,nominal_poses_dict):
+    def get_euclidean_errors(self, data_dict, nominal_poses_dict):
         error_dict = {}
         Num_samples = len(data_dict[self.data_keys[0]])
 
         for key in self.data_keys:
             error_dict[key] = []
-            if Num_samples!=len(data_dict[key]):
+            if Num_samples != len(data_dict[key]):
                 print('[Warning]: Number of samples found inconsistent for each Marker tag.')
 
         for key in error_dict.keys():
             for i in range(Num_samples):
                 if type(data_dict[key][i]) != type(None):
-                    error_dict[key].append(self.euclidean_error(nominal_poses_dict[key],data_dict[key][i]))
+                    error_dict[key].append(self.euclidean_error(nominal_poses_dict[key], data_dict[key][i]))
                 else:
                     error_dict[key].append(None)
         return error_dict
 
-    def get_angle_rot_errors(self,data_dict,nominal_poses_dict):
+    def get_angle_rot_errors(self, data_dict, nominal_poses_dict):
         error_dict = {}
         Num_samples = len(data_dict[self.data_keys[0]])
 
         for key in self.data_keys:
             error_dict[key] = []
-            if Num_samples!=len(data_dict[key]):
+            if Num_samples != len(data_dict[key]):
                 print('[Warning]: Number of samples found inconsistent for each Marker tag.')
 
         for key in error_dict.keys():
             for i in range(Num_samples):
                 if type(data_dict[key][i]) != type(None):
-                    error_dict[key].append(self.angle_rot_error(nominal_poses_dict[key],data_dict[key][i]))
+                    error_dict[key].append(self.angle_rot_error(nominal_poses_dict[key], data_dict[key][i]))
                 else:
                     error_dict[key].append(None)
         return error_dict
 
-    def angle_rot_error(self,r1,r2):
-        rot1 = np.array(r1)[:3,:3]
-        rot2 = np.array(r2)[:3,:3]
-        rot12 = np.matmul(rot1.T,rot2)
-        theta_error = np.arccos((np.trace(rot12)-1)/2)
+    def angle_rot_error(self, r1, r2):
+        rot1 = np.array(r1)[:3, :3]
+        rot2 = np.array(r2)[:3, :3]
+        rot12 = np.matmul(rot1.T, rot2)
+        theta_error = np.arccos((np.trace(rot12) - 1) / 2)
         return float(theta_error)
 
-    def euclidean_error(self,r1,r2):
-        dist = np.linalg.norm(r1[:3,3] - r2[:3,3])
+    def euclidean_error(self, r1, r2):
+        dist = np.linalg.norm(r1[:3, 3] - r2[:3, 3])
         return float(dist)
 
-    def get_nominal_pose_dict(self,filename):
+    def get_nominal_pose_dict(self, filename):
         try:
             with open(filename, 'r') as file:
                 data = yaml.safe_load(file)
@@ -136,13 +189,13 @@ class TestRig_analyze:
         except IOError:
             print('[Error]:Unable to open Testrig Nominal Poses file: {0}'.format(filename))
 
-    def save_error_results(self):
+    def save_error_computations(self):
         t = time.localtime()
         capture_date = self.data_capture_date
         testrig_results = []
 
         for i in range(self.number_of_samples):
-            error = {'error_data':{}}
+            error = {'error_data': {}}
             for key in self.data_keys:
                 vals = {}
                 vals['angle_rotation_error'] = self.angle_rotation_error_dict[key][i]
@@ -156,17 +209,26 @@ class TestRig_analyze:
             documents = yaml.dump(testrig_results, file)
         print('Test Rig Computed Error Data Saved to : {}'.format(filename))
 
-    def get_observations(self):
+    def save_testrig_results(self):
+        filename = self.data_directory + '/results/testrig_results_' + self.data_capture_date + '.yaml'
+        with open(filename, 'w') as file:
+            documents = yaml.dump(self.test_results_dict, file)
+        print('Test Rig Results Data Saved to : {}'.format(filename))
+
+    def generate_error_observations(self):
         self.data_dict = self.testrg_data_parse(self.data_filename)
         self.nominal_poses_dict = self.get_nominal_pose_dict(self.nominal_poses_filename)
 
-        self.euclidean_error_dict = self.get_euclidean_errors(self.data_dict,self.nominal_poses_dict)
-        self.angle_rotation_error_dict = self.get_angle_rot_errors(self.data_dict,self.nominal_poses_dict)
+        self.euclidean_error_dict = self.get_euclidean_errors(self.data_dict, self.nominal_poses_dict)
+        self.angle_rotation_error_dict = self.get_angle_rot_errors(self.data_dict, self.nominal_poses_dict)
+
 
 if args.observation:
-    test_rig = TestRig_analyze()
-    test_rig.get_observations()
-    test_rig.save_error_results()
+    test_rig = TestRig_Analyze(data_file)
+    test_rig.generate_error_observations()
+    test_rig.populate_performance_metrics()
+    test_rig.save_error_computations()
+    test_rig.save_testrig_results()
 
 if args.f:
     data_file = args.f

stretch_camera_testrig/nodes/testrig_dashboard.py

@@ -0,0 +1,212 @@
+#!/usr/bin/env python
+from tkinter import *
+from testrig_analyze_data import TestRig_Analyze
+import os
+import yaml
+import numpy as np
+from tabulate import tabulate
+import math
+
+
+class TestRig_dashboard():
+    def __init__(self):
+        self.data_directory = os.path.expanduser('~/catkin_ws/src/stretch_ros/stretch_camera_testrig/data')
+        self.nominal_poses_filename = os.path.expanduser(
+            '~/catkin_ws/src/stretch_ros/stretch_camera_testrig/config/testrig_marker_info.yaml')
+        self.nominal_poses_filename = os.path.expanduser(
+            '~/catkin_ws/src/stretch_ros/stretch_camera_testrig/config/testrig_marker_info.yaml')
+
+        self.data_keys = ['base_left_marker_pose',
+                          'base_right_marker_pose',
+                          'wrist_inside_marker_pose',
+                          'wrist_top_marker_pose',
+                          'shoulder_marker_pose']
+        self.metrics_keys = ["maximum", "mean", "median", "rmse"]
+        self.error_keys = ["angle_rotation_error", "euclidean_error"]
+        self.window = Tk()
+        self.window.title("Camera Test rig Dashboard")
+        self.window.geometry('600x900')
+
+        self.data_file_name = None
+        self.test_rig = None
+        self.table_entry = None
+
+        #################### Top Pane ##########################
+
+        self.frame_top = Frame(self.window, width=100, height=25)
+        self.frame_top.grid(column=0, row=0, sticky="w")
+
+        self.load_data_recent_btn = Button(self.frame_top, text="Load Most Recent Data",
+                                           command=self.load_recent_clicked, font=("Arial", 14))
+        self.load_data_recent_btn.pack(side=TOP)
+
+        self.data_file_entry_lbl = Label(self.frame_top, text="Enter Data file path ")
+        self.data_file_entry_lbl.pack(side=LEFT)
+
+        self.data_file_path = StringVar()
+        self.data_file_path_entry = Entry(self.frame_top, textvariable=self.data_file_path, width=30)
+        self.data_file_path_entry.pack(side=LEFT)
+
+        self.load_data_btn = Button(self.frame_top, text="Load", command=self.load_clicked)
+        self.load_data_btn.pack(side=LEFT)
+
+        self.test_capture_id_lbl = Label(self.window, text=" ")
+        self.test_capture_id_lbl.grid(column=0, row=1, sticky="w")
+
+        self.LOGGER = Label(self.window, text="", anchor='w')
+        self.LOGGER.grid(column=0, row=2, sticky="w")
+
+        ############## Nominal Poses Update Section Pane ###########
+
+        x_off = 0
+        y_off = 0
+        self.nominal_poses_label = Label(self.window, text="Nominal Poses", font=("Arial", 18))
+        self.nominal_poses_label.place(x=x_off + 20, y=y_off + 120)
+        self.matrix_text_var = None
+        self.matrix_entries = None
+        self.create_matrix_entry(x_off + 50, y_off + 150)
+        self.update_nominal_poses_btn = Button(self.window, text="Update Nominal Poses",
+                                               command=self.update_nominal_poses)
+        self.update_nominal_poses_btn.place(x=x_off + 20, y=y_off + 280)
+
+        self.nominal_poses_radiobuttons = []
+        self.nominal_poses_selector_vars = []
+        for key in self.data_keys:
+            self.nominal_poses_selector_vars.append(IntVar)
+            r = Radiobutton(self.window, text=key, value=0,
+                            command=self.radiobutton_sel)
+            self.nominal_poses_radiobuttons.append(r)
+        for i in range(len(self.nominal_poses_radiobuttons)):
+            self.nominal_poses_radiobuttons[i].place(x=x_off + 220, y=y_off + 165 + i * 20)
+
+        ################## Test Results Display Pane ########################
+        self.x_off_mid = 0
+        self.y_off_mid = 0
+        self.test_results_title = Label(self.window, text="Testrig Computed Results", font=("Arial", 18))
+        self.test_results_title.place(x=self.x_off_mid + 20, y=self.y_off_mid + 330)
+
+        ################### Bottom Pane #########################
+        x_off_bottom = 0
+        y_off_bottom = 300
+        self.save_results_btn = Button(self.window, text="Save Computed Results", command=self.save_results_clicked,
+                                       font=("Arial", 14))
+        self.save_results_btn.place(x=x_off_bottom + 150, y=y_off_bottom + 500)
+
+        self.run_new_test_btn = Button(self.window, text="Run a new Test", command=self.run_new_test,
+                                       font=("Arial", 14))
+        self.run_new_test_btn.place(x=x_off_bottom + 150, y=y_off_bottom + 540)
+
+        self.target_samples = IntVar()
+        self.target_samples_entry = Entry(self.window, textvariable=self.target_samples, width=6)
+        self.target_samples_entry.place(x=x_off_bottom + 450, y=y_off_bottom + 546)
+
+        self.target_samples_lbl = Label(self.window, text="Enter Target frames:")
+        self.target_samples_lbl.place(x=x_off_bottom + 310, y=y_off_bottom + 546)
+
+    def metrics_table_print(self, pos_x, pos_y, error_key):
+        n_rows = len(self.data_keys) + 1
+        n_columns = len(self.metrics_keys) + 1
+        data_list = []
+        for key in self.data_keys:
+            d = [key]
+            for mkey in self.metrics_keys:
+                results = self.test_rig.test_results_dict['performance_metrics'][error_key]
+                d.append(results[key][mkey])
+            data_list.append(d)
+        mkeys = self.metrics_keys
+        mkeys = ['Marker Tag'] + mkeys
+        data_list = [mkeys] + data_list
+        x2 = 0
+        y2 = 0
+        for i in range(n_rows):
+            for j in range(n_columns):
+                if j == 0:
+                    self.table_entry = Entry(self.window, width=22, font=('Arial', 10, 'bold'))
+                    self.table_entry.place(x=pos_x + x2 - 110, y=pos_y + y2)
+                    self.table_entry.insert(END, data_list[i][j])
+                elif i == 0:
+                    self.table_entry = Entry(self.window, width=7, font=('Arial', 10, 'bold'))
+                    self.table_entry.place(x=pos_x + x2, y=pos_y + y2)
+                    self.table_entry.insert(END, data_list[i][j])
+                else:
+                    self.table_entry = Entry(self.window, width=7)
+                    self.table_entry.place(x=pos_x + x2, y=pos_y + y2)
+                    self.table_entry.insert(END, data_list[i][j])
+                x2 += 70
+            y2 += 30
+            x2 = 0
+
+    def create_matrix_entry(self, pos_x, pos_y):
+        self.matrix_text_var = []
+        self.matrix_entries = []
+        x2 = 0
+        y2 = 0
+        rows, cols = (4, 4)
+        for i in range(rows):
+            self.matrix_text_var.append([])
+            self.matrix_entries.append([])
+            for j in range(cols):
+                # append your StringVar and Entry
+                self.matrix_text_var[i].append(StringVar())
+                self.matrix_entries[i].append(Entry(self.window, textvariable=self.matrix_text_var[i][j], width=3))
+                self.matrix_entries[i][j].place(x=pos_x + x2, y=pos_y + y2)
+                x2 += 30
+            y2 += 30
+            x2 = 0
+
+    def radiobutton_sel(self):
+        print('Radio Log')
+
+    def run_new_test(self):
+        print('Run New Test')
+
+    def update_nominal_poses(self):
+        self.log('Updated new Nominal Poses File.')
+
+    def log(self, txt):
+        self.LOGGER.configure(text=txt)
+        print(txt)
+        self.window.update_idletasks()
+
+    def save_results_clicked(self):
+        if self.test_rig:
+            self.test_rig.save_error_computations()
+            self.test_rig.save_testrig_results()
+            self.log('Testrig Analyze results saved in /stretch_camera_testrig/data/results')
+
+    def load_clicked(self):
+        if len(self.data_file_path.get()) > 0:
+            try:
+                self.log('Loading given Test data file....')
+                self.test_rig = self.get_testrig(self.data_directory + '/' + self.data_file_path.get())
+                self.test_capture_id_lbl.configure(
+                    text="Testrig Data Capture ID : {} loaded".format(self.test_rig.data_capture_date))
+                self.log('Loaded Test Rig data file : {}'.format(self.test_rig.data_filename.split('/')[-1]))
+            except:
+                self.log('Unable to load given data file:{}'.format(self.data_file_path.get()))
+        else:
+            self.log('Enter a data file path.')
+
+    def load_recent_clicked(self):
+        self.log('Loading most Recent Test data file....')
+        self.test_rig = self.get_testrig()
+        self.test_capture_id_lbl.configure(
+            text="Testrig Capture ID : {} loaded".format(self.test_rig.data_capture_date))
+        self.log('Loaded Test Rig data file : {}'.format(self.test_rig.data_filename.split('/')[-1]))
+
+    def get_testrig(self, data_file=None):
+        test_rig = TestRig_Analyze(data_file)
+        test_rig.generate_error_observations()
+        test_rig.populate_performance_metrics()
+        self.test_rig = test_rig
+        self.metrics_table_print(self.x_off_mid + 130, self.y_off_mid + 420, 'euclidean_error')
+        self.metrics_table_print(self.x_off_mid + 130, self.y_off_mid + 620, 'angle_rotation_error')
+        return test_rig
+
+    def mainloop(self):
+        self.window.mainloop()
+
+
+if __name__ == "__main__":
+    dashboard = TestRig_dashboard()
+    dashboard.mainloop()