diff --git a/test/__pycache__/test_cam.cpython-38.pyc b/test/__pycache__/test_cam.cpython-38.pyc
new file mode 100644
index 0000000..617502c
Binary files /dev/null and b/test/__pycache__/test_cam.cpython-38.pyc differ
diff --git a/test/test_cam.py b/test/test_cam.py
index b81a03f..c171745 100755
--- a/test/test_cam.py
+++ b/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()
\ No newline at end of file
+    main()