Testing saving calib result to file

launch/calib_camera_pose.launch

@@ -1,19 +1,27 @@
 <launch>
 
 
-    <arg name="topic_pub_images_with_chessboard"  default="my/image_chessboard_pose" doc=""/>
+    <arg name="debug_topic_to_pub_image_result"  default="my/image_chessboard_pose" doc=""/>
 
 
     <node name="calib_camera_pose" type="calib_camera_pose.py" pkg="scan3d_by_baxter" output = "screen">
         <!-- <param name="topic_baxter_left_hand_camera" value="cameras/left_hand_camera/image" />     
         <param name="topic_rgbd_camera" value="XXX RGBD CAMERA'S COLOR TOPIC XXX" />      -->
-        <param name="topic_pub_images_with_chessboard" value="$(arg topic_pub_images_with_chessboard)" />
+        <param name="debug_topic_to_pub_image_result" value="$(arg debug_topic_to_pub_image_result)" />
+        <param name="the_frame_depth_camera_connected_to" value="/left_lower_forearm" />
+
+
+        <param name="file_folder" value="$(find scan3d_by_baxter)/config/" /> 
+        <param name="file_name_T_baxter_to_chess" value="T_baxter_to_chess.txt"  />
+        <param name="file_name_T_arm_to_depth" value="T_arm_to_depth.txt"  />
+
     </node>
 
-    <!-- Display result in rqt_image_view --> -->
+
+    <!-- Display result in rqt_image_view -->
     <!-- rosrun rqt_image_view rqt_image_view image:=/my/image_chessboard_pese -->
     <node name="rqt_image_view" type="rqt_image_view" pkg="rqt_image_view"
-        output = "screen" args="image:=/$(arg topic_pub_images_with_chessboard)">
+        output = "screen" args="image:=/$(arg debug_topic_to_pub_image_result)">
     </node>
 
 

src_main/calib_camera_pose/calib_camera_pose.py

@@ -35,6 +35,7 @@
 from lib_geo_trans_ros import form_T, quaternion_to_R, toRosPose, pose2T
 from lib_cam_calib import *
 from lib_baxter import MyBaxter
+from lib_keyboard_input import waitKeyPress
 
 getStrBeforeLastChar = lambda s, c: s[:(-s[::-1].find(c))]
 
@@ -61,18 +62,29 @@ def pubImage(publisher, cv2_image):
 # -- Main
 if __name__ == "__main__":
     rospy.init_node("calib_camera_pose")
+    # TWO THINGS WE WANT!:
+    T_baxter_to_chess = None
+    T_arm_to_depth = None
+    folder = rospy.get_param("file_folder")
+    file_name1 = folder+rospy.get_param("file_name_T_baxter_to_chess")
+    file_name2 = folder+rospy.get_param("file_name_T_arm_to_depth")
 
     # -- Get the two cameras topic.
     # -- Note: Both cameras are using color image for locating chessboard!!!
-    CAMERA_FOR_DEBUG = "usb_cam/image_raw"
-    topic_cam_color = rospy.get_param("topic_baxter_left_hand_camera",
-        default=CAMERA_FOR_DEBUG)
-    topic_cam_depth = rospy.get_param("topic_rgbd_camera",
-        default=CAMERA_FOR_DEBUG)
+    DEBUG_MODE = True
+    if DEBUG_MODE: # Use my usb cam to debug
+        CAMERA_FOR_DEBUG = "usb_cam/image_raw"
+        topic_cam_color = CAMERA_FOR_DEBUG
+        topic_cam_depth = CAMERA_FOR_DEBUG
+    else:
+        topic_cam_color = rospy.get_param("topic_baxter_left_hand_camera")
+        topic_cam_depth = rospy.get_param("topic_rgbd_camera")
+        my_baxter = MyBaxter("left")
+        THE_FRAME_DEPTHCAM_CONNECTED_TO = rospy.get_param("the_frame_depth_camera_connected_to")
 
     # -- Set a publisher for visualization chessboard in image
-    topic_pub_images_with_chessboard = rospy.get_param("~topic_pub_images_with_chessboard")
-    pub = rospy.Publisher(topic_pub_images_with_chessboard, Image, queue_size=5)
+    debug_topic_to_pub_image_result = rospy.get_param("~debug_topic_to_pub_image_result")
+    pub = rospy.Publisher(debug_topic_to_pub_image_result, Image, queue_size=5)
 
     # -- Set params 
     # Camera info
