Add fake node1. Adjust pcl view angle

include/my_pcl/pcl_visualization.h

@@ -22,10 +22,11 @@ initPointCloudRGBViewer(pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr cloud,
                         );
 
 // set the initial viewing angle (Copied from pcl website)
-void  setViewerPose (pcl::visualization::PCLVisualizer& viewer, const Eigen::Affine3f& viewer_pose);
+void setViewerPose(boost::shared_ptr<visualization::PCLVisualizer> viewer,
+    const Eigen::Affine3f& viewer_pose);
 
 // set the initial viewing angle (Overload by using pos and euler angles)
-void  setViewerPose (pcl::visualization::PCLVisualizer& viewer,
+void setViewerPose(boost::shared_ptr<visualization::PCLVisualizer> viewer,
     double x, double y, double z, double ea_x, double ea_y, double ea_z);
 
 

launch/main_3d_scanner.launch

@@ -3,7 +3,7 @@
     <!-- This launches the 3 main nodes for "Baxter 3D Object Scanner"-->
 
 
-   <!--================================== Parameters ======================================== -->
+   <!--=============================== Parameters ======================================== -->
 
     <!-- Number of poses the Baxter will move to for taking picture -->
     <param name="num_goalposes" type="int" value="6" /> 
@@ -36,22 +36,34 @@
     <param name="topic_n2_to_rviz" value="my/cloud_rotated" /> 
     <param name="topic_n2_to_n3" value="my/cloud_segmented" /> 
 
-   <!--================================== Debug Settings ======================================== -->
 
-    <arg name="run_node1"  default="true" doc="DEBUG: whether run node 1">
-    <!-- <arg name="run_node1_sim"  default="true" doc="Run a fake node 1. All data are read from file. No need for Baxter or Kinect. "> -->
+   <!--=============================== Nodes: setup ============================================== -->
+
+    <!-- enable robot -->
+    <!-- <node name="enable_robot" pkg="baxter_tools" type="enable_robot.py" output="screen" args="-e"/> -->
+
+
+   <!--=============================== Debug Settings ======================================== -->
+
+    <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"/>
 
-   <!--================================== Nodes: setup ============================================== -->
+    <arg name="run_node1_fake"  default="true"
+        doc="Run a fake node 1. All data are read from file. No need for Baxter or Kinect."/>
+
 
-    <!-- enable robot -->
-    <!-- <node name="enable_robot" pkg="baxter_tools" type="enable_robot.py" output="screen" args="-e"/> -->
+   <!--=============================== Run setup nodes =========================================== -->
+    <group if="$(arg run_node1)">
 
-    <!-- trajectory action server -->
-    <node name="joint_trajectory_action_server" pkg="baxter_interface" type="joint_trajectory_action_server.py" args="-m position"/>
+       <!-- trajectory action server -->
+        <node name="joint_trajectory_action_server" pkg="baxter_interface"
+            type="joint_trajectory_action_server.py" args="-m position"
+        />
+
+    </group>
 
-   <!--=============================== Nodes: my main 3 nodes =========================================== -->
+   <!--=============================== Nodes: Main 3 =========================================== -->
 
    <!-- node 1: main node for controlling work flow -->
     <node if="$(arg run_node1)" 
@@ -66,6 +78,12 @@
 
     </node>
 
+    <node if="$(arg run_node1_fake)" 
+        name="n1_fake" pkg="scan3d_by_baxter" type="n1_fake.py" output = "screen">
+    </node>
+
+
+
    <!-- node 2: read cloud from kinect, filter, remove plane, do clustering, pub -->
     <node if="$(arg run_node2)" 
         name="node2"
@@ -114,5 +132,4 @@
     <node type="rviz" name="rviz" pkg="rviz"
        args="-d $(find scan3d_by_baxter)/config/rviz_baxter_and_clouds.rviz" /> 
 
-</launch>
-
+</launch>

launch/scripts_for_debug/rviz_for_debug_baxter.launch

@@ -0,0 +1,3 @@
+<launch>
+  <node type="rviz" name="rviz" pkg="rviz" args="-d $(find scan3d_by_baxter)/config/rviz_debug_baxter.rviz" />
+</launch>

src_cpp/my_pcl/pcl_visualization.cpp

@@ -2,7 +2,6 @@
 #include "my_pcl/pcl_visualization.h"
 #include "my_basics/eigen_funcs.h"
 
-
 using namespace std;
 
 #define DEBUG_RESULT true
@@ -32,23 +31,29 @@ initPointCloudRGBViewer(pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr cloud,
     // Other properties
     viewer->setBackgroundColor(0, 0, 0);
     viewer->initCameraParameters();
+
+    // -- Set viewer angle
+    // double rot_scale = 1.0;
+    // double x = 0.5, y = 0.0, z = 0.0,
+    //        rx = 1.0 * rot_scale, ry = 1.0 * rot_scale, rz = 1.0 * rot_scale;
+    // my_pcl::setViewerPose(viewer, x, y, z, rx, ry, rz);
     return (viewer);
 }
 
-void setViewerPose(pcl::visualization::PCLVisualizer& viewer, const Eigen::Affine3f& viewer_pose)
+void setViewerPose(boost::shared_ptr<visualization::PCLVisualizer> viewer, const Eigen::Affine3f &viewer_pose)
 {
-  Eigen::Vector3f pos_vector = viewer_pose * Eigen::Vector3f(0, 0, 0);
-  Eigen::Vector3f look_at_vector = viewer_pose.rotation () * Eigen::Vector3f(0, 0, 1) + pos_vector;
-  Eigen::Vector3f up_vector = viewer_pose.rotation () * Eigen::Vector3f(0, -1, 0);
-  viewer.setCameraPosition (pos_vector[0], pos_vector[1], pos_vector[2],
-                            look_at_vector[0], look_at_vector[1], look_at_vector[2],
-                            up_vector[0], up_vector[1], up_vector[2]);
+    Eigen::Vector3f pos_vector = viewer_pose * Eigen::Vector3f(0, 0, 0);
+    Eigen::Vector3f look_at_vector = viewer_pose.rotation() * Eigen::Vector3f(0, 0, 1) + pos_vector;
+    Eigen::Vector3f up_vector = viewer_pose.rotation() * Eigen::Vector3f(0, -1, 0);
+    viewer->setCameraPosition(pos_vector[0], pos_vector[1], pos_vector[2],
+                             look_at_vector[0], look_at_vector[1], look_at_vector[2],
+                             up_vector[0], up_vector[1], up_vector[2]);
 }
-void setViewerPose(pcl::visualization::PCLVisualizer& viewer,
-    double x, double y, double z, double rotaxis_x, double rotaxis_y, double rotaxis_z){
-    Eigen::Affine3d T = my_basics::getAffine3d(x, y, z, rotaxis_x, rotaxis_y, rotaxis_z); 
+void setViewerPose(boost::shared_ptr<visualization::PCLVisualizer> viewer,
+                   double x, double y, double z, double rotaxis_x, double rotaxis_y, double rotaxis_z)
+{
+    Eigen::Affine3d T = my_basics::getAffine3d(x, y, z, rotaxis_x, rotaxis_y, rotaxis_z);
     setViewerPose(viewer, T.cast<float>());
 }
 
-
-} // namespace pcl_funcs
+} // namespace my_pcl

