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 deleted file mode 100644 index 3648efb..0000000 --- a/scenarios/final project/bt_nodes.py +++ /dev/null @@ -1,270 +0,0 @@ -import math -import rclpy -from rclpy.node import Node as RosNode - -from modules.base_bt_nodes import ( - Node, - Status, - Sequence, - BTNodeList as BaseBTNodeList -) -from modules.base_bt_nodes_ros import ( - ActionWithROSAction, -) - -from geometry_msgs.msg import PoseStamped, Quaternion -from nav2_msgs.action import NavigateToPose -from std_msgs.msg import String - - -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) # 택배 배달 장소 - - -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) - return q - - -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 : 충전 장소로 이동 -# ============================================ -class MoveToCharge(ActionWithROSAction): - def __init__(self, name, agent, action_name="/navigate_to_pose"): - super().__init__(name, agent, (NavigateToPose, action_name)) - - 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 : 택배 수령 장소로 이동 -# ============================================ -class MoveToPickup(ActionWithROSAction): - def __init__(self, name, agent, action_name="/navigate_to_pose"): - super().__init__(name, agent, (NavigateToPose, action_name)) - - 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 : 택배 배달 장소로 이동 -# ============================================ -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 취급됨 - - goal = NavigateToPose.Goal() - goal.pose = pose - return goal - - def _interpret_result(self, result, agent, blackboard, status_code=None): - return Status.SUCCESS - - - -# ============================================ -# 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 - ) - - 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 → 택배 수령 완료" - ) - else: - # 아직 안 눌렸으면 계속 기다리기 - self.status = Status.RUNNING - - return self.status - - - -# ============================================ -# 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 - ) - - 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 → 택배 배달 완료" - ) - else: - self.status = Status.RUNNING - - return self.status - - -# ============================================ -# 6) WaitForQRPose : /qr_warehouse_pose 들어올 때까지 기다리는 노드 -# ============================================ -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 처럼 사용 - self._node: RosNode = agent.ros_bridge.node - self._sub = self._node.create_subscription( - PoseStamped, - "/qr_warehouse_pose", - self._callback, - 10 - ) - - 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}" - ) - - 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 - - - -# ============================================ -# BT Node Registration -# ============================================ -class BTNodeList: - # Sequence / Fallback 등 기본 CONTROL_NODES는 그대로 재사용 - CONTROL_NODES = BaseBTNodeList.CONTROL_NODES - - # 우리가 새로 정의한 Action/Leaf 노드들 이름 - ACTION_NODES = [ - "MoveToCharge", - "MoveToPickup", - "MoveToDelivery", - "ReceiveParcel", - "DropoffParcel", - "WaitForQRPose", - ] - - CONDITION_NODES = [] - DECORATOR_NODES = [] - - #limo/button - #pressed, release 둘중에 값 하나를 받음 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 new file mode 100644 index 0000000..ff2ef0e --- /dev/null +++ b/scenarios/final_project/bt_nodes.py @@ -0,0 +1,215 @@ +import math +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, + Sequence, + Fallback, + ReactiveSequence, + ReactiveFallback, + Parallel, + BTNodeList as BaseBTNodeList +) + + +from modules.base_bt_nodes_ros import ( + ActionWithROSAction, +) + +from geometry_msgs.msg import PoseStamped, Quaternion +from nav2_msgs.action import NavigateToPose +from std_msgs.msg import String + + +def deg(d: float) -> float: + return math.radians(d) + +# ================================ +# 고정 위치 좌표 (충전소, 수령장소 등) +# ================================ +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: + q = Quaternion() + q.z = math.sin(yaw / 2.0) + q.w = math.cos(yaw / 2.0) + return q + + +def _build_nav_goal(x: float, y: float, yaw: float, agent) -> NavigateToPose.Goal: + goal = NavigateToPose.Goal() + ps = PoseStamped() + ps.header.frame_id = "map" + 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 +# ============================================ +class MoveToCharge(ActionWithROSAction): + def __init__(self, name, agent, action_name="/navigate_to_pose"): + super().__init__(name, agent, (NavigateToPose, action_name)) + + def _build_goal(self, agent, blackboard): + return _build_nav_goal(CHARGE_X, CHARGE_Y, CHARGE_YAW, agent) + + +# ============================================ +# 2) MoveToPickup +# ============================================ +class MoveToPickup(ActionWithROSAction): + def __init__(self, name, agent, action_name="/navigate_to_pose"): + super().__init__(name, agent, (NavigateToPose, action_name)) + + def _build_goal(self, agent, blackboard): + return _build_nav_goal(PICKUP_X, PICKUP_Y, PICKUP_YAW, agent) + + +# ============================================ +# 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): + 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") + return None + goal = NavigateToPose.Goal() + goal.pose = pose + return goal + + +# ============================================ +# 4) ReceiveParcel +# ============================================ +class ReceiveParcel(Node): + 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) + + def _callback(self, msg: String): + self._button_state = msg.data.strip() + + async def run(self, agent, blackboard): + if self._button_state.lower() == "pressed": + self.status = Status.SUCCESS + self._node.get_logger().info("[ReceiveParcel] Button Pressed") + else: + self.status = Status.RUNNING + return self.status + + +# ============================================ +# 5) DropoffParcel +# ============================================ +class DropoffParcel(Node): + 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) + + def _callback(self, msg: String): + self._button_state = msg.data.strip() + + async def run(self, agent, blackboard): + if self._button_state.lower() == "release": + self.status = Status.SUCCESS + self._node.get_logger().info("[DropoffParcel] Button Released") + else: + self.status = Status.RUNNING + return self.status + + +# ========================================================= +# 6) WaitForQRPose (New Async Version) +# ========================================================= +class WaitForQRPose(Node): + def __init__(self, name, agent): + super().__init__(name) + self.agent = agent + + # 비동기 프로세서 생성 (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( + Image, + topic_name, + self._img_callback, + 10 + ) + self._node.get_logger().info(f"[WaitForQRPose] Async Scan Started on {topic_name}") + + def _img_callback(self, msg: Image): + # 콜백은 최대한 가볍게! 이미지만 던져주고 끝냄 + self.processor.update_image(msg) + + async def run(self, agent, blackboard): + # 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: + CONTROL_NODES = BaseBTNodeList.CONTROL_NODES + ACTION_NODES = [ + "MoveToCharge", + "MoveToPickup", + "MoveToDelivery", + "ReceiveParcel", + "DropoffParcel", + "WaitForQRPose", + ] + CONDITION_NODES = [] + DECORATOR_NODES = [] \ No newline at end of file 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