Parameterizing the variables in data collection node,Head Data Collection Node Clean up

stretch_camera_testrig/config/testrig_marker_info.yaml

@@ -1,59 +1,94 @@
 # This file contains the nominal poses of the each Aruco markers
 # that is to be used in the test rig in Millimeters. Each aruco marker's 
-# pose(mm)/orientation(rad) are to be measured and inserted carefully.
+# pose(mm)/orientation(rad) are to be measured and inserted as a 4x4 Homogeneous matrix
 
+# 'testrig_data_directory': "/catkin_ws/src/stretch_ros/stretch_camera_testrig/data/"
 'testrig_aruco_marker_info':
-  'base_left':
-    'pose':
-      'x': 0
-      'y': 0
-      'z': 0
-    'orientation':
-      'x': 0
-      'y': 0
-      'z': 0
-      'w': 0
+  'base_left_marker_pose':
+    - - 0.0
+      - 0.0
+      - 0.0
+      - 0.0
+    - - 0.0
+      - 0.0
+      - 0.0
+      - 0.0
+    - - 0.0
+      - 0.0
+      - 0.0
+      - 0.0
+    - - 0.0
+      - 0.0
+      - 0.0
+      - 1.0    
+  'base_right_marker_pose':
+    - - 0.0
+      - 0.0
+      - 0.0
+      - 0.0
+    - - 0.0
+      - 0.0
+      - 0.0
+      - 0.0
+    - - 0.0
+      - 0.0
+      - 0.0
+      - 0.0
+    - - 0.0
+      - 0.0
+      - 0.0
+      - 1.0
 
-  'base_right':
-    'pose':
-      'x': 0
-      'y': 0
-      'z': 0
-    'orientation':
-      'x': 0
-      'y': 0
-      'z': 0
-      'w': 0
+  'shoulder_marker_pose':
+    - - 0.0
+      - 0.0
+      - 0.0
+      - 0.0
+    - - 0.0
+      - 0.0
+      - 0.0
+      - 0.0
+    - - 0.0
+      - 0.0
+      - 0.0
+      - 0.0
+    - - 0.0
+      - 0.0
+      - 0.0
+      - 1.0
 
-  'shoulder_top':
-    'pose':
-      'x': 0
-      'y': 0
-      'z': 0
-    'orientation':
-      'x': 0
-      'y': 0
-      'z': 0
-      'w': 0
+  'wrist_inside_marker_pose':
+    - - 0.0
+      - 0.0
+      - 0.0
+      - 0.0
+    - - 0.0
+      - 0.0
+      - 0.0
+      - 0.0
+    - - 0.0
+      - 0.0
+      - 0.0
+      - 0.0
+    - - 0.0
+      - 0.0
+      - 0.0
+      - 1.0
 
-  'wrist_top':
-    'pose':
-      'x': 0
-      'y': 0
-      'z': 0
-    'orientation':
-      'x': 0
-      'y': 0
-      'z': 0
-      'w': 0
-
-  'wrist_inside':
-    'pose':
-      'x': 0
-      'y': 0
-      'z': 0
-    'orientation':
-      'x': 0
-      'y': 0
-      'z': 0
-      'w': 0
+  'wrist_top_marker_pose':
+    - - 0.0
+      - 0.0
+      - 0.0
+      - 0.0
+    - - 0.0
+      - 0.0
+      - 0.0
+      - 0.0
+    - - 0.0
+      - 0.0
+      - 0.0
+      - 0.0
+    - - 0.0
+      - 0.0
+      - 0.0
+      - 1.0

stretch_camera_testrig/launch/test_basic.launch

