[ROS2][Turtlebot4] 경로 추종시, 총 회전 각도를 측정하는 노드

1.

cd ~/ros2_ws/src
ros2 pkg create --build-type ament_python path_rotation_eval_node

vim setup.py

==> 추가 ==> 'rotation_timer = path_rotation_eval_node.rotation_timer:main',


cd path_rotation_eval_node/path_rotation_eval_node
touch rotation_timer.py

 

2. 파일작성

import rclpy
from rclpy.node import Node
from rcl_interfaces.msg import Log
from nav_msgs.msg import Odometry
import math

def quaternion_to_yaw(qx, qy, qz, qw):
    siny_cosp = 2.0 * (qw * qz + qx * qy)
    cosy_cosp = 1.0 - 2.0 * (qy * qy + qz * qz)
    return math.atan2(siny_cosp, cosy_cosp)

class RotationTimer(Node):
    def __init__(self):
        super().__init__('rotation_timer')
        self.get_logger().info('RotationTimer 노드 시작')

        # 상태 플래그
        self.active = False
        # 누적 회전량, 이전 yaw, 최신 yaw
        self.cumulative = 0.0
        self.prev_yaw = None
        self.latest_yaw = None

        # 1) controller_server 로그 파싱 (/rosout)
        self.create_subscription(
            Log,
            '/rosout',
            self.log_cb,
            10)

        # 2) odom 구독
        self.create_subscription(
            Odometry,
            '/odom',
            self.odom_cb,
            10)

    def log_cb(self, msg: Log):
        # controller_server 로그만 필터
        if msg.name != 'controller_server':
            return

        text = msg.msg
        now = self.get_clock().now()

        if 'Received a goal, begin computing control effort' in text:
            # 경로 추종 시작!
            self.active = True
            self.cumulative = 0.0
            # 시작 시점의 yaw 기준으로 prev_yaw 세팅
            if self.latest_yaw is not None:
                self.prev_yaw = self.latest_yaw
            self.get_logger().info(f'[START] {now.to_msg()}  시작 yaw={math.degrees(self.prev_yaw):.1f}°')

        elif 'Reached the goal!' in text and self.active:
            # 종료!
            self.active = False
            # 최종 yaw
            end_yaw = self.latest_yaw
            # 순회전량: 최종 - 시작 ( 방향 고려 )
            net = 0.0
            if self.prev_yaw is not None:
                d = end_yaw - (self.prev_yaw - (self.cumulative if self.cumulative else 0))
                net = (d + math.pi) % (2*math.pi) - math.pi
            self.get_logger().info(f'[END]   {now.to_msg()}')
            self.get_logger().info(f'         누적 회전량: {math.degrees(self.cumulative):.2f}°')
            self.get_logger().info(f'         순회전량  : {math.degrees(net):.2f}°')

    def odom_cb(self, odom: Odometry):
        # 항상 최신 yaw 저장
        ori = odom.pose.pose.orientation
        yaw = quaternion_to_yaw(ori.x, ori.y, ori.z, ori.w)
        self.latest_yaw = yaw

        if not self.active or self.prev_yaw is None:
            return

        # delta yaw 계산 (–π~π)
        delta = yaw - self.prev_yaw
        delta = (delta + math.pi) % (2*math.pi) - math.pi

        self.cumulative += abs(delta)
        self.prev_yaw = yaw

def main(args=None):
    rclpy.init(args=args)
    node = RotationTimer()
    rclpy.spin(node)
    rclpy.shutdown()

if __name__ == '__main__':
    main()

 

3. 실행

cd ~/ros2_ws

colcon build --packages-select path_rotation_eval_node
source install/setup.bash


ros2 run path_rotation_eval_node rotation_timer