src_main/calib_camera_pose/calib_camera_pose.py

@@ -77,11 +77,13 @@ def pubImage(publisher, cv2_image):
         CAMERA_FOR_DEBUG = "usb_cam/image_raw"
         topic_cam_color = CAMERA_FOR_DEBUG
         topic_cam_depth = CAMERA_FOR_DEBUG
+        SLEEP_TIME = 1000
     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", turn_on_traj_action_server=False)
         THE_FRAME_DEPTHCAM_CONNECTED_TO = rospy.get_param("~the_frame_depth_camera_connected_to")
+        SLEEP_TIME = 2000
 
     # -- Set a publisher for visualization chessboard in image
     topic_to_pub_calib_result_image = rospy.get_param("~topic_to_pub_calib_result_image")
@@ -149,7 +151,7 @@ def resize_to_fixed_height(I, rows_goal):
 
         print("\n"+s1+"\n"+s2+"\n"+s3+"\n"+s4)
         # c = waitKeyPress(time_out=1.0) # Why can't I use this in ROS???
-        key = cv2.waitKey(2000)
+        key = cv2.waitKey(SLEEP_TIME)
         c = chr(key)
         print "--- User pressed: ", key, ", ", c, "\n"
 

src_main/n1_fake.py

@@ -0,0 +1,127 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+
+# -------------------------------------------------------------
+# ------------------ Includes
+# -- Standard
+import open3d
+import numpy as np
+import sys, os, cv2
+from copy import copy
+PYTHON_FILE_PATH = os.path.join(os.path.dirname(__file__))+"/"
+
+# -- ROS
+import rospy
+from tf.transformations import euler_from_quaternion, quaternion_from_euler, euler_matrix
+
+# -- My lib
+sys.path.append(PYTHON_FILE_PATH + "../src_python")
+from lib_cloud_conversion_between_Open3D_and_ROS import convertCloudFromOpen3dToRos
+
+# -- 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 scan3d_by_baxter.msg import T4x4
+
+
+# -------------------------------------------------------------
+# ------------------ Variables 
+FILE_FOLDER = PYTHON_FILE_PATH+"../data_debug/from_real/"
+ith_goalpose = 0
+
+# -------------------------------------------------------------
+# ------------------ Functions 
+
+
+def getCloudSize(open3d_cloud):
+    return np.asarray(open3d_cloud.points).shape[0]
+
+
+class ClassPubDepthCamPose(object):
+    def __init__(self):
+
+        # File to read in pose
+        self.openFile()
+
+        # Topic to publish pose
+        topic_endeffector_pos = "my/robot_end_effector_pose"
+        self.pub = rospy.Publisher(topic_endeffector_pos, T4x4, queue_size=10)
+
+    def openFile(self):
+        file_name_camera_pose = "camera_pose.txt"
+        self.fin = open(FILE_FOLDER+file_name_camera_pose, "r")
+
+    def closeFile(self):
+        self.fin.close()
+
+    def readNextPose(self):
+        s_blank = self.fin.readline()
+        s_ithpose = self.fin.readline()
+        T=np.identity(4)
+        for i in range(4):
+            s_vals = self.fin.readline()
+            vals = [float(s) for s in s_vals.split()]
+            for j in range(4):
+                T[i][j]=vals[j]
+        return T
+
+    def pub_pose(self):
+        T = self.readNextPose()
+        # Trans to 1x16 array
+        pose_1x16 = []
+        for i in range(4):
+            for j in range(4):
+                pose_1x16 += [T[i, j]]
+        self.pub.publish(pose_1x16)
+        return
+
+class ClassPubPointClous(object):
+    def __init__(self):
+
+        # File to read in cloud
+        file_name_cloud_src = "src_"
+        self.getFileName = lambda: FILE_FOLDER + file_name_cloud_src+\
+            "{:02d}".format(ith_goalpose)+".pcd"
+
+        # Topic to publish cloud
+        topic_name_rgbd_cloud = "/camera/depth_registered/points"
+        self.pub = rospy.Publisher(
+            topic_name_rgbd_cloud, PointCloud2, queue_size=10)
+
+    def pub_cloud(self):
+        filename = self.getFileName()
+        open3d_cloud = open3d.read_point_cloud(filename)
+        print "node1: sim: load cloud file:\n  " + filename
+        print "  points = " + str(getCloudSize(open3d_cloud))
+
+        ros_cloud = convertCloudFromOpen3dToRos(open3d_cloud)
+        print("node1: sim: publishing cloud "+str(ith_goalpose))
+        self.pub.publish(ros_cloud)
+
+
+# -- Main
+if __name__ == "__main__":
+    rospy.init_node('node1_fake')
+
+    # -- Publish poses
+    pub_poses = ClassPubDepthCamPose()
+
+    # # -- Publishes clouds
+    pub_clouds = ClassPubPointClous()
+
+    # Move Baxter to all goal positions
+    num_goalposes = 6
+    while ith_goalpose < num_goalposes and not rospy.is_shutdown():
+        ith_goalpose += 1
+        print "\n----------------------------------------"
+        print ith_goalpose,"th pose:"
+        pub_poses.pub_pose()
+        pub_clouds.pub_cloud()
+        rospy.sleep(3)
+        if ith_goalpose == num_goalposes:
+            pub_poses.closeFile()
+            pub_poses.openFile()
+            ith_goalpose = 0
+
+    print("!!!!! Node 1 stops.")
+    pub_poses.closeFile()

