"""Chế độ lệnh chuyển động rời rạc (NaVILA-style mid-level action). 2 cấp: - parse_motion(): "não/parser" — tách câu lệnh -> MotionCmd (quãng đường / góc). - MotionController: "tủy sống" — chạy vòng kín bằng /odom, publish TwistStamped vào /cmd_vel_joy (priority cao, đè Nav2), tự dừng đúng quãng đường/góc; chặn /scan + timeout + cancel để an toàn. parse_motion thuần (chỉ re/math) -> unit-test được. ROS import nằm trong MotionController (lazy) nên module import được khi không có ROS. """ import os import re import math from dataclasses import dataclass _NUM = r"([-+]?\d*\.?\d+)" _MOVE_DIR = re.compile(r"\b(forward|ahead|backward|back|tiến|lùi|tien|lui)\b") _MOVE_VERB = re.compile(r"\b(move|go|drive|forward|backward|back|ahead|tiến|lùi|tien|lui)\b") _MOVE_UNIT = re.compile(_NUM + r"\s*(cm|centimet\w*|mm|m|met\w*|meter\w*)\b") _TURN_VERB = re.compile(r"\b(turn|rotate|spin|xoay|quay)\b") _TURN_DIR = re.compile(r"\b(left|right|trái|phải|trai|phai)\b") _TURN_UNIT = re.compile(_NUM + r"\s*(deg|degree\w*|°|rad|radian\w*)") @dataclass class MotionCmd: kind: str # "move" | "turn" value: float # mét (move; + tiến, - lùi) | radian (turn; + trái/CCW, - phải/CW) raw: str def parse_motion(command): """Trả MotionCmd nếu câu lệnh là lệnh chuyển động; None nếu không phải.""" t = (command or "").lower().strip() # TURN if _TURN_VERB.search(t) or _TURN_DIR.search(t): mu = _TURN_UNIT.search(t) if mu and (_TURN_DIR.search(t) or _TURN_VERB.search(t)): val = float(mu.group(1)) unit = mu.group(2) rad = val if unit.startswith("rad") else math.radians(val) if re.search(r"\b(right|phải|phai)\b", t): rad = -abs(rad) else: rad = abs(rad) return MotionCmd("turn", rad, command) # MOVE mu = _MOVE_UNIT.search(t) if mu and _MOVE_VERB.search(t): val = float(mu.group(1)) unit = mu.group(2) if unit == "cm" or unit.startswith("centi"): meters = val / 100.0 elif unit == "mm": meters = val / 1000.0 else: meters = val if re.search(r"\b(back|backward|lùi|lui)\b", t): meters = -abs(meters) else: meters = abs(meters) return MotionCmd("move", meters, command) return None def _yaw(q): return math.atan2(2 * (q.w * q.z + q.x * q.y), 1 - 2 * (q.y * q.y + q.z * q.z)) class MotionController: def __init__(self, node, cmd_topic="/cmd_vel_joy", lin_speed=None, ang_speed=None, rate_hz=20.0, scan_topic="/scan", front_stop=None, front_fov_deg=20.0, timeout_s=40.0): # Đi bằng env MOTION_LIN_SPEED; xoay bằng MOTION_ANG_SPEED (mặc định ~20°/s để # xoay dứt khoát), có TRẦN an toàn MOTION_ANG_MAX_DEG (mặc định 30°/s) để xoay # không quá nhanh gây mất cam. Hạ trần xuống nếu xoay mà mất camera. if lin_speed is None: lin_speed = float(os.getenv("MOTION_LIN_SPEED", "0.3")) if ang_speed is None: ang_speed = float(os.getenv("MOTION_ANG_SPEED", "0.35")) # ~20°/s ang_max = math.radians(float(os.getenv("MOTION_ANG_MAX_DEG", "30"))) ang_speed = min(abs(ang_speed), ang_max) # Khoảng cách (m) lidar coi là vật cản phía trước -> dừng. HẠ xuống để robot # được lại GẦN mục tiêu hơn (đáy box xuống sát mép dưới ảnh). Mặc định 0.30m. if front_stop is None: front_stop = float(os.getenv("MOTION_FRONT_STOP", "0.30")) from geometry_msgs.msg import TwistStamped from nav_msgs.msg import Odometry from sensor_msgs.msg import LaserScan from rclpy.qos import qos_profile_sensor_data self.node = node self.lin_speed = lin_speed self.ang_speed = ang_speed self.rate_hz = rate_hz self.front_stop = front_stop self.front_fov = math.radians(front_fov_deg) self.timeout_s = timeout_s self._TwistStamped = TwistStamped self._odom = None self._scan = None self.pub = node.create_publisher(TwistStamped, cmd_topic, 10) node.create_subscription(Odometry, "/odom", self._on_odom, 10) node.create_subscription(LaserScan, scan_topic, self._on_scan, qos_profile_sensor_data) def _on_odom(self, msg): self._odom = msg.pose.pose def _on_scan(self, msg): self._scan = msg def _front_clear(self): s = self._scan if s is None: return True # không có scan -> không chặn được (cẩn thận: đi chậm) vals = [] for i, r in enumerate(s.ranges): a = s.angle_min + i * s.angle_increment if -self.front_fov <= a <= self.front_fov and r > 0.05 and r == r: vals.append(r) return (min(vals) > self.front_stop) if vals else True def front_distance(self): """Khoảng cách vật gần nhất trong nón phía trước (m), hoặc None nếu chưa có scan.""" s = self._scan if s is None: return None vals = [] for i, r in enumerate(s.ranges): a = s.angle_min + i * s.angle_increment if -self.front_fov <= a <= self.front_fov and r > 0.05 and r == r: vals.append(r) return min(vals) if vals else None def _publish(self, vx, wz): msg = self._TwistStamped() msg.header.stamp = self.node.get_clock().now().to_msg() msg.header.frame_id = "base_link" msg.twist.linear.x = float(vx) msg.twist.angular.z = float(wz) self.pub.publish(msg) def _stop(self): for _ in range(3): self._publish(0.0, 0.0) def estop(self): """Phanh khẩn cấp: spam vận tốc 0 lên cmd_vel_joy (đè mọi nguồn).""" for _ in range(8): self._publish(0.0, 0.0) def _move(self, distance, cancel): import time if self._odom is None: yield {"kind": "nodom"} return x0, y0 = self._odom.position.x, self._odom.position.y target = abs(distance) vx = self.lin_speed if distance >= 0 else -self.lin_speed t0 = time.time() while True: if cancel is not None and cancel.is_set(): yield {"kind": "cancel"} return if distance > 0 and not self._front_clear(): yield {"kind": "obstacle"} return od = self._odom cur = math.hypot(od.position.x - x0, od.position.y - y0) rem = target - cur if rem <= 0.03: yield {"kind": "done"} return if time.time() - t0 > self.timeout_s: yield {"kind": "timeout"} return self._publish(vx, 0.0) yield {"kind": "progress", "remaining": rem} time.sleep(1.0 / self.rate_hz) def _turn(self, angle, cancel): import time if self._odom is None: yield {"kind": "nodom"} return prev = _yaw(self._odom.orientation) acc = 0.0 target = abs(angle) # Dung sai HOÀN THÀNH lệnh xoay (deadband chống vọt/rung ở 20Hz). KHÔNG phải ngưỡng # căn giữa 5° (cái đó ở prompt VLM). Hạ xuống nếu muốn xoay tới sát góc lệnh hơn. turn_tol = math.radians(float(os.getenv("MOTION_TURN_TOL_DEG", "5"))) # MOTION_TURN_SIGN=-1 nếu robot xoay NGƯỢC chiều mong muốn (lệch mounting/sign). sign = float(os.getenv("MOTION_TURN_SIGN", "1")) wz = (self.ang_speed if angle >= 0 else -self.ang_speed) * sign t0 = time.time() while True: if cancel is not None and cancel.is_set(): yield {"kind": "cancel"} return cur = _yaw(self._odom.orientation) d = (cur - prev + math.pi) % (2 * math.pi) - math.pi acc += abs(d) prev = cur rem = target - acc if rem <= turn_tol: yield {"kind": "done"} return if time.time() - t0 > self.timeout_s: yield {"kind": "timeout"} return self._publish(0.0, wz) yield {"kind": "progress", "remaining": rem} time.sleep(1.0 / self.rate_hz) def run(self, cmd, cancel=None): """Generator phát step-events cho GUI (tái dùng schema demo1).""" if cmd.kind == "move": label = "forward" if cmd.value >= 0 else "backward" title = f"Moving {label} {abs(cmd.value):.2f} m" steps = self._move(cmd.value, cancel) else: label = "left" if cmd.value >= 0 else "right" title = f"Turning {label} {math.degrees(abs(cmd.value)):.0f}°" steps = self._turn(cmd.value, cancel) yield {"type": "step", "id": "motion", "status": "running", "title": title} result = {"kind": "fail"} for ev in steps: if ev["kind"] == "progress": if cmd.kind == "move": yield {"type": "nav", "distance_remaining": ev["remaining"]} else: yield {"type": "step", "id": "motion", "status": "running", "title": f"Turning {label} — {math.degrees(ev['remaining']):.0f}° left"} else: result = ev self._stop() kind = result["kind"] if kind == "done": yield {"type": "step", "id": "motion", "status": "done"} yield {"type": "answer", "text": f"Done — {title.lower()}.", "state": "UNKNOWN"} elif kind == "obstacle": yield {"type": "step", "id": "motion", "status": "error"} yield {"type": "error", "message": "⛔ Vật cản phía trước — đã dừng an toàn."} elif kind == "cancel": yield {"type": "step", "id": "motion", "status": "error"} yield {"type": "error", "message": "⏹ Đã dừng theo yêu cầu."} elif kind == "nodom": yield {"type": "step", "id": "motion", "status": "error"} yield {"type": "error", "message": "Không có /odom — robot chưa kết nối?"} else: # timeout / fail yield {"type": "step", "id": "motion", "status": "error"} yield {"type": "error", "message": "Quá thời gian chuyển động."}