@@ -95,11 +107,12 @@ def pubImage(publisher, cv2_image):
         Rs=[]
         ps=[]
         rospy.loginfo("-------------------------------------------")
+
         # -- Subscribe to image and locate chessboard
         for i in range(2):
             image_topic, K, D = cameras[i][0],cameras[i][1],cameras[i][2]
             I = sub_image(image_topic)
-            rospy.loginfo("Subscribed a image from " + image_topic)
+            # rospy.loginfo("Subscribed a image from " + image_topic)
             res, R, p, imgpoints = getChessboardPose(I, K, D, SQUARE_SIZE , CHECKER_ROWS, CHECKER_COLS)
 
             # Draw to image
@@ -117,10 +130,46 @@ def pubImage(publisher, cv2_image):
         # -- Display result
         I_disp = np.hstack([Is_disp[0], Is_disp[1]])
         pubImage(pub, I_disp)
-        rospy.loginfo("Published a new image with chessboard result")
+        # rospy.loginfo("Published a new image with chessboard result")
 
         # -- Wait for user keypress to calib poses
-
+        s1 = "Press key to compute transformations between Baxter_base, Baxter_forearm, Baxter_color_camera, depth_camera, and chessboard:"
+        s2 = "Press a: compute T_Base_to_Chess ( from T_ColorCam_to_Chess )"
+        s3 = "Press b: compute T_Arm_to_DepthCam ( from T_DepthCam_to_Chess )"
+        print("\n"+s1+"\n"+s2+"\n"+s3)
+        c = waitKeyPress(time_out=1.0)
+        print("\n")
+
+        if (c=='a'): # Return T_baxter_to_chess
+            T_color_to_chess=form_T(Rs[0],ps[0])
+            if DEBUG_MODE:
+                T_baxter_to_color = np.identity(4)
+            else:
+                pos, quaternion = my_baxter.getCameraPose()
+                T_baxter_to_color = pose2T(pos, quaternion)
+            T_baxter_to_chess = T_baxter_to_color.dot(T_color_to_chess)
+            print "Get/Save T_baxter_to_chess:\n", T_baxter_to_chess
+            np.savetxt(file_name1, T_baxter_to_chess, delimiter=" ")
+
+        elif(c=='b'): # Return T_arm_to_depth
+            if T_baxter_to_chess is None:
+                print("Please press a to compute T_baxter_to_chess first!")
+            else:
+                T_depth_to_chess=form_T(Rs[1],ps[1]) # from image processing
+                if DEBUG_MODE:
+                    T_baxter_to_arm = np.identity(4)
+                else:
+                    pos, quaternion = my_baxter.getFramePose(THE_FRAME_DEPTHCAM_CONNECTED_TO)
+                    T_baxter_to_arm= pose2T(pos, quaternion)
+
+                # Combine the above transformations
+                T_chess_to_depth = np.linalg.inv(T_depth_to_chess)
+                T_arm_to_baxter = np.linalg.inv(T_baxter_to_arm)
+
+                T_arm_to_depth = T_arm_to_baxter.dot(T_baxter_to_chess).dot(T_chess_to_depth)
+                print "Get/Save T_arm_to_depth:\n", T_arm_to_depth
+                np.savetxt(file_name2, T_arm_to_depth, delimiter=" ")
+
         rospy.sleep(0.05)
 
     rospy.spin()

src_python/lib_commons.py

@@ -5,4 +5,10 @@
 def list2str(vals):
     str_val = ["{:.3f} ".format(val) for val in vals]
     str_val = ", ".join(str_val)
-    return str_val
+    return str_val
+
+def savetxt(filename, data):
+    np.savetxt(filename, data, delimiter=",")
+
+def loadtxt(filename):
+    return np.loadtxt(filename, delimiter=",")

src_python/lib_keyboard_input.py

@@ -3,10 +3,10 @@
 
 ''' 4 functions for reading input from keyboard
 {
-    getRawInput
-    waitRawInput
-    getKeyPress
-    waitKeyPress
+    getRawInput(), return string
+    waitRawInput(time_out), return string or None
+    getKeyPress(), return char
+    waitKeyPress(time_out), return char or None
 }
 Tested on Ubuntu
 '''
@@ -83,6 +83,6 @@ def _getKeyPress(key):
 if __name__=="__main__":
 
     c = waitKeyPress(time_out=2.0)
-    print c
+    print "\nDetected keypress: ", c
 
     None