Added Kinect2 and tested on hardware

examples/kinect2/README.md

@@ -0,0 +1,18 @@
+# Exmaples for using PyRobot to use Kinect2
+
+## Getting started
+
+Make sure your system has [IAI Kinect2](https://github.com/code-iai/iai_kinect2) installed. If not, please follow the [install steps](https://github.com/code-iai/iai_kinect2#install) to setup KinectV2.
+
+## Getting started
+1. Launch Kinect2 camera
+```bash
+cd ~/catkin_ws # or the appropriate catkin workspace in which IAI Kinect2 package is in
+source devel/setup.bash
+roslaunch kinect2_bridge kinect2_bridge.launch
+```
+
+2. Run Pyrobot examples in a new terminal
+```bash
+python camera_image.py
+```

examples/kinect2/camera_image.py

@@ -0,0 +1,29 @@
+# Copyright (c) Facebook, Inc. and its affiliates.
+
+# This source code is licensed under the MIT license found in the
+# LICENSE file in the root directory of this source tree.
+
+"""
+Example to show how to read images from the camera. 
+"""
+from __future__ import print_function
+from IPython import embed
+
+import sys
+ros_path = '/opt/ros/kinetic/lib/python2.7/dist-packages'
+if ros_path in sys.path:
+    sys.path.remove(ros_path)
+    import cv2
+sys.path.append('/opt/ros/kinetic/lib/python2.7/dist-packages')
+
+import numpy as np
+from pyrobot import Robot
+
+if __name__ == "__main__":
+    bot = Robot('kinect2')
+    rgb, depth = bot.camera.get_rgb_depth()
+    actual_depth_values = depth.astype(np.float64) / bot.camera.DepthMapFactor
+    cv2.imshow('Color', rgb[:, :, ::-1])
+    cv2.imshow('Depth', depth)
+    print("Press any Key to Exit")
+    cv2.waitKey(0)

examples/kinect2/vis_point_cloud.py

@@ -0,0 +1,55 @@
+# Copyright (c) Facebook, Inc. and its affiliates.
+
+# This source code is licensed under the MIT license found in the
+# LICENSE file in the root directory of this source tree.
+
+from __future__ import print_function
+
+import signal
+import sys
+import time
+
+import open3d
+from pyrobot import Robot
+
+
+def signal_handler(sig, frame):
+    print('Exit')
+    sys.exit(0)
+
+
+signal.signal(signal.SIGINT, signal_handler)
+
+
+def main():
+    bot = Robot('kinect2')
+    vis = open3d.Visualizer()
+    vis.create_window("3D Map")
+    pcd = open3d.PointCloud()
+    coord = open3d.create_mesh_coordinate_frame(1, [0, 0, 0])
+    vis.add_geometry(pcd)
+    vis.add_geometry(coord)
+    # We update the viewer every a few seconds
+    update_time = 4
+    while True:
+        start_time = time.time()
+        pts, colors = bot.camera.get_current_pcd()
+        pcd.clear()
+        # note that open3d.Vector3dVector is slow
+        pcd.points = open3d.Vector3dVector(pts)
+        pcd.colors = open3d.Vector3dVector(colors / 255.0)
+        vis.update_geometry()
+        vis.poll_events()
+        vis.update_renderer()
+        s_t = time.time()
+        while time.time() - s_t < update_time:
+            vis.poll_events()
+            vis.update_renderer()
+            time.sleep(0.1)
+        end_time = time.time()
+        process_time = end_time - start_time
+        print("Updating every {0:.2f}s".format(process_time))
+
+
+if __name__ == "__main__":
+    main()

robots/kinect2/README.md

@@ -0,0 +1,15 @@
+# Exmaples for using PyRobot to use Kinect2
+
+## Getting started
+
+Make sure your system has [IAI Kinect2](https://github.com/code-iai/iai_kinect2) installed. If not, please follow the [install steps](https://github.com/code-iai/iai_kinect2#install) to setup KinectV2.
+
+## Getting started
+1. Launch Kinect2 camera
+```bash
+cd ~/catkin_ws # or the appropriate catkin workspace in which IAI Kinect2 package is in
+source devel~/setuo.bash
+roslaunch kinect2_bridge kinect2_bridge.launch
+```
+
+2. Run Pyrobot examples in a new terminal

src/pyrobot/cfg/kinect2_config.py

@@ -0,0 +1,29 @@
+# Copyright (c) Facebook, Inc. and its affiliates.
+
+# This source code is licensed under the MIT license found in the
+# LICENSE file in the root directory of this source tree.
+
+from pyrobot.cfg.config import get_cfg_defaults
+
+_C = get_cfg_defaults()
+
+_C.HAS_ARM = False
+_C.HAS_BASE = False
+_C.HAS_GRIPPER = False
+
+_CAMERAC = _C.CAMERA
+# CAMERA class name
+_CAMERAC.CLASS = 'Kinect2Camera'
+# topic name of the camera info
+_CAMERAC.ROSTOPIC_CAMERA_INFO_STREAM = '/kinect2/qhd/camera_info'
+# TOD0: Make sure the topic names are right
+# topic name of the RGB images
+_CAMERAC.ROSTOPIC_CAMERA_RGB_STREAM = '/kinect2/qhd/image_color_rect'
+# topic name of the depth images
+_CAMERAC.ROSTOPIC_CAMERA_DEPTH_STREAM = '/kinect2/qhd/image_depth_rect'
+# depth map factor
+_CAMERAC.DEPTH_MAP_FACTOR = 1000.0
+
+
+def get_cfg():
+    return _C.clone()

src/pyrobot/core.py

@@ -17,13 +17,16 @@
 import rospy
 import tf
 from geometry_msgs.msg import Twist, Pose, PoseStamped
-from sensor_msgs.msg import JointState
+from sensor_msgs.msg import JointState, CameraInfo, Image
 
 import pyrobot.utils.util as prutil
 
 from pyrobot.utils.move_group_interface import MoveGroupInterface as MoveGroup
 from pyrobot_bridge.srv import *
 
+from cv_bridge import CvBridge, CvBridgeError
+import message_filters
+
 class Robot:
     """
     This is the main interface class that is composed of
@@ -303,10 +306,90 @@ def __init__(self, configs):
         :type configs: YACS CfgNode
         """
         self.configs = configs
+        self.cv_bridge = CvBridge()
+        self.camera_info_lock = threading.RLock()
+        self.camera_img_lock = threading.RLock()
+        self.rgb_img = None
+        self.depth_img = None
+        self.camera_info = None
+        self.camera_P = None
+        rospy.Subscriber(self.configs.CAMERA.ROSTOPIC_CAMERA_INFO_STREAM,
+                         CameraInfo,
+                         self._camera_info_callback)
+
+        rgb_topic = self.configs.CAMERA.ROSTOPIC_CAMERA_RGB_STREAM
+        self.rgb_sub = message_filters.Subscriber(rgb_topic, Image)
+        depth_topic = self.configs.CAMERA.ROSTOPIC_CAMERA_DEPTH_STREAM
+        self.depth_sub = message_filters.Subscriber(depth_topic, Image)
+        img_subs = [self.rgb_sub, self.depth_sub]
+        self.sync = message_filters.ApproximateTimeSynchronizer(img_subs,
+                                                                queue_size=10,
+                                                                slop=0.2)
+        self.sync.registerCallback(self._sync_callback)
+
+    def _sync_callback(self, rgb, depth):
+        self.camera_img_lock.acquire()
+        try:
+            self.rgb_img = self.cv_bridge.imgmsg_to_cv2(rgb, "bgr8")
+            self.rgb_img = self.rgb_img[:, :, ::-1]
+            self.depth_img = self.cv_bridge.imgmsg_to_cv2(depth, "passthrough")
+        except CvBridgeError as e:
+            rospy.logerr(e)
+        self.camera_img_lock.release()
+
+    def _camera_info_callback(self, msg):
+        self.camera_info_lock.acquire()
+        self.camera_info = msg
+        self.camera_P = np.array(msg.P).reshape((3, 4))
+        self.camera_info_lock.release()
+
+    def get_rgb(self):
+        '''
+        This function returns the RGB image perceived by the camera.
+
+        :rtype: np.ndarray or None
+        '''
+        self.camera_img_lock.acquire()
+        rgb = copy.deepcopy(self.rgb_img)
+        self.camera_img_lock.release()
+        return rgb
+
+    def get_depth(self):
+        '''
+        This function returns the depth image perceived by the camera.
+
+        :rtype: np.ndarray or None
+        '''
+        self.camera_img_lock.acquire()
+        depth = copy.deepcopy(self.depth_img)
+        self.camera_img_lock.release()
+        return depth
+
+    def get_rgb_depth(self):
+        '''
+        This function returns both the RGB and depth
+        images perceived by the camera.
+
+        :rtype: np.ndarray or None
+        '''
+        self.camera_img_lock.acquire()
+        rgb = copy.deepcopy(self.rgb_img)
+        depth = copy.deepcopy(self.depth_img)
+        self.camera_img_lock.release()
+        return rgb, depth
+
+    def get_intrinsics(self):
+        """
+        This function returns the camera intrinsics.
 
-    @abstractmethod
-    def get_rgb(self, **kwargs):
-        pass
+        :rtype: np.ndarray
+        """
+        if self.camera_P is None:
+            return self.camera_P
+        self.camera_info_lock.acquire()
+        P = copy.deepcopy(self.camera_P)
+        self.camera_info_lock.release()
+        return P[:3, :3]
 
 
 class Arm(object):

src/pyrobot/kinect2/__init__.py

@@ -0,0 +1,4 @@
+# Copyright (c) Facebook, Inc. and its affiliates.
+
+# This source code is licensed under the MIT license found in the
+# LICENSE file in the root directory of this source tree.

src/pyrobot/kinect2/camera.py

@@ -0,0 +1,120 @@
+# Copyright (c) Facebook, Inc. and its affiliates.
+
+# This source code is licensed under the MIT license found in the
+# LICENSE file in the root directory of this source tree.
+
+import os
+import rospkg
+import threading
+import yaml
+from copy import deepcopy
+
+import message_filters
+import numpy as np
+import pyrobot.utils.util as prutil
+import rospy
+
+from pyrobot.core import Camera
+from sensor_msgs.msg import CameraInfo
+from sensor_msgs.msg import Image
+from std_msgs.msg import Float64
+
+import sys
+ros_path = '/opt/ros/kinetic/lib/python2.7/dist-packages'
+if ros_path in sys.path:
+    sys.path.remove(ros_path)
+    import cv2
+sys.path.append(ros_path)
+from cv_bridge import CvBridge, CvBridgeError
+
+class Kinect2Camera(Camera):
+    """
+    This is camera class that interfaces with the KinectV2 camera
+    """
+
+    def __init__(self, configs):
+        """
+        Constructor of the KinectV2Camera class.
+
+        :param configs: Camera specific configuration object
+
+        :type configs: YACS CfgNode
+        """
+        super(Kinect2Camera, self).__init__(configs=configs)
+        self.DepthMapFactor = float(self.configs.CAMERA.DEPTH_MAP_FACTOR)
+        self.intrinsic_mat = None
+
+    def get_current_pcd(self):
+        """
+        Return the point cloud at current time step (one frame only)
+
+        :returns: tuple (pts, colors)
+
+                  pts: point coordinates in camera frame (shape: :math:`[N, 3]`)
+
+                  colors: rgb values for pts_in_cam (shape: :math:`[N, 3]`)
+        :rtype: tuple(np.ndarray, np.ndarray)
+        """
+        rgb_im, depth_im = self.get_rgb_depth()
+        depth = depth_im.reshape(-1) / self.DepthMapFactor
+        rgb = rgb_im.reshape(-1, 3)
+        if self.intrinsic_mat is None:
+            self.intrinsic_mat = self.get_intrinsics()
+            self.intrinsic_mat_inv = np.linalg.inv(self.intrinsic_mat)
+            #TODO: image height --> rgb_im.shape[0] and width--> rgb_im.shape[1]
+            img_pixs = np.mgrid[0: rgb_im.shape[0]: 1,
+                    0: rgb_im.shape[1]: 1]
+            img_pixs = img_pixs.reshape(2, -1)
+            img_pixs[[0, 1], :] = img_pixs[[1, 0], :]
+            self.uv_one = np.concatenate((img_pixs,
+                                        np.ones((1, img_pixs.shape[1]))))
+            self.uv_one_in_cam = np.dot(self.intrinsic_mat_inv, self.uv_one)
+
+        pts_in_cam = np.multiply(self.uv_one_in_cam, depth)
+        pts_in_cam = np.concatenate((pts_in_cam,
+                                     np.ones((1, pts_in_cam.shape[1]))),
+                                    axis=0)
+        pts = pts_in_cam[:3, :].T
+        return pts, rgb
+
+    def pix_to_3dpt(self, rs, cs, reduce='none', k=5):
+        """
+        Get the 3D points of the pixels in RGB images.
+
+        :param rs: rows of interest in the RGB image.
+                   It can be a list or 1D numpy array
+                   which contains the row indices.
+                   The default value is None,
+                   which means all rows.
+        :param cs: columns of interest in the RGB image.
+                   It can be a list or 1D numpy array
+                   which contains the column indices.
+                   The default value is None,
+                   which means all columns.
+        :param reduce: whether to consider the depth at nearby pixels
+                    'none': no neighbour consideration
+                    'mean': depth based on the mean of kernel sized k  centered at [rs,cs] 
+                    'max': depth based on the max of kernel sized k  centered at [rs,cs] 
+                    'min': depth based on the min of kernel sized k  centered at [rs,cs] 
+        :param k: kernel size for reduce type['mean', 'max', 'min']
+        
+        :type rs: list or np.ndarray
+        :type cs: list or np.ndarray
+        :type reduce: str
+        :tyep k: int
+
+        :returns: tuple (pts, colors)
+
+                  pts: point coordinates in world frame
+                  (shape: :math:`[N, 3]`)
+
+                  colors: rgb values for pts_in_cam
+                  (shape: :math:`[N, 3]`)
+
+        :rtype: tuple(np.ndarray, np.ndarray)
+        """
+        rgb_im, depth_im = self.get_rgb_depth()
+        pts_in_cam = prutil.pix_to_3dpt(depth_im, rs, cs, self.get_intrinsics(), self.DepthMapFactor, reduce, k)
+        pts = pts_in_cam[:3, :].T
+        colors = rgb_im[rs, cs].reshape(-1, 3)
+        return pts, colors