add pub image-RGB

test/test_cam.py

@@ -19,6 +19,12 @@ def main():
         queue_size=1
     )
 
+    color_pub = rospy.Publisher(
+        "/camera/color/image_raw",
+        Image,
+        queue_size=1
+    )
+
     info_pub = rospy.Publisher(
         "/camera/depth/camera_info",
         CameraInfo,
@@ -40,6 +46,13 @@ def main():
         rs.format.z16,
         fps
     )
+    config.enable_stream(
+        rs.stream.color,
+        640,
+        480,
+        rs.format.rgb8,
+        fps
+    )
 
     profile = pipeline.start(config)
 
@@ -55,17 +68,23 @@ def main():
 
         frames = pipeline.wait_for_frames()
         depth = frames.get_depth_frame()
+        color = frames.get_color_frame()
 
-        if not depth:
+        if not depth or not color:
             continue
 
         depth_image = np.asanyarray(depth.get_data())
+        color_image = np.asanyarray(color.get_data())
 
         # Image message
         img_msg = bridge.cv2_to_imgmsg(depth_image, encoding="16UC1")
         img_msg.header.stamp = rospy.Time.now()
         img_msg.header.frame_id = "camera_depth_optical_frame"
 
+        color_msg = bridge.cv2_to_imgmsg(color_image, encoding="rgb8")
+        color_msg.header.stamp = img_msg.header.stamp
+        color_msg.header.frame_id = "camera_color_optical_frame"
+
         # CameraInfo message
         info_msg = CameraInfo()
         info_msg.header = img_msg.header
@@ -100,6 +119,7 @@ def main():
         ]
 
         depth_pub.publish(img_msg)
+        color_pub.publish(color_msg)
         info_pub.publish(info_msg)
 
         rate.sleep()
@@ -108,4 +128,4 @@ def main():
 
 
 if __name__ == "__main__":
-    main()
+    main()