장애물 감지 조건 노드 (Obstacle Detection Condition Node)

1. 개요

장애물 감지 조건 노드는 로봇 주변에 장애물이 존재하는지를 판정하는 조건 노드이다. 단순한 거리 임계값 비교를 넘어, 감지 영역의 정의, 유효 측정값의 필터링, 포인트 수 기반 판정 등 장애물의 존재를 보다 안정적으로 판정하기 위한 기법을 포함한다. 장애물 감지 결과는 장애물 회피, 정지 명령, 경로 재계획 등의 행동을 유발하는 트리거로 활용된다.

2. 장애물 감지의 정의

2.1 장애물 존재 판정 기준

장애물의 존재 여부를 판정하는 기준은 다음과 같이 정의할 수 있다.

1. 최소 거리 기준: 감지 영역 내의 최소 측정 거리가 임계값 미만이면 장애물이 존재하는 것으로 판정한다.
2. 포인트 수 기준: 감지 영역 내에서 임계값 미만의 거리를 가진 측정 포인트의 수가 지정된 최소 포인트 수 이상이면 장애물이 존재하는 것으로 판정한다. 이 방법은 잡음에 의한 단일 포인트의 이상값(outlier)에 대해 강건하다.
3. 포인트 비율 기준: 감지 영역 내 전체 유효 측정 포인트 대비 임계값 미만의 포인트 비율이 지정된 비율 이상이면 장애물이 존재하는 것으로 판정한다.

2.2 감지 영역의 정의

장애물 감지 영역은 로봇을 중심으로 한 극좌표(polar coordinate) 기반의 섹터(sector)로 정의된다.

• 각도 범위: 시작 각도 \theta_{\text{start}}와 종료 각도 \theta_{\text{end}} 사이의 부채꼴 영역
• 거리 범위: 최소 감지 거리 r_{\min}부터 최대 감지 거리 r_{\max}까지의 환형(annular) 영역

감지 영역 내의 포인트 (r_i, \theta_i)가 장애물 포인트로 판정되는 조건은 다음과 같다.

r_{\min} \leq r_i \leq r_{\text{threshold}} \quad \text{and} \quad \theta_{\text{start}} \leq \theta_i \leq \theta_{\text{end}}

포인트 수 기반 장애물 감지 구현

class IsObstacleDetected
    : public BT::RosTopicSubNode<sensor_msgs::msg::LaserScan>
{
public:
    IsObstacleDetected(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>("detection_range", 1.0,
                                  "장애물 감지 거리 (m)"),
            BT::InputPort<double>("angle_start", -1.5708,
                                  "감지 시작 각도 (rad)"),
            BT::InputPort<double>("angle_end", 1.5708,
                                  "감지 종료 각도 (rad)"),
            BT::InputPort<int>("min_points", 3,
                               "장애물 판정 최소 포인트 수")
        });
    }

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

        double detection_range, angle_start, angle_end;
        int min_points;
        getInput("detection_range", detection_range);
        getInput("angle_start", angle_start);
        getInput("angle_end", angle_end);
        getInput("min_points", min_points);

        int obstacle_count = 0;

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

            if (angle < angle_start || angle > angle_end)
            {
                continue;
            }

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

이 구현에서 장애물이 감지되면 SUCCESS를 반환한다. 즉, “장애물이 감지되었는가?“라는 질문에 대한 답이다. 행동 트리에서의 사용 맥락에 따라, 장애물 감지 시 회피 행동을 실행하거나, 반전 조건(Inverter 데코레이터)을 적용하여 “장애물이 없는가?“로 변환할 수 있다.

방향별 장애물 감지

로봇의 전방, 좌측, 우측 등 방향별로 장애물을 감지하여, 방향에 따른 차별적 행동을 구현하는 패턴이다.

class IsObstacleInDirection
    : public BT::RosTopicSubNode<sensor_msgs::msg::LaserScan>
{
public:
    IsObstacleInDirection(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>("detection_range", 1.0,
                                  "감지 거리 (m)"),
            BT::InputPort<std::string>("direction", "front",
                "감지 방향 (front, left, right, back)"),
            BT::InputPort<double>("sector_width", 1.0472,
                "섹터 너비 (rad, 기본 60°)")
        });
    }

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

        double detection_range, sector_width;
        std::string direction;
        getInput("detection_range", detection_range);
        getInput("direction", direction);
        getInput("sector_width", sector_width);

        double center_angle = getDirectionAngle(direction);
        double half_width = sector_width / 2.0;
        double angle_start = center_angle - half_width;
        double angle_end = center_angle + half_width;

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

            if (angle < angle_start || angle > angle_end)
            {
                continue;
            }

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

private:
    double getDirectionAngle(const std::string& direction) const
    {
        if (direction == "front") return 0.0;
        if (direction == "left")  return M_PI / 2.0;
        if (direction == "right") return -M_PI / 2.0;
        if (direction == "back")  return M_PI;
        return 0.0;
    }
};

방향별 중심 각도는 ROS2의 REP-103 좌표 규약에 따라 다음과 같이 정의된다.

코스트맵 기반 장애물 감지

센서 원시 데이터가 아닌 코스트맵(costmap)을 기반으로 장애물을 감지하는 방식이다. 코스트맵은 센서 데이터를 통합하여 2차원 점유 격자(occupancy grid)로 변환한 것이므로, 복수 센서의 정보를 반영한 보다 포괄적인 장애물 감지가 가능하다.

