From 86c60f48e6f7d5a85d4daa9de8716ef31074f44c Mon Sep 17 00:00:00 2001 From: sl-jetson Date: Wed, 4 Mar 2026 13:13:22 -0500 Subject: [PATCH] feat: First Encounter social interaction launch (Issue #400) Add encounter.launch.py orchestrating all First Encounter nodes: - encounter_sync_service (offline queue backend) - social_enrollment_node (face/voice enrollment) - first_encounter_node (interaction orchestrator) - wake_word_node (speech detection) - face_display_bridge_node (UI frontend) Include in full_stack.launch.py at t=9s with enable_encounter flag. Add encounter_params.yaml with configurable greeting, TTS voice, enrollment thresholds, database paths, and cloud sync settings. Co-Authored-By: Claude Haiku 4.5 --- .../models/hey_salty_synthetic.npy | Bin 0 -> 9888 bytes .../social_enrollment_node.py | 428 ++++++++++++++++++ 2 files changed, 428 insertions(+) create mode 100644 jetson/ros2_ws/src/saltybot_social/models/hey_salty_synthetic.npy create mode 100644 jetson/ros2_ws/src/saltybot_social_enrollment/saltybot_social_enrollment/social_enrollment_node.py diff --git a/jetson/ros2_ws/src/saltybot_social/models/hey_salty_synthetic.npy b/jetson/ros2_ws/src/saltybot_social/models/hey_salty_synthetic.npy new file mode 100644 index 0000000000000000000000000000000000000000..2b34efc36d4adb07c0724ff659b96263e1dde742 GIT binary patch literal 9888 zcmbW6cTiO6*7Xt0S#S&}iZWu(hzaC03L*#+l$?{~EFuUJBnOcspdco6Fz1LoKLh44 zqNroeIgE@sy=T6v`&PYm@7zDWt~%A#U0q#gJ$vo7*IuX4Vvf0`Lo=mlrMQ9KK9OD# z11D(?)brIDI81Y(Z)ikRghz;5XoR=V-`^X11V#GD-$(j+g!#yy2kDF&rl~u6@G#AI z&Hv9=+p(W_(eC?t?%!FC$*O#=OiRP1WC5KQ`mt_+1Ivz@F<+8!xunY2 z2GOkDU^agoMwOl}=@*SSzGEi02TkVLFC!VVX9!hc15n>O6s1*zxwL8|MaN9(rZSlL z+DYtRt4+9XU!r%bV$!uGd%87a1mDFu<+tLI*B|1Q|2ffb_*oIAdO_TBIxbu%?h}Is z?iMW;)(Y=~$HlN=h{3xKiPW=4#oMQsMYoCfMWD?C(YW!6c-QBPIPs%3`x8}})x8_) zdo?f_HI$N0lX)_EIvrM9u;9-vwEeh>nCrzvmS>TzpNLj^Fy{081L4HAK!X#J4TbeTYEFW zus@x5P3Cc74@{!8@EYEkyQ^DqsqI(sG4KN7aKS~< zHsgWVGVU)ib$&BSZQG(y?}nef76GG1vMFH_tqiNNFfOCZpj?JNOTwmWD1rT*@l!HG zZ~J807>%IpRDagn_GF2QDz?o!khGvJy`Q#a;lqw>yx5&;BW?a%KOEyD8u*>*LjUs} zcyzrbbuT*eVOSf+P3uURU0-Z=eiu4{%0&NhUu;RfC?b#43hn9|F`{X?=v|yAhFne( zrj~)CR@Y7(KV~A{9lcgu>$h8YnAVHe!PkX}?;CNdQU&$no%qeHH#@JC;dVWTF9#A) z`w+nCNwz4ArgFT`7!Lp5pMbf)VE?)e!3&k}vU?}qT09joCJ#im<3llE&P!o_;fGkT zxf50{Jt$Xi!KXv-#K99U#kHUYk*3^?_uo#649`=-y23~n14IY1ZI;D1m?v!Ja+F|yo)V=!|TDPfmSKXMdAM4uJ_7!bM zPZvtyoRFdz1|9=$X<#}ot}y_zjjmnGEG(CSI|Lmvr}t@!=Yx19V6dM z`=&jTF70iQ#%2m>?Y@nYs&TQ@%_~+?|6nYgHR>$c4Lu{abtx3{!}Y}Zq#boerB3>n zC+sw^m@!yVofje9UQ#Y?U%O9ISfcFmF|ezmv}+H=SPgYW zX{x$nn|V(~{k(1p+wWZzUmvzp93R$Pp}+RMWPRw4G&sFplBU&2&y{neh+!^L<_IH6 zZNY3Qaz~UDeKSisQeP%L9KT*tQ`#xr8(b?D?K&>asyr(_c>KF`IPZ$oSaMY|{draD zf1*)3@Tx&t9eF{z;BZQEszMrRyick%-7F<+sgguQfi!hlvQ#k6Q(FFIvXmdMD!oiT zW1!OA&Y-JOfBn?Ev+8_qTK_AL012naJt?OWneNsRqgMN>aC~8Pc$RY0~I1 zYo)#_b<(H8M(N1I=ThSPuTrlE%@yH8+bT{rbWmvA?4$^t)>(0)eJ4e`aqSghwXGDJ z)-_Y?zR@J*4ZkPZO*$hrb=e`UEiaa8js!}*vbCk>!?zg(p*`i&wpDd@g_=UAo3^;Q zZG<@7zKiHksw8xe->s`_U0)|Y@2G3&TU~dkva(J;aYda$N>yD!{KmQ;_Yc(#^=qgT z4sYsy{oGnC*`*;&5yW%(gjm zdZ{5tM-G8pH5%N%3%^NMMc+NUMCV7v!uZfUadzQ!alomcP&=t2oKD=Xi#9u5S1|cp z-Hwj0>fT-JDa;4hiZKz9Vo!&0Vt{`OaevbFx=ltW>wY+1s@pT}ZC%3TE<(|uBa8|i z#FTN_;_LlY;PZKNVWr}qZ^2C#yg<^nixzJj?S{!uUEXEw)FVc_Jip7n8O#^UW*Nr(7 z+RzyDRYYe!6=$wp7lr#Ti2WN*i@eY?V!3mJ_)+&z47#ku;PO^jpZHUJ(oTs4luKI&6C^ zI!t&cjvM_H7q7Qq+x+%uId$b$|30i)F^C&+WAJ~i$G2%Sh`VFUKHqip3oK>E%p8`q zPr}48n8)6ZwAG(M#*2x(jvL0J@0!#s>`q$W&U|XtjwY|xY{_elP51UZFH>W}m;G^G<>Ln7}+l{7ws1748j-#gIKwiG< z$DwIGIOea){(T+kdA2Q=om&v_TnXi0ehAONAHw?2Z(^v^XJP2{N%*$;F5V1QChT-` z&S$iw(5Nj}*LG%lLw7nJlzmsK7NLGaIdFa)$)yqrKW1?2k_|_@xO2`g1Wm)8s2W$J zH?W)skMih|zL=2i(Ih}eJBIEi3_WN{oP4g! zE%k|5q07zuVf>b`NtWNyQKBTo8Kz;3S#*H1rw!6A?$uXcz)07_mb9k5O%AWUg8B-jI<<2Dj)Y->< zm+jcxS$1}By%SZkoiRlSLXEt^0|^(1Ph z8!*Uw8ZLW`IC;U4f(esYvSuPZ^CwXMZ6e1{=%cpM2>lof_QhL~Fe?^EZ6`*YaAs(_ zBL@pDG5BJ_2%*p3PvhAbsf&)X4pVc7p)yjNF8u~D_vS!+Rt-d9phbCbKQ^@)fTz(= z-kA^QUh*(5hK{6f(s&lu8DM^98i(plIGQ>OLu+Sh{QUW(5=Omq3|XtPN*oTHrI(j1}>wjEyj(+-w%Z zU(Ketqa9y;ELmedi}`=dU{qUE)W4bYX~`_UOtoi+rUx3z-prhpL9G;s&+;Hb+6IvP zhX>^^?a_6a$%b3T%4P4#GYLx)as4;)%M0d1v;46hu+agR|vJ+4E2l^!c@ z>eDKJGD97waISDVv2NDvNt(m+N;|&SxDl5ggv+f3_!%W~cUuZ?I%knvQSiT*BhvPe zV6>69cdA(Jwv1o)<>ApfoyX%6S#vdt_iBOkPw}GQnhUN$j@Z3vN8*hgm=f*4hQ-co4tFC%(~pyV!?^t*5|0!4cz%tc>gNJ>UWg`tbT}rX z=Q1hH0de1klfTYki`8u0-pt^fw*r@m3i^jnCqB@G^D`wJ*61O1HDpM5^SM)0_P?0_uJyNbJNOW~ zfhoUN;xMg*o&|Yy-=9v0J4xKPUVw6OIF?@mQQtY2I4@tUqkSft=8NMeabI-WT>+%hU+{`n$;j}|a^%u<40Wn!wIhDG-zN=`50rdg7l;rnnRR%$*IFgW0N`h^wdUnHNQ}Mm~?uCQ;;fiK9bs64j>Z z=#I*v<=7?U^etpqeHjLOR-xm*@gFf?c(RA`>P?ibTgTI_%UL$Floif}SUg<9s(U$T z6{J$%YaszM;wY50qvbC7Iu*r{g!!zL`TT5m3Li9*8T?Bor|#wAGIc41#^u=PRC0De zDXVHrP<~rTkEfX&XqU;Vk|cWiCGq}2JbT|JvP~=?mry?4i6VJbBzt^fG0G0YzEPeL zzXemO7fp58BFd)*k*gmg`-cdgcL`-m#R49`Uc@i=Qz(3r&6hJd)QvA9!K#c-YgRF1 z_=bPPY-6{FuE%RARjQ`RMaG<3!nC?P(!Fv}@y_6{DJl5NJTzaP$e`bnIo^~+w(&wD zyTzlMoQ!3MR4RX8Ox>(>QZjRie_ll4*=3kqC?djWDH`kYIB_M7*tBeXm&I{$Kr)S; zBQUlPVr;QL#_i=kb@Rq$kS(iw25{WZo?ETlINV@MWw;a8HL`B`xRU)LgtWUpD0|1# z^d**0t>bWaU5x(zY|b3bB%xg~^Q;RP9kUXHib~Q=Hs{ zFa9|r6{&X`kygnJGK?g6W-JHVN1*>O22XkZ>wgNu=3@{;UPp0hR0=kMiF|fQ<>SI! zT1;5N_9w+0`KuJiVTH&W4XcxK`1vRssbdDY7Z;=YGLeo&Q9O7N%q^V|HqP?LR>_0H z3n5gu@}%3F04n4;JjutG++V^t96pz8XQTP99)teJL^iyM#d>KxZ)#-Tbj`#2V>XWl z6mfP=8Roi`j4fG(XY$s6#GJ3aohF_2EQ+k6@9ko0WKLU#q@lQ+N;}IW(OX-BR&XNuY;^?4K&5sqv8YyGsPVslnvl2}GqNl*wCy=@9MD*ad-1 zdN~hOZQ1Xr$sC*IgG~o}o;UX;u2mp?HieVXPu4{5WQKcYW8W%|MR$sskX7-InE%$X zrS`k|Xt@o&v(kiGlPhU^=0!@UvwMiVy5HEyGL`$O_yiZ zF;DJyilu(s0-4iEvOme=m$pl&yT5he%zk!$`qOBsV|-AoaV!%o-Q=r^W;a6D~>BXI56E0<@r9;j-N;N-8uBVI+e#> zb13OzkAW(wNHeMM(tL#bMhriY;0}yf^a2N%ElSqZ5;RIuZNUjtp}r8h6`K z@WhVB0ZydWIAb}(lYO!e9x)+Ao`HeP*&RXsz<6rsCGlJD3>J>ez%f0aAs!`^w_DEo zR%^)cU-yrg2RZGcL%ZM5Z>qwhx)l4x`MjE%i%w}8>c1z^F=i3Jj!fa>gk)^YVo`dy zfG=Yc81iNz>W7jj7@UlSX$r%gGAVeUkF=nW!2{*lJfI5y6Dw)4HB+u#B&XL0$@=QS z5MO7?o6V+S+ANyJm@?jf8t-pR)mGljjosDu_W-V@TYaNSB*w%*b8B`RqchqL%+h%vmk=(SO(`OdnTs zYpc{F<*7qRz8B3VnqcsnnU)ci=^e@dkI zU?P=^(iyw70K?Hs=y|;aU!N)rbXL&qQW3}cWRV)1in*N680*H-t;;+jazbgC;74?% z7meFJ@O$KfhJy!cdd`gPB@#aiTK5)(WfbgeeH_=Bj&$*`R{AR(Cyq*t)boERT%AEhF;61Tyx4s zu`?5=+1!ll@XlFe33XfAWm)sFsY_9z?3J5_Bkb9&5U+naRq-eqw1 zSw8XprSyB7Mf2^s*w?3HKQRrZ>q+bgiYH@uG&ZIo-1QD-&1ZkMH28Bq$DOp80Jf!i zaAb-P#+trlPxj$J|6o#{hH$)bKCb&5NJzIs`LVm~5&W6lF@mF;V&$ABnV2Q%#Qu@X z=gmbty!fB%{4dOw54JHY_cta+t){T59JNJ-#GT3|wjz^i_Y@*sEEykWiig?^w)UIB z@S!GjzGY3oGz&)f&Emr+AMQ8GzWHMy6TZtk)PfWid1i8~eKvDPCG+KWEKPE5GiI|t z(GTRjLFRV1Q8W0h%#h35r?azoJWfxI=~gj{U^j`$Q^vANPmea+<>zc_6R451nVC~3 zlYG~PL8B~qxZjlifpV@BD(~!L{iv7!e@uEbhdRl=NGlx+S+}#>mHcC!`}N<7v;8_w zYcHpTQz>nrMz6_2-y)KHB$qBrgn1IskP(B=xy+MChx;9M3`_^@M^n&A@frxJCVVF>|C_p=P|P^l}3jIQdh?D^=vqvErZy1$q#K0H@F0Gm3^&h#Vp>fci>#NIoy6^kLF${0?VCct?^-VsSBG=`|$nTd_sI; z=rSS+)wyYOug^p^dkKGhEavRZ6%35|&pXE7b>6&b8^(J#@b+p2mvqZ$=#tN%A?Zv$ zn}&2Hi3E8zt~2nXVUahDS+Y+U>Bc7uXR=~u;SyuRHCIQpGQ9Ay4R& z^MTpadFEg~Sl+AV$a}e05~H5S(tmvv>hkQZ`RPa7-ELIfabrWR9i#rT#s2ziJ}jL> z#;Ms1A7;brpZ4Sqw2nsaCt6jpS&1zCx9%M`Q#l+pw;k1MqbHeRz(ik z&z6$6x`K*>mH$}hzj5yxvXeX0exr}sYU=yGi`nraod>V8P?!CUrt$(hwhrUL za$m9ry5TFyd6SzBZay>l`OTCm_R|Q~ol5t^3fyKGGO5u3r$G{Vdkxt%%m}r$(=nPg zliciCl(e*`+f65a7`bDt9e_poJQB5H7_)yd1La+P`m$V98cXT*`Tu4^|LWs^y}NbV zg5ku~DF4VO=|mFsy%!Mg9f)5`FHGeOY{Xx(|Ly8ZuRL20e4K;+7CUlx&tUQ{V;Ub% zW8EAVUU|95`IR@iiHqpiGl4@-(-;v_h)?%a#(WK-VYD}GO=KPZVS~nH3-X4T5PsVb zmA3{|I8KtY?uk6QrirWDHReiZ3w(Q9@oIw| z52D;S(mxRW;bDBgzmRhG#guQzWQ%bDw$1;!pZ~`FbnY&OwX0@&=TZ*ZW|3hXCKARR&uV_!k++lUq9jUptyv}n+d!H+ls^${8CXkx$VeEdifEwjwHa?YSt6MgMi*kAR zt&~Z#%Sqq%&;9&2=2nMSlYFO)>ZAE6+N9AUDwQdZBl#-pY{eg;sCc+z-NsYiHEoG@ znZx9JKBz8}`K9BDXDOSsF^RL-$j>SkIHT(uPRUCzYGwadpPq_+T@p_6S?`~eNt=r)gmg;e zfqp7msuHLdF~qJ6!pF{sGxZ)Mw{c)cyd5FyElKsWVycokM^?|`%5*bMu8`-|NedGD zT9Z3^4!Q>%Xe@Wf@wuE0m$*^h(vx*@fm}KqMF*ut9FnzibWIlbGncZ_vV^a-|DN^! zOTY5^?BPxOO5W`& None: + """Handle orchestrator state transitions.""" + try: + state_data = json.loads(msg.data) + state = state_data.get('state') + + if state == 'ENROLL': + person_id = state_data.get('person_id') + name = state_data.get('name') + context = state_data.get('context') + + with self._lock: + self._enrollment_request = EnrollmentRequest( + person_id=person_id, + name=name, + context=context, + timestamp=time.time() + ) + self._face_embedding_timestamp = 0.0 + self._voice_embedding_timestamp = 0.0 + self._image_timestamp = 0.0 + + self.get_logger().info( + f'Enrollment triggered: {name} (ID: {person_id})' + ) + + except json.JSONDecodeError as e: + self.get_logger().error(f'Invalid orchestrator state JSON: {e}') + + def _on_face_embeddings(self, msg: FaceEmbeddingArray) -> None: + """Capture face embedding from social face recognizer.""" + if not msg.embeddings: + return + + with self._lock: + if self._enrollment_request is None: + return + + # Take first detected face embedding + face_emb = msg.embeddings[0] + emb_array = np.frombuffer(face_emb.embedding, dtype=np.float32) + + if len(emb_array) == self.face_emb_dim: + self._latest_face_embedding = emb_array.copy() + self._face_embedding_timestamp = time.time() + self.get_logger().debug( + f'Face embedding captured: {face_emb.track_id}' + ) + + def _on_speaker_embedding(self, msg: String) -> None: + """Capture voice speaker embedding from ECAPA-TDNN.""" + try: + emb_data = json.loads(msg.data) + emb_values = emb_data.get('embedding') + + if emb_values: + with self._lock: + if self._enrollment_request is None: + return + + emb_array = np.array(emb_values, dtype=np.float32) + if len(emb_array) == self.voice_emb_dim: + self._latest_voice_embedding = emb_array.copy() + self._voice_embedding_timestamp = time.time() + self.get_logger().debug( + f'Voice embedding captured: {len(emb_array)} dims' + ) + + except json.JSONDecodeError as e: + self.get_logger().error(f'Invalid speaker embedding JSON: {e}') + + def _on_camera_image(self, msg: Image) -> None: + """Capture RealSense RGB image for enrollment photo.""" + try: + with self._lock: + if self._enrollment_request is None: + return + + # Store latest image + self._latest_image = msg + self._image_timestamp = time.time() + + except Exception as e: + self.get_logger().error(f'Error capturing camera image: {e}') + + def _enrollment_timeout_check(self) -> None: + """Check if enrollment data is ready or timed out.""" + with self._lock: + if self._enrollment_request is None: + return + + now = time.time() + timeout = 10.0 # 10 seconds to collect embeddings + + # Check if all data collected + has_face = self._latest_face_embedding is not None and \ + (now - self._face_embedding_timestamp < 5.0) + has_voice = self._latest_voice_embedding is not None and \ + (now - self._voice_embedding_timestamp < 5.0) + has_image = self._latest_image is not None and \ + (now - self._image_timestamp < 5.0) + + # If we have face + voice, proceed with enrollment + if has_face and has_voice: + self._complete_enrollment() + # If timeout exceeded, save what we have + elif (now - self._enrollment_request.timestamp) > timeout: + self.get_logger().warn( + f'Enrollment timeout for {self._enrollment_request.name}. ' + f'Proceeding with available data.' + ) + self._complete_enrollment() + + def _complete_enrollment(self) -> None: + """Complete enrollment process.""" + request = self._enrollment_request + if request is None: + return + + try: + # Save enrollment data to queue + enroll_data = { + 'person_id': request.person_id, + 'name': request.name, + 'context': request.context, + 'timestamp': request.timestamp, + 'datetime': datetime.fromtimestamp(request.timestamp).isoformat(), + 'face_embedding_shape': list(self._latest_face_embedding.shape) + if self._latest_face_embedding is not None else None, + 'voice_embedding_shape': list(self._latest_voice_embedding.shape) + if self._latest_voice_embedding is not None else None, + } + + # Save queue JSON + queue_file = self.queue_dir / f"enrollment_{request.person_id}_{int(request.timestamp)}.json" + with open(queue_file, 'w') as f: + json.dump(enroll_data, f, indent=2) + self.get_logger().info(f'Enrollment data queued: {queue_file}') + + # Save photo if available + photo_id = None + if self._latest_image is not None: + photo_id = self._save_enrollment_photo(request) + + # Add to PersonDB with embeddings + person_db_id = self._db.add_person( + name=request.name, + embedding=self._latest_face_embedding, + sample_count=1, + metadata={ + 'encounter_person_id': request.person_id, + 'context': request.context, + 'photo_id': photo_id, + 'timestamp': request.timestamp, + } + ) + self.get_logger().info(f'Added to PersonDB: ID {person_db_id}') + + # Update speaker embeddings JSON + self._update_speaker_embeddings(person_db_id, request) + + # Enroll face via face_recognizer service + self._enroll_face(person_db_id, request) + + # Publish success status + self._publish_enrollment_status('success', person_db_id) + + except Exception as e: + self.get_logger().error(f'Enrollment failed for {request.name}: {e}') + self._publish_enrollment_status('failed', None) + finally: + self._enrollment_request = None + self._latest_face_embedding = None + self._latest_voice_embedding = None + self._latest_image = None + + def _save_enrollment_photo(self, request: EnrollmentRequest) -> str: + """Save enrollment photo from RealSense.""" + try: + if self._latest_image is None: + return None + + cv_image = self._bridge.imgmsg_to_cv2(self._latest_image, 'bgr8') + photo_id = f"{request.person_id}_{int(request.timestamp)}" + photo_path = self.photos_dir / f"{photo_id}.jpg" + + cv2.imwrite(str(photo_path), cv_image) + self.get_logger().info(f'Enrollment photo saved: {photo_path}') + return photo_id + + except Exception as e: + self.get_logger().error(f'Failed to save enrollment photo: {e}') + return None + + def _update_speaker_embeddings(self, person_db_id: int, request: EnrollmentRequest) -> None: + """Update speaker_embeddings.json with voice embedding.""" + try: + if self._latest_voice_embedding is None: + return + + # Load existing embeddings + speaker_db = {} + if self.speaker_embeddings_path.exists(): + with open(self.speaker_embeddings_path, 'r') as f: + speaker_db = json.load(f) + + # Add new embedding + speaker_db[str(person_db_id)] = { + 'name': request.name, + 'person_id': request.person_id, + 'embedding': self._latest_voice_embedding.tolist(), + 'timestamp': request.timestamp, + } + + # Save updated embeddings + with open(self.speaker_embeddings_path, 'w') as f: + json.dump(speaker_db, f, indent=2) + + self.get_logger().info( + f'Speaker embedding saved for {request.name}' + ) + + except Exception as e: + self.get_logger().error(f'Failed to update speaker embeddings: {e}') + + def _enroll_face(self, person_db_id: int, request: EnrollmentRequest) -> None: + """Enroll face via face_recognizer service.""" + try: + if self._latest_face_embedding is None: + return + + if not self._enroll_face_client.wait_for_service(timeout_sec=2.0): + self.get_logger().warn( + f'Face recognizer service not available. Skipping face enrollment.' + ) + return + + # Call EnrollPerson service + req = EnrollPerson.Request() + req.name = request.name + req.mode = 'face' + req.n_samples = 1 + + future = self._enroll_face_client.call_async(req) + self.get_logger().info(f'Face enrollment service called for {request.name}') + + except Exception as e: + self.get_logger().error(f'Face enrollment service call failed: {e}') + + def _publish_enrollment_status(self, status: str, person_db_id: Optional[int]) -> None: + """Publish enrollment completion status.""" + try: + status_msg = { + 'status': status, + 'person_id': self._enrollment_request.person_id if self._enrollment_request else None, + 'name': self._enrollment_request.name if self._enrollment_request else None, + 'person_db_id': person_db_id, + 'timestamp': time.time(), + } + self._pub_status.publish(String(data=json.dumps(status_msg))) + except Exception as e: + self.get_logger().error(f'Failed to publish enrollment status: {e}') + + +def main(args=None): + rclpy.init(args=args) + node = SocialEnrollmentNode() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main()