거리 센서 임계값 조건 노드 (Range Sensor Threshold Condition Node)

1. 개요

거리 센서 임계값 조건 노드는 로봇에 장착된 거리 측정 센서(라이다, 초음파 센서, 적외선 거리 센서 등)의 측정값을 임계값과 비교하여 조건을 판정하는 조건 노드이다. 측정된 거리가 지정된 임계값 이상인지 또는 미만인지를 평가하여, 장애물 접근 경고, 안전 거리 유지 확인, 충돌 위험 판정 등의 의사 결정에 활용된다.

2. 거리 센서 메시지 타입

2.1 sensor_msgs::msg::Range

단일 빔(single beam) 거리 센서는 sensor_msgs::msg::Range 메시지를 사용한다. 이 메시지는 하나의 거리 값을 포함하며, 초음파 센서, 적외선 거리 센서 등에 사용된다.

2.2 sensor_msgs::msg::LaserScan

2D 라이다는 sensor_msgs::msg::LaserScan 메시지를 사용한다. 이 메시지는 복수의 거리 값을 각도별 배열로 포함한다.

3. 단일 빔 거리 센서 조건 노드

3.1 Range 메시지 기반 구현

class IsRangeAboveThreshold
    : public BT::RosTopicSubNode<sensor_msgs::msg::Range>
{
public:
    IsRangeAboveThreshold(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_range", 0.5,
                                  "최소 허용 거리 (m)")
        });
    }

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

        // 측정값 유효성 검증
        if (!std::isfinite(msg->range) ||
            msg->range < msg->min_range ||
            msg->range > msg->max_range)
        {
            return BT::NodeStatus::FAILURE;
        }

        double min_range;
        getInput("min_range", min_range);

        if (msg->range >= min_range)
        {
            return BT::NodeStatus::SUCCESS;
        }
        return BT::NodeStatus::FAILURE;
    }
};

측정값이 센서의 유효 범위(min_range ~ max_range) 밖에 있거나 NaN/Inf인 경우, 유효하지 않은 것으로 판정하여 FAILURE를 반환한다.

4. D 라이다 기반 거리 임계값 조건 노드

4.1 전방위 최소 거리 확인

라이다의 전체 스캔 범위에서 최소 거리를 추출하여 임계값과 비교한다.

class IsMinLaserRangeAbove
    : public BT::RosTopicSubNode<sensor_msgs::msg::LaserScan>
{
public:
    IsMinLaserRangeAbove(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_distance", 0.3,
                                  "최소 허용 거리 (m)")
        });
    }

    BT::NodeStatus onTick(
        const sensor_msgs::msg::LaserScan::SharedPtr& msg) override
    {
        if (!msg || msg->ranges.empty())
        {
            return BT::NodeStatus::FAILURE;
        }

        double min_distance;
        getInput("min_distance", min_distance);

        for (const auto& range : msg->ranges)
        {
            if (std::isfinite(range) &&
                range >= msg->range_min &&
                range <= msg->range_max &&
                range < min_distance)
            {
                return BT::NodeStatus::FAILURE;
            }
        }
        return BT::NodeStatus::SUCCESS;
    }
};

유효한 측정값 중 하나라도 임계값 미만이면 FAILURE를 반환한다. 모든 유효 측정값이 임계값 이상이면 SUCCESS를 반환한다.

4.2 특정 각도 범위 내 거리 확인

로봇의 전방, 측방, 후방 등 특정 방향의 거리만을 평가하는 조건 노드이다.

