Executor와 Callback Group 활용법

rclpy.init()
node = MyNode()
rclpy.spin(node)  # 기본 Executor를 이용해 종료될 때까지 처리
node.destroy_node()
rclpy.shutdown()

executor = rclpy.executors.SingleThreadedExecutor()
executor.add_node(node)
try:
    executor.spin()
finally:
    executor.shutdown()

#!/usr/bin/env python3
import time
import rclpy
from rclpy.node import Node

class BlockingDemo(Node):
    def __init__(self):
        super().__init__('blocking_demo')
        self.last_fast = time.perf_counter()
        self.create_timer(0.5, self.slow_callback)
        self.create_timer(0.1, self.fast_callback)

    def slow_callback(self):
        self.get_logger().info('slow start')
        time.sleep(0.4)  # I/O 대기를 흉내 낸다. 실제 제어 Callback에서는 금지한다.
        self.get_logger().info('slow end')

    def fast_callback(self):
        now = time.perf_counter()
        interval_ms = (now - self.last_fast) * 1000.0
        self.last_fast = now
        self.get_logger().info(f'fast interval={interval_ms:.1f} ms')

def main():
    rclpy.init()
    node = BlockingDemo()
    executor = rclpy.executors.SingleThreadedExecutor()
    executor.add_node(node)
    try:
        executor.spin()
    except KeyboardInterrupt:
        pass
    finally:
        executor.shutdown()
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

executor = rclpy.executors.MultiThreadedExecutor(num_threads=4)
executor.add_node(node)
executor.spin()

#!/usr/bin/env python3
import threading
import time
import math
import rclpy
from rclpy.callback_groups import MutuallyExclusiveCallbackGroup
from rclpy.executors import MultiThreadedExecutor
from rclpy.node import Node
from geometry_msgs.msg import Twist
from sensor_msgs.msg import LaserScan
from std_srvs.srv import Trigger

class RobotExecutorNode(Node):
    def __init__(self):
        super().__init__('robot_executor_node')
        self.sensor_group = MutuallyExclusiveCallbackGroup()
        self.control_group = MutuallyExclusiveCallbackGroup()
        self.service_group = MutuallyExclusiveCallbackGroup()

        self.state_lock = threading.Lock()
        self.nearest = math.inf
        self.last_scan_ns = 0

        self.cmd_pub = self.create_publisher(Twist, '/lab/cmd_vel', 10)
        self.scan_sub = self.create_subscription(
            LaserScan, '/scan', self.on_scan, 10,
            callback_group=self.sensor_group)
        self.control_timer = self.create_timer(
            0.05, self.control, callback_group=self.control_group)
        self.service = self.create_service(
            Trigger, '~/status', self.status,
            callback_group=self.service_group)

    def on_scan(self, msg):
        valid = [x for x in msg.ranges if math.isfinite(x) and x > 0.0]
        nearest = min(valid) if valid else math.inf
        with self.state_lock:
            self.nearest = nearest
            self.last_scan_ns = self.get_clock().now().nanoseconds

    def control(self):
        now_ns = self.get_clock().now().nanoseconds
        with self.state_lock:  # 값만 Snapshot하고 즉시 해제한다.
            nearest = self.nearest
            age_ms = (now_ns - self.last_scan_ns) / 1e6

        cmd = Twist()
        if age_ms <= 250.0 and nearest >= 0.5:
            cmd.linear.x = 0.2
        self.cmd_pub.publish(cmd)  # Lock 밖에서 Publish한다.

    def status(self, request, response):
        del request
        with self.state_lock:
            response.message = f'nearest={self.nearest:.3f} m'
        response.success = True
        return response

def main():
    rclpy.init()
    node = RobotExecutorNode()
    executor = MultiThreadedExecutor(num_threads=3)
    executor.add_node(node)
    try:
        executor.spin()
    except KeyboardInterrupt:
        pass
    finally:
        stop = Twist()
        node.cmd_pub.publish(stop)
        executor.shutdown()
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

#include <chrono>
#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"

using namespace std::chrono_literals;

class GroupDemo : public rclcpp::Node {
public:
  GroupDemo() : Node("group_demo") {
    sensor_group_ = create_callback_group(
      rclcpp::CallbackGroupType::MutuallyExclusive);
    control_group_ = create_callback_group(
      rclcpp::CallbackGroupType::MutuallyExclusive);

    rclcpp::SubscriptionOptions options;
    options.callback_group = sensor_group_;
    sub_ = create_subscription<std_msgs::msg::String>(
      "input", 10,
      [this](std_msgs::msg::String::ConstSharedPtr msg) {
        std::lock_guard<std::mutex> guard(mutex_);
        latest_ = msg->data;
      }, options);

    timer_ = create_wall_timer(20ms, [this]() {
      std::string snapshot;
      {
        std::lock_guard<std::mutex> guard(mutex_);
        snapshot = latest_;
      }
      RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 2000,
                           "latest=%s", snapshot.c_str());
    }, control_group_);
  }

private:
  rclcpp::CallbackGroup::SharedPtr sensor_group_, control_group_;
  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr sub_;
  rclcpp::TimerBase::SharedPtr timer_;
  std::mutex mutex_;
  std::string latest_;
};

int main(int argc, char ** argv) {
  rclcpp::init(argc, argv);
  auto node = std::make_shared<GroupDemo>();
  rclcpp::executors::MultiThreadedExecutor executor(
    rclcpp::ExecutorOptions(), 2);
  executor.add_node(node);
  executor.spin();
  rclcpp::shutdown();
}

def request_async(self):
    if not self.client.service_is_ready():
        self.get_logger().warn('service not ready')
        return
    future = self.client.call_async(Trigger.Request())
    future.add_done_callback(self.on_response)

def on_response(self, future):
    try:
        response = future.result()
        self.get_logger().info(response.message)
    except Exception as exc:
        self.get_logger().error(f'service failed: {exc}')

with self.lock:
    snapshot = (self.pose, self.stamp_ns, self.valid)

# 긴 계산은 Lock 밖에서
command = compute_command(*snapshot)
self.publisher.publish(command)

def control_callback(self):
    started = time.perf_counter_ns()
    interval_ms = (started - self.last_started_ns) / 1e6
    self.last_started_ns = started

    self.run_control()

    elapsed_ms = (time.perf_counter_ns() - started) / 1e6
    if elapsed_ms > 10.0 or interval_ms > 60.0:
        self.get_logger().warn(
            f'control runtime={elapsed_ms:.2f} ms interval={interval_ms:.2f} ms',
            throttle_duration_sec=2.0)

ros2 topic hz /control/heartbeat
ros2 topic delay /scan
ros2 topic info /scan --verbose
top -H -p <PID>
pidstat -t -p <PID> 1

ros2 run my_executor_demo blocking_demo --ros-args --log-level info
top -H -p $(pgrep -n -f blocking_demo)

# Single-threaded component container
ros2 run rclcpp_components component_container

# 지원 배포판에서 Multi-threaded container와 Thread 수 지정
ros2 run rclcpp_components component_container \
  --executor-type multi-threaded --ros-args -p thread_num:=4