import rclpy from rclpy.node import Node from rclpy.action import ActionClient from geometry_msgs.msg import TwistStamped, PoseWithCovarianceStamped from nav2_msgs.action import NavigateToPose from sensor_msgs.msg import BatteryState from nav_msgs.msg import Odometry from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy, HistoryPolicy import socket import time import math import requests import threading from collections import deque UDP_PORT_UWB = 44444 UDP_PORT_QT = 55555 BUFFER_SIZE = 1024 SERVER_IP = "192.168.123.43" API_REPORT_URL = f"http://{SERVER_IP}:8000/dashboard/bot/report" API_WEIGHT_URL = f"http://{SERVER_IP}:8000/cart/check_weight" TAG_HEIGHT = 1.85 ANCHOR_Z = 2.37 H_DIFF = abs(ANCHOR_Z - TAG_HEIGHT) # 고정 오프셋 보정값 FIXED_OFFSETS = {0: -7.00, 1: -7.00, 2: -7.50} # 앵커 절대 좌표 ANCHORS_POS = { 0: {'x': 0.00, 'y': 0.00}, 1: {'x': 6.00, 'y': 0.00}, 2: {'x': 3.00, 'y': 15.00} } MIN_UPDATE_DIST = 1.2 NAV_UPDATE_INTERVAL = 2.0 class IntegratedRosController(Node): def __init__(self): super().__init__('integrated_ros_controller_node') amcl_qos = QoSProfile( depth=1, reliability=ReliabilityPolicy.RELIABLE, durability=DurabilityPolicy.TRANSIENT_LOCAL, history=HistoryPolicy.KEEP_LAST ) self.cmd_vel_pub = self.create_publisher(TwistStamped, '/cmd_vel', 10) self.battery_sub = self.create_subscription(BatteryState, '/battery_state', self.battery_callback, 10) self.odom_sub = self.create_subscription(Odometry, '/odom', self.odom_callback, 10) self.amcl_sub = self.create_subscription( PoseWithCovarianceStamped, '/amcl_pose', self.amcl_callback, amcl_qos ) self.nav_to_pose_client = ActionClient(self, NavigateToPose, 'navigate_to_pose') self.sock_uwb = self.create_udp_socket(UDP_PORT_UWB) self.sock_qt = self.create_udp_socket(UDP_PORT_QT) self.current_mode = 0 self.battery_level = 0.0 self.current_speed = 0.0 self.qt_addr = None self.robot_x = 0.0 self.robot_y = 0.0 self.human_x = 0.0 self.human_y = 0.0 self.x_his = deque(maxlen=15) self.y_his = deque(maxlen=5) self.uwb_dists = [0.0, 0.0, 0.0] self.uwb_last_update = [0.0, 0.0, 0.0] self.nav_goal_handle = None self.last_goal_time = 0.0 self.last_target_pos = [0.0, 0.0] self.is_weight_ok = True self.last_weight_check_time = 0.0 self.weight_check_interval = 3.0 self.get_logger().info("Waiting for Nav2 Server...") if not self.nav_to_pose_client.wait_for_server(timeout_sec=5.0): self.get_logger().warn("Nav2 Server not ready yet.") self.create_timer(0.05, self.control_loop) self.create_timer(1.0, self.send_bot_status) self.get_logger().info("ROS Controller Started") def create_udp_socket(self, port): sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) sock.bind(('0.0.0.0', port)) sock.setblocking(False) return sock # 무게 체크 def trigger_async_weight_check(self): def _check(): try: response = requests.get(API_WEIGHT_URL, timeout=0.5) if response.status_code == 200: data = response.json() movable = data.get("movable", True) self.is_weight_ok = movable if not movable: self.send_udp_to_qt("ERROR:WEIGHT_MISMATCH") except: pass threading.Thread(target=_check, daemon=True).start() def send_udp_to_qt(self, message): if self.qt_addr: try: self.sock_qt.sendto(message.encode(), self.qt_addr) except: pass def battery_callback(self, msg): self.battery_level = int(msg.percentage) if msg.percentage >= 0 else 0.0 def odom_callback(self, msg): self.current_speed = msg.twist.twist.linear.x def amcl_callback(self, msg): self.robot_x = msg.pose.pose.position.x self.robot_y = msg.pose.pose.position.y # 로봇 상태 전송 def send_bot_status(self): def _post(): try: payload = { "status": "ONLINE", "mode": self.current_mode, "battery": round(self.battery_level, 1), "speed": round(self.current_speed, 2), "x": self.robot_x, "y": self.robot_y, "human_x": self.human_x, "human_y": self.human_y } requests.post(API_REPORT_URL, json=payload, timeout=2.0) except: pass # 스레드 비동기 전송 후 종료 threading.Thread(target=_post, daemon=True).start() # 삼변측량 함수 def calculate_trilateration(self, distances): try: x1, y1, r1 = ANCHORS_POS[0]['x'], ANCHORS_POS[0]['y'], distances[0] x2, y2, r2 = ANCHORS_POS[1]['x'], ANCHORS_POS[1]['y'], distances[1] x3, y3, r3 = ANCHORS_POS[2]['x'], ANCHORS_POS[2]['y'], distances[2] A, B = 2*(x2-x1), 2*(y2-y1) C = r1**2 - r2**2 - x1**2 + x2**2 - y1**2 + y2**2 D, E = 2*(x3-x2), 2*(y3-y2) F = r2**2 - r3**2 - x2**2 + x3**2 - y2**2 + y3**2 den = (A * E) - (B * D) if abs(den) < 0.0001: return None return ((C * E) - (B * F)) / den, ((A * F) - (C * D)) / den except: return None def transform_to_map(self, ux, uy): mx = 3.25 + (ux * -0.9725) my = -7.35 + (uy * 0.5966) return mx, my # UWB 데이터 수신 def receive_uwb_data(self): try: for _ in range(10): data, _ = self.sock_uwb.recvfrom(BUFFER_SIZE) msg = data.decode('utf-8').strip() idx = -1 if msg.startswith("A1:"): idx = 0 elif msg.startswith("A2:"): idx = 1 elif msg.startswith("A3:"): idx = 2 if idx != -1: raw_val = float(msg.split(":")[1]) corrected_dist = raw_val - FIXED_OFFSETS[idx] h_dist = math.sqrt(corrected_dist**2 - H_DIFF**2) if corrected_dist > H_DIFF else 0.1 self.uwb_dists[idx] = h_dist self.uwb_last_update[idx] = time.time() except BlockingIOError: pass except: pass def receive_qt_command(self): try: while True: data, addr = self.sock_qt.recvfrom(BUFFER_SIZE) self.qt_addr = addr text = data.decode('utf-8').strip() if text.startswith("MODE:"): self.change_mode(int(text.split(":")[1])) elif text.startswith("GOAL:"): if self.current_mode != 2: self.change_mode(2) p = text.split(":")[1].split(",") self.start_navigation(float(p[0]), float(p[1])) elif text.startswith("STOP"): self.change_mode(0) except BlockingIOError: pass except: pass def control_loop(self): self.receive_uwb_data() self.receive_qt_command() now = time.time() if now - self.last_weight_check_time > self.weight_check_interval: self.trigger_async_weight_check() self.last_weight_check_time = now if all(now - self.uwb_last_update[i] < 1.0 for i in range(3)): pos = self.calculate_trilateration(self.uwb_dists) if pos: raw_x = max(0, min(pos[0], 6.0)) raw_y = max(0, min(pos[1], 15.0)) self.x_his.append(raw_x) self.y_his.append(raw_y) avg_x = sum(self.x_his) / len(self.x_his) avg_y = sum(self.y_his) / len(self.y_his) final_uwb_x = (avg_x / 6.0) * (3.80 - 0.10) + 0.10 final_uwb_y = avg_y mx, my = self.transform_to_map(final_uwb_x, final_uwb_y) self.human_x = mx self.human_y = my if self.current_mode == 1: dist_diff = math.sqrt((mx - self.last_target_pos[0])**2 + (my - self.last_target_pos[1])**2) if self.is_weight_ok: if (now - self.last_goal_time > NAV_UPDATE_INTERVAL) and (dist_diff > MIN_UPDATE_DIST): self.get_logger().info(f"Target Moved -> Goal: ({mx:.2f}, {my:.2f})") self.start_navigation(mx, my) self.last_goal_time = now self.last_target_pos = [mx, my] if self.current_mode == 0: if self.nav_goal_handle: self.cancel_navigation() self.stop_robot() def change_mode(self, new_mode): if self.current_mode == new_mode: return self.get_logger().info(f"Mode Switching: {self.current_mode} -> {new_mode}") self.cancel_navigation() self.stop_robot() self.current_mode = new_mode self.last_target_pos = [0.0, 0.0] self.trigger_async_weight_check() # 네비게이션 실행 def start_navigation(self, x, y): if not self.is_weight_ok: return if not self.nav_to_pose_client.server_is_ready(): self.get_logger().error("Nav2 Server not ready!") return goal_msg = NavigateToPose.Goal() goal_msg.pose.header.frame_id = 'map' goal_msg.pose.header.stamp = self.get_clock().now().to_msg() goal_msg.pose.pose.position.x = x goal_msg.pose.pose.position.y = y goal_msg.pose.pose.orientation.w = 1.0 self._send_goal_future = self.nav_to_pose_client.send_goal_async(goal_msg) self._send_goal_future.add_done_callback(self.goal_response_callback) def goal_response_callback(self, future): handle = future.result() if handle.accepted: self.nav_goal_handle = handle self._get_result_future = handle.get_result_async() self._get_result_future.add_done_callback(self.get_result_callback) def get_result_callback(self, future): result = future.result() if result.status == 4: self.send_udp_to_qt("STATUS:ARRIVED") self.nav_goal_handle = None def cancel_navigation(self): if self.nav_goal_handle: self.nav_goal_handle.cancel_goal_async() self.nav_goal_handle = None def stop_robot(self): msg = TwistStamped() msg.header.stamp = self.get_clock().now().to_msg() msg.header.frame_id = 'base_link' self.cmd_vel_pub.publish(msg) def main(): rclpy.init() node = IntegratedRosController() try: rclpy.spin(node) except KeyboardInterrupt: pass finally: node.destroy_node() rclpy.shutdown() if __name__ == '__main__': main()