Nominal Poses Entry through GUI Working

stretch_camera_testrig/data/results/testrig_results_202204221727.yaml

@@ -0,0 +1,68 @@
+Realsense Details:
+  firmware: 05.13.00.50
+  serial: '134322070297'
+  usb: '3.2'
+capture_id: '202204221727'
+lighting_condition:
+  brightness: null
+  temperature: null
+null_frames:
+  base_left_marker_pose: 45
+  base_right_marker_pose: 0
+  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: 3.1415102398810113
+      mean: 3.123058951288247
+      median: 3.124318210965746
+      rmse: 3.1230811369254297
+    shoulder_marker_pose:
+      maximum: 3.1335815142187062
+      mean: 3.083481179422443
+      median: 3.0833914703359806
+      rmse: 3.0835367100259767
+    wrist_inside_marker_pose:
+      maximum: .nan
+      mean: .nan
+      median: .nan
+      rmse: .nan
+    wrist_top_marker_pose:
+      maximum: .nan
+      mean: .nan
+      median: .nan
+      rmse: .nan
+  euclidean_error:
+    base_left_marker_pose:
+      maximum: 0.915788144606525
+      mean: 0.9154234022733942
+      median: 0.9154204292171265
+      rmse: 0.9154234113171679
+    base_right_marker_pose:
+      maximum: 0.9214418418458941
+      mean: 0.920910422416328
+      median: 0.9209082137309761
+      rmse: 0.9209104385493279
+    shoulder_marker_pose:
+      maximum: 0.9262525606179585
+      mean: 0.9255763067776382
+      median: 0.9255706057771111
+      rmse: 0.9255763231902148
+    wrist_inside_marker_pose:
+      maximum: 0.9306561678487971
+      mean: 0.9299437368304545
+      median: 0.9299687248825842
+      rmse: 0.9299437768626291
+    wrist_top_marker_pose:
+      maximum: 0.934151074834107
+      mean: 0.9339303855491827
+      median: 0.9339337648183319
+      rmse: 0.9339303887654067

stretch_camera_testrig/launch/rs_d435i.launch

@@ -0,0 +1,30 @@
+<launch>
+
+  <arg name="rviz" default="false"/>
+  
+  <!-- REALSENSE D435i -->
+  <include file="$(find stretch_core)/launch/d435i_high_resolution.launch"></include>
+  <node name="d435i_configure" pkg="stretch_core" type="d435i_configure" output="screen">
+    <param name="initial_mode" type="string" value="High Accuracy"/>
+  </node>
+
+  <!-- REALSENSE D435i Model Load -->
+  <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find realsense2_description)/urdf/test_d435i_camera.urdf.xacro' use_nominal_extrinsics:=true" />
+
+  <node
+      name="robot_state_publisher"
+      pkg="robot_state_publisher"
+      type="robot_state_publisher" >
+    <param name="publish_frequency" value="15.0"/>
+  </node>
+
+  <!-- ARUCO MARKER DETECTOR -->
+  <rosparam command="load" file="$(find stretch_core)/config/stretch_marker_dict.yaml" />
+  <node name="detect_aruco_markers" pkg="stretch_core" type="detect_aruco_markers" output="screen"/>
+
+  <!-- Optional RVIZ Visualization -->
+  <group if="$(arg rviz)">
+  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find stretch_camera_testrig)/rviz/rviz.rviz" required="true"/>
+  </group>
+
+</launch>

stretch_camera_testrig/nodes/testrig_dashboard.py

@@ -176,7 +176,8 @@ class TestRig_dashboard():
     def run_new_test(self):
         ros_log = None
         try:
-            ros_log = Popen("roslaunch stretch_camera_testrig testrig_collect_data.launch", shell=True, bufsize=64, stdin=PIPE, stdout=PIPE,
+            ros_log = Popen("roslaunch stretch_camera_testrig testrig_collect_data.launch", shell=True, bufsize=64,
+                            stdin=PIPE, stdout=PIPE,
                             close_fds=True).stdout.read()
         except:
             print('Unable to Collect Testrig Data.')
@@ -184,10 +185,11 @@ class TestRig_dashboard():
 
     def radiobutton_sel(self):
         selected_data_key = self.data_keys[self.radio_var.get()]
-        print('selected {}'.format(selected_data_key))
-        nominal_poses_dict = self.get_nominal_pose_dict(self.nominal_poses_filename)
+        self.log('selected {}'.format(selected_data_key))
+        nominal_poses_dict = self.get_nominal_pose_dict()
         for key in self.data_keys:
             if key == selected_data_key:
+                print('Current Value:')
                 print(nominal_poses_dict[key])
                 for i in range(4):
                     for j in range(4):
@@ -195,7 +197,8 @@ class TestRig_dashboard():
         # print(self.matrix_text_var)
         # print(self.matrix_entries)
 
-    def get_nominal_pose_dict(self, filename):
+    def get_nominal_pose_dict(self):
+        filename = self.nominal_poses_filename
         try:
             with open(filename, 'r') as file:
                 data = yaml.safe_load(file)
@@ -206,8 +209,38 @@ class TestRig_dashboard():
         except IOError:
             print('[Error] Unable to open Testrig Nominal Poses file: {0}'.format(filename))
 
+    def get_current_entry_matrix(self):
+        mat = None
+        try:
+            mat = []
+            rows, cols = (4, 4)
+            for i in range(rows):
+                mat.append([])
+                for j in range(cols):
+                    mat[i].append(float(self.matrix_entries[i][j].get()))
+            return np.array(mat)
+        except:
+            self.log('Non-Valid Matrix Entered.')
+            return None
+
+    def set_nominal_pose_dict(self, pose_matrix, marker_name):
+        filename = self.nominal_poses_filename
+        try:
+            with open(filename, 'r') as file:
+                data = yaml.safe_load(file)
+            data['testrig_aruco_marker_info'][marker_name] = pose_matrix.tolist()
+            with open(filename, 'w') as file:
+                yaml.dump(data, file)
+        except IOError:
+            print('[Error] Unable to update Nominal Poses file: {0}'.format(filename))
+            self.log('Unable to set Nominal Pose Matrix')
+
     def update_nominal_poses(self):
-        self.log('Updated new Nominal Poses File.')
+        mat = self.get_current_entry_matrix()
+        selected_data_key = self.data_keys[self.radio_var.get()]
+        print(selected_data_key)
+        self.set_nominal_pose_dict(mat, selected_data_key)
+        self.log('Updated "{}" Nominal Pose.'.format(selected_data_key))
 
     def log(self, txt):
         self.LOGGER.configure(text=txt)