Resurrect camera node

This reverts commit 6c5040d.
kini5gowda · Jun 28, 2019 · f67148a · f67148a
1 parent b6b8058
commit f67148a
Show file tree

Hide file tree

Showing 3 changed files with 57 additions and 6 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -170,6 +170,7 @@ include_directories(
 # )
 
 catkin_install_python(PROGRAMS
+  nodes/camera
   nodes/dope
   scripts/train.py
   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})

diff --git a/nodes/camera b/nodes/camera
@@ -0,0 +1,52 @@
+#!/usr/bin/env python
+# Copyright (c) 2018 NVIDIA Corporation. All rights reserved.
+# This work is licensed under a Creative Commons Attribution-NonCommercial-ShareAlike 4.0 International License.
+# https://creativecommons.org/licenses/by-nc-sa/4.0/legalcode
+
+"""
+This file opens an RGB camera and publishes images via ROS. 
+It uses OpenCV to capture from camera 0.
+"""
+
+from __future__ import print_function
+
+import cv2
+import rospy
+from cv_bridge import CvBridge
+from sensor_msgs.msg import Image as Image_msg
+
+# Global variables
+cam_index = 0  # index of camera to capture
+topic = '/dope/webcam_rgb_raw'  # topic for publishing
+cap = cv2.VideoCapture(cam_index)
+if not cap.isOpened():
+    print("ERROR:  Unable to open camera for capture.  Is camera plugged in?")
+    exit(1)
+
+def publish_images(freq=5):
+    rospy.init_node('dope_webcam_rgb_raw', anonymous=True)
+    images_out = rospy.Publisher(topic, Image_msg, queue_size=10)
+    rate = rospy.Rate(freq)
+
+    print ("Publishing images from camera {} to topic '{}'...".format(
+            cam_index, 
+            topic
+            )
+    )
+    print ("Ctrl-C to stop")
+    while not rospy.is_shutdown():
+        ret, frame = cap.read()
+
+        if ret:
+            msg_frame_edges = CvBridge().cv2_to_imgmsg(frame, "bgr8")
+            images_out.publish(msg_frame_edges)
+
+        rate.sleep()
+
+if __name__ == "__main__":
+
+    try :
+        publish_images()
+    except rospy.ROSInterruptException:
+        pass
+
diff --git a/readme.md b/readme.md
@@ -57,12 +57,10 @@ This is the official DOPE ROS package for detection and 6-DoF pose estimation of
       $ roscore
       ```
 
-2. **Start camera node**
-
-    Start the ROS driver for your camera. If you are using a webcam, try one of these:
-
-    * [usb_cam](http://wiki.ros.org/usb_cam)
-    * [libuvc_camera](http://wiki.ros.org/libuvc_camera)
+2. **Start camera node** (or start your own camera node)
+      ```
+      $ rosrun dope camera  # Publishes RGB images to `/dope/webcam_rgb_raw`
+      ```
 
     The camera must publish a correct `camera_info` topic to enable DOPE to compute the correct poses. Basically all ROS drivers have a `camera_info_url` parameter where you can set the calibration info (but most ROS drivers include a reasonable default).