From 653ee76cd1f3b10e5dd1e5998632551959be16e7 Mon Sep 17 00:00:00 2001
From: duongtd <tdd149cm@gmail.com>
Date: Mon, 27 Jul 2026 09:19:52 +0700
Subject: [PATCH] add pub image-RGB

---
 test/__pycache__/test_cam.cpython-38.pyc | Bin 0 -> 2056 bytes
 test/test_cam.py                         |  24 +++++++++++++++++++++--
 2 files changed, 22 insertions(+), 2 deletions(-)
 create mode 100644 test/__pycache__/test_cam.cpython-38.pyc

diff --git a/test/__pycache__/test_cam.cpython-38.pyc b/test/__pycache__/test_cam.cpython-38.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..617502c8882467471ea16941871903c5b6436770
GIT binary patch
literal 2056
zcmZuyNpIUm6rSNGilQXjvg0Ie6KCn7E>@fXK@LSX=aLi+f;73zg`hMec~ov0j_lZ=
zfCA|?Kz{;tYkx&=MScTM1$y!=hn^Y~=$oNzCq+5n+uxfvpWaYU+HDWP^ZK{(blgJd
zPo=#17@&LwZ~g!XMHEMf)EpB->n5>)8<8D5#EBcEftBBk+_*`aAY0V_2@#LAsPls%
z?lbQ>0xSL=^&7`f6Fuk~m3=P`C#>>z!<dQiUNTPmxN>**zY&~H*hqz)JR2bR@b1E!
zpNIA&Dlx?ijFB{+qdDkPlN!>ZCbeFebF6jyR5!HlNPFowrBgN*Xl?;_f!nk(cc?pW
z(B|AN?6N7HsW)vEPSF6#pSBBkVPkaa=}?chw5PlDSmjD#FYvrsG|SaG8#J%*%C!Y5
zd$30fdS0i#?irOEkca0!*v>7`&qnSSR=EjL?V??t2i^gG0eH9Qh?hk-|Lf^Xd9i$_
z=#=j+5M-`st-I7#3p9PNSfOp&d11`g;Ix;@%L`PjO}Bz8w5$159kWVTbWBg#rdNud
zj#$<Fs<Z*ib>%76gRP=9y$1Xm@aw>@(>0*)%MU>R@HqzgBar&E2lQi2KLP1e+Jm(|
zX@dVVx~?_QPq?;kznS-+OdUJ%A$w5Z@<y?y@G1<DdRj6;TGx_a?kr%ap1uU+ZYqf)
z7_W^7q)S;Q_kwITjCj7s#L?zpq(^L^9Rse$E)e0tF@|yKW8yuXu^9_;e#FjVM`@Ib
zv)H2xZx^X61{|iA{{nT6UqP+oOF;OKFrWMnupMpORh?|rv0J$eg=BP_*gLnM?Cz*x
zXUQm~JeiP-%X)R|X(st7jDoR%zAH;ihIdXQbYH<+5u}+#vv?Q`)8Paf(9cqM^=1E7
zWs5Y=4oQ<ITn0%>nXn*+G#{QIB-Di$CXmVydF5t2V-ZhS<&4r~%qOJHl5iLSL{hLY
zuAC*eb-R@_PDLC_Vjk_>u58_)XsTkiJ~HtpOfFN&W|#;2oU(Lj2r`k5dBjLZ1@S}*
zp5%O#lSY(I6uQKE6iP-~JP-1{nWX7KLb?YboCJ2Q5z0wdCD+(#%xXgoRSlw%WW)>e
zFgXl`2oEb)XM{2&?a}_tK&AnYCviR@)?*$sVkYT9<?OK#z#{4xaYkHSDd4oS4>*;3
z%FiczlB`gk%T!37CP57FMwJDVCkdd(#^b!gcZo56M$9aG2Ew5d4lDd^g&$S;A!#fJ
z70`+p%PI?MW&I7}4E1-AH0yIg6lvBcQlhJ{Q8O;ocTuzzIj6`b5MquqK=1@wzphaK
zXv9(=6H6#m9et!+bXDf6%52rK`i_uRy<e>~0H8o2wCqBObr&H0io|6||NB-hMuV@v
zdlWprbK}O~G0SCr=>T1I>7b+0Kq{*u0Sx^%Kw-rpPvCzD<-Y?#*fSi%!iHM^IL`k_
z4Hvr*0Sl<3BZ0oQ8gFT?XLhh}czQtw<ha-{I=BP&4rt)TuIXsIYdEHFS;#=JAF?zx
z5bW;)MbOC^1)Gl3R){?Ds%Xc<uwX}V1x`}AL7>hS1bttqzg*&Hhj4?UoFzHCDb$&X
zoy=me_LSUJ4j5}dG~Whtb}<3<{h*!*eS1&qC-8{~0*(F~WDBZQ?&mO#vxt46Zg$=Q
NQlqefebYT~@xOAgGP?i(

literal 0
HcmV?d00001

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()