src_main/n2_filt_and_seg_object.cpp

@@ -163,6 +163,10 @@ int main(int argc, char **argv)
                                         PCL_VIEWER_NAME, PCL_VIEWER_CLOUD_NAME,
                                         viwer_axis_unit_length);
 
+    double rot_scale = 1.0;
+    double x = -0.5, y = 0.0, z = -0.2, rx = 0.0 * rot_scale, ry = 3.14/2 * rot_scale, rz = 0.0 * rot_scale;
+    my_pcl::setViewerPose(viewer, x, y, z, rx, ry, rz);
+
     // Loop, subscribe ros_cloud, and view
     main_loop(viewer, pub_to_node3, pub_to_rviz);
 
@@ -197,7 +201,6 @@ void update_cloud_rotated()
             assert(0);
         if (!nh.getParam("z_grid_size", z_grid_size))
             assert(0);
-
     }
 
     // -- filtByVoxelGrid
@@ -211,7 +214,6 @@ void update_cloud_rotated()
     // -- rotate cloud to Baxter's frame
     for (PointXYZRGB &p : cloud_rotated->points)
         my_basics::preTranslatePoint(T_baxter_to_depthcam, p.x, p.y, p.z);
-
 }
 
 // -----------------------------------------------------
@@ -291,7 +293,7 @@ void update_cloud_segmented()
     }
 
     // -- filtByPassThrough (by range)
