From ca6bcf6b58d0bdc689179f7b810740080f295c5d Mon Sep 17 00:00:00 2001 From: Miguelmelon Date: Sat, 29 Jun 2024 18:39:50 +0200 Subject: [PATCH] Revert "Drawing operator" Solved problem --- .../operator_data/latest_detection_1_data.npz | Bin 0 -> 7128 bytes .../operator_data/operator_data.npz | Bin 0 -> 7128 bytes people_tracking_v2/scripts/comparison_node.py | 22 ++++++++++++++---- people_tracking_v2/scripts/decision_node.py | 2 +- people_tracking_v2/scripts/yolo_seg.py | 4 ++-- 5 files changed, 20 insertions(+), 8 deletions(-) create mode 100644 people_tracking_v2/operator_data/latest_detection_1_data.npz create mode 100644 people_tracking_v2/operator_data/operator_data.npz diff --git a/people_tracking_v2/operator_data/latest_detection_1_data.npz b/people_tracking_v2/operator_data/latest_detection_1_data.npz new file mode 100644 index 0000000000000000000000000000000000000000..da252cf91d0fef7c881d7b2c7013d01ebf4232c3 GIT binary patch literal 7128 zcmdT}3vgD&61)@6d^Z|*B>#&Ja~w~YXSs> zDk;yTsos`G9mWCrkdi58u1!&fQX(A$q=8uM}Gzm z=eg(Xp4~lrZYVRhYj2@`{lvDyt2^yDA>`kiL~l`4nSW17>549*CrDCEvgB{`j5*Uo zbGwM8Vr6n(e)*!Z=CwyRP z+Q`9!1{+nz|NOg!Trkg0XdF+_9knN2LVManvn1r7)$R2~5;yGU^8)nd`Z9@|^#gOk z|I@)rKi~dtrVakt2^;-%vGvq?&|k}{^V7ZbPk#)&>ijkd-GQe!`T0iCZphn*Z-+kn zXQmWCCgq%0+iU#tX59VHN!;@;6-j6(ln3-E&T>gNduHeR^~CG9!EfV{($(N=`y{G< z|9%hi23nbsFnXTg2if(Ak51?0B z`9xrxUVna8;=%wcZaXqBJt>Wt&;0cLNCfsH|rF} z_2tuqfW6zE{t4_Qo3(zrHRxTH7dpS)G1=SweCN{6y%IOhpThps;Rx^O=^db-8PO1s z-*^uBZ(cdF9Q@;>f9~gd4Y9Cit^LOfe!BH>(+0?Y)DqZtV$=xCi?Jq*bIBcl0Q)cE zM_UErH2Yj%=eOssKCxKh93SQB_yAw8hgsJr(@$r+)~}KRe(*kEzBoBA+plLXuPc#w zv>dHRA4zwQeLTc3Z`H0r-jd!7Kb`G!9y08J9^=<@Q~!qZC0d^K7*f7y!x?s{01H#U6}Wh5B`yNvKYMYZTf3TYkhw<-pl@HT zz=7%QK7)e6J#h#Ztaa!b=yt=%&C;HE{?v~obo0vrtrshxFKVXaf^#z_gKiE>gT8p7 z1P7DzL62vkzwLeC;+{K!GvEA8`g-uo_JdDx;6O4@g+aG3j)cF)k>8@gv5NXb-x?EI z3xDoC0eSDQ=YWe(rvO^v1Mr*mopqbl_*)Lco;jcncHDPv!Mxb|Ahl-hP;qRE$M{BK z4eVR5PJ_OC-xKP>(uIuYRjGBjbsy~6$6o<7T8;sF6@}`6ow^f#8Gk>l;;ieV?ENuO z`Sra@@V$K(*2uW5h11|CL+8w!)sk+EiwECm?xx0Dlnp=K?X}Q%W4eNG-gQyodty}~ z(6NivRPr5NIEp)tl}X$@ zxF3a#dnM;_iBtT=kUM;PSVH6Eyh6xzSp>Q{d>J-6*#UHuQc-}?I;exl8UnhwtO_>w z?g1#=$PNxt>&lUbVgH;nM?$M-wt^|e()GR{!@$$IQJvpc}qF9lhU?K5OL!)SUm&T^f>FDl=e zov#Z$I^Jmf>k`NR%TJld!NMiah+pe3T$bi8{t6c^Uv!?Ratj;`gsX$96FS>xIhPN# z8|Br(#T^Q+RCT3K6}-9#n4hK&c774_3z+Pu;Na$pdf$aKiSx^a<@qJWFJSU-f|R2U zLY9Q$jk6Z?_Fce*73KjtrvpwM!T7Qi2>_)(BJAASdghSm1&3i#*R%Otc~@=)o! zm$#H6p6*v4m5@3)s&mpb6)#8bM0vUUfE&8d@ZK!MOV)XIv7$eC1a|0!T_o*Le1H@e zpx67k$1v{L1&XfqgdSHU)_`v50*79*g;L)Bap8MVb*r84n_|#zyej)OG*7-a@ z-Rb5|Y2-ZO&XGQ!?7aMWQl8E;6z0@%ohxzU;C|rrqPHYY`7mEX>J&kq`1O zl27{zIK>5dyXs#7I-QrZVQ<)>EZAEbHw${Y6K!u>0Y1$q9s-fh&@&?-Z)-ia%aHap z{GxMF`Il0R@gx3c{soVzO3I`0vu)!q!8GX6D6HGa}NOO&I|crN}M`%8KCPg7f8*6yzVsf{Y3MS zF6lJ>F!1@|!W|!eu<(8O8H|BmRA-e;-S`U@E>EqxY2`W+l7HjqQ&H)X^2oO$Ns~W@ zU+P=Ym-|j6KQPH}MNXQq%}{2SnBK9SeeQzG_{Z)df&TavtrToE`IVaEgwTzrXr*A8 n=T|E0C4?^eqLqSE>{m*;OGG9-l=+QrlGl~~yiM>T@r3v_26geX)ZQ-czb26N`f zER`X>dWPNa_gvTe&-?uIu5<0PuJ!q@?_TSyz4qGo*-Kl4gp>*Q&%}khb|N#KK#9Zs zJ}7WZID5R!5f?Wc|Tm{Y*k z)x*of!sVi?hqcXbu43WjX|uyU?JeAFcFKp2OGpX}3UdZ={+|=ozA4LlIvm+Rtw3|u zkJSL^a@h(*_L#swdxygFYO$ zYi*%j%4X|Vdo-rU6$Zoe3Oq1U7Ul%znPFTi{3pp~M=+Qe&#Nz1M8nEkiURvIJf;)a z(%4hg^x~QW1a`;vHCTW@l^v#x4tkpZ4pxA6~_{fb`)kN1Zs3D~>2I zFfs}6U%fd^xKhst+}!jv zbH~iV^eA8Zm;1Vy9;V+cp0IWVLdMJ5W(@wUC!&m17uF0e!O7cphds2;0(D>=X{qHg z%onOFu{QgW8l1|)n1AY>#P|tT4K(@;2gky7TuW3lG2T-xK4Jf94FO(S0u0xY756n6 zr8C-?KJkk4nCW?Y_$b^~GVa5L@s<{KHRrWGFrfO`z~5N}|8< zx`MU(FZ8uiSO3B7c}%CNx2zkl>O)nOD#m2lfA0Sj4y$?BWP!o7Ew54qz zci+XQLZ*N8DPqn74pc{4tTGQubsGUv&#c9_Ku)Q@&IBh|~5 z4+0A3(8a18*V|gZFrQtM8^4+_7Yqa+%xs)D#eD4P!?We0IN;hS9Elt}1r{-z#9JYM z)|C?c`ymZYN}%)B=(c(ziTS*SZ3Y)c1fld?Z(Q51JglAR=O6a=d^-s3hL_61M|l7a zU14v%MS=N(4kVGi6cU52a)S&-VRMW-I0h#Z4~T(e%3^sy=@@oC0m(fMt1b2rlubME zp1=bpe6||;S0ylCTNR&z@;(4}n0H(w{8^tut~R@?ZL2~lE2WMyK@F>~{kOnnfVPYH zuYrrFh1dVrz{T{N!H>>^X%M=_U$;m+fKDhf7Ty}RfZ^=4Xcr(~yah!Kvv@RkunCbUxvqz{#e7a|!yv6qW|O-N9iTH`4)M6vCQ5mkhyi z-C?ttsUY-8UrjwX1yo93Pcw(7fMA9_YMn@dv8W~XS%VZ!pNRf?1m>f@CC0dOGRZr6C#>x~JZj~nPX-)O63zZ|L*O$H~1FOFOG_(88 z@48Ug*;k$bT2F{&Kbb{>myf(9>C1TFUs2q0o{I;`YwazElA{1G>E$qB7X{aji;cf? zjfE+hm-u?AXh`$i?&R+Y2aRlnXWS#Pkn~I?o%VGkR5_FO5*tRs;u=dI*T*mr7I}X5 z$7C4zSSixP#)JTOXi*iLU?k9J_ISwWg#okCx1p1DVeow^(6*e00F4(ft}Xgs2ItcL zA6FF#5KSc|%v5q2o_mo~UGTdMTwTT`hJiZ^K50Jn{fJ;#oT#sw$qfWqwj99*G9NJ4 z@7*-c^Mx97zx(oxfp7y6mpNH@0k>4z7yUeMsNOZ@DcbG{sod&#&t)&LrOE0D74d|< zM+%-gXnMhq_R+jaXAf|WB2qoR?gnd=DoS@rJi)a0cDDtYCoo(ic_)6<6|(7V3KnSG z;G~J#7$=1X_y=0D-;i?w?o>w~f!A)}TfIcntL_HTLvh88Q7)hwNk`vu#}(cL+q!7m zy8#V#olz8{8@&0J_03<~4GxLy$p|O70awbiMFtKxSbTXkyi3>(_IoA}Z5O%10X{1F zqDEKfC{1v?F60VCQ~4&$DlSkMbPGR3>IhYOuMX)?J3{u?F`u!g_8|12mXqDp9y0nu zdz(Jk!0flts_&{+aL)D46~|Qz=!3%8$SZYe`K&l@JVR6(19p z5rP{Bj2N@^`60A?bc&3H5ArqZk7THE!`mRrOGJnZ_-$K7_g*~!^(?joX2X48&UsIP zHH`!IwTo=zI zXrw9wd?W3ZCOWzsLKQFfn0V9ejHCH*Wg;D%t=&~FKC}x=bLB~K8#KTdRq^4$$z9Oa zV}7G`A2~cp!S6B3p@b{>je1?lMDQT0Zm--L2@qfK)ptlIhm}n$i?Dhem`wZ8=BMJo zVVDs}#&O{D!|lAL(NE;kCm(Cp^9v20BQa47+(a9;eH`%y+i0ysK#?%Gj=tq?>IPT; zL_b?`orQGkXxmz%&nJ5YofO+D5L(OG4j99T0(mzZZ!22E+cw6^#bXDB@|V6 zI8uCi5|s+@b>AQViNZ8{O{U6b5U=jtW8XxUkcqHsp}5%+A_)?SW22cvpQyhPT)k&e znTP)eo{AZi6U)%x@_7l3t$&W1`80v0k;sb`ev+{T3%`BIYn>BwTlglz%ZH{gaNnS-YrOhK@ zTdQb##NkWs&#$P$S9Z^+?mQZEU)pNZUqvoWCx5u~Pa-IGvO{sp$Tfy8oGJJV+9J9W zeD(VRqCB@z-_tOOp4^u$mMfn^6KAjIM2bwK!gb>8_X2~+d?NkbnB_3KNji69U+XK> znz+GusHYEUh*LOqJ?=)rm9u(pZug*w7mFqS`p;2YR*hrV{yxNFpw&d!^Bk?j*hwE8 zY(Yio7dL7b-y-Liu9YGz?dax&+as0Q<3Y|E{{-E z4zm>Hp#mhfTs`Nrs|A%@WfC4Ne1zJEU-$krt470*d%lwEWuUt)lQYUXRfutPUQd0X z8HpX@xisO_hFH6VZt|Wjzyh8p(KtQVq6n-!cm5X(g*83=J#Znnb^Q6)z{T6b>Hlls z^6!7-Y10d)-s7D}_|dr&RE+OY`FkyUUz*n_VdS=ZSAQQ0llpjngLn`LS(VCHWPU;< z;|5o&XeZDVgBiV|^f*#6%`5!iJ%h%MY?G$1%%DxHm(D6V%g8&BwzkZ23zccef2Tc9 z1nC}oyro;nA+cfhYJxp6SVnx^y^D+*e3r<}X?^J7l4{-k)a5;(r+v?RZ!v&- zb@u76L@nT`oRRi3ygjHrAz06tIfIG+u{XUt57dC!=AU+nLBNRP9UW#40}TlUO-2QR!ceHsQopHSA^ zeh~vTb$+y?-4P%@KAZZoV<&)0YAMkA6$w^XX-Abgqd@R}48i7b4A5V`>m#Wc3FO<9 z4p-{pz}`YBR3|bV*79Gn6>UU8ye6fE-<=T1o3bkCBnkx{k;X_6LE( zXDSX@27vbF$Y`D+9(>;KKex>74a`lXaXNACaQJ%WqO`dir1)yp#PT`AWs1ftF*Ek? zZ7Jl@IY|eY8z-tBps<7OT^5DLm+c@!I4wju+7^nH*YCw~+rZ@ts7$?U4YBOYj%1P6 zpkMWNKigd^*k!3`WEXJ>2qcg2LC@^qTBq<+pL-54)!HIK=)VMqPYjp%nK=U9!M9oc zwJU6us71O@xosq1>zb>tI6|SWK9lmQ9KP3ij!E$ON-E!WIFp%%hzrd_sY3*Y*8I0tATo zl`_R96$*2TDS5QxVK6O=4-t)s08zDvznIj*L57QWWZ66%WK*(U^yY*@dpN$hfH?$O zKA**RmW9BOhj(#*R|wSS>`q&_5(YMnC-2Y{heGC{{#!jlC~WL|HoI4g07+Xdrz$R7 zh7>c$&BXN}i1e)uUM#%~1ENz7_hJcP_xNiJiAgX#?6*DoU_2Ny+5APa>H+}GRU(^V zKX@o_l3*L*3!Dne1|nDefW+{Lsy2&1?7m~4Aff3C{!wm~%IAGSi>j+bA`A~9-n&YQ z^}GP_GxXg5;0bY$NFBLaJb^3|y>5Hq37g8n&)vRzKqxO>CKVd& zKi%Dc%$GszN0u9i7ut$W8Mp!k!E8fQ+!f{+UmqWM>;%I_lV?dio!}JtWTf^^S?C?8 z*S1`OM$WFGw`~qk%V$6{Z!3x&Kf%C2Xod)tzk%elP}uL8s;Ty+j%4`L3_jh;?R;MgiPc=5|FWg zP6bc-I~f+hW}J~dcj_XrJ-1`hq8Iu=gsZOmsAm9R;CP$wgDz~mq#H1n(}( zsz977L!VWl3Jf}q#crIcASCA%LqV+qeun#fgNRgszG21pd4&=X7fUY%bSS~Q%Sp$T zh82PArB&v8O+`ql;z(Y4q5!#lqQ|eN$O8?NY|kfK1xVu+CdGvUgd`a}_If4@c%{es zWIs;Aq}@!nIQa<(5~DS49F~HQERj8L2P7bXExU!tT^y7uCpfG&#DMT=@?uHUahM;J z9KDE+!$6`&=48e(IB@xVMas2fu(C^FHZkuwn9uZ&8}1c{W|<>LpRb9-6M<6gx6Oya zU*Ftxe>)evtKDpCpk&#ZA8XMZ!c1^Xlea5Il?r@A!{JuNxMel?JzYoftts*y9Pi(K~H~XFb zo&Q&RMgQGC(eDi=s-3PZ;Ufw1+8RW}OeFu?cGo|;{EsgJ(eLbEn_vHF`unSkaQ_4B CZo)GF literal 0 HcmV?d00001 diff --git a/people_tracking_v2/scripts/comparison_node.py b/people_tracking_v2/scripts/comparison_node.py index 95912ee..e7bc5ee 100755 --- a/people_tracking_v2/scripts/comparison_node.py +++ b/people_tracking_v2/scripts/comparison_node.py @@ -44,11 +44,23 @@ def __init__(self): self.operator_data_set = False # Define the file paths for saving the data - self.operator_npz_file_path = os.path.expanduser('~/hoc_data/operator_data.npz') - self.detection_npz_file_path = os.path.expanduser('~/hoc_data/latest_detection_1_data.npz') + self.data_directory = '/home/miguel/ros/noetic/system/src/people_tracking_v2/operator_data' + self.operator_npz_file_path = os.path.join(self.data_directory, 'operator_data.npz') + self.detection_npz_file_path = os.path.join(self.data_directory, 'latest_detection_1_data.npz') + + # Ensure the directories exist + self.ensure_directories_exist([self.operator_npz_file_path, self.detection_npz_file_path]) rospy.spin() + def ensure_directories_exist(self, paths): + """Ensure that the directories for the given paths exist.""" + for path in paths: + directory = os.path.dirname(path) + if not os.path.exists(directory): + os.makedirs(directory) + rospy.loginfo(f"Created directory: {directory}") + def sync_callback(self, hoc_array, pose_array): """Callback function to handle synchronized HoC and pose data.""" rospy.loginfo("sync_callback invoked") @@ -147,17 +159,17 @@ def compute_distance(self, vector1, vector2): """Compute the Euclidean distance between two vectors (General).""" return np.linalg.norm(vector1 - vector2) - #def save_operator_data(self): + def save_operator_data(self): """Save the accumulated operator data to an .npz file.""" np.savez(self.operator_npz_file_path, hue=self.operator_hue_avg, sat=self.operator_sat_avg, val=self.operator_val_avg, pose=self.operator_pose_median) rospy.loginfo(f"Saved accumulated operator data to {self.operator_npz_file_path}") - #def save_latest_detection_data(self, hue_vector, sat_vector, val_vector, head_feet_distance): + def save_latest_detection_data(self, hue_vector, sat_vector, val_vector, head_feet_distance): """Save the latest data for detection ID 1 to an .npz file.""" np.savez(self.detection_npz_file_path, hue=hue_vector, sat=sat_vector, val=val_vector, pose=head_feet_distance) rospy.loginfo(f"Saved latest detection 1 data to {self.detection_npz_file_path}") - #def publish_debug_info(self, hoc_distance_score, pose_distance_score, detection_id): + def publish_debug_info(self, hoc_distance_score, pose_distance_score, detection_id): """Publish debug information about the current comparison (General).""" debug_msg = String() debug_msg.data = f"Detection ID {detection_id}: HoC Distance score: {hoc_distance_score:.2f}, Pose Distance score: {pose_distance_score:.2f}" diff --git a/people_tracking_v2/scripts/decision_node.py b/people_tracking_v2/scripts/decision_node.py index cf209aa..b629af9 100755 --- a/people_tracking_v2/scripts/decision_node.py +++ b/people_tracking_v2/scripts/decision_node.py @@ -33,7 +33,7 @@ def __init__(self): # Publisher for decision results and marked images self.decision_pub = rospy.Publisher('/decision/result', DecisionResult, queue_size=10) - #self.marked_image_pub = rospy.Publisher('/marked_image', Image, queue_size=10) + self.marked_image_pub = rospy.Publisher('/marked_image', Image, queue_size=10) # Initialize variables for saving data self.save_data = save_data diff --git a/people_tracking_v2/scripts/yolo_seg.py b/people_tracking_v2/scripts/yolo_seg.py index 42edb88..8d215ee 100755 --- a/people_tracking_v2/scripts/yolo_seg.py +++ b/people_tracking_v2/scripts/yolo_seg.py @@ -19,8 +19,8 @@ laptop = sys.argv[1] name_subscriber_RGB = 'Webcam/image_raw' if laptop == "True" else '/hero/head_rgbd_sensor/rgb/image_raw' -depth_camera = False #if sys.argv[2] == "False" else True -save_data = False #if sys.argv[3] == "False" else True +depth_camera = False if sys.argv[2] == "False" else True +save_data = False if sys.argv[3] == "False" else True class YoloSegNode: def __init__(self):