From 478fb26d416fea7f331dcfb480c550d39530218b Mon Sep 17 00:00:00 2001 From: Xu Shiyuan Date: Mon, 18 May 2026 16:43:29 +0800 Subject: [PATCH] update --- .../config/example_object_detection_vlm.json | 50 +- .../__pycache__/semantic_nav.cpython-310.pyc | Bin 6235 -> 3405 bytes src/vlm_nav_pkg/vlm_nav_pkg/semantic_nav.py | 439 +++++++++++++----- .../__pycache__/vlm_detection.cpython-310.pyc | Bin 16793 -> 9008 bytes .../vlm_perception_pkg/vlm_detection.py | 344 +++++++------- 5 files changed, 507 insertions(+), 326 deletions(-) diff --git a/src/vlm_nav_pkg/config/example_object_detection_vlm.json b/src/vlm_nav_pkg/config/example_object_detection_vlm.json index 2023e54..2befd64 100755 --- a/src/vlm_nav_pkg/config/example_object_detection_vlm.json +++ b/src/vlm_nav_pkg/config/example_object_detection_vlm.json @@ -1,47 +1,47 @@ { "Refrigerator": { - "highest_score": 0.301025390625, - "last_detected": "2026-04-29T11:38:32.954873", + "highest_score": 0.301513671875, + "last_detected": "2026-05-18T14:13:25.085735", "position": { - "x": -3.258748492688493, - "y": -0.657160436573238, - "z": 0.23105746596100607 + "x": -1.3181, + "y": 0.5165, + "z": 0.7319 } }, "water dispenser": { - "highest_score": 0.310791015625, - "last_detected": "2026-04-29T11:38:29.064743", + "highest_score": 0.316162109375, + "last_detected": "2026-05-18T14:15:56.440127", "position": { - "x": -2.7678260060491566, - "y": -0.2820617355615878, - "z": -0.006539989127681528 + "x": -1.6292, + "y": -1.7906, + "z": 0.661 } }, "sofa": { - "highest_score": 0.316650390625, - "last_detected": "2026-04-29T11:36:16.740557", + "highest_score": 0.307861328125, + "last_detected": "2026-05-18T14:17:27.585956", "position": { - "x": -1.444039469788105, - "y": -1.389487320024688, - "z": 0.9400984640933344 + "x": 1.4535, + "y": -0.4709, + "z": 0.5445 } }, "white toilet": { - "highest_score": 0.29931640625, - "last_detected": "2026-04-29T11:32:23.241802", + "highest_score": 0.269287109375, + "last_detected": "2026-05-18T14:13:07.308074", "position": { - "x": 0.5281400126286391, - "y": 7.222577558743074, - "z": 0.24172188472363199 + "x": -4.3598, + "y": -1.1593, + "z": 0.3363 } }, "office chair with wheels": { - "highest_score": 0.316650390625, - "last_detected": "2026-04-29T11:34:58.447898", + "highest_score": 0.3037109375, + "last_detected": "2026-05-18T14:12:39.416197", "position": { - "x": 3.982877977168086, - "y": 1.6895332207339573, - "z": 0.5514825779390112 + "x": -2.8461, + "y": 2.6732, + "z": 0.4811 } } } \ No newline at end of file diff --git a/src/vlm_nav_pkg/vlm_nav_pkg/__pycache__/semantic_nav.cpython-310.pyc b/src/vlm_nav_pkg/vlm_nav_pkg/__pycache__/semantic_nav.cpython-310.pyc index b5c4f83afd78d6fa61e2398a2bfd3355b9d6e08c..3aafcdcf032927b27be9f69a5fb963460854c0b5 100644 GIT binary patch delta 1627 zcmYjR%WvF77@x6cz25gbn-`mH2`UR}lQa@V5kyUaB7|5W1u4Q5xtokP*{!`^#*UUm zUPWSq;8LMri33o{dqL`@2gJX@38@F?!l|Mh5a%8!-{(9UTl4eG_nw*G=i|lSr?Yk@ zlO*t@)mG=9%u#lh{&4hq?LabRi)oA!PB=AVS}gi1ngU1W60RERNY?T^!IOtXD;Pzdx+jSbjKYY32VIXwBmI=* zp`vDkhAKCV{+8X}JNm0fSN#n-H*d&oiO0Bd$e<_|tuQ##ly3+lT6~=}68^9B4e1&2 z|E9MNT%{Wi)G-lPxYVOu_K2QvlU>G{N4g0fyD#05K$aTHZW<)9ATuT-3X-gw400gJ zj%kob+c6z7R|yJZ^4%mQAV~#9kAXDi!P_ly1-E%$ngN|&-hiWHge{SgOi52&B2Ifi zDeRMyPhq%~5t>avn2De|28l<4$uSA0{3p^jo%WmZqW`n}_}V-i#X}kHfR5DV_+vOaJYHg`KF_aCvVr3|<@HM~@sl(Tg9d71hDvTO69(igVX zcef2EhZWr_rO9)rEbJ;jT+B{)^3DY9VmiO2y83J|J&+v&I+ zyVod)X_U?()Df@-F^O;qVFuwc!pjKremq~RVu_*(5X#$z+fYRV1xx1l zN3e)i=`VH-v8UUcU1Q6|b8*T5WtyaETBfuA6&Y-p<|UQ-*9&j1+=Uw9P>Dy!^o}%? zhO#GfvI{9eO5vy_PCe--@<1BH#c;AtTzN!>j7$D6g}DQHig>J>>ZbJ!__6a<){}!A zGH@)N>hoNQW(7UZy&E*P^`xQbbM!jmC4@F(Ju5=@MVa}ap} z9fq+3FDCX+3$5@Z5NwjCsstR<59EtZ!ib!vz)A5Gce*)y?sDbcx+Z|>KccKu$@9`x#G*IsYiqJDB6EA>#eTK$lI8PdIw?r+1* z5aKVFZb(n5Z+Uwh$p6du?w|LcGD{mG~2!&p(2hqjb{%_?= zsSC97|14K8FM)xx0+6LD5Q$L*C^SK{@D_nkNm}%$Dz~ZX->uAl6v~b(LVP{#CxA#s2D3J6Zx6ZW6A3N!j!N0ScaW AP5=M^ literal 6235 zcmZ`-&6C?kc1Hss2!bE;snLuiyI^}aGPBaCJo36yO1x`XmUg|d$BHFewG(ep7&gcO z2L!kcP%~tKPAcQno?NbcU1GbI+{0G+kpCd3RQ`k9`kF(^x2>EKS0d&28svN?WTE?Y z_v^3M{eJKDOX_t;!|$u#{a5%mFKXJqQQ`E@MBy!b>2Hy6jk8GGDRQ>MR9WBARc`DU z$o0tVm3B(3;IVcr)mGjqtK8nPksDE^=j=F&$Be4I+D=W;rKsL(>@<1{I}568MT@gukW|^ z{Z8Ped%g%f9*Up^&S8sYIsGG9(J%cNNuZH@AR87KZT(kTn{k~RpVf9uZt@aVU*Z-o zBe%HCE6B^-Ax>W9wa-k*sVLQX1EtEZG*jDg9%;=5x$r3H`EjP!Ok0wbZkohcWVYAR z)%-x#pFg;M7d~j|`^8_G?;t z^#Hu||2e)~n^;q%UOBV7m5DRe#0{>;j2oz{X0H9TK&Pu{y2Pm z^X*%=w{ZFv51xjt;M?c;J^n_Q1`m6(ydMO85Tk|TOXG^b=fmbTSxN`}KuFv3=+=3j zG?RW1OOsAWno;6&=_I>d+#fFrcjcl-RbJ2Ud%MXX=Bac}(Is56)}E;1{A4XmJ^!g6 zM*eOTh&ol7ttjlvQV;8oFm{+`(%cV*sjSUri2YuWN;^yTgE$-oQqK}ujuWpVu#9RD zx6sqefElcpujG>F=wLfu(gGEY|3z-k?-_&wBr!(_$tT9Kcop7)LR-3Pb!l3sA@aIm#? zOC+gxklso~>(H;c=Z?N= ztg%I7l{u`&*4VXYcFAGOY_xt(qc-Qq*Mim@{~Dm^Ebie;|3m3hmrbDGT%YO_W2%i= z&csrtb0gl6)hCVIAl9Y-rq}@^Pt3C0!p|<8rnLj+)}O)oPY<8$0@<*oR7&X^*JY$9A>?O-9mrFh0p4-6e_AKZD5wLRL-WeEtZ(1_c*}eL?nCe4-N!$bnp}dqWKpo2WVgMJKLQ~Q zXz^$vX}19$q}`iCzZZ=jz3=zkLeJd+F6vC1-7t1f-`_uU+kO=7!sehl0#t-a>>h+s zlA!WbMNNz^@nF7XofV3C;kZ z{`WCUvmt&!li3ifrzR|CySK?t5I3maXtg`y4~Wu}LwrN&DB&xUEse0(k!(-018L2S z01L!xU@wv_qSldI$CvIRS!Szjk?G7vUPG!sU(}H@^SNp2sHp+#YWmBHT4N1xG=Ort z?tsT()_>a;^);WXPdA_0y7kO_Zj~As58QvDKO4Ch5b>O$I^$`Or65m3A>$#442YGn zBRz%J_edM9k+;E_LJnQg(05>tLG;An1~;d`1466Vfp(h1>vOMz`X6XnDM!0_9XXIw z<>nDf-=Nl9&kf*G=_>=)-P)GyP_m(9qkqRb)URm>lA0iQO>iVIQBIhKahmyY3$~w! ztxRlF>pPVEj1r|4rQ%$Tev+OW;@tVjWiRz%@L^>=FoQ_q4>3$i=jJdIf~RAl$(YQ^zsEEUl`D;XQaVQv8M3~iq!FDjnW z2IyGV;T|Qgs~Su3m5=$hR4vTHKE@?J2)45Q;IY#7LJ*|sg@!AszJ+3wi4DqhB{Z=| z9A|H_Y``lz{Z=m`ZH6<1$^;>Y|eC*R$nB&es)GaH5v+^lFz>= z13iIB?!Y8X;Es05AnF;6Gn~!K%GtCgWpaJT2Ck6NwaJuo<8%E5ZERdHoUfiXoZ#2G zm9Dc$Mwr}HGdCwSnB+R#fD+moIpjRHKrKubb1T$f1Yyo5i!k+UZ0C3e;jBJZH8A3+ zp|&!p3Jj7mtYb$}FkCQ;4)~c6s9DaPDN$DLwCSXamfy@}41^!X<>KJ_807{B)a_MtUt;$PK#l+SwLYll9#o#j9+lXwp8W>hLH}8op!KN3UFW&oH~m z05-6F#sH08c>seO#vNA~U9wT9hjH79aIcSy>v!DI#z(-7&i~o|=sE_?V3xVNi#hiE z)QuB&0231h?tF!0>wos>gY8Wh#>6FWmIv9)#G~o%`}f{+$)R`Kp-8irMsHkK10``Z zq?O-H+Bato#C*h8)8-9#X4~OYkgpzWHZO^5B(0Yyd6^P=F^DUayh6zflzfkp?^8lP zxnz%JIsN4z2=G*}e9?g?0GB=sdcq~1%aka942dHAQRY_sF{oy((E5GQ(keWC>0mH1 zR1XTg3Bn%UBI+zcekd!7QfAl2a|OWAPGoI4II zq3QIrX8?(O6ncd#K>9NP5)=WDJZ128bpjowCU~z-7!(#SiCl-m>V*Q3ZOe=!ZEWIw zZFWtMUB)#vOq8BsXnU##3*XXTLe5%pCymp z;nrj&Hwh@Amc?pT0UWd%jDHLX#CK_`rJt%EYYOU0VtCvGemng6+GP#>=1M;6Be?3b zjy~}4vyJW*#BKiixMzcAR@R)_uA=SN$88H|+ODDPACKD*M=1JH-{#Wx=+%3Ln>$y9 zJEVk9{V3!oI#}rQ%hxyEhf(0Cfh#WPVxfNG4IJKi%O9YwgAc#o{dm;aE_9Ai0l+ab zT|aY2EB6nB794U^9{9sPoYc$}chF;`-+FDdI`0F&J{`1LLBIn(S~*n@=^gM4xNo!vkaRLuBOT1p%A@>^b zFSyqTg^3=Gcj6)`9p}`$ipN9{XTEZ+$y&>Wb`nL&!IRmhQsw{3Mwn92>>ey~1UQpb z%=!SkA~=e$;zwix0&ICcm4$~8!gl9zVL6`RqKDlV=5Um=aAjk6n?SqKun&z^Jt~X=IN}BcCo+UDP_H9+0oNPgr6Y8q>^OP? zjAr4r(zdLg3erfcmBj6^GkPgF#D&A&3;CazJrCTQ6doIGJlP9G;PS*BCIjdVbOsvL zN$$8!N0vk@>JKTz6lUt}C*H=Q1+84XO9|}|FS@ut$b^!Zo$d`Xo*cw@+%8>+;L3&y zn$lCUc!nVahB%-@Z%h~?8s(hxYWDmv{%6cm$XDG9n~_zh>EbsuJC2%oJP+pg60J$u zU}bb^u@WO>d+KE&-lJxEA&7sV7Dw%Fla?S1s#xj-2#95{p3v5G6G@@w-^U%>@?c9z zl&z$PKrp&&iUiRIihUKKIu*Jrbn7ba9&|4R+o9JVW_wAzITJ=&$dLr6)-V2!sFPhQ oif)#)|ERdQKcav_`aLArxcbxclm7I&SyZ3)=k^cn8kx%f1?$v}<^TWy diff --git a/src/vlm_nav_pkg/vlm_nav_pkg/semantic_nav.py b/src/vlm_nav_pkg/vlm_nav_pkg/semantic_nav.py index dc07467..8bc0c5f 100755 --- a/src/vlm_nav_pkg/vlm_nav_pkg/semantic_nav.py +++ b/src/vlm_nav_pkg/vlm_nav_pkg/semantic_nav.py @@ -1,24 +1,41 @@ #!/usr/bin/env python3 import json +import math +import time import clip import torch +from copy import deepcopy import os -from math import isfinite -import yaml -import cv2 import rclpy from rclpy.node import Node from geometry_msgs.msg import PoseStamped +from tf2_ros import Buffer, TransformListener +from rclpy.duration import Duration +from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy, HistoryPolicy +from nav_msgs.msg import OccupancyGrid from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult from ament_index_python.packages import get_package_share_directory + + class SemanticNavNode(Node): def __init__(self, json_path: str): super().__init__('semantic_nav_node') + self.declare_parameter('approach_offset_m', 0.7) + self.declare_parameter('approach_offset_refrigerator_m', 0.7) + self.declare_parameter('approach_offset_water_dispenser_m', 0.7) + self.declare_parameter('approach_offset_sofa_m', 0.5) + self.declare_parameter('approach_offset_white_toilet_m', 0.7) + self.declare_parameter('approach_offset_office_chair_with_wheels_m', 0.45) + self.declare_parameter('approach_candidate_count', 8) + self.declare_parameter('approach_clearance_m', 0.30) + self.declare_parameter('approach_clearance_office_chair_with_wheels_m', 0.18) + self.declare_parameter('approach_candidate_count_office_chair_with_wheels', 16) + self.declare_parameter('approach_offset_office_chair_with_wheels_search_radii', [0.45, 0.60, 0.75]) + self.map_msg = None # Load object library with open(json_path) as f: self.object_lib = json.load(f) - self.map_bounds = self._load_map_bounds() # Load CLIP self.device = "cuda" if torch.cuda.is_available() else "cpu" @@ -34,73 +51,21 @@ class SemanticNavNode(Node): # Initialize navigator self.navigator = BasicNavigator() self.navigator.waitUntilNav2Active() - self.get_logger().info("Navigator ready") - - def _load_map_bounds(self): - map_yaml = os.environ.get('NAV2_MAP_PATH', '').strip() - map_yaml = os.path.expanduser(map_yaml) if map_yaml else '' - if not map_yaml or not os.path.isfile(map_yaml): - try: - tb3_dir = get_package_share_directory('turtlebot3_gazebo') - map_yaml = os.path.join(tb3_dir, 'map', 'office_map.yaml') - except Exception: - map_yaml = '' - if not map_yaml or not os.path.isfile(map_yaml): - self.get_logger().warn( - "Map bounds unavailable in semantic_nav; " - "fallback selection will skip bounds checks." - ) - return None - try: - with open(map_yaml, 'r') as f: - cfg = yaml.safe_load(f) - resolution = float(cfg['resolution']) - ox, oy = float(cfg['origin'][0]), float(cfg['origin'][1]) - image_path = str(cfg['image']) - if not os.path.isabs(image_path): - image_path = os.path.join(os.path.dirname(map_yaml), image_path) - img = cv2.imread(image_path, cv2.IMREAD_UNCHANGED) - if img is None: - raise RuntimeError(f"cannot open map image: {image_path}") - h, w = img.shape[:2] - bounds = { - "min_x": ox, - "max_x": ox + w * resolution, - "min_y": oy, - "max_y": oy + h * resolution, - } - self.get_logger().info( - "Semantic map bounds: " - f"x=[{bounds['min_x']:.3f}, {bounds['max_x']:.3f}], " - f"y=[{bounds['min_y']:.3f}, {bounds['max_y']:.3f}]" - ) - return bounds - except Exception as e: - self.get_logger().warn(f"Failed to load map bounds: {e}") - return None - - def _sanitize_position(self, pos): - if not isinstance(pos, dict): - return None - try: - x = float(pos["x"]) - y = float(pos["y"]) - z = float(pos.get("z", 0.0)) - except Exception: - return None - if not (isfinite(x) and isfinite(y) and isfinite(z)): - return None - return {"x": x, "y": y, "z": z} - - def _in_map_bounds(self, pos): - if pos is None: - return False - if self.map_bounds is None: - return True - return ( - self.map_bounds["min_x"] <= pos["x"] <= self.map_bounds["max_x"] - and self.map_bounds["min_y"] <= pos["y"] <= self.map_bounds["max_y"] + self.tf_buffer = Buffer(cache_time=Duration(seconds=10)) + self.tf_listener = TransformListener(self.tf_buffer, self, spin_thread=True) + map_qos = QoSProfile( + depth=1, + reliability=ReliabilityPolicy.RELIABLE, + durability=DurabilityPolicy.TRANSIENT_LOCAL, + history=HistoryPolicy.KEEP_LAST, ) + self.map_sub = self.create_subscription( + OccupancyGrid, + "/map", + self.map_callback, + map_qos, + ) + self.get_logger().info("Navigator ready") def query_object(self, prompt: str): tokens = clip.tokenize([prompt]).to(self.device) @@ -112,46 +77,18 @@ class SemanticNavNode(Node): best_idx = sims.argmax().item() best_name = self.object_names[best_idx] - # Align with vlm_detection: - # prefer best_position/position, fallback to last_position when best is out-of-bounds. + # Align with vlm_detection: position may be None if not yet detected obj_info = self.object_lib.get(best_name, {}) - best_pos_raw = obj_info.get("best_position") - if best_pos_raw is None: - best_pos_raw = obj_info.get("position") - last_pos_raw = obj_info.get("last_position") - best_pos = self._sanitize_position(best_pos_raw) - last_pos = self._sanitize_position(last_pos_raw) + pos = obj_info.get("position") - if best_pos is not None and self._in_map_bounds(best_pos): - return best_name, best_pos - - if best_pos is not None and not self._in_map_bounds(best_pos): + if pos is None: self.get_logger().warn( - f"'{best_name}' best_position out of map bounds: {best_pos}" - ) - if last_pos is not None and self._in_map_bounds(last_pos): - self.get_logger().warn( - f"Falling back to last_position for '{best_name}': {last_pos}" - ) - return best_name, last_pos - - if last_pos is not None and self._in_map_bounds(last_pos): - self.get_logger().warn( - f"Using last_position for '{best_name}': {last_pos}" - ) - return best_name, last_pos - - if best_pos is None and last_pos is None: - self.get_logger().warn( - f"'{best_name}' matched but has no usable position in JSON. " + f"'{best_name}' matched but has no position in JSON. " f"Run vlm_detection in AMCL mode first!" ) return best_name, None - self.get_logger().warn( - f"'{best_name}' has only out-of-bounds position(s), cannot navigate." - ) - return best_name, None + return best_name, pos def navigate_to_object(self, prompt: str): name, pos = self.query_object(prompt) @@ -159,35 +96,299 @@ class SemanticNavNode(Node): self.get_logger().warn(f"Cannot navigate: no valid position for '{name}'. Please run vlm_detection first!") return + robot_pose = self.get_robot_pose_in_map() + if robot_pose is None: + self.get_logger().warn("Cannot navigate: failed to get robot pose in map frame.") + return + + offset_m = self.get_approach_offset(name) + goal_x, goal_y, yaw = self.compute_approach_goal(name, robot_pose, pos, offset_m) + robot_x, robot_y = robot_pose + distance_to_goal = math.hypot(goal_x - robot_x, goal_y - robot_y) + goal_pose = PoseStamped() goal_pose.header.frame_id = "map" goal_pose.header.stamp = self.navigator.get_clock().now().to_msg() - goal_pose.pose.position.x = pos["x"] - goal_pose.pose.position.y = pos["y"] - goal_pose.pose.position.z = pos.get("z", 0.0) - goal_pose.pose.orientation.z = 0.0 - goal_pose.pose.orientation.w = 1.0 + goal_pose.pose.position.x = goal_x + goal_pose.pose.position.y = goal_y + goal_pose.pose.position.z = 0.0 + goal_pose.pose.orientation.z = math.sin(yaw / 2.0) + goal_pose.pose.orientation.w = math.cos(yaw / 2.0) - self.navigator.followWaypoints([goal_pose]) - self.get_logger().info(f"Navigating to {name} at {pos}") + self.navigator.goToPose(deepcopy(goal_pose)) + self.get_logger().info( + f"Navigating to {name}: object={pos}, goal={{'x': {goal_x:.4f}, 'y': {goal_y:.4f}}}, " + f"approach_offset={offset_m:.2f}m" + ) + self.get_logger().info( + f"Robot pose={{'x': {robot_x:.4f}, 'y': {robot_y:.4f}}}, " + f"distance_to_goal={distance_to_goal:.4f}m" + ) # Wait until navigation completes while not self.navigator.isTaskComplete(): feedback = self.navigator.getFeedback() if feedback: self.get_logger().info( - f"Executing waypoint {feedback.current_waypoint + 1}/1" + "Navigation task in progress" ) result = self.navigator.getResult() + final_robot_pose = self.get_robot_pose_in_map() + if final_robot_pose is not None: + final_x, final_y = final_robot_pose + final_distance_to_goal = math.hypot(goal_x - final_x, goal_y - final_y) + self.get_logger().info( + f"Final robot map pose={{'x': {final_x:.4f}, 'y': {final_y:.4f}}}, " + f"distance_to_goal={final_distance_to_goal:.4f}m" + ) if result == TaskResult.SUCCEEDED: self.get_logger().info("Navigation succeeded") elif result == TaskResult.CANCELED: self.get_logger().info("Navigation canceled") elif result == TaskResult.FAILED: self.get_logger().info("Navigation failed") - - + + def map_callback(self, msg: OccupancyGrid): + self.map_msg = msg + + def get_robot_pose_in_map(self): + target_frame = "map" + source_frame = "base_footprint" + last_error = None + for _ in range(15): + try: + if not self.tf_buffer.can_transform( + target_frame, + source_frame, + rclpy.time.Time(), + timeout=Duration(seconds=0.2) + ): + continue + + transform = self.tf_buffer.lookup_transform( + target_frame, + source_frame, + rclpy.time.Time(), + timeout=Duration(seconds=0.2) + ) + return ( + transform.transform.translation.x, + transform.transform.translation.y, + ) + except Exception as e: + last_error = e + + if last_error is None: + self.get_logger().warn( + "Robot pose lookup failed: transform map -> base_footprint was not available within timeout." + ) + else: + self.get_logger().warn(f"Robot pose lookup failed: {last_error}") + return None + + def compute_approach_goal(self, object_name, robot_pose, obj_pos, offset_m): + robot_x, robot_y = robot_pose + obj_x = obj_pos["x"] + obj_y = obj_pos["y"] + search_radii = self.get_approach_search_radii(object_name, offset_m) + candidate_count = self.get_approach_candidate_count(object_name) + approach_candidates = [] + for radius in search_radii: + approach_candidates.extend( + self.generate_approach_candidates(obj_x, obj_y, radius, candidate_count) + ) + feasible_candidates = [ + candidate for candidate in approach_candidates + if self.is_candidate_feasible(object_name, candidate[0], candidate[1]) + ] + + path_candidates = [] + start_pose = self.build_pose_stamped(robot_x, robot_y, 0.0) + for candidate in feasible_candidates: + candidate_x, candidate_y, candidate_yaw = candidate + candidate_goal_pose = self.build_pose_stamped(candidate_x, candidate_y, candidate_yaw) + path = self.navigator.getPath(start_pose, candidate_goal_pose, use_start=True) + if path and len(path.poses) >= 2: + path_length = self.compute_path_length(path) + if path_length >= 0.2: + path_candidates.append((candidate_x, candidate_y, candidate_yaw, path_length)) + + if path_candidates: + goal_x, goal_y, yaw, path_length = min( + path_candidates, + key=lambda candidate: candidate[3] + ) + self.get_logger().info( + f"Approach candidates for {object_name}: total={len(approach_candidates)} " + f"feasible={len(feasible_candidates)} path_feasible={len(path_candidates)} " + f"selected=({goal_x:.4f}, {goal_y:.4f}) path_length={path_length:.4f}m" + ) + return goal_x, goal_y, yaw + + if feasible_candidates: + goal_x, goal_y, yaw = min( + feasible_candidates, + key=lambda candidate: math.hypot(candidate[0] - robot_x, candidate[1] - robot_y) + ) + self.get_logger().warn( + f"Approach candidates for {object_name}: total={len(approach_candidates)} " + f"feasible={len(feasible_candidates)} path_feasible=0, " + f"falling back to nearest map-feasible candidate=({goal_x:.4f}, {goal_y:.4f})" + ) + return goal_x, goal_y, yaw + + dx = obj_x - robot_x + dy = obj_y - robot_y + distance = math.hypot(dx, dy) + if distance < 1e-6: + yaw = 0.0 + return obj_x, obj_y, yaw + + ux = dx / distance + uy = dy / distance + goal_x = obj_x - ux * min(offset_m, max(distance - 0.05, 0.0)) + goal_y = obj_y - uy * min(offset_m, max(distance - 0.05, 0.0)) + yaw = math.atan2(dy, dx) + self.get_logger().warn( + f"Approach candidates for {object_name}: no feasible candidate found, " + f"falling back to line-of-sight offset goal=({goal_x:.4f}, {goal_y:.4f})" + ) + return goal_x, goal_y, yaw + + def generate_approach_candidates(self, obj_x, obj_y, offset_m, candidate_count): + candidates = [] + for i in range(candidate_count): + angle = (2.0 * math.pi * i) / candidate_count + goal_x = obj_x + offset_m * math.cos(angle) + goal_y = obj_y + offset_m * math.sin(angle) + yaw = math.atan2(obj_y - goal_y, obj_x - goal_x) + candidates.append((goal_x, goal_y, yaw)) + return candidates + + def is_candidate_feasible(self, object_name: str, world_x, world_y): + map_msg = self.wait_for_map() + if map_msg is None: + return True + + info = map_msg.info + resolution = info.resolution + origin_x = info.origin.position.x + origin_y = info.origin.position.y + width = info.width + height = info.height + grid_x = int((world_x - origin_x) / resolution) + grid_y = int((world_y - origin_y) / resolution) + + if grid_x < 0 or grid_x >= width or grid_y < 0 or grid_y >= height: + return False + + clearance_m = self.get_approach_clearance(object_name) + clearance_cells = max(0, int(math.ceil(clearance_m / resolution))) + + for dy in range(-clearance_cells, clearance_cells + 1): + for dx in range(-clearance_cells, clearance_cells + 1): + if dx * dx + dy * dy > clearance_cells * clearance_cells: + continue + xx = grid_x + dx + yy = grid_y + dy + if xx < 0 or xx >= width or yy < 0 or yy >= height: + return False + cost = map_msg.data[yy * width + xx] + if cost < 0 or cost >= 50: + return False + return True + + def build_pose_stamped(self, x, y, yaw): + pose = PoseStamped() + pose.header.frame_id = "map" + pose.header.stamp = self.navigator.get_clock().now().to_msg() + pose.pose.position.x = x + pose.pose.position.y = y + pose.pose.position.z = 0.0 + pose.pose.orientation.z = math.sin(yaw / 2.0) + pose.pose.orientation.w = math.cos(yaw / 2.0) + return pose + + def compute_path_length(self, path): + if not path.poses or len(path.poses) < 2: + return 0.0 + length = 0.0 + for prev_pose, next_pose in zip(path.poses[:-1], path.poses[1:]): + dx = next_pose.pose.position.x - prev_pose.pose.position.x + dy = next_pose.pose.position.y - prev_pose.pose.position.y + length += math.hypot(dx, dy) + return length + + def wait_for_map(self, timeout_sec: float = 2.0): + if self.map_msg is not None: + return self.map_msg + end_time = time.time() + timeout_sec + while self.map_msg is None and time.time() < end_time: + time.sleep(0.05) + return self.map_msg + + def get_approach_candidate_count(self, object_name: str) -> int: + default_count = int( + self.get_parameter('approach_candidate_count').get_parameter_value().integer_value + ) + param_name = ( + "approach_candidate_count_" + + object_name.lower().replace(" ", "_") + ) + if self.has_parameter(param_name): + default_count = int(self.get_parameter(param_name).get_parameter_value().integer_value) + return min(max(default_count, 4), 16) + + def get_approach_search_radii(self, object_name: str, default_offset: float): + param_name = ( + "approach_offset_" + + object_name.lower().replace(" ", "_") + + "_search_radii" + ) + if self.has_parameter(param_name): + value = self.get_parameter(param_name).value + if value: + return [float(radius) for radius in value] + return [default_offset] + + def get_approach_clearance(self, object_name: str) -> float: + default_clearance = self.get_parameter( + 'approach_clearance_m' + ).get_parameter_value().double_value + param_name = ( + "approach_clearance_" + + object_name.lower().replace(" ", "_") + + "_m" + ) + if self.has_parameter(param_name): + return self.get_parameter(param_name).get_parameter_value().double_value + return default_clearance + + def get_approach_offset(self, object_name: str) -> float: + default_offset = self.get_parameter( + 'approach_offset_m' + ).get_parameter_value().double_value + param_name = ( + "approach_offset_" + + object_name.lower().replace(" ", "_") + + "_m" + ) + if self.has_parameter(param_name): + return self.get_parameter(param_name).get_parameter_value().double_value + return default_offset + + def cleanup(self): + if hasattr(self, 'tf_listener'): + try: + if hasattr(self.tf_listener, 'executor'): + self.tf_listener.executor.shutdown(timeout_sec=0.5) + if hasattr(self.tf_listener, 'dedicated_listener_thread'): + self.tf_listener.dedicated_listener_thread.join(timeout=0.5) + self.tf_listener.unregister() + except Exception: + pass + @@ -197,10 +398,14 @@ def main(): json_file_path = os.path.join(package_share_dir, 'config', 'example_object_detection_vlm.json') node = SemanticNavNode(json_file_path) - user_input = input("Where do you want to go: ") - node.navigate_to_object(user_input) - - rclpy.shutdown() + try: + user_input = input("Where do you want to go: ") + node.navigate_to_object(user_input) + finally: + node.cleanup() + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() if __name__ == "__main__": diff --git a/src/vlm_perception_pkg/vlm_perception_pkg/__pycache__/vlm_detection.cpython-310.pyc b/src/vlm_perception_pkg/vlm_perception_pkg/__pycache__/vlm_detection.cpython-310.pyc index 3485a9faae1f789a4e6452b9775b913935704746..bf68c90e9faba54d7b52e6d39bcf57a6795c7de3 100644 GIT binary patch delta 4996 zcmZ`-O>i5@b)KFX07DP}L5kv^_yhhE5=H&4sFf*Eu~uB~I;J<#+VV0=CLnr14mg;> z_6$T383dc6;&oi1qxCrDa#mXg_Y}pK%_)^rDpjsj<(M3j)Ra?p;zOcCQa z0BTo^1603${rbH|_v`o9_})JIWG&;Q(@6=RlvXfz&VG;?WlJBNdE8{yz{Vh#c)}X0 z3~vlqMm9#6gfX*DZJa`{S)-M)jj@QM*zWhoVP zL#@ng%tT{m&2G%SF7Y(aJd}9GkoT31dEUo+c=jRNSQs+$ymv}8=;gA_xB_B=)07s$t_UV0*zKjfG*@}-fbsYN15d{5@uhOcm-c0hvR-ewk0(G$ zaP2_8N3;`Y{ocv)KzXn{#JhO%fVGAXY5kEciFY?Yre#j?6yT^YwZ@{UabOc(4!Ttj zX+x73PkGZEKUAIpO$BYXL!%AMb!hVuZGmS&%Xes}qqU}aZ)dGDT!QlY*28oW+6(T) z`{k#}z)u8A^OL}fsVU+ z$+V2H&vFV}e`z92zdXzqg;DSfy;|F}Om`d0rGtm*4VDRhoW6E}g{o^Ib$}(6xRPX9CHSw*7lTnvYu+OMIRKw-5e5#3){+mUmcnuUW%i-;kxcd$X!D;=8{*?!Ga%Umzy#MU1-r^ zz5&F;wp+A$<&cRL55`tjINO*1WWK@bX%F7wvqN@Qx+}>NmsR-VzQSNss^rDWiM>Sz ze`m1pmIgI+aje3mcO;wbLgG_?Te~2+%e!p6&HfGr3Aoln2$qwtN&5&G`FMRPcw=&O zz!ACjmFH|@H|N-f8#1vTd~Zyei`aGx z!WI{RKIvQ}D;?W~xP;lSpt%D889Bpb2CMvKM(OKf@^hJu{vw+Q-kRwTzBc{QSD&h2 z%Cj(T+T5_c^?Xc_Y_uuyRbZjYYn7_FOvHBxylgaKpIf+ZyzGw!DY!NBYfZ8uT0JtH zM?gG8S6n%)zoC*xl$HBTjQFyz`Y@o!h-78OXDehH>LJ}X?tv?f7&xMtVOYShw4;ip z&r(*v2cF0beFmNwR#14nS!3d+-&qvo1jsS5cC@%3!zF9=YhS5y#0v5$iZMB}l=HSt z^e%cMw{3218?Kj&0!*$1LyKnHxBo?)>FqV~>*&On0M-S04>U+N76$$K>>H8V$QJVP z$nV5kG!=`Mi@3v7u_gpGuX|?22otqvJN&qo`vbPnf35^!cLpFOCuMwav#DoVOl7J} zF-T<@S&MoNczrHtEzAY~e){9)IQ?iEVF6CBdyZbbe^JmfVSL*taD;}K>lJXx)1ZkM zfymfmh@6fH(^cUV4TQ-k&OA}t2l)cYGvnAs#Z{2sLv#B8WR}HtGJ(8!`||H#T<^w3 zB6`5x;A#hy|Kz)pJ%-4j>@6^h%Ew>1YT@Y zlxes_z#{OneTr+KeG|>i0?2G2c4jd3s6nuH-waH)}N9efN~B_4@%_>Opv=J|=oN+XTM7k$l7_-WpcA484N zI*TnWq4lHAm@1#t2XFxDhj2|ZxTbTD*xq-WxT$H})L`cf=lMy0`b8p(HB!*W1wKF< zxQoaR4Nd?4v>j{ryNL70_2nl09288NgR-Mjp67!k-%gCahQkf{2|oOx`jOIT&iE;? z{*6DwN0z0=%#kYn84`K-`ukFe@l$;Cq2{Zti-!_ri^eP;14RPmQin1Jxyy({v;G_| zX1-3<(<@N6RK8MP_UHLHpE!tebH_xlK9cv+bo84zJ4&d|K69U*{a2rsPu?R-#~tfP zGl)7SAv1I>Gw&yb`hpA=?XC zJ&(+Z_3C|ZujMSq*{M}?B~)w%zn0tjv+vR8uUG36$0u`zl4pqAZo%|Wkt_v2xl~?z z;fSa?zKkV9W}@ioDHd#QQq0viod<^7_E*XgNS`Rz+UfHb7)R-|KY0Ig;x(Y+~ZL$2UNGb|gO~iBj}GZ9R(gw9|HNrBn*lP%&*UjFnJ`dZBDr!L@^26CbAaA(57y36afw@P13cvPhgk817hlSgo%t$pjV6dEuzG$dr^8&35r=F zW!smddq)-$#zfJo?xBi)LFtZAtGy6~bn!ZY?&}1v&?<>ejVufrby2lgw4CCO_}yrH z`#}*!C~bp+EYgH!7}em>l~eH>7>n`XFRu)$`312W{L_`|tRD0)=fz&7$%W_L^}nF@6ass!i2MVkxW)pKnVVI`RYhyP`^h% zOy&}W{BHwse<%g#uHI{^3Z=b1CZlqdfnoGaOQ1BDQ&C#P_&GJexQ5yIvuwPdrI4fg z!9BpV_vB}NKTS?%AfHv^;6Um0-arhg6{&Ul4-|L0}7R1H45oC?17>VMvS z7kgS41mTnlH!Ff(n)yC4k1o_aXUDM3x)I8rBL<1Y*1|3uK3=du7I`9d8#Yybx@SD_ z#MeO)?+4$-+vRld_t#Emse+Fx3Gscxs)6#ki?==|^v?-AA@COjegF`vMd4IKEqWE; zT@)%Vw)Da|+q(QiFeX^=;B!BIRgmBbfR4LAU4_T}0}<&M?Xx7rL&Dn|0~RVMDS|&< zIr}?{3K%h?G|J&a2ojeBfN%rmQ%cvW!UY~8!yDNH`M0KsuX-LKzRT@!t zZ|8nGfgs^9-Fem(4a?H?&#ZrwSR&Ku0XBgamB<=B=a#gDmP*F75iO}{S}mIq#8YHJ z&yAs`50qX#J{}1NNHO&10Ny%h>IsyzC|TQ?&Q2pis(YOWi_cmyDq``nNzuy&WBiK% zS*ewvo-)C&rXJ_qJtgJkb=Zan(w}wDBL0PGfjqio+p|3 zDJ`SvI>|t<|BPY&G;JpMtJNEMQfGJqCSBZKvhAf(&5r(cDOd=Rw)i1t#orP*6O7*+ zx=gMh{*eH^F~n~YNC1Q}MB(b5_#?s-Ug@GP9Jl=!L09nj=E{7MCNfc^TjDi@YYI+O z%7w``F^Z6>;Q7tLW)zJxm=hlmctn71H9Cp-7s5Vug9wY1qODXMUbBoFWbZE3j~b~2 g{;wgrv~h|b^rb%nh)(lIJlTZQOg6QYnnbYqU+jJ7bpQYW literal 16793 zcmbt*d2k$8dS74D(=(VE3{C#36e#wv>J=<9n9+i2AGSl zdkCP@qvaY}TlR)t*-pxFR!RX$*;Oo`iJjPST&_4i%10`-f2As&s{E0p%Ec-t*(8-z zS-T?g``+sr%mAP^sQ@*v-`(%{?(geHI-OMT*Zkh!RK8VIl>bD9-ai9{m+^D2sfxlB zrdE{#J+-P@(+ZkOdA+LFjDjKZMm1J53uY}|h>NmXwF;IPm#8H*MOA78g;dm%E~L>C ztEOw2LMEDN5Hn??_MyU%7&%-R7SEBwh5x-c!;64e8>gN1{&Lxn?8-&`SwKFR9g+Du_a)D2XR)Q%R8ihQbitaiL` zT;$W$6Sb3tlOmsyJFevmc~$w6!UkFP1BGQBZCx*%Vxw$`4S%2(o^VdHk!6i*zJzWWNVumvYfcXC%bZ*cTUdc0V7%MD(0 zQL* z5S~LU#}4B;%x2gTJV$;~F_gmSO(lOU9J*YsG_N=w4h`F;8!7?zyMG#T7JjrT*F@?m z>&gbUvZ<!d=Dz@$!OR)`eQ{h8QtE)^$UHn63!(!OtCa^~2O&0IwEtcr!lWd@y zPqB12pYak)$&CTp{HE5e1*~=RI0njlDa@Z{#-_TISy%DyK`$$6%x=w4RAY5(hNGHf zw`Rl}6{Aw!nz5)RBWpIsH)*%~ya~~7sM~Kcnroz6v;RY7Y0UjamPY?E8_uDN2!k*ig)27 zbq$n;{xR;0o65#Tc9h_RVP*F7?<#e5g=Qc8NaZgHggB1(Sv~K20pniW+#dfDJAu~c zzoUGj(L8m!p_j#acK1Bl+Y|jRd9ScMJ0(_l8RaY9RrUn>Ttl6K^?WI+JB_;QQQZwu z_bNMsx-Um{uZg*?ve{^^*O>yuIXf4oOTZkP3Djxu&?vQ7G1N=VRye^O7la9fsKJ?Q zH|nCvsc#^B);8vIm~q1eY;kvM-9rq>~AQpMh&Ac zhhrr{itIAS(=HdQ)p-!aaEv>}s_WEUkVL2HEz0&VT`M;2wPLLbvgs{`>A6d9pR-@T z^rrphrCZlSC7hmjT#(exB1?^Wxx#C5u=|jxz~j2`BCE9Azxw;XNPiDsdPr2{ujIZI z<>4xF57aR3;!Ns{XnL|#EG;@VQFY&XyK?KyD`(H2L%qqIyOoj?#;;txcIhjx-9lA- z#i=YTdTu@zrrvVOys`k=3mWeau7EG_9ILoZ+9@uMhpxlnN)j(ZPeqOty-BcbZ{0wbF3$@uN z!&tRA?^H#NNIw0svx|+Ib9S|L?%Y}4aP1ZMtjkMh?^bJPT&Gs7dzI3edhzZ#s>+RZ3o|QER^VHCiN*=3&L3-9^A>ZJQXRZO>k;HcEhk z7iYJJh6D6^tb^Vgn&N^N{KL*F_*>Pn8}m!x1QArz>dJ2YfZnWy` zbj~f_1)Szyz4^u*PCQp?aqiST+)2cT{mIA9x$6RA*wP5?Z5)ZXfH++MFi_cyE$l2;&b`1{UE=s8=g;~szk%oelc=farVLR)BcAb=A{hvvMLvSPrQ z(juo54pSA^F5WFx0L4{@pQNfG@ct^vP|YII5@1%SHQX@n)bCb!qaNyzwnAgR(Wr)L zDR_0d#qJiXEr*ZMEU_{U&f|20d>UVqgS?9io6~UijpLiRKe*kd756H zK%aRbFY*B@51?F)#6=Ky(E89IkO1K#x?h8&R1F73zR8^?ZC>6Pluv z8z#DV9PTe;IS%KgdL5wS)*bKZC+%_*cy=x{92{dfP!^4Hep3(`H=Ni(mWy>(A(_|> zvo{Vr=NgF=RhtE&+$3X^oN)y4t{Y-orR zOZN@H_~KoN+P#y620ZqF1Q>y$oR1G=fQOZammsHIyO{a8N zACAi7dT+g{o77AEC@=pl@f_AN>Vz?_CNVatYKE*$Ze?@}a~WGnUHgF*OR9!uJ+jbB z?@1b#Mmb|E8#6SG`r&8(z%YCB2F?FyRQLBko^_OnyLA4Dh!APMjGy~+NCGubY@|!- zlGe1Cx}k3>Z7tB;K@yGvEl`#WsDk5EVyM5X@aLHxXx~wofwmZ)u}@-cJBf9kT3q;dq#QRh~ZunbHfpX9G%`p{f-z!gAY zB@L-C{v}lNIZBAU@i!@PC@E7?L6SElcP&6F>L1^IoxO11!I?g;OH!JNpSW7WASkJ1 z<)DA<;}|4eeU2YNJ!G%+BBU^)&jQKAw4kMuv^h*5wUBJXbRV&b`gKG~xlMsjBuw@2 z>ZeisP5j&x5{;mj{#5gC{6o7p*2n$(c0|BR++OU64iTWD@T3P&-5_vNHdW}T&|?CX zkBeF(fHup=16VBl2`|PpXgQi_F$0a1^-aC4M?C?a{B)qd1HgVq*f*f}#MhpaTF<+p zW(8*@fiWENTHLET^9}F3y-@U#33X6se^82~M73v07|&aRAM%&5TwX*HCY{wLhX9HKkNlH+T*98y_1y;1dt#^JHa-Nj0fiyU(doN`$g4nl zOB3%NHV3R@MSN{tRO)oXkGnrHApUz^)B&EZBltpYY((jfrArSmaiqhv&!U{2zV2*vv_(fpjR61VQThU}Ia-pUvHL zyj-8Ym}~G{S61vFHyf%vObYmD5mOIMP~?S5ofE;~#6ZKC5L*3xCBchxpwxAmBPXga z2ftGE3VJ>xq^kz*}VB6Bof4zf#wgAa`IpQdqL3LhG@@X#P^3KMRz z>S%bm#)}fnY(=b(%hG> zUV*~fg63Frgkco&{bCUUK5-RE1O^3b;eQ7ep;`{(bRebj0{`38<380|^5AiHo!lD9 z$3jJc1e=suAtH*=5S@w3k-{CuRw@iu0yB~bROgkBkawNP|oRb_V+XeJf5R$Ji1-&3I&K<|AXvj8R{zgT!-B`++~R z2Lak4`<8Spqw`03`U%qWHZnx~1@gwpI4w#**FZJUZCP!MiyDxTWwox6@xF0N)PS4> zTGZ#Hr~z47#thU44APvtCBRHT*EUq;RsJ=+G_OnMudasb+K%H3hiqtJQmc%l_y2(* zZUzZ)`lH&ksy%`r_sBo8$L4pA?UUJvS#%lu&!FI8r|Xd0mQL=TW%S#Rv9*I5kY75#2cw__l#AgF;hp|pw6f}jRE@^LXX2GR#(FXjS?4UA=j zUjnH$?i>q9`ZMY5^48q7K1M;bFDU-_2N(+q2e-%yQ7$0c- zEb^h++K;-3Z&PxF60r{&m5+yd)u~H0n5agm)tgWq9absU!`R|lv*8K# zIG>P^l;|!xv`R3wIGk`h$_bjd3Q+)B_CFDmvDHvxYoQ^k#FDz}3dg#ZzTN})6yx0e zNHT!I3H%dU6B1_$9aZgKQuR;#Umj*3OwvVkMYDMnAOr~M-cvl)gLo!9Bt5Xm^mVP< z21xCN)4nzcZ9ChT`LPZ-i_d-j-Ek5*KmX?7!IL-VKeNZKA7KS~BaCq}BS`-BJG7|ZrFy9$^WW>PavO-U z_KqX4Fuf4sNn@D?4VzZY&-NtWI|lXvOAN2-0?T(%03lEzd6`7I{Ba^@4CGr7EXqsy zPp5kLr-bK;&2O2^h zLKd$Ts}&!*Y6rO?kd-_Vu;v=>W97{BpbWcIi(EAjPr~4F* zWT1Kj>zaGLJKi8+oW|ov>SYT2u}XY$BOMqO1u6q<7#kU=Jm8aXQ-W`zF7BJ&U=Rm@ z#<8{mtS!6Nlq+LqJ1*Da4N(twg=)NE*gWFvT41u+MMcaMhk^p9DoV=6h^T>k6T7v# zWhe?LC%R=gJ5h$R!*n3hKO*%9>I~%{Adh;?m%w~UJd=1**}@a*RbX6H?z|olMIG=j zz9uB%RWO;z%F<&^ht|3c&%GW27w(=~ah87zx9A_gDNOLB*XEGpttQMYr*py_NORVm zl^g(n&Og)Bk-`CrUI!B(&pX}zku9Hp9g|CpxJNkw(LYYPbtDkpw}o-jq4mQwwM+K%@^7w6<;Swce!jS0KgLzO~ILXNQ z0m;7y;2G!`VEHB*!sIso^Czi7$Ff45frrz6VD5GnilsHexX|htS3*nJUr@ILNBI)Q zJOK=G-$cfM88HL2q?80|?U8nG`m>}-yxo8hPA1G0%q+y;(Yi%QOg*z&miRqN#Pi9l zC=83*UZ^XiaA6ec?5huT``G>!tO+z4h$|3v7)Y!w5#j)4NEE%Vt7KU*I!MTjn+o*( zsa`asx_r!^xd~fz*Wo1IPar`)SAsiX+g4XNkw!2@vr=b}mgYe0=(gp({{?cm#a6A! z$)nm~b95^^PjwM>U!_wesXC*Lsxw=AoyBhZ_*}`rh|V9$_FcC2uQ8t&89_iHa0Eob z?y7tkIBRUuF~J-{7*0?2H1{+@j&Le2yNDbiG-M7L`TfECnGwdx3!3l#Hok- zEJF?E6*g9q%$dpBoL+nV+ZW8uR~sk+2kSujzF&z3cw)$Cn#1TNNl9gO3X{p zOfMZI1Y692{Vk$(kb(Zd7z5n^GlIql4Is%7-~rhzLHb2|pl4K!|1wsS4F*8Ff7u&i z2>$5wL(de9CrQlwOXxF9ePA+4VQoa8(s*W~=b&6G(a8F9;?0BD3#|ZQ8`#l5@8mHi z>RUfbMk4I}FERHBxWFjO1}bdQLxB#niJ<6v%=902&lH&aKVy!uO^6mj96&i-OyWwv zBCN)_j@LI=b7}}klCEdgDpt=R?x?ifW%cfCGMS71&g44yEo`Y5$h``w01oRq=p>x+ zfZ-CkF3EZB=8A$)l3kW;-Q1b&nW#e^qKTxNEGLAB?yHH0E)+Sd!grNxly^_k5l1fM zD5?OqYIcScE5t~_Sd^<($SOhu`&PZZifjBH;9U^pIYEMN0mZ(7Dp)dTz0w>+L~a+w zAfzE>u%ltoD1nn*#jmszMM=&237o%Bv-}IYWejP~?{`w|C9v?ySHEQ zpZ|YeaAzJaSR9>@KXXIMiNciDxxF0YjV5_Qz(L^v(NDmTHO}L-7`@(DfjtOu6N0%4 zGPoBX6D&+}IkNSHN%+hoTXSd@F*$UGe})F3$A)^d(d2(f#eYM|pHkvbQl^ARX+9%m zPU3gs$Rt>#qWA$37ZRG1oQHA5ZqygZ$pE>)CRhtcws~e(5~Y}uj$>4E&MYPX(eL(f z;j!&>iQE{;Z6#y}JcXls!OLyBvnp1diYLKali-jf*lJoY2Tg)s61N2}MLRj)tVbCm z39bs>iZ%@{LE@*xRjC(g)uR|ePvW)6ZH?-ZN26Wa%eS-F;`%fw9g9rjcNssI4&H~w zeF^LaV5@F|71$bFcse+*V#mNf4nPbw*G|KljHo(cO%^UWa^UT-CTs6&ku@1PvL=HF zCWr_MSDZeFD1in3oS*GPb#|?%N&ZEwBTU1{2$z@bHEdWv`D>`-J~hOI|DJ!DDo6no zKoiD<@ik0GPIl=zgYHUBTOLsIE0pvbH+UU&zeJZcjEn|hhQt~=WLOg)rF8$`ZglJQ zIY0g=34Hx59p#cZN~Ya^++m*Cb(q2nD#O_#)BNWjrh%Z2^XINzehMN?mN_oMYM2ck z&|nag{qZa0{?0w-ocVvHW4S>Ioi6`xl)O$!_iXw1sQBM0A=Bf|Gi;*nH@44kc;^}B zcAsItgT9ZFp1^D*{}5ohe+{}vXGj)VH;Ei*DKwE3fsg=G4HA(`7D~uL(k=;oZd3Vo z7Q7aI;6Ph|Hp56N+U*{i?G+cn?O`bRDbmsm58;OE>J{Z9_4a>+qb3GvN^i&EAc5Aj z1R3S_sL+>47lN*ZH|Xez_936vpIbT{9AE?93}nL;f>_h{_4Yv!pg#uz zI)Wh5?*eNONb3K-cQiNzSuh93Sne+Fj4$|kj6a6i#%$Wb_DqA@GeMd|A0k=p!@*(X zk1y*OdkIi;0@B{e;4sOl)M6jUTi2tvmhsjZycMS;`sE=H&U6SL-DZTZGw;ttxkK+C zy7L0=7&Sx$*>JSV-}aA)wVe{X#EG#H?6tGPSoJ)uq#_N6B_P6r3YiOF+0!^VPxbk|E$Y#dI@eNkyzlqRCm0V;ih5Nw=u z)VtHK+hg)$UEs$o^ols#o%;a?&@mv1PXW88F0b(@T`JTw1(ga&3xwt z5k^c=?(j2goZVD}mT?sE?udIw4MLnn%`uF73fk4t;250w#|0Fg!@iWI^Gja{j|c{V8V^jm-ws2)J`$gFy#x(SiRL!kE!ujhg>*->XERZFfGq+^SZ$-+8xEgnqtC z5-G(H=1$NAC;THa&=Q&sl=w=W0^IN>xe)3{K3z^hF}^`QTuS{HgxQmaRGZ6n!cnBM z)uS;<^8j4I=gNL2cXICPJGr;6UVh`PD~I!aJc^7JB$1Ec=l1%cK&wGdr5|dWO+wz6 zUZ~0gwO6(jcPzVgJwPvDZmF7_-_17rWSbwVlMn|bI|M06?hF#)ElYz}!4e>JS@`Cd z3hdNAG31)_y07I<@=wwEQ#CJ^2H&Juyk4EJN*R2I2t^9ji&(XYHJ(xUe?b`_Xm#a} z!8c`OUsu4>=H~LxNr8^!4g^j%nrobv8d|p+w-BWbW8Xz8#_J7x0YT&ZJt`%fdW*iO zfj5iHY2hFYeWgWPgg?SHx+1hQWSQcQ^n~Ig|wf;QZgxfFGd&i7pU}Kv8K_zedd>;Oh;_Ih2ryE##>% z(;XtAnAJsxp?HH8(Ux^eh558CeD;1EP9`V;|o*;he*i3?&7zXttm+2_0Pqlv4b;IM(meGIYE%Oi+M@z>+Yz z-Ba2lRa<}u5#Kt%NV-`58HmUdnkNbE9hVIRUwkqJLM&p24IGS_Q{lQYH(DfkS2nGZ)#=u|Hw$#oslA}5Ko1-4Kicp=0pl2s58Xu07hW+<3xr- z%WEtr{y?!-%ttP>(mjJjn9b?z+ds;?#Bl(wS2b6#xjDn^8ZC6 z5#|F-uhb%60wI5M3$A0JdQ6Q2oQZ+2IcI-v?o2)9BhYmQa8p4hC+( z%xnZ{@F@f_0XHxUw<*dGY9z!zxX_@UA#Se><_OmxmKW&!99m;&g_8?mEa<}!%2GxU z%#uldWc0(=y>~DkaA3}XX<}2#!!0Dzd?`e?=csrX3EHoqgKVOn`8dbd!b;C&e~j8N z0U`=7(gFhT!5O?;IPh@v+8cAX?CV$Gx_SNTyY|f+ufO@))vv;%ONNe6Cu&6(C>>5j zxgUEuJMRHHv+`BC2V##|P^bN%A4hTehU!o6KK*S*^gx}h+Yz@*9K~TeK z$@t1ghTxZ+Y87Gl+t78FItzk5gxq2L+~Y_r3dwh^DQmz=Tc<3`%2)|&zjY=Vv#wi7 zE1`Vd8p&ojjoBsjb&1x`p_M$#0Z7+Jvu=KXT z-y4H&X-a=`$Cr#+n9u-ZDtVhRlQ5I9MZ^hh8M6Oe-Br%vL#Cn+llVRJ-$mNQ$g&-Rd5tzPHPVtW1sO5VW?Qc%_3 zyzyFSA-O@qlpyY$P?rA%B`;ATOy}4mVyp>l!Wg7i$hp5mZGVp{rhqQ6I}5qUZmxNY z^bMcejYa= diff --git a/src/vlm_perception_pkg/vlm_perception_pkg/vlm_detection.py b/src/vlm_perception_pkg/vlm_perception_pkg/vlm_detection.py index 20dc727..f8998dc 100755 --- a/src/vlm_perception_pkg/vlm_perception_pkg/vlm_detection.py +++ b/src/vlm_perception_pkg/vlm_perception_pkg/vlm_detection.py @@ -1,27 +1,27 @@ #!/usr/bin/env python3 import rclpy from rclpy.node import Node -from sensor_msgs.msg import Image +from rclpy.executors import MultiThreadedExecutor +from rclpy.callback_groups import ReentrantCallbackGroup, MutuallyExclusiveCallbackGroup from cv_bridge import CvBridge, CvBridgeError import torch import torch.nn.functional as F import torchvision from torchvision import transforms -from PIL import Image as PILImage import clip import cv2 import json import os +import time from datetime import datetime import numpy as np from ament_index_python.packages import get_package_share_directory from tf2_ros import Buffer, TransformListener -import tf2_geometry_msgs -from tf2_geometry_msgs.tf2_geometry_msgs import do_transform_pose from rclpy.duration import Duration -from rclpy.time import Time from geometry_msgs.msg import PoseStamped from sensor_msgs.msg import Image, CameraInfo +import tf2_geometry_msgs +from tf2_geometry_msgs.tf2_geometry_msgs import do_transform_pose @@ -31,16 +31,24 @@ from sensor_msgs.msg import Image, CameraInfo class ClipDetectionNode(Node): def __init__(self): super().__init__('clip_detector') + self.declare_parameter('reset_records_on_startup', False) + self.declare_parameter('detection_interval_sec', 1.0) + self.declare_parameter('frcnn_min_score', 0.3) + self.declare_parameter('frcnn_top_k', 2) self.bridge = CvBridge() self.device = "cuda" if torch.cuda.is_available() else "cpu" - #camera init + # Camera cache: subscriber callbacks only keep the latest data. self.latest_depth = None - self.camera_info = None - self.latest_stamp = None self.latest_depth_stamp = None - self.last_tf_warn_time = None + self.camera_info = None + self.latest_camera_info_stamp = None + self.latest_stamp = None self.latest_rgb = None + self.img_frame = "realsense_depth_frame" + self.processing_frame = False + self.sensor_callback_group = ReentrantCallbackGroup() + self.timer_callback_group = MutuallyExclusiveCallbackGroup() self.tf_buffer = Buffer(cache_time=Duration(seconds=10)) self.tf_listener = TransformListener(self.tf_buffer, self) @@ -51,32 +59,31 @@ class ClipDetectionNode(Node): # Labels and score tracking self.labels = ["Refrigerator","water dispenser", "sofa", "white toilet", "office chair with wheels"] - - # Save directly to vlm_nav_pkg config so semantic_nav reads the same file. + + # Initialize score tracking - save directly to vlm_nav_pkg config + # so semantic_nav reads the same file without manual copy try: nav_pkg_dir = get_package_share_directory('vlm_nav_pkg') - default_records_file = os.path.join( + self.score_records_file = os.path.join( nav_pkg_dir, 'config', 'example_object_detection_vlm.json' ) except Exception: self.get_logger().warn( "vlm_nav_pkg not found, saving JSON to current directory" ) - default_records_file = "object_detection_vlm.json" - - self.declare_parameter("score_records_file", default_records_file) - self.declare_parameter("reset_records_on_startup", False) - configured_file = self.get_parameter("score_records_file").value - self.score_records_file = configured_file if configured_file else default_records_file - self.reset_records_on_startup = self.get_parameter("reset_records_on_startup").value - - if self.reset_records_on_startup: - self.score_records = self.get_empty_score_records() + self.score_records_file = "object_detection_vlm.json" + reset_records = self.get_parameter( + 'reset_records_on_startup' + ).get_parameter_value().bool_value + if reset_records: + self.score_records = { + label: {"highest_score": 0.0, "last_detected": None, "position": None} + for label in self.labels + } self.save_score_records() self.get_logger().info(f"Score records reset: {self.score_records_file}") else: self.score_records = self.load_score_records() - self.save_score_records() self.get_logger().info(f"Score records loaded: {self.score_records_file}") # Subscribers & Publishers @@ -86,84 +93,53 @@ class ClipDetectionNode(Node): Image, "/intel_realsense_r200_rgb/image_raw", self.rgb_callback, - 10 + 10, + callback_group=self.sensor_callback_group ) self.depth_sub = self.create_subscription( Image, "/intel_realsense_r200_depth/depth/image_raw", self.depth_callback, - 10 + 10, + callback_group=self.sensor_callback_group ) self.camera_info_sub = self.create_subscription( CameraInfo, "/intel_realsense_r200_depth/camera_info", - self.camera_info_callback, 10) + self.camera_info_callback, 10, + callback_group=self.sensor_callback_group) self.annotated_pub = self.create_publisher(Image, "/percep/annotated_image", 10) + interval_sec = self.get_parameter( + 'detection_interval_sec' + ).get_parameter_value().double_value + self.detection_timer = self.create_timer( + interval_sec, + self.process_frame, + callback_group=self.timer_callback_group, + ) + self.get_logger().info( + f"Detection timer enabled: processing latest frame every {interval_sec:.2f}s" + ) def camera_info_callback(self, msg): self.camera_info = msg - self.img_frame = "realsense_depth_frame" - self.get_logger().info(f"Camera info received, using frame: {self.img_frame}") + self.latest_camera_info_stamp = msg.header.stamp + if msg.header.frame_id: + self.img_frame = msg.header.frame_id - def get_empty_score_records(self): - return { - label: {"highest_score": 0.0, "last_detected": None, "position": None} - for label in self.labels - } - def load_score_records(self): - """Load existing score records and normalize missing labels/fields.""" - empty_records = self.get_empty_score_records() - if not os.path.exists(self.score_records_file): - self.get_logger().warn( - f"Score records file not found, creating new one: {self.score_records_file}" - ) - return empty_records - - with open(self.score_records_file, 'r') as f: - try: - loaded = json.load(f) - except json.JSONDecodeError: - self.get_logger().warn("Score file corrupted, creating new one") - return empty_records - - if not isinstance(loaded, dict): - self.get_logger().warn("Score file format is invalid, creating new one") - return empty_records - - def normalize_record(record): - if not isinstance(record, dict): - record = {} - - highest_score = record.get("highest_score", 0.0) - try: - highest_score = float(highest_score) - except (TypeError, ValueError): - highest_score = 0.0 - highest_score = max(0.0, highest_score) - - last_detected = record.get("last_detected") - position = record.get("position") - if position is not None and not isinstance(position, dict): - position = None - - return { - "highest_score": highest_score, - "last_detected": last_detected, - "position": position - } - - normalized = {} - for label, record in loaded.items(): - normalized[label] = normalize_record(record) - - for label in self.labels: - if label not in normalized: - normalized[label] = empty_records[label] - - return normalized + """Load existing score records or create new ones""" + if os.path.exists(self.score_records_file): + with open(self.score_records_file, 'r') as f: + try: + return json.load(f) + except json.JSONDecodeError: + self.get_logger().warn("Score file corrupted, creating new one") + + # Initialize with empty records + return {label: {"highest_score": 0.0, "last_detected": None, "position":None} for label in self.labels} def save_score_records(self): """Save current score records to file""" @@ -171,40 +147,40 @@ class ClipDetectionNode(Node): json.dump(self.score_records, f, indent=4) def update_score_records(self, label, score, position=None): - """Keep highest score per label; equal score is treated as an update.""" + """Update records when confidence matches or exceeds the best score.""" current_time = datetime.now().isoformat() - + if label not in self.score_records: self.score_records[label] = { - "highest_score": 0.0, - "last_detected": None, - "position": None + "highest_score": score, + "last_detected": current_time, + "position": position } - - record = self.score_records[label] - updated = False - if score >= record["highest_score"]: - # Equal score is also accepted so records can refresh over time. - record["highest_score"] = score - if position is not None: - record["position"] = position - updated = True - - record["last_detected"] = current_time - if updated: self.save_score_records() - + return True + + updated = False + if score >= self.score_records[label]["highest_score"]: + # Refresh the best score and associated position on ties or improvements. + self.score_records[label]["highest_score"] = score + if position is not None: + self.score_records[label]["position"] = position + updated = True + + # Always refresh timestamp (even without position/score change) + self.score_records[label]["last_detected"] = current_time + self.save_score_records() + return updated def rgb_callback(self, msg): try: self.latest_rgb = self.bridge.imgmsg_to_cv2(msg, "bgr8") - self.latest_stamp = msg.header.stamp # <-- Correct assignment + self.latest_stamp = msg.header.stamp except CvBridgeError as e: self.get_logger().error(f"CvBridge error: {e}") return - self.process_frame() def depth_callback(self, msg): @@ -214,85 +190,87 @@ class ClipDetectionNode(Node): except CvBridgeError as e: self.get_logger().error(f"Depth CvBridge error: {e}") return - self.process_frame() def process_frame(self): - if self.latest_rgb is None or self.latest_depth is None: + if self.processing_frame: return - cv_img = self.latest_rgb.copy() - image_tensor = transforms.ToTensor()(cv_img) + if self.latest_rgb is None or self.latest_depth is None or self.camera_info is None: + return - with torch.no_grad(): - frcnn_output = self.frcnn_model([image_tensor])[0] - candidate_boxes = frcnn_output['boxes'] + if self.latest_stamp is None or self.latest_depth_stamp is None: + return - best_labels_per_box = {} # key: box index, value: (label, score) + self.processing_frame = True + start_time = time.perf_counter() + try: + cv_img = self.latest_rgb.copy() + depth_img = self.latest_depth.copy() + frame_stamp = self.latest_depth_stamp + camera_info = self.camera_info + image_tensor = transforms.ToTensor()(cv_img) + frcnn_min_score = self.get_parameter( + 'frcnn_min_score' + ).get_parameter_value().double_value + frcnn_top_k = self.get_parameter( + 'frcnn_top_k' + ).get_parameter_value().integer_value - for i, box in enumerate(candidate_boxes): - max_score = -1 - best_label = None - for label in self.labels: - _, score = self.match_label_box(label, image_tensor, [box]) - if score > max_score: - max_score = score - best_label = label + with torch.no_grad(): + frcnn_output = self.frcnn_model([image_tensor])[0] + candidate_boxes = frcnn_output['boxes'] + candidate_scores = frcnn_output['scores'] + total_candidate_count = len(candidate_boxes) - if max_score >= 0.25 and best_label is not None: - best_labels_per_box[i] = (best_label, max_score) - # Initialize record_updated as False first - record_updated = False - position = None + filtered_indices = [ + i for i, score in enumerate(candidate_scores) + if float(score) >= frcnn_min_score + ] + filtered_indices.sort(key=lambda i: float(candidate_scores[i]), reverse=True) + selected_indices = filtered_indices[:frcnn_top_k] + candidate_boxes = [candidate_boxes[i] for i in selected_indices] + self.get_logger().info( + f"FRCNN candidates: total={total_candidate_count} " + f"filtered={len(filtered_indices)} top_k={frcnn_top_k} " + f"selected={len(candidate_boxes)}" + ) + + best_labels_per_box = {} # key: box index, value: (label, score) + + for i, box in enumerate(candidate_boxes): + max_score = -1 + best_label = None + for label in self.labels: + _, score = self.match_label_box(label, image_tensor, [box]) + if score > max_score: + max_score = score + best_label = label + + if max_score >= 0.25 and best_label is not None: + best_labels_per_box[i] = (best_label, max_score) + is_new_record = False + position = None - if self.latest_depth is not None and self.camera_info is not None: box_index = i box = candidate_boxes[box_index] x_min, y_min, x_max, y_max = map(int, box) x_center = (x_min + x_max) // 2 y_center = (y_min + y_max) // 2 - depth = float(self.latest_depth[y_center, x_center]) + depth = float(depth_img[y_center, x_center]) if np.isnan(depth) or depth <= 0.0: continue - fx = self.camera_info.k[0] - fy = self.camera_info.k[4] - cx = self.camera_info.k[2] - cy = self.camera_info.k[5] + fx = camera_info.k[0] + fy = camera_info.k[4] + cx = camera_info.k[2] + cy = camera_info.k[5] X = (x_center - cx) * depth / fx Y = (y_center - cy) * depth / fy Z = depth - stamp_msg = self.latest_depth_stamp if self.latest_depth_stamp is not None else self.latest_stamp - if stamp_msg is None: - continue - query_time = Time.from_msg( - stamp_msg, clock_type=self.get_clock().clock_type - ) - tf_stamp_msg = stamp_msg - has_tf_at_stamp = self.tf_buffer.can_transform( - "map", self.img_frame, query_time, timeout=Duration(seconds=0.05) - ) - if not has_tf_at_stamp: - latest_query_time = Time(clock_type=self.get_clock().clock_type) - has_tf_latest = self.tf_buffer.can_transform( - "map", self.img_frame, latest_query_time, timeout=Duration(seconds=0.05) - ) - if not has_tf_latest: - now_time = self.get_clock().now() - if ( - self.last_tf_warn_time is None - or (now_time - self.last_tf_warn_time).nanoseconds > 2_000_000_000 - ): - self.get_logger().warn( - "TF unavailable at sensor stamp and latest time; skip this detection frame." - ) - self.last_tf_warn_time = now_time - continue - tf_stamp_msg = latest_query_time.to_msg() - pose = PoseStamped() - pose.header.stamp = tf_stamp_msg + pose.header.stamp = frame_stamp pose.header.frame_id = self.img_frame pose.pose.position.x = X pose.pose.position.y = Y @@ -302,7 +280,7 @@ class ClipDetectionNode(Node): # TF transform: camera frame -> map frame (semantic_nav 需要 map 坐标) try: ob2map = self.tf_buffer.transform( - pose, "map", timeout=Duration(seconds=0.05) + pose, "map", timeout=Duration(seconds=1.0) ) self.pose_pub.publish(ob2map) @@ -316,33 +294,28 @@ class ClipDetectionNode(Node): "y": round(ob2map.pose.position.y, 4), "z": round(ob2map.pose.position.z, 4) } - record_updated = self.update_score_records(best_label, max_score, position=position) + is_new_record = self.update_score_records(best_label, max_score, position=position) except Exception as e: self.get_logger().error(f"TF transform to map failed: {e}") - record_updated = False + is_new_record = False + log_msg = f"{best_label} detected with confidence score {max_score:.2f}" + if is_new_record: + log_msg += " (NEW RECORD!)" + self.get_logger().info(log_msg) - # Logging - log_msg = f"{best_label} detected with confidence score {max_score:.2f}" - if record_updated: - log_msg += " (BEST SCORE RECORD UPDATED)" - self.get_logger().info(log_msg) + matched_boxes = [candidate_boxes[i] for i in best_labels_per_box.keys()] + matched_labels = [f"{lbl} ({score:.2f})" for lbl, score in best_labels_per_box.values()] - -############################################### - - matched_boxes = [candidate_boxes[i] for i in best_labels_per_box.keys()] - matched_labels = [f"{lbl} ({score:.2f})" for lbl, score in best_labels_per_box.values()] - - annotated_img = self.draw_boxes(cv_img, matched_boxes, matched_labels) - try: - self.annotated_pub.publish(self.bridge.cv2_to_imgmsg(annotated_img, encoding="bgr8")) - except CvBridgeError as e: - self.get_logger().error(f"Publish error: {e}") - - self.latest_rgb, self.latest_depth = None, None - self.latest_stamp = None # 同步清空时间戳，避免TF查询残留旧stamp - self.latest_depth_stamp = None + annotated_img = self.draw_boxes(cv_img, matched_boxes, matched_labels) + try: + self.annotated_pub.publish(self.bridge.cv2_to_imgmsg(annotated_img, encoding="bgr8")) + except CvBridgeError as e: + self.get_logger().error(f"Publish error: {e}") + finally: + elapsed_ms = (time.perf_counter() - start_time) * 1000.0 + self.get_logger().info(f"Detection cycle took {elapsed_ms:.1f} ms") + self.processing_frame = False def match_label_box(self, label, image_tensor, boxes): max_sim, best_box = -1, None @@ -369,12 +342,15 @@ class ClipDetectionNode(Node): def main(args=None): rclpy.init(args=args) node = ClipDetectionNode() + executor = MultiThreadedExecutor(num_threads=2) + executor.add_node(node) try: - rclpy.spin(node) + executor.spin() except KeyboardInterrupt: node.get_logger().info("KeyboardInterrupt received, shutting down...") finally: node.save_score_records() + executor.shutdown() node.destroy_node() if rclpy.ok(): rclpy.shutdown()