1293.68 Tick 내 토픽 메시지 수신 처리

1. 토픽 메시지 수신과 Tick의 관계

ROS2 토픽 구독에서 메시지 수신은 콜백 함수를 통해 이루어진다. 콜백은 실행기(executor)의 spin() 또는 spin_some() 호출 시 처리된다. Tick 실행 중에 콜백이 처리되는 시점은 실행기의 유형과 Tick 루프의 설계에 따라 결정되며, 이 시점이 Tick 내에서 읽는 토픽 데이터의 신선도와 일관성에 직접적 영향을 미친다(Faconti, 2022).

2. 콜백 기반 메시지 수신 패턴

2.1 콜백에서 멤버 변수로 저장

가장 기본적인 패턴으로, 구독 콜백에서 수신된 메시지를 노드의 멤버 변수에 저장하고, Tick에서 해당 멤버 변수를 읽는다.

class LaserScanCondition : public BT::ConditionNode {
public:
    LaserScanCondition(const std::string& name, const BT::NodeConfig& config)
        : BT::ConditionNode(name, config) {
        auto node = config.blackboard->get<rclcpp::Node::SharedPtr>("node");
        scan_sub_ = node->create_subscription<sensor_msgs::msg::LaserScan>(
            "scan", rclcpp::SensorDataQoS(),
            [this](const%20sensor_msgs::msg::LaserScan::SharedPtr%20msg) {
                latest_scan_ = msg;
            }
        );
    }

    BT::NodeStatus tick() override {
        if (!latest_scan_) {
            return BT::NodeStatus::FAILURE;  // 메시지 미수신
        }
        
        double min_range = *std::min_element(
            latest_scan_->ranges.begin(), 
            latest_scan_->ranges.end()
        );
        
        double threshold;
        getInput("min_distance", threshold);
        return (min_range > threshold) 
            ? BT::NodeStatus::SUCCESS 
            : BT::NodeStatus::FAILURE;
    }

private:
    rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr scan_sub_;
    sensor_msgs::msg::LaserScan::SharedPtr latest_scan_;
};

2.2 콜백에서 블랙보드로 직접 기록

콜백에서 수신된 데이터를 블랙보드에 직접 기록하여, 트리 내의 임의의 노드가 접근할 수 있도록 하는 패턴이다.

class ScanUpdater : public BT::SyncActionNode {
public:
    ScanUpdater(const std::string& name, const BT::NodeConfig& config)
        : BT::SyncActionNode(name, config) {
        auto node = config.blackboard->get<rclcpp::Node::SharedPtr>("node");
        scan_sub_ = node->create_subscription<sensor_msgs::msg::LaserScan>(
            "scan", rclcpp::SensorDataQoS(),
            [this](const%20sensor_msgs::msg::LaserScan::SharedPtr%20msg) {
                latest_scan_ = msg;
            }
        );
    }

    BT::NodeStatus tick() override {
        if (latest_scan_) {
            setOutput("scan_data", *latest_scan_);
            return BT::NodeStatus::SUCCESS;
        }
        return BT::NodeStatus::FAILURE;
    }

private:
    rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr scan_sub_;
    sensor_msgs::msg::LaserScan::SharedPtr latest_scan_;
};

3. SingleThreadedExecutor에서의 메시지 수신 시점

SingleThreadedExecutor와 인터리빙 패턴을 사용하면, 콜백은 spinSome() 호출 시에만 처리된다. Tick 실행 중에는 콜백이 처리되지 않으므로, Tick 내에서 읽는 데이터는 Tick 시작 시점의 스냅샷이다.

시간축:
  ├─ spinSome()          ← 콜백 처리: latest_scan_ 갱신
  ├─ tickOnce() 시작
  │   ├─ Node_A: latest_scan_ 읽기 → 스냅샷 값
  │   ├─ Node_B: latest_scan_ 읽기 → 동일 스냅샷 값
  │   └─ (Tick 중 새 메시지 도착해도 콜백 미처리)
  ├─ tickOnce() 종료
  ├─ spinSome()          ← 콜백 처리: Tick 중 도착한 메시지 처리
  └─ ...

