From 707f1f1c26bef1205b4d5f436663e8676157930c Mon Sep 17 00:00:00 2001 From: Bidulgi-99 Date: Tue, 2 Dec 2025 19:07:22 +0900 Subject: [PATCH 1/2] Feat: Integrate Async QR Processor and Refactor BT nodes --- config.yaml | 25 ++++ modules/qr_processor.py | 92 ++++++++++++++ scenarios/final project/bt_nodes.py | 185 ++++++++++------------------ 3 files changed, 179 insertions(+), 123 deletions(-) create mode 100644 modules/qr_processor.py diff --git a/config.yaml b/config.yaml index 582f443..f2fcee8 100644 --- a/config.yaml +++ b/config.yaml @@ -11,3 +11,28 @@ bt_runner: screen_width: 600 screen_height: 600 profiling_mode: False + +# ... (기존 내용 아래에 추가) + +qr_system: + # LIMO Pro 카메라 토픽 (보통 기본값이지만 확인 필요) + camera_topic: "/camera/color/image_raw" + + # 성능 최적화를 위한 리사이즈 (LIMO Pro CPU 부하 감소용) + process_width: 640 + + # 지역별 키워드 + areas: + SEOUL: ["서울특별시", "서울시"] + GYEONGGI: ["경기도"] + + # 지역별 목표 좌표 (map 프레임 기준) + locations: + SEOUL: + x: -6.326 + y: 3.209 + yaw: 90.0 + GYEONGGI: + x: -4.198 + y: 0.200 + yaw: 0.0 \ No newline at end of file diff --git a/modules/qr_processor.py b/modules/qr_processor.py new file mode 100644 index 0000000..7a9da77 --- /dev/null +++ b/modules/qr_processor.py @@ -0,0 +1,92 @@ +import cv2 +import json +import threading +import time +from cv_bridge import CvBridge + +class AsyncQRProcessor: + def __init__(self, config): + self.config = config + self.bridge = CvBridge() + self.qr_detector = cv2.QRCodeDetector() + + # 설정 로드 + self.target_width = config['qr_system'].get('process_width', 640) + self.areas = config['qr_system']['areas'] + self.locations = config['qr_system']['locations'] + + # 쓰레드 관련 변수 + self._current_image = None + self._lock = threading.Lock() + self._running = True + self._latest_result = None # (x, y, yaw) + + # 백그라운드 워커 시작 + self._worker_thread = threading.Thread(target=self._process_loop, daemon=True) + self._worker_thread.start() + + def update_image(self, ros_image_msg): + """ROS 이미지 메시지를 받아서 최신 프레임으로 업데이트""" + try: + cv_image = self.bridge.imgmsg_to_cv2(ros_image_msg, desired_encoding='bgr8') + # LIMO Pro 성능을 위해 리사이즈 (인식률 유지하며 속도 향상) + if cv_image.shape[1] > self.target_width: + scale = self.target_width / cv_image.shape[1] + cv_image = cv2.resize(cv_image, None, fx=scale, fy=scale) + + with self._lock: + self._current_image = cv_image + except Exception as e: + print(f"[QRProcessor] Error converting image: {e}") + + def get_result(self): + """현재까지 인식된 결과 반환 (Non-blocking)""" + with self._lock: + return self._latest_result + + def _process_loop(self): + """별도 쓰레드에서 계속 돌아가는 QR 인식 루프""" + while self._running: + img_to_process = None + + # 1. 처리할 이미지 가져오기 (Lock 최소화) + with self._lock: + if self._current_image is not None: + img_to_process = self._current_image.copy() + self._current_image = None # 처리 했으면 비움 + + # 2. 이미지가 없으면 잠시 대기 + if img_to_process is None: + time.sleep(0.05) + continue + + # 3. QR 인식 (무거운 작업) + try: + data, points, _ = self.qr_detector.detectAndDecode(img_to_process) + if data: + parsed_pose = self._parse_data(data) + if parsed_pose: + with self._lock: + self._latest_result = parsed_pose + except Exception as e: + print(f"[QRProcessor] Detection Error: {e}") + + def _parse_data(self, data): + """문자열 -> 좌표 변환 로직""" + try: + data_list = json.loads(data) + if len(data_list) <= 1: return None + area_name = str(data_list[1]) + + # Config에 정의된 지역 매칭 + for region_key, keywords in self.areas.items(): + if area_name in keywords: + loc = self.locations[region_key] + return (loc['x'], loc['y'], loc['yaw']) + except: + pass + return None + + def stop(self): + self._running = False + self._worker_thread.join() \ No newline at end of file diff --git a/scenarios/final project/bt_nodes.py b/scenarios/final project/bt_nodes.py index 3648efb..b5fcff0 100644 --- a/scenarios/final project/bt_nodes.py +++ b/scenarios/final project/bt_nodes.py @@ -2,6 +2,12 @@ import rclpy from rclpy.node import Node as RosNode +# --- [새로 추가된 모듈] --- +from modules.utils import config +from modules.qr_processor import AsyncQRProcessor +from sensor_msgs.msg import Image +# ------------------------ + from modules.base_bt_nodes import ( Node, Status, @@ -21,16 +27,14 @@ def deg(d: float) -> float: return math.radians(d) # ================================ -# 좌표 정의 (네 맵에 맞게 수정 필요!) +# 고정 위치 좌표 (충전소, 수령장소 등) # ================================ -# 예시 값이니까, 실제 map 좌표로 꼭 바꿔줘! -CHARGE_X, CHARGE_Y, CHARGE_YAW = -4.198, 0.2, deg(89.274) # 충전 장소 -PICKUP_X, PICKUP_Y, PICKUP_YAW = -6.326, 3.209, deg(-78.415) # 택배 수령 장소 -DELIV_X, DELIV_Y, DELIV_YAW = -4.19, 2.063, deg(-6.810) # 택배 배달 장소 +CHARGE_X, CHARGE_Y, CHARGE_YAW = -4.198, 0.2, deg(89.274) +PICKUP_X, PICKUP_Y, PICKUP_YAW = -6.326, 3.209, deg(-78.415) +DELIV_X, DELIV_Y, DELIV_YAW = -4.19, 2.063, deg(-6.810) def yaw_to_quaternion(yaw: float) -> Quaternion: - """2D yaw(라디안) -> Quaternion (z, w만 사용)""" q = Quaternion() q.z = math.sin(yaw / 2.0) q.w = math.cos(yaw / 2.0) @@ -38,25 +42,20 @@ def yaw_to_quaternion(yaw: float) -> Quaternion: def _build_nav_goal(x: float, y: float, yaw: float, agent) -> NavigateToPose.Goal: - """주어진 (x, y, yaw)로 Nav2 NavigateToPose.Goal 생성 헬퍼""" goal = NavigateToPose.Goal() - ps = PoseStamped() ps.header.frame_id = "map" - # agent 안에 ros_bridge가 있다고 가정 (WaitForGoal에서 쓰던 패턴) ps.header.stamp = agent.ros_bridge.node.get_clock().now().to_msg() - ps.pose.position.x = x ps.pose.position.y = y ps.pose.position.z = 0.0 ps.pose.orientation = yaw_to_quaternion(yaw) - goal.pose = ps return goal # ============================================ -# 1) MoveToCharge : 충전 장소로 이동 +# 1) MoveToCharge # ============================================ class MoveToCharge(ActionWithROSAction): def __init__(self, name, agent, action_name="/navigate_to_pose"): @@ -65,13 +64,9 @@ def __init__(self, name, agent, action_name="/navigate_to_pose"): def _build_goal(self, agent, blackboard): return _build_nav_goal(CHARGE_X, CHARGE_Y, CHARGE_YAW, agent) - def _interpret_result(self, result, agent, blackboard, status_code=None): - # Nav2 결과를 세부적으로 보고 싶으면 여기서 처리 - return Status.SUCCESS - # ============================================ -# 2) MoveToPickup : 택배 수령 장소로 이동 +# 2) MoveToPickup # ============================================ class MoveToPickup(ActionWithROSAction): def __init__(self, name, agent, action_name="/navigate_to_pose"): @@ -80,180 +75,128 @@ def __init__(self, name, agent, action_name="/navigate_to_pose"): def _build_goal(self, agent, blackboard): return _build_nav_goal(PICKUP_X, PICKUP_Y, PICKUP_YAW, agent) - def _interpret_result(self, result, agent, blackboard, status_code=None): - return Status.SUCCESS - # ============================================ -# 3) MoveToDelivery : 택배 배달 장소로 이동 +# 3) MoveToDelivery # ============================================ class MoveToDelivery(ActionWithROSAction): def __init__(self, name, agent, action_name="/navigate_to_pose"): super().__init__(name, agent, (NavigateToPose, action_name)) def _build_goal(self, agent, blackboard): - # WaitForQRPose 노드가 저장해둔 PoseStamped 를 가져옴 pose: PoseStamped = blackboard.get("qr_target_pose", None) - if pose is None: - agent.ros_bridge.node.get_logger().warn( - "[MoveToDelivery] qr_target_pose is None (QR 인식 정보가 아직 없음)" - ) - return None # Goal이 None이면 이 액션 노드는 FAILURE 취급됨 - + agent.ros_bridge.node.get_logger().warn("[MoveToDelivery] qr_target_pose is None") + return None goal = NavigateToPose.Goal() goal.pose = pose return goal - def _interpret_result(self, result, agent, blackboard, status_code=None): - return Status.SUCCESS - - # ============================================ -# 4) ReceiveParcel : 택배 수령 (더미, 항상 성공) +# 4) ReceiveParcel # ============================================ class ReceiveParcel(Node): - """ - /limo/button 토픽이 'pressed' 가 되었을 때 SUCCESS. - 그 전까지는 RUNNING. - """ def __init__(self, name, agent): super().__init__(name) self.agent = agent - - # 버튼 상태 초기값 (안 눌린 상태라고 가정) self._button_state = "release" - - # rclpy Node 핸들 self._node: RosNode = agent.ros_bridge.node - - # /limo/button 구독 - self._sub = self._node.create_subscription( - String, - "/limo/button", - self._callback, - 10 - ) + self._sub = self._node.create_subscription(String, "/limo/button", self._callback, 10) def _callback(self, msg: String): self._button_state = msg.data.strip() - self._node.get_logger().info( - f"[ReceiveParcel] button state: {self._button_state}" - ) async def run(self, agent, blackboard): - # 버튼이 pressed 가 되어야 수령 완료 if self._button_state.lower() == "pressed": self.status = Status.SUCCESS - self._node.get_logger().info( - "[ReceiveParcel] 버튼 pressed → 택배 수령 완료" - ) + self._node.get_logger().info("[ReceiveParcel] Button Pressed") else: - # 아직 안 눌렸으면 계속 기다리기 self.status = Status.RUNNING - return self.status - # ============================================ -# 5) DropoffParcel : 택배 배달 (더미, 항상 성공) +# 5) DropoffParcel # ============================================ class DropoffParcel(Node): - """ - /limo/button 토픽이 'release' 인 상태여야 SUCCESS. - (즉, 손을 떼서 놓은 상태) - """ def __init__(self, name, agent): super().__init__(name) self.agent = agent - self._button_state = "release" self._node: RosNode = agent.ros_bridge.node - - self._sub = self._node.create_subscription( - String, - "/limo/button", - self._callback, - 10 - ) + self._sub = self._node.create_subscription(String, "/limo/button", self._callback, 10) def _callback(self, msg: String): self._button_state = msg.data.strip() - self._node.get_logger().info( - f"[DropoffParcel] button state: {self._button_state}" - ) async def run(self, agent, blackboard): - # 버튼이 release 상태여야 배달 완료 if self._button_state.lower() == "release": self.status = Status.SUCCESS - self._node.get_logger().info( - "[DropoffParcel] 버튼 release → 택배 배달 완료" - ) + self._node.get_logger().info("[DropoffParcel] Button Released") else: self.status = Status.RUNNING - return self.status -# ============================================ -# 6) WaitForQRPose : /qr_warehouse_pose 들어올 때까지 기다리는 노드 -# ============================================ +# ========================================================= +# 6) WaitForQRPose (New Async Version) +# ========================================================= class WaitForQRPose(Node): - """ - /qr_warehouse_pose (PoseStamped)를 한 번 받을 때까지 RUNNING. - 받으면 blackboard['qr_target_pose'] 에 저장하고 SUCCESS. - """ - def __init__(self, name, agent): super().__init__(name) self.agent = agent - self._pose = None - - # agent.ros_bridge.node 를 rclpy Node 처럼 사용 + + # 비동기 프로세서 생성 (Config 전달) + self.processor = AsyncQRProcessor(config) + self._node: RosNode = agent.ros_bridge.node + topic_name = config['qr_system']['camera_topic'] + self._sub = self._node.create_subscription( - PoseStamped, - "/qr_warehouse_pose", - self._callback, + Image, + topic_name, + self._img_callback, 10 ) + self._node.get_logger().info(f"[WaitForQRPose] Async Scan Started on {topic_name}") - def _callback(self, msg: PoseStamped): - self._pose = msg - self._node.get_logger().info( - f"[WaitForQRPose] got pose from QR: " - f"x={msg.pose.position.x:.3f}, y={msg.pose.position.y:.3f}" - ) + def _img_callback(self, msg: Image): + # 콜백은 최대한 가볍게! 이미지만 던져주고 끝냄 + self.processor.update_image(msg) async def run(self, agent, blackboard): - # 아직 pose가 안 들어왔으면 계속 RUNNING - if self._pose is None: - self.status = Status.RUNNING - return self.status - - # 받은 pose를 블랙보드에 저장 - blackboard["qr_target_pose"] = self._pose - self._node.get_logger().info("[WaitForQRPose] stored pose to blackboard") - - # 한 번만 쓰고 초기화 (원하면 안 지워도 됨) - self._pose = None - - self.status = Status.SUCCESS - return self.status - + # 1. 프로세서에게 "결과 나왔니?" 하고 물어봄 (즉시 리턴됨) + result = self.processor.get_result() + + if result is None: + return Status.RUNNING + + # 2. 결과가 있으면 처리 + x, y, yaw_deg = result + + ps = PoseStamped() + ps.header.stamp = self._node.get_clock().now().to_msg() + ps.header.frame_id = "map" + ps.pose.position.x = float(x) + ps.pose.position.y = float(y) + ps.pose.position.z = 0.0 + + yaw_rad = math.radians(yaw_deg) + ps.pose.orientation.z = math.sin(yaw_rad / 2.0) + ps.pose.orientation.w = math.cos(yaw_rad / 2.0) + + blackboard["qr_target_pose"] = ps + self._node.get_logger().info(f"[WaitForQRPose] Target Found: ({x}, {y})") + + return Status.SUCCESS # ============================================ # BT Node Registration # ============================================ class BTNodeList: - # Sequence / Fallback 등 기본 CONTROL_NODES는 그대로 재사용 CONTROL_NODES = BaseBTNodeList.CONTROL_NODES - - # 우리가 새로 정의한 Action/Leaf 노드들 이름 ACTION_NODES = [ "MoveToCharge", "MoveToPickup", @@ -262,9 +205,5 @@ class BTNodeList: "DropoffParcel", "WaitForQRPose", ] - CONDITION_NODES = [] - DECORATOR_NODES = [] - - #limo/button - #pressed, release 둘중에 값 하나를 받음 + DECORATOR_NODES = [] \ No newline at end of file From 1f254b6825c610d07a65e837e51f39345645ae8d Mon Sep 17 00:00:00 2001 From: Bidulgi-99 Date: Tue, 2 Dec 2025 19:46:55 +0900 Subject: [PATCH 2/2] Fix: Rename scenario folder (remove spaces) and update BT config --- scenarios/final project/default_bt.xml | 68 ------------------- .../bt_nodes.py | 6 ++ scenarios/final_project/default_bt.xml | 29 ++++++++ .../limo_nav_action_server.py | 0 4 files changed, 35 insertions(+), 68 deletions(-) delete mode 100644 scenarios/final project/default_bt.xml rename scenarios/{final project => final_project}/bt_nodes.py (98%) create mode 100644 scenarios/final_project/default_bt.xml rename scenarios/{final project => final_project}/limo_nav_action_server.py (100%) diff --git a/scenarios/final project/default_bt.xml b/scenarios/final project/default_bt.xml deleted file mode 100644 index 49668e5..0000000 --- a/scenarios/final project/default_bt.xml +++ /dev/null @@ -1,68 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/scenarios/final project/bt_nodes.py b/scenarios/final_project/bt_nodes.py similarity index 98% rename from scenarios/final project/bt_nodes.py rename to scenarios/final_project/bt_nodes.py index b5fcff0..ff2ef0e 100644 --- a/scenarios/final project/bt_nodes.py +++ b/scenarios/final_project/bt_nodes.py @@ -12,8 +12,14 @@ Node, Status, Sequence, + Fallback, + ReactiveSequence, + ReactiveFallback, + Parallel, BTNodeList as BaseBTNodeList ) + + from modules.base_bt_nodes_ros import ( ActionWithROSAction, ) diff --git a/scenarios/final_project/default_bt.xml b/scenarios/final_project/default_bt.xml new file mode 100644 index 0000000..41a681b --- /dev/null +++ b/scenarios/final_project/default_bt.xml @@ -0,0 +1,29 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/scenarios/final project/limo_nav_action_server.py b/scenarios/final_project/limo_nav_action_server.py similarity index 100% rename from scenarios/final project/limo_nav_action_server.py rename to scenarios/final_project/limo_nav_action_server.py