Delete useless debug funcs in node1. Give node2 a buff for storing re…

…ceived point clouds.

doc/bug_list.md

@@ -61,6 +61,10 @@ p_A_to_B, q_A_to_B = self.tf_listener.lookupTransform(
 Besides, it might returns error if the node has just been initialized.
 rospy.sleep(1) for one second and then call this function.
 
+* When starting a node, sleep for 2 seconds!!!
+I run a node, in which it immediately publishes a lot of data. However, the 1st data was never really published out.
+After I set it sleep 2 seconds after init node, the problem was solved.
+
 # ============================================================
 # Bugs related to Hardware
 

include/my_basics/basics.h

@@ -29,6 +29,7 @@ string int2str(int num, int width, char char_to_fill='0');
 vector<int> getIntersection(vector<int> v1, vector<int> v2);
 
 void preTranslatePoint(const float T[4][4], float &x, float &y, float &z);
+void preTranslatePoint(const vector<vector<float>> &T, float &x, float &y, float &z);
 
 void inv(const float T_src[4][4], float T_dst[4][4]);
 

launch/main_3d_scanner.launch

@@ -6,7 +6,7 @@
    <!--=============================== Parameters ======================================== -->
 
     <!-- Number of poses the Baxter will move to for taking picture -->
-    <param name="num_goalposes" type="int" value="11" /> 
+    <param name="num_goalposes" type="int" value="10" /> 
 
     <!-- The topic for receiving PointCloud2 from RgbdCam -->
     <param name="topic_name_rgbd_cloud" value="/camera/depth_registered/points" /> 
@@ -39,15 +39,15 @@
 
    <!--=============================== Debug Settings ======================================== -->
 
-    <arg name="run_node1"  default="true" doc="DEBUG: whether run node 1"/>
-    <!-- <arg name="run_node1"  default="false" doc="DEBUG: whether run node 1"/> -->
+    <!-- <arg name="run_node1"  default="true" doc="DEBUG: whether run node 1"/> -->
+    <arg name="run_node1"  default="false" doc="DEBUG: whether run node 1"/>
     <arg name="run_node2"  default="true" doc="DEBUG: whether run node 2"/>
     <arg name="run_node3"  default="true" doc="DEBUG: whether run node 3"/>
 
-    <!-- Run a fake node 1. All data are read from file. No need for Baxter or Kinect. -->
-    <!-- This should be false, if run_node1==true  -->
-    <arg name="run_node1_fake"  default="false"/>
-    <!-- <arg name="run_node1_fake"  default="true"/> -->
+    <!--    Run a fake node 1. All data are read from file. No need for Baxter or Kinect.
+            This should be false, if run_node1==true  -->
+    <!-- <arg name="run_node1_fake"  default="false"/> -->
+    <arg name="run_node1_fake"  default="true"/>
 
    <!--=============================== Run setup nodes =========================================== -->
     <group if="$(arg run_node1)">
@@ -64,14 +64,6 @@
    <!-- node 1: main node for controlling work flow -->
     <node if="$(arg run_node1)" 
         name="node1" type="n1_move_baxter.py"  pkg="scan3d_by_baxter" output = "screen">
-
-        <!-- DEBUG_MODE: If true, no need to connect to real Baxter or Gazebo. Publish fake poses.  -->
-        <param name="DEBUG_MODE_FOR_BAXTER" value="false" type="bool" />
-
-        <!-- DEBUG_MODE: If true, simulate Kinect point cloud by reading from file -->
-        <param name="DEBUG_MODE_FOR_RGBDCAM" value="false" type="bool" />
-        <!-- <param name="topic_name_rgbd_cloud" value="/if_DEBUG_MODE_is_false_Please_comment_this_line_out" />  -->
-
     </node>
 
     <node if="$(arg run_node1_fake)" 
@@ -87,9 +79,9 @@
         pkg="scan3d_by_baxter" output = "screen">  
 
             <!-- filtering -->
-            <param name="x_grid_size" type="double" value="0.001" />     
-            <param name="y_grid_size" type="double" value="0.001" />     
-            <param name="z_grid_size" type="double" value="0.001" />   
+            <param name="x_grid_size" type="double" value="0.002" />     
+            <param name="y_grid_size" type="double" value="0.002" />     
+            <param name="z_grid_size" type="double" value="0.002" />   
 
             <!-- filtering by range. Centered at the chessboard -->
             <param name="flag_do_range_filt" type="bool" value="true" />     

src_cpp/my_basics/basics.cpp

@@ -38,6 +38,19 @@ void preTranslatePoint(const float T[4][4], float &x, float &y, float &z){
     z=res[2];
 }
 
+void preTranslatePoint(const vector<vector<float>> &T, float &x, float &y, float &z){
+    assert(T.size()==4 && T[0].size()==4);
+    double p[4]={x,y,z,1};
+    double res[3]={0,0,0};
+    for(int row=0;row<3;row++){
+        for(int j=0; j<4; j++)
+            res[row]+=T[row][j]*p[j];
+    }
+    x=res[0];
+    y=res[1];
+    z=res[2];
+}
+
 void inv(const float T_src[4][4], float T_dst[4][4]){
     cv::Mat T_src_ = (cv::Mat_<double>(4,4)<<
      T_src[0][0], T_src[0][1], T_src[0][2], T_src[0][3],

src_main/calib_camera_pose/notes_AsusXtion.md

@@ -21,7 +21,7 @@ $ rviz
 Set fix frame to "camera_rgb_frame" (because I need to use rgb image to do PnP with the chessboard.)  
 Set PointCloud2's topic to: "camera/depth_registered/points"
 
-# Frames
+# Frames and their poses
 
 $ rosrun tf tf_echo /camera_link /camera_rgb_frame
 - Translation: [0.000, -0.045, 0.000]
@@ -58,3 +58,9 @@ At time 0.000
             in RPY (degree) [0.000, -0.000, 0.000]
 
 
+# Hardware
+
+The connection of the wire is very loose on the camera side.
+Push it tight, and use tape to fix it.
+
+

src_main/n1_fake_data_publisher.py

@@ -27,8 +27,9 @@
 
 # -------------------------------------------------------------
 # ------------------ Variables 
-FILE_FOLDER = PYTHON_FILE_PATH+"../data/data/driller_2/"
-ith_goalpose = 0
+FILE_FOLDER = PYTHON_FILE_PATH+"../data/data/driller_1/" # 10 goals
+# FILE_FOLDER = PYTHON_FILE_PATH+"../data/data/bottle_1/" # 11 goals
+
 
 # -------------------------------------------------------------
 # ------------------ Functions 
@@ -68,9 +69,11 @@ def readNextPose(self):
 
     def pub_pose(self):
         T = self.readNextPose()
-        # ------ DEBUG ------
-        # T = T.dot(transXYZ(x=0, y=0, z=0.1))
-        # ------
+
+        # Manual offset
+        T_rgb_to_depth = transXYZ(x=0, y=-0.025, z=0.1) 
+        T = T.dot(T_rgb_to_depth)
+
         # Trans to 1x16 array
         pose_1x16 = []
         for i in range(4):
@@ -99,8 +102,8 @@ def pub_cloud(self):
         print "  points = " + str(getCloudSize(open3d_cloud))
 
         ros_cloud = convertCloudFromOpen3dToRos(open3d_cloud)
-        print("Node 1: sim: publishing cloud "+str(ith_goalpose))
         self.pub.publish(ros_cloud)
+        print("Node 1: sim: publishing cloud "+str(ith_goalpose))
 
 
 # -- Main
@@ -115,18 +118,21 @@ def pub_cloud(self):
 
     # Move Baxter to all goal positions
     num_goalposes = rospy.get_param("num_goalposes")
+    ith_goalpose = 0
+    rospy.sleep(3.0)
     while ith_goalpose < num_goalposes and not rospy.is_shutdown():
         ith_goalpose += 1
         print "\n----------------------------------------"
         print "Node 1: Moving to ", ith_goalpose,"th pose:"
         pub_poses.pub_pose()
         pub_clouds.pub_cloud()
-        rospy.sleep(1)
+        rospy.sleep(1.0)
         if ith_goalpose == num_goalposes:
             pub_poses.closeFile()
             break
             # pub_poses.openFile()
             # ith_goalpose = 0
-
-    print("!!!!! Node 1 stops.")
+
     pub_poses.closeFile()
+    rospy.spin()
+    print("!!!!! Node 1 stops.")

src_main/n1_move_baxter.py

@@ -20,7 +20,6 @@
 
 # -- Message types
 # from std_msgs.msg import Int32  # used for indexing the ith robot pose
-from sensor_msgs.msg import PointCloud2  # for DEBUG_MODE_FOR_BAXTER
 from geometry_msgs.msg import Pose, Point, Quaternion
 from scan3d_by_baxter.msg import T4x4
 
@@ -31,25 +30,15 @@
 
 
 def moveBaxterToJointAngles(joint_angles, time_cost = 3.0):
-    if DEBUG_MODE_FOR_BAXTER:
-        None
-    else:
-        my_Baxter.moveToJointAngles(joint_angles, time_cost)
-    return
+    my_Baxter.moveToJointAngles(joint_angles, time_cost)
 
 
 def readKinectCameraPose():
-    if DEBUG_MODE_FOR_BAXTER:  # Manually define the T4x4 matrix
-        pos = Point(0, 0, 0)
-        quaternion = quaternion_from_euler(0, 0, 0, 'rxyz')
-        T = pose2T(pos, quaternion)
-    else:
-        # (pos, quaternion) = my_Baxter.getFramePose('/left_hand_camera')
-        # (pos, quaternion) = my_Baxter.getFramePose('/left_gripper')
-        T_base_to_arm = my_Baxter.getFramePose('/left_lower_forearm')
-        T_arm_to_depth  # This is read from file
-        T = T_base_to_arm.dot(T_arm_to_depth)
-    return T
+    # (pos, quaternion) = my_Baxter.getFramePose('/left_hand_camera')
+    # (pos, quaternion) = my_Baxter.getFramePose('/left_gripper')
+    T_base_to_arm = my_Baxter.getFramePose('/left_lower_forearm')
+    T_arm_to_depth  # This is read from file
+    T = T_base_to_arm.dot(T_arm_to_depth)
 
 
 # Change int to str and filled prefix with 0s
@@ -88,9 +77,6 @@ def getCloudSize(open3d_cloud):
 # -- Main
 if __name__ == "__main__":
     rospy.init_node('Node 1')
-    DEBUG_MODE_FOR_BAXTER = rospy.get_param("~DEBUG_MODE_FOR_BAXTER")
-    DEBUG_MODE_FOR_RGBDCAM = rospy.get_param("~DEBUG_MODE_FOR_RGBDCAM")
-
     file_folder = rospy.get_param("file_folder")
     file_name_pose = rospy.get_param("file_name_pose")
     file_name_index_width = rospy.get_param("file_name_index_width")
@@ -101,9 +87,8 @@ def getCloudSize(open3d_cloud):
     # T_baxter_to_chess = np.loadtxt(config_folder+"T_baxter_to_chess.txt")
 
     # Set Baxter
-    if not DEBUG_MODE_FOR_BAXTER:
-        my_Baxter = MyBaxter(['left', 'right'][0])
-        my_Baxter.enableBaxter()
+    my_Baxter = MyBaxter(['left', 'right'][0])
+    my_Baxter.enableBaxter()
 
     # Start node when pressing enter
     rospy.loginfo("\n\nWaiting for pressing 'enter' to start ...")
@@ -118,8 +103,10 @@ def getCloudSize(open3d_cloud):
     def publishPose(pose):
         T = pose
 
-        # This seems like existing an offset on cloud in Baxter base frame.
-        T_rgb_to_depth = transXYZ(x=0, y=-0.025, z=0.1) # This is I manually set to amend the bug.
+        # The segmented and rotated object in Chessboard frame is NOT at the center of chessboard, but has a offset.
+        # Maybe I ignored some offset inside /tf. I've double checked but still could find it.
+        # Here I manually set a translation to amend the offset.
+        T_rgb_to_depth = transXYZ(x=0, y=-0.025, z=0.1) 
         T = T.dot(T_rgb_to_depth)
         # Test result:  increase x makes object move along -y direction
         #               increase z makes object move along -z direction
@@ -132,11 +119,11 @@ def publishPose(pose):
                 pose_1x16 += [T[i, j]]
         pub_pose.publish(pose_1x16)
         return
+
     pub_pose = rospy.Publisher(topic_endeffector_pos, T4x4, queue_size=10)
 
     # -- Speicify the goalpose_joint_angles that the Baxter needs to move to
     # 7 numbers of the 7 joint angles
-    NUM_JOINTS = 7
     # base_angles = [-1.0, -0.29145634174346924, 0.021859226748347282, 1.4894953966140747, -3.0480198860168457, -0.9146360158920288, -3.0269274711608887]
     base_angles = [-1.0, 0.0019174759509041905, -0.003451456781476736, 1.2252671718597412, -3.0491702556610107, -1.0227817296981812, -3.0480198860168457]
 
@@ -152,8 +139,8 @@ def addListVal(l0, idx, val):
     assert len(goalposes_joint_angles)>=num_goalposes
 
     # Move Baxter to initial position
-    DEBUG_NOT_MOVE_BAXTER=False
-    if not DEBUG_NOT_MOVE_BAXTER:
+    NOT_MOVE_BAXTER=False
+    if not NOT_MOVE_BAXTER:
         rospy.sleep(1)
         rospy.loginfo("]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]]")
         init_joint_angles = goalposes_joint_angles[0]
@@ -170,7 +157,7 @@ def addListVal(l0, idx, val):
         joint_angles = goalposes_joint_angles[ith_goalpose-1]
 
         # Move robot to the next pose for taking picture
-        if not DEBUG_NOT_MOVE_BAXTER:
+        if not NOT_MOVE_BAXTER:
             rospy.loginfo("--------------------------------")
             rospy.loginfo("Node 1: {}th pos".format(ith_goalpose))
             rospy.loginfo("Node 1: Baxter is moving to pos: "+str(joint_angles))
@@ -186,11 +173,7 @@ def addListVal(l0, idx, val):
         publishPose(pose)
         savePoseToFile(pose, ith_goalpose)
 
-        if DEBUG_MODE_FOR_RGBDCAM:  # simulate publishing a point cloud
-            rospy.sleep(0.01)
-            sim_pub_point_cloud(ith_goalpose)
         rospy.loginfo("--------------------------------")
-
         rospy.sleep(3)
         # if ith_goalpose==num_goalposes: ith_goalpose = 0