이 패턴에서 단일 Tick 내의 데이터 일관성이 자연스럽게 보장된다.

4. MultiThreadedExecutor에서의 메시지 수신 시점

MultiThreadedExecutor를 사용하면, 콜백이 Tick 실행 중에도 별도 스레드에서 처리될 수 있다. 이 경우 Tick 실행 도중에 latest_scan_이 갱신되어 데이터 경쟁이 발생할 수 있다.

Tick 스레드:     latest_scan_ 읽기 (Node_A)
콜백 스레드:     latest_scan_ 갱신 (새 메시지 도착)  ← 동시 접근
Tick 스레드:     latest_scan_ 읽기 (Node_B)          ← 다른 값 읽힘

이를 방지하기 위해 뮤텍스 보호 또는 원자적 포인터 교환이 필요하다.

5. 메시지 미수신 상태의 처리

Tick 시작 시점에 토픽 메시지가 아직 한 번도 수신되지 않은 경우, 노드는 이 상태를 적절히 처리해야 한다.

BT::NodeStatus tick() override {
    if (!latest_scan_) {
        RCLCPP_WARN_THROTTLE(
            node_->get_logger(), *node_->get_clock(), 1000,
            "Scan data not yet received");
        return BT::NodeStatus::FAILURE;
    }
    // 정상 처리
}

TRANSIENT_LOCAL QoS를 사용하면, 구독 이전에 발행된 마지막 메시지를 수신할 수 있어 초기 메시지 부재 문제를 완화할 수 있다.

6. 메시지 신선도 확인

수신된 메시지가 현재 시점에서 유효한지 확인하기 위해, 메시지의 타임스탬프와 현재 시간을 비교하는 신선도 검사를 수행한다.

BT::NodeStatus tick() override {
    if (!latest_scan_) {
        return BT::NodeStatus::FAILURE;
    }
    
    auto age = node_->now() - latest_scan_->header.stamp;
    if (age > rclcpp::Duration::from_seconds(1.0)) {
        // 메시지가 1초 이상 오래됨: 센서 장애 의심
        return BT::NodeStatus::FAILURE;
    }
    
    return processScan(*latest_scan_);
}

7. 고빈도 토픽과 Tick 주기의 불일치

센서 토픽(LiDAR: 10~40Hz, 카메라: 30~60Hz, IMU: 100~1000Hz)의 발행 주기가 Tick 주기(통상 10~30Hz)보다 높은 경우, 복수의 메시지가 Tick 간에 도착한다. KEEP_LAST(1) QoS를 사용하면 가장 최근 메시지만 콜백에서 처리되므로, Tick에서는 항상 최신 데이터를 읽게 된다.

\frac{f_{sensor}}{f_{tick}} > 1 \quad \Rightarrow \quad \text{Tick당 복수 메시지 도착, 최신 것만 사용}

반대로 Tick 주기가 토픽 발행 주기보다 높은 경우, 일부 Tick에서는 이전 Tick과 동일한 데이터를 읽게 된다.

8. Tick 내 토픽 발행

노드가 Tick 실행 중에 토픽 메시지를 발행하는 것은 비차단 연산이므로, Tick 실행 시간에 미미한 영향을 미친다.

class PublishCommand : public BT::SyncActionNode {
    BT::NodeStatus tick() override {
        geometry_msgs::msg::Twist cmd;
        getInput("linear_x", cmd.linear.x);
        getInput("angular_z", cmd.angular.z);
        
        cmd_pub_->publish(cmd);  // 비차단, 즉시 반환
        return BT::NodeStatus::SUCCESS;
    }
};

발행 연산은 DDS 미들웨어에 메시지를 전달하는 것으로 완료되며, 구독자의 수신을 대기하지 않는다.

9. 설계 권장 사항

참고 문헌

• Colledanchise, M., & Ogren, P. (2018). Behavior Trees in Robotics and AI: An Introduction. CRC Press.
• Faconti, D. (2022). BehaviorTree.CPP documentation and API reference. https://www.behaviortree.dev/