Cam calib has bug

felixchenfy · Jan 31, 2019 · 5747912 · 5747912
1 parent 5ae1c93
commit 5747912
Show file tree

Hide file tree

Showing 5 changed files with 28 additions and 16 deletions.
diff --git a/launch/calib_camera_pose.launch b/launch/calib_camera_pose.launch
@@ -5,7 +5,10 @@
 
 
     <node name="calib_camera_pose" type="calib_camera_pose.py" pkg="scan3d_by_baxter" output = "screen">
-        <param name="DEBUG_MODE" value="true" />     
+
+        <!-- !!! Set to false if not debug !!! -->
+        <param name="DEBUG_MODE" value="false" />     
+        <!-- <param name="DEBUG_MODE" value="true" />      -->
 
         <!-- Input image topics -->
         <param name="topic_baxter_left_hand_camera" value="/cameras/left_hand_camera/image" />     
@@ -22,6 +25,8 @@
         <param name="chessboard_square_size" value="0.02455555"  />
         <param name="chessboard_checker_rows" value="6"  />
         <param name="chessboard_checker_cols" value="8"  />
+        <!-- <param name="chessboard_checker_rows" value="4"  />
+        <param name="chessboard_checker_cols" value="6"  /> -->
 
         <!-- Baxter: which frame is rgb-d camera connected to -->
         <param name="the_frame_depth_camera_connected_to" value="/left_lower_forearm" />

diff --git a/launch/main_3d_scanner.launch b/launch/main_3d_scanner.launch
@@ -45,11 +45,11 @@
 
    <!--=============================== Debug Settings ======================================== -->
 
-    <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_node2"  default="true" doc="DEBUG: whether run node 2"/>
     <arg name="run_node3"  default="true" doc="DEBUG: whether run node 3"/>
 
-    <arg name="run_node1_fake"  default="true"
+    <arg name="run_node1_fake"  default="false"
         doc="Run a fake node 1. All data are read from file. No need for Baxter or Kinect."/>
 
 

diff --git a/launch/open_rgbd_camera.launch b/launch/open_rgbd_camera.launch
@@ -1,18 +1,24 @@
 <launch>
 
-<!-- This is for launching Asus rgbd-camera.
-This is also a replacement of the following operations:
+    <!-- This is for launching Asus rgbd-camera.
+    This is also a replacement of the following operations:
 
-$ roslaunch openni2_launch openni2.launch
+    $ roslaunch openni2_launch openni2.launch
 
-$ rosrun rqt_reconfigure rqt_reconfigure  
-Select camera/driver, set "depth_registration" to true
-(Optional: Set "color_depth_synchronization" to true.)
+    $ rosrun rqt_reconfigure rqt_reconfigure  
+    Select camera/driver, set "depth_registration" to true
+    (Optional: Set "color_depth_synchronization" to true.)
 
- -->
+    -->
 
-<include file="$(find openni2_launch)/launch/openni2.launch">
-    <arg name="depth_registration" default="true" />
-</include>
+    <include file="$(find openni2_launch)/launch/openni2.launch">
+        <arg name="depth_registration" default="true" />
+    </include>
 
+    <!-- <node type="rviz" name="rviz_depth_image" pkg="rviz"
+       args="-d $(find scan3d_by_baxter)/config/rviz_depth_image.rviz" 
+    />  -->
+    <node type="rviz" name="rviz_depth_image" pkg="rviz"
+       args="-d $(find scan3d_by_baxter)/config/rviz_depth_image_2.rviz" 
+    /> 
 </launch>
diff --git a/src_main/calib_camera_pose/calib_camera_pose.py b/src_main/calib_camera_pose/calib_camera_pose.py
@@ -83,7 +83,7 @@ def pubImage(publisher, cv2_image):
         topic_cam_depth = rospy.get_param("~topic_rgbd_camera")
         my_baxter = MyBaxter("left", turn_on_traj_action_server=False)
         THE_FRAME_DEPTHCAM_CONNECTED_TO = rospy.get_param("~the_frame_depth_camera_connected_to")
-        SLEEP_TIME = 2000
+        SLEEP_TIME = 1000
 
     # -- Set a publisher for visualization chessboard in image
     topic_to_pub_calib_result_image = rospy.get_param("~topic_to_pub_calib_result_image")
@@ -121,7 +121,8 @@ def pubImage(publisher, cv2_image):
             I = sub_image(image_topic)
             # rospy.loginfo("Subscribed a image from " + image_topic)
             res, R, p, imgpoints = getChessboardPose(I, K, D, SQUARE_SIZE , CHECKER_ROWS, CHECKER_COLS)
-
+            print ("left " if i==0 else "right")+": chessboard "+("found" if res else "not found")
+
             img_display=I.copy()
             if res==True:
 

diff --git a/src_python/lib_baxter.py b/src_python/lib_baxter.py
@@ -173,7 +173,7 @@ def __init__(self, limb_name):
 
     def add_point(self, positions, time):
         point = JointTrajectoryPoint()
-        point.positions = copy(positions)
+        point.positions = copy.copy(positions)
         point.time_from_start = rospy.Duration(time)
         self._goal.trajectory.points.append(point)