class IsRangeAboveInSector
    : public BT::RosTopicSubNode<sensor_msgs::msg::LaserScan>
{
public:
    IsRangeAboveInSector(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_distance", 0.5,
                                  "최소 허용 거리 (m)"),
            BT::InputPort<double>("sector_start", -0.5236,
                                  "검사 시작 각도 (rad)"),
            BT::InputPort<double>("sector_end", 0.5236,
                                  "검사 종료 각도 (rad)")
        });
    }

    BT::NodeStatus onTick(
        const sensor_msgs::msg::LaserScan::SharedPtr& msg) override
    {
        if (!msg || msg->ranges.empty())
        {
            return BT::NodeStatus::FAILURE;
        }

        double min_distance, sector_start, sector_end;
        getInput("min_distance", min_distance);
        getInput("sector_start", sector_start);
        getInput("sector_end", sector_end);

        for (size_t i = 0; i < msg->ranges.size(); ++i)
        {
            double angle = msg->angle_min +
                           static_cast<double>(i) * msg->angle_increment;

            if (angle < sector_start || angle > sector_end)
            {
                continue;
            }

            float range = msg->ranges[i];
            if (std::isfinite(range) &&
                range >= msg->range_min &&
                range <= msg->range_max &&
                range < min_distance)
            {
                return BT::NodeStatus::FAILURE;
            }
        }
        return BT::NodeStatus::SUCCESS;
    }
};

sector_start와 sector_end는 라디안 단위의 각도로, 라이다 좌표계에서 검사할 각도 범위를 정의한다. 기본값 \pm 0.5236 rad(약 \pm 30°)는 로봇 전방의 좁은 영역을 나타낸다.

검사 대상 각도 \theta_i는 다음과 같이 계산된다.

\theta_i = \theta_{\min} + i \cdot \Delta\theta

여기서 \theta_{\min}은 angle_min, \Delta\theta는 angle_increment, i는 배열 인덱스이다.

히스테리시스를 적용한 거리 임계값 조건 노드

센서 잡음에 의한 조건 결과의 진동을 방지하기 위해 히스테리시스를 적용한다.

class IsRangeSafeWithHysteresis
    : public BT::RosTopicSubNode<sensor_msgs::msg::LaserScan>
{
public:
    IsRangeSafeWithHysteresis(const std::string& name,
                              const BT::NodeConfiguration& config,
                              const BT::RosNodeParams& params)
        : RosTopicSubNode(name, config, params),
          was_safe_(true)
    {}

    static BT::PortsList providedPorts()
    {
        return providedBasicPorts({
            BT::InputPort<double>("danger_distance", 0.3,
                "위험 거리 (m) - 이 이하면 FAILURE"),
            BT::InputPort<double>("safe_distance", 0.5,
                "안전 거리 (m) - 이 이상이면 SUCCESS")
        });
    }

    BT::NodeStatus onTick(
        const sensor_msgs::msg::LaserScan::SharedPtr& msg) override
    {
        if (!msg || msg->ranges.empty())
        {
            return BT::NodeStatus::FAILURE;
        }

        double danger_dist, safe_dist;
        getInput("danger_distance", danger_dist);
        getInput("safe_distance", safe_dist);

        // 최소 유효 거리 추출
        double min_valid_range = std::numeric_limits<double>::max();
        for (const auto& range : msg->ranges)
        {
            if (std::isfinite(range) &&
                range >= msg->range_min &&
                range <= msg->range_max)
            {
                min_valid_range = std::min(
                    min_valid_range, static_cast<double>(range));
            }
        }

        if (min_valid_range == std::numeric_limits<double>::max())
        {
            return BT::NodeStatus::FAILURE;
        }

        // 히스테리시스 적용
        if (min_valid_range < danger_dist)
        {
            was_safe_ = false;
        }
        else if (min_valid_range > safe_dist)
        {
            was_safe_ = true;
        }
        // danger_dist <= min_valid_range <= safe_dist: 이전 상태 유지

        return was_safe_ ? BT::NodeStatus::SUCCESS
                         : BT::NodeStatus::FAILURE;
    }

private:
    bool was_safe_;
};

예를 들어, danger_distance를 0.3m, safe_distance를 0.5m로 설정하면, 최소 거리가 0.3m 미만으로 감소할 때 FAILURE로 전환되고, 0.5m 이상으로 증가할 때에야 SUCCESS로 복귀한다. 0.3m~0.5m 사이에서는 이전 상태를 유지한다.

XML 행동 트리에서의 활용

전방 안전 거리 확인 후 주행

