4.5시뮬 → 실기기 전환

scan_pub = this->create_publisher<sensor_msgs::msg::LaserScan>(
    topic_name, rclcpp::QoS(rclcpp::KeepLast(10)));

ros2 topic info /scan --verbose

import math

import rclpy
from rclpy.node import Node
from rclpy.qos import qos_profile_sensor_data
from rclpy.signals import SignalHandlerOptions
from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry

MAX_LIN = 0.2    # tiki_controllers.yaml linear.x 한계 (m/s)
MAX_ANG = 0.85   # tiki_controllers.yaml angular.z 한계 (rad/s)
SIDE = 0.5       # 한 변 길이 (m)
LIN = 0.10       # 직진 속도 (m/s), 한계의 절반
ANG = 0.40       # 회전 속도 (rad/s)
TURN = math.pi / 2


def clamp(v, limit):
    return max(-limit, min(limit, v))


def wrap(a):
    return math.atan2(math.sin(a), math.cos(a))


def yaw_of(q):
    return math.atan2(2.0 * (q.w * q.z + q.x * q.y),
                      1.0 - 2.0 * (q.y * q.y + q.z * q.z))


class SquareDriver(Node):
    def __init__(self):
        super().__init__('square_driver')
        self.pub = self.create_publisher(Twist, 'cmd_vel', 10)
        # 구독 QoS를 센서 데이터 프로파일로 명시한다 (Best effort, depth 5)
        self.create_subscription(Odometry, 'odom', self.on_odom,
                                 qos_profile_sensor_data)
        self.create_timer(0.05, self.tick)   # 20 Hz: cmd_vel 타임아웃보다 훨씬 짧게
        self.pose = None                     # (x, y, yaw)
        self.start = None
        self.state = 'FORWARD'
        self.sides = 0

    def on_odom(self, msg):
        p = msg.pose.pose
        self.pose = (p.position.x, p.position.y, yaw_of(p.orientation))
        if self.start is None:
            self.start = self.pose

    def send(self, lin, ang):
        cmd = Twist()
        cmd.linear.x = clamp(lin, MAX_LIN)
        cmd.angular.z = clamp(ang, MAX_ANG)
        self.pub.publish(cmd)

    def stop(self):
        self.send(0.0, 0.0)

    def tick(self):
        if self.pose is None:
            self.stop()          # odom이 올 때까지 움직이지 않는다
            return
        x, y, yaw = self.pose
        sx, sy, syaw = self.start
        if self.state == 'FORWARD':
            if math.hypot(x - sx, y - sy) >= SIDE:
                self.state = 'TURN'
                self.start = self.pose   # 회전 각도는 회전을 시작한 방향 기준
                self.stop()
            else:
                self.send(LIN, 0.0)
        elif self.state == 'TURN':
            turned = wrap(yaw - syaw)
            if turned >= TURN:
                self.sides += 1
                self.start = self.pose
                self.state = 'DONE' if self.sides >= 4 else 'FORWARD'
                self.stop()
            else:
                self.send(0.0, ANG)
        else:
            self.stop()
            self.get_logger().info('정사각형 완료', once=True)


def main():
    # 종료 시 제로 Twist를 보내려면 rclpy가 먼저 컨텍스트를 닫지 않아야 한다
    rclpy.init(signal_handler_options=SignalHandlerOptions.NO)
    node = SquareDriver()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        for _ in range(5):     # 한 번만 보내면 유실될 수 있어 여러 번 보낸다
            node.stop()
        node.destroy_node()
        rclpy.shutdown()


if __name__ == '__main__':
    main()

import math

import rclpy
from rclpy.node import Node
from rclpy.qos import qos_profile_sensor_data
from rclpy.signals import SignalHandlerOptions
from geometry_msgs.msg import Twist
from sensor_msgs.msg import LaserScan

LASER_YAW = math.pi   # URDF: laser 링크가 base_link 기준 yaw 3.14159로 장착
STOP_DIST = 0.35      # laser 기준 거리 (m). 라이다는 base_footprint 바로 위라 로봇 중심에서 잰 값과 같다
SLOW_DIST = 0.60
LIN = 0.10
ANG = 0.40            # 한계 0.85 rad/s 이내
SCAN_TIMEOUT = 0.5    # 이 시간 동안 /scan이 없으면 정지 (s)


def wrap(a):
    return math.atan2(math.sin(a), math.cos(a))


class ScanGuard(Node):
    def __init__(self):
        super().__init__('scan_guard')
        self.pub = self.create_publisher(Twist, 'cmd_vel', 10)
        self.create_subscription(LaserScan, 'scan', self.on_scan,
                                 qos_profile_sensor_data)
        self.create_timer(0.1, self.tick)    # 10 Hz
        self.front = None
        self.left = None
        self.right = None
        self.last_scan = None

    def sector(self, msg, lo, hi):
        """로봇 기준 각도 lo..hi (rad) 구간에서 유효한 가장 가까운 거리"""
        best = None
        for i, r in enumerate(msg.ranges):
            if not math.isfinite(r) or r < msg.range_min or r > msg.range_max:
                continue
            a = wrap(msg.angle_min + i * msg.angle_increment + LASER_YAW)
            if lo <= a <= hi and (best is None or r < best):
                best = r
        return best

    def on_scan(self, msg):
        d30, d90 = math.radians(30), math.radians(90)
        self.front = self.sector(msg, -d30, d30)
        self.left = self.sector(msg, d30, d90)
        self.right = self.sector(msg, -d90, -d30)
        self.last_scan = self.get_clock().now()

    def send(self, lin, ang):
        cmd = Twist()
        cmd.linear.x = lin
        cmd.angular.z = ang
        self.pub.publish(cmd)

    def stop(self):
        self.send(0.0, 0.0)

    def tick(self):
        if self.last_scan is None:
            self.stop()
            return
        age = (self.get_clock().now() - self.last_scan).nanoseconds / 1e9
        if age > SCAN_TIMEOUT:
            self.stop()              # 센서가 끊기면 달리지 않는다
            self.get_logger().warn('scan 없음, 정지', throttle_duration_sec=2.0)
            return
        if self.front is None:
            self.stop()              # 정면에 유효한 측정이 하나도 없으면 모른다고 보고 멈춘다
            return
        if self.front < STOP_DIST:
            # 더 트인 쪽으로 제자리 회전 (측정이 없는 쪽은 트인 것으로 본다)
            left = self.left if self.left is not None else 99.0
            right = self.right if self.right is not None else 99.0
            self.send(0.0, ANG if left >= right else -ANG)
        elif self.front < SLOW_DIST:
            self.send(LIN / 2.0, 0.0)
        else:
            self.send(LIN, 0.0)


def main():
    rclpy.init(signal_handler_options=SignalHandlerOptions.NO)
    node = ScanGuard()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        for _ in range(5):
            node.stop()
        node.destroy_node()
        rclpy.shutdown()


if __name__ == '__main__':
    main()

# 터미널 1 (로봇): 기본 구동
ros2 launch tiki_bringup tiki.launch.py

# 터미널 2 (VM): 명령 확인용
ros2 topic echo /cmd_vel

# 터미널 3 (VM): 노드 실행
source /opt/ros/humble/setup.bash
python3 square_driver.py