@@ -1,33 +0,0 @@
-<launch>
-
-  <!-- REALSENSE D435i -->
-  <include file="$(find stretch_core)/launch/d435i_basic.launch" pass_all_args="true">
-    <!--
-	HIGHEST RESOLUTION, but also has the highest minimum depth
-	(280mm Min-Z) below which objects generate bad noise, such as
-	when the arm and gripper are raised close to the camera.
-    -->
-    <arg name="depth_width" value="1280"/>
-    <arg name="depth_height" value="720"/>
-    <arg name="color_width" value="1280"/>
-    <arg name="color_height" value="720"/>
-  </include> 
-
-  <!-- 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" />
-
-  <!-- RVIZ Visualization -->
-  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find stretch_camera_testrig)/rviz/rviz.rviz"/>
-
-  <!-- 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"/>
-
-  <!-- ARUCO Marker Actual Poses in Testrig -->
-  <rosparam command="load" file="$(find stretch_camera_testrig)/config/testrig_marker_info.yaml" />
-
-  <!-- Test Rig Node -->
-  <!-- <node name="test_rig_node" pkg="stretch_camera_testrig" type="test_basic.py" output="screen"/> -->
-  
-</launch>

stretch_camera_testrig/launch/testrig_collect_data.launch

@@ -0,0 +1,35 @@
+<launch>
+
+  <arg name="rviz" default="false"/>
+  <arg name="samples" default="600"/>
+  <arg name="testrig_data_directory" default="$(find stretch_camera_testrig)/data/"/>
+
+  <!-- ARUCO Marker Actual Poses in Testrig -->
+  <rosparam command="load" file="$(find stretch_camera_testrig)/config/testrig_marker_info.yaml" />
+
+  <!-- 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" />
+
+  <!-- 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>
+
+  <!-- Main Test Rig Node -->
+  <node name="test_rig_node" pkg="stretch_camera_testrig" type="testrig_collect_data" output="screen">
+    <param name="testrig_data_directory" type="string" value="$(arg testrig_data_directory)"/>
+    <param name="samples" type="int" value="$(arg samples)"/>
+  </node>
+  
+</launch>

stretch_camera_testrig/nodes/collect_head_calibration_data.py

@@ -25,6 +25,7 @@ import math
 import time
 import threading
 import sys
+import os
 
 import argparse as ap
 
@@ -270,15 +271,13 @@ class CollectHeadCalibrationDataNode:
         return samples
 
     def get_samples_testrig(self, number_of_samples):
-        # Collect N observations (samples) centered around the
-        # provided head and wrist extension pose. If first_move is
-        # true, it indicates that this is the first movement to a new
-        # region for calibration, which can induce more vibrations and
-        # thus benefit from more time to settle prior to collecting a
-        # sample.
+        # Collect N observations (samples) within the
+        # testrig rigid setup. This function allows to
+        # collect samples in the same data dictionary format 
+        # used for calibration without any robot motion.
         
         samples = []
-        wait_before_each_sample_s = 0.2
+        wait_before_each_sample_s = 0.1
         for sample_number in range(number_of_samples):
             rospy.sleep(wait_before_each_sample_s)
             observation = {'joints': {'joint_head_pan': None,
@@ -298,37 +297,6 @@ class CollectHeadCalibrationDataNode:
                                                     'shoulder_marker_pose': None}
                             }
 
-            # wait for a sample taken after the robot has
-            # settled
-            # timeout_duration = rospy.Duration(secs=10)
-            # start_wait = rospy.Time.now()
-            # timeout = False
-            # data_ready = False
-            # while (not data_ready) and (not timeout):
-            #     # check the timing of the current sample
-            #     with self.data_lock:
-            #         if (self.data_time is not None) and (self.data_time > settled_time):
-            #             if (self.marker_time is None):
-            #                 # joint_states and acceleration
-            #                 # were taken after the robot
-            #                 # settled and there are no aruco
-            #                 # marker poses
-            #                 data_ready = True
-            #             elif (self.marker_time > settled_time):
-            #                 # joint_states, acceleration,
-            #                 # and aruco marker poses were
-            #                 # taken after the robot
-            #                 # settled
-            #                 data_ready = True
-            #     if not data_ready:
-            #         #rospy.sleep(0.2) #0.1
-            #         rospy.sleep(1.0)
-            #     timeout = (rospy.Time.now() - start_wait) > timeout_duration
-
-            # if timeout:
-            #     rospy.logerr('collect_head_calibration_data get_samples: timeout while waiting for sample.')
-            #     raise Exception('Timed out waiting for joint_states/accelerations/markers messages after 10 seconds')
-
             with self.data_lock:
 
                 # record the settled camera measurments