<BehaviorTree ID="SafeForwardDrive">
    <ReactiveSequence>
        <Condition ID="IsRangeAboveInSector"
                   topic_name="/scan"
                   min_distance="0.5"
                   sector_start="-0.5236"
                   sector_end="0.5236"/>
        <Action ID="DriveForward"/>
    </ReactiveSequence>
</BehaviorTree>

히스테리시스 기반 안전 주행

<BehaviorTree ID="HysteresisNavigation">
    <ReactiveSequence>
        <Condition ID="IsRangeSafeWithHysteresis"
                   topic_name="/scan"
                   danger_distance="0.3"
                   safe_distance="0.5"/>
        <Action ID="NavigateToGoal"/>
    </ReactiveSequence>
</BehaviorTree>

다중 거리 센서 조합

<BehaviorTree ID="MultiRangeSafety">
    <ReactiveSequence>
        <Sequence name="AllDirectionsSafe">
            <Condition ID="IsRangeAboveThreshold"
                       topic_name="/sonar_front"
                       min_range="0.4"/>
            <Condition ID="IsRangeAboveThreshold"
                       topic_name="/sonar_left"
                       min_range="0.2"/>
            <Condition ID="IsRangeAboveThreshold"
                       topic_name="/sonar_right"
                       min_range="0.2"/>
        </Sequence>
        <Action ID="Navigate"/>
    </ReactiveSequence>
</BehaviorTree>

설계 시 고려 사항

센서 시야각과 사각 지대

초음파 센서는 넓은 시야각(약 30°~60°)을 가지며, 시야각 외부에 존재하는 장애물은 감지하지 못한다. 라이다의 경우에도 수직 시야각이 제한되어 지면에 낮게 위치한 장애물이나 높은 위치의 장애물을 감지하지 못할 수 있다. 조건 노드 설계 시 센서의 물리적 한계를 인지하고, 사각 지대를 보완하기 위해 복수 센서를 조합하는 전략을 고려하여야 한다.

다중 반사와 크로스토크

초음파 센서에서는 다중 반사(multipath reflection)에 의해 실제보다 긴 거리가 측정되거나, 인접 센서 간의 간섭(crosstalk)으로 인해 잘못된 거리가 보고될 수 있다. 이러한 오류는 잘못된 안전 판정을 초래할 수 있으므로, 단일 측정에 의존하지 않고 연속적인 측정값의 일관성을 검증하는 것이 바람직하다.

라이다 데이터의 무효 값 처리

라이다의 ranges 배열에는 다음과 같은 무효 값이 포함될 수 있다.

• NaN: 반사체 부재, 표면 흡수
• Inf: 최대 측정 거리 초과
• range_min 미만: 최소 측정 거리 미만의 근접 물체

이러한 무효 값을 장애물 부재로 해석하면 안전상 위험하다. 특히 NaN 값은 “측정 불가“를 의미하므로, 이를 “장애물 없음“으로 간주하여서는 안 된다. 안전이 중요한 응용에서는 무효 측정값이 포함된 방향을 잠재적 위험으로 간주하는 보수적 접근이 적절하다.

임계값 설정 기준

임계값은 로봇의 물리적 크기, 제동 거리, 반응 시간을 고려하여 설정하여야 한다. 최소 안전 거리 d_{\min}은 다음과 같이 추정할 수 있다.

d_{\min} = v \cdot t_{\text{react}} + \frac{v^2}{2a_{\max}} + d_{\text{margin}}

여기서 v는 현재 속도, t_{\text{react}}는 시스템 반응 시간, a_{\max}는 최대 감속도, d_{\text{margin}}은 안전 여유 거리이다.

5. 참고 문헌

• Colledanchise, M., & Ogren, P. (2018). Behavior Trees in Robotics and AI: An Introduction. CRC Press.
• Siegwart, R., Nourbakhsh, I., & Scaramuzza, D. (2011). Introduction to Autonomous Mobile Robots. MIT Press.
• BehaviorTree.CPP 공식 문서. https://www.behaviortree.dev/
• ROS2 공식 문서. https://docs.ros.org/en/humble/