속도 범위 확인 조건 노드 (Speed Range Check Condition Node)

1. 개요

속도 범위 확인 조건 노드는 로봇의 현재 선속도(linear velocity)와 각속도(angular velocity)가 허용 범위 내에 있는지를 판정하는 조건 노드이다. 속도가 설정된 한계를 초과하면 제어 불안정, 센서 데이터 품질 저하, 충돌 위험 증가 등의 문제가 발생할 수 있으며, 속도가 비정상적으로 낮으면 로봇이 정체(stuck) 상태에 빠진 것을 나타낼 수 있다.

2. ROS2 오도메트리 메시지

속도 정보는 nav_msgs::msg::Odometry 메시지의 twist 필드에 포함된다.

3. 구현

3.1 선속도 범위 확인

class IsSpeedInRange
    : public BT::RosTopicSubNode<nav_msgs::msg::Odometry>
{
public:
    IsSpeedInRange(const std::string& name,
                   const BT::NodeConfiguration& config,
                   const BT::RosNodeParams& params)
        : RosTopicSubNode(name, config, params)
    {}

    static BT::PortsList providedPorts()
    {
        return providedBasicPorts({
            BT::InputPort<double>("max_linear_speed", 2.0,
                "최대 허용 선속도 (m/s)"),
            BT::InputPort<double>("max_angular_speed", 2.0,
                "최대 허용 각속도 (rad/s)")
        });
    }

    BT::NodeStatus onTick(
        const nav_msgs::msg::Odometry::SharedPtr& msg) override
    {
        if (!msg)
        {
            return BT::NodeStatus::FAILURE;
        }

        double max_linear, max_angular;
        getInput("max_linear_speed", max_linear);
        getInput("max_angular_speed", max_angular);

        double linear_speed = std::sqrt(
            msg->twist.twist.linear.x * msg->twist.twist.linear.x +
            msg->twist.twist.linear.y * msg->twist.twist.linear.y);

        double angular_speed =
            std::abs(msg->twist.twist.angular.z);

        if (linear_speed <= max_linear &&
            angular_speed <= max_angular)
        {
            return BT::NodeStatus::SUCCESS;
        }
        return BT::NodeStatus::FAILURE;
    }
};

선속도의 크기 v는 v = \sqrt{v_x^2 + v_y^2}로 계산된다. 비홀로노믹(non-holonomic) 로봇의 경우 v_y = 0이므로 v = \lvert v_x \rvert가 된다.

3.2 정체 상태 감지

로봇이 이동하여야 하는 상황에서 속도가 비정상적으로 낮은 경우(정체 상태)를 감지한다.

class IsRobotMoving
    : public BT::RosTopicSubNode<nav_msgs::msg::Odometry>
{
public:
    IsRobotMoving(const std::string& name,
                  const BT::NodeConfiguration& config,
                  const BT::RosNodeParams& params)
        : RosTopicSubNode(name, config, params)
    {}

    static BT::PortsList providedPorts()
    {
        return providedBasicPorts({
            BT::InputPort<double>("min_speed", 0.05,
                "최소 이동 속도 (m/s)")
        });
    }

    BT::NodeStatus onTick(
        const nav_msgs::msg::Odometry::SharedPtr& msg) override
    {
        if (!msg)
        {
            return BT::NodeStatus::FAILURE;
        }

        double min_speed;
        getInput("min_speed", min_speed);

        double speed = std::sqrt(
            msg->twist.twist.linear.x * msg->twist.twist.linear.x +
            msg->twist.twist.linear.y * msg->twist.twist.linear.y +
            msg->twist.twist.angular.z * msg->twist.twist.angular.z);

        return (speed >= min_speed) ? BT::NodeStatus::SUCCESS
                                    : BT::NodeStatus::FAILURE;
    }
};

4. XML 행동 트리에서의 활용

4.1 속도 안전 감시

<BehaviorTree ID="SpeedMonitoring">
    <ReactiveSequence>
        <Condition ID="IsSpeedInRange"
                   topic_name="/odom"
                   max_linear_speed="1.5"
                   max_angular_speed="1.0"/>
        <Action ID="FollowPath"/>
    </ReactiveSequence>
</BehaviorTree>

4.2 정체 상태 복구

<BehaviorTree ID="StuckRecovery">
    <Sequence>
        <Action ID="NavigateToGoal"/>
        <Inverter>
            <Condition ID="IsRobotMoving"
                       topic_name="/odom"
                       min_speed="0.05"/>
        </Inverter>
        <Action ID="RecoverFromStuck"/>
    </Sequence>
</BehaviorTree>

5. 설계 시 고려 사항

5.1 오도메트리 잡음

저속에서 오도메트리의 잡음이 실제 속도보다 클 수 있다. 정체 감지 시 min_speed 임계값을 오도메트리 잡음 수준 이상으로 설정하여 오탐을 방지한다.

5.2 속도 한계의 상황별 조정

실내 환경에서는 낮은 속도 한계(예: 0.5 m/s)를, 실외 환경에서는 높은 속도 한계(예: 2.0 m/s)를 적용하는 것이 적절하다. 블랙보드를 통해 동적으로 임계값을 전달할 수 있다.

6. 참고 문헌

• Colledanchise, M., & Ogren, P. (2018). Behavior Trees in Robotics and AI: An Introduction. CRC Press.
• BehaviorTree.CPP 공식 문서. https://www.behaviortree.dev/
• ROS2 공식 문서. https://docs.ros.org/en/humble/