class IsObstacleInCostmap
    : public BT::RosTopicSubNode<nav_msgs::msg::OccupancyGrid>
{
public:
    IsObstacleInCostmap(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>("check_radius", 0.5,
                "로봇 중심으로부터 검사 반경 (m)"),
            BT::InputPort<int>("obstacle_threshold", 90,
                "장애물 비용 임계값 (0~100)")
        });
    }

    BT::NodeStatus onTick(
        const nav_msgs::msg::OccupancyGrid::SharedPtr& msg) override
    {
        if (!msg || msg->data.empty())
        {
            return BT::NodeStatus::FAILURE;
        }

        double check_radius;
        int obstacle_threshold;
        getInput("check_radius", check_radius);
        getInput("obstacle_threshold", obstacle_threshold);

        double resolution = msg->info.resolution;
        int width = msg->info.width;
        int height = msg->info.height;
        double origin_x = msg->info.origin.position.x;
        double origin_y = msg->info.origin.position.y;

        // 로봇 위치를 맵 중앙으로 가정 (간략화)
        int center_x = width / 2;
        int center_y = height / 2;
        int radius_cells =
            static_cast<int>(check_radius / resolution);

        for (int dy = -radius_cells; dy <= radius_cells; ++dy)
        {
            for (int dx = -radius_cells; dx <= radius_cells; ++dx)
            {
                if (dx * dx + dy * dy >
                    radius_cells * radius_cells)
                {
                    continue;
                }

                int mx = center_x + dx;
                int my = center_y + dy;

                if (mx < 0 || mx >= width ||
                    my < 0 || my >= height)
                {
                    continue;
                }

                int index = my * width + mx;
                if (msg->data[index] >= obstacle_threshold)
                {
                    return BT::NodeStatus::SUCCESS;
                }
            }
        }
        return BT::NodeStatus::FAILURE;
    }
};

코스트맵의 셀 값은 0(자유 공간)부터 100(확실한 장애물)까지의 범위를 가지며, -1은 미지(unknown) 영역을 나타낸다. obstacle_threshold를 90으로 설정하면, 높은 확률로 장애물이 존재하는 셀만을 감지한다.

XML 행동 트리에서의 활용

장애물 감지 시 회피 행동 실행

<BehaviorTree ID="ObstacleAvoidance">
    <Fallback>
        <Sequence>
            <Condition ID="IsObstacleDetected"
                       topic_name="/scan"
                       detection_range="1.0"
                       min_points="3"/>
            <Action ID="AvoidObstacle"/>
        </Sequence>
        <Action ID="ContinueNavigation"/>
    </Fallback>
</BehaviorTree>

방향별 장애물 감지 기반 행동 선택

<BehaviorTree ID="DirectionalAvoidance">
    <Fallback>
        <Sequence>
            <Condition ID="IsObstacleInDirection"
                       topic_name="/scan"
                       direction="front"
                       detection_range="0.8"/>
            <Fallback>
                <Sequence>
                    <Inverter>
                        <Condition ID="IsObstacleInDirection"
                                   topic_name="/scan"
                                   direction="left"
                                   detection_range="0.5"/>
                    </Inverter>
                    <Action ID="TurnLeft"/>
                </Sequence>
                <Sequence>
                    <Inverter>
                        <Condition ID="IsObstacleInDirection"
                                   topic_name="/scan"
                                   direction="right"
                                   detection_range="0.5"/>
                    </Inverter>
                    <Action ID="TurnRight"/>
                </Sequence>
                <Action ID="TurnAround"/>
            </Fallback>
        </Sequence>
        <Action ID="DriveForward"/>
    </Fallback>
</BehaviorTree>

이 구조는 전방에 장애물이 감지되면, 좌측과 우측의 장애물 유무를 확인하여 비어 있는 방향으로 회전한다. 양측 모두 장애물이 있으면 후진 회전을 수행한다.

설계 시 고려 사항

잡음에 대한 강건성

단일 측정 포인트에 의한 장애물 판정은 센서 잡음에 취약하다. min_points 매개변수를 설정하여, 지정된 수 이상의 포인트가 감지 범위 내에 존재하는 경우에만 장애물로 판정하면 단발성 잡음에 의한 오탐(false positive)을 줄일 수 있다.

동적 장애물과 정적 장애물의 구분

본 절에서 다루는 조건 노드는 장애물의 존재 여부만을 판정하며, 장애물이 정적인지 동적인지를 구분하지 않는다. 동적 장애물의 감지와 추적은 별도의 인지(perception) 모듈에서 처리하고, 그 결과를 토픽이나 블랙보드를 통해 조건 노드에 전달하는 구조가 적절하다.

감지 영역과 로봇 형상의 고려

감지 영역을 설정할 때 로봇의 물리적 형상(footprint)을 고려하여야 한다. 원형 로봇의 경우 전방위 감지가 적합하며, 직사각형 로봇의 경우 진행 방향의 폭에 맞춘 섹터 설정이 필요하다. 로봇의 형상이 감지 범위보다 크면 장애물과의 접촉이 감지되기 전에 발생할 수 있다.

센서 장착 위치 오프셋

센서가 로봇의 기하학적 중심이 아닌 다른 위치에 장착된 경우, 센서 좌표계와 로봇 좌표계 간의 변환을 고려하여야 한다. tf2 프레임워크를 통해 센서 프레임에서 로봇 기저 프레임(base frame)으로의 좌표 변환을 적용하면 정확한 장애물 위치를 결정할 수 있으나, 이 변환을 조건 노드 내부에서 수행하면 계산 오버헤드가 증가한다.

참고 문헌

• 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.
• Macenski, S., et al. (2020). “The Marathon 2: A Navigation System.” arXiv preprint arXiv:2003.00368.
• BehaviorTree.CPP 공식 문서. https://www.behaviortree.dev/