-    // PointCloud<PointXYZRGB>::Ptr tmp_cloud(new PointCloud<PointXYZRGB>); 
+    // PointCloud<PointXYZRGB>::Ptr tmp_cloud(new PointCloud<PointXYZRGB>);
     copyPointCloud(*cloud_rotated, *cloud_segmented);
     if (flag_do_range_filt)
     {
@@ -307,7 +309,6 @@ void update_cloud_segmented()
             cloud_segmented, "z", chessboard_z + z_range_up, chessboard_z + z_range_low);
         // cout<<"Debug: after range filter"<<endl;
         // my_pcl::printCloudSize(cloud_segmented);
-
     }
 
     // -- Remove planes

src_main/n3_register_clouds_to_object.py

@@ -85,8 +85,9 @@ def callback(ros_cloud):
 
 
         # Update viewer
-        vis.poll_events()
-        vis.update_renderer()
+        if VIEW_RES_BY_OPEN3D:
+            vis.poll_events()
+            vis.update_renderer()
 
         # Sleep
         rate.sleep()

src_python/lib_baxter.py

@@ -64,7 +64,7 @@ def getGripperPose(self, flag_return_euler=True):
         else:
             return position, quaternion
 
-    def getFramePose(self, frame_name):
+    def getFramePose(self, frame_name): # return 4x4 transformation matrix
         A='/base'
         B=frame_name
         # print A, " -> ", B
@@ -75,7 +75,7 @@ def getFramePose(self, frame_name):
         # print "quaternion:", q_A_to_B
         return T_A_to_B
 
-    def getCameraPose(self):
+    def getCameraPose(self): # return 4x4 transformation matrix
         return self.getFramePose('/'+self.limb_name+'_hand_camera')
 
     def moveToJointAngles(self, goal_angles, time_cost=3.0):