@@ -671,8 +639,10 @@ class CollectHeadCalibrationDataNode:
         rospy.loginfo('COLLECT DATA FROM TEST RIG')
         rospy.loginfo('*************************************')
         rospy.loginfo('')
+        rospy.loginfo('Target Number of Frames :{0}'.format(number_of_samples))
+        rospy.loginfo('')
 
-        while len(calibration_data)<=number_of_samples:
+        while len(calibration_data)<number_of_samples:
             observation = self.get_samples_testrig(1)
             first_pan_tilt = False
             calibration_data.extend(observation)
@@ -728,8 +698,13 @@ class CollectHeadCalibrationDataNode:
         rospy.loginfo('Move to the calibration start pose.')
         self.move_to_pose(initial_pose)
 
-    def main(self, collect_check_data):
-        rospy.init_node('collect_head_calibration_data')
+    def main(self, collect_check_data=None):
+        if not self.test_rig:
+            rospy.init_node('collect_head_calibration_data')
+            print('yep!')
+        else:
+            rospy.init_node('collect_testrig_data')
+
         self.node_name = rospy.get_name()        
         rospy.loginfo("{0} started".format(self.node_name))
 
@@ -797,23 +772,11 @@ class CollectHeadCalibrationDataNode:
             self.synchronizer = message_filters.ApproximateTimeSynchronizer([accel_subscriber, aruco_subscriber], 10, slop_time, allow_headerless=True)
             self.synchronizer.registerCallback(self.calibration_data_callback)
 
-
             rate = rospy.Rate(self.rate)
-            self.testrig_data_path = '/home/hello-robot/catkin_ws/src/stretch_ros/stretch_camera_testrig/data/' #Change it
+            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)
-    def get_testrig_metrics(self):
-        return None
+
         
-if __name__ == '__main__':    
-    parser = ap.ArgumentParser(description='Collect head calibration data.')
-    parser.add_argument('--check', action='store_true', help='Collect data to check the current calibration, instead of data to perform a new calibration.')
-    
-    args, unknown = parser.parse_known_args()
-    collect_check_data = args.check
-     
-    try:
-        node = CollectHeadCalibrationDataNode()
-        node.main(collect_check_data)
-    except rospy.ROSInterruptException:
-        pass
+
 

stretch_camera_testrig/nodes/test_basic.py

@@ -1,38 +0,0 @@
-#!/usr/bin/env python
-
-# Goal Aruco Marker Detection subscribe
-import rospy
-import actionlib
-import math
-import time
-import threading
-import sys
-import argparse as ap
-import numpy as np
-import ros_numpy
-import yaml
-import time
-
-from sensor_msgs.msg import JointState
-from geometry_msgs.msg import Twist
-from nav_msgs.msg import Odometry
-from std_msgs.msg import String
-from visualization_msgs.msg import Marker
-from visualization_msgs.msg import MarkerArray
-
-from collect_head_calibration_data import CollectHeadCalibrationDataNode
-
-
-if __name__ == '__main__':    
-    parser = ap.ArgumentParser(description='Collect Test Rig data.')
-    parser.add_argument('--check', action='store_true', help='Collect data to check the current calibration, instead of data to perform a new calibration.')
-    
-    args, unknown = parser.parse_known_args()
-    collect_check_data = args.check
-     
-    try:
-        node = CollectHeadCalibrationDataNode(test_rig=True)
-        node.number_of_testrig_samples = 500
-        node.main(collect_check_data)
-    except rospy.ROSInterruptException:
-        pass

stretch_camera_testrig/nodes/testrig_collect_data

@@ -0,0 +1,13 @@
+#!/usr/bin/env python
+
+from collect_head_calibration_data import CollectHeadCalibrationDataNode
+import rospy
+
+
+if __name__ == '__main__':
+    rospy.sleep(15) # Wait until Aruco Detector stabilizes
+    try:
+        node = CollectHeadCalibrationDataNode(test_rig=True)
+        node.main()
+    except rospy.ROSInterruptException:
+        pass