제6부 하드웨어 인터페이스

6.4 컨트롤러 작성과 체이닝

6.4.1 이 장에서 배우는 것 / 선수 지식

표준 컨트롤러(6.5)로 대부분의 요구가 충족되지만, 언젠가는 직접 써야 한다 — 로봇 고유의 제어 법칙, 특수한 명령 인터페이스, 여러 컨트롤러를 조합한 계층 제어.

컨트롤러 작성이 하드웨어 컴포넌트(6.2)보다 어려운 이유는 ROS 통신과 실시간 경로가 한 클래스 안에 공존하기 때문이다. update()는 실시간이고 콜백은 아니며, 이 둘 사이의 데이터 전달을 잘못하면 지터나 경쟁 조건이 된다(2.9).

배우는 것:

• 구현할 함수들 — 인터페이스 선언·라이프사이클·update().
• update()의 제약 — 무엇이 금지되고 무엇으로 대신하는가.
• realtime_tools — 비실시간 ↔ 실시간 안전한 통신.
• 파라미터 — generate_parameter_library.
• 활성화 시 초기화 — 전환 순간 튀지 않게.
• 체이닝 — 컨트롤러를 겹쳐 계층 제어를 구성하는 법.
• 테스트 — 하드웨어 없이 컨트롤러 검증.

선수 지식: 6.1~6.3(아키텍처·하드웨어·매니저), 2.6(메모리), 2.9(스레드 안전), 1.12(라이프사이클), 4.2.6(pluginlib).

정직: 컨트롤러 API는 배포판마다 변경되어 왔고, 체이닝은 비교적 최근 기능이라 특히 그렇다. 아래 코드는 구조를 보이기 위한 것이며 컴파일 검증하지 않았다(목차 3). 정확한 시그니처는 controller_interface 헤더가 정본이다. 예외: 6.4.8(체이닝)의 서술은 Jazzy 실물과 대조해 확인했다 — 본문의 (실행 확인) 표기가 그것이다.

6.4.2 구현 골격

#include "controller_interface/controller_interface.hpp"
#include "realtime_tools/realtime_buffer.hpp"    // .h 는 폐기 예정 — 6.4.5
#include "realtime_tools/realtime_publisher.hpp"

namespace my_controllers
{

class MyController : public controller_interface::ControllerInterface
{
public:
  // ① 초기화 — 파라미터 선언
  controller_interface::CallbackReturn on_init() override;

  // ② 필요한 인터페이스 선언 (점유 대상 — 6.3.2)
  controller_interface::InterfaceConfiguration
      command_interface_configuration() const override;
  controller_interface::InterfaceConfiguration
      state_interface_configuration() const override;

  // ③ 라이프사이클
  controller_interface::CallbackReturn on_configure(
      const rclcpp_lifecycle::State & previous_state) override;
  controller_interface::CallbackReturn on_activate(
      const rclcpp_lifecycle::State & previous_state) override;
  controller_interface::CallbackReturn on_deactivate(
      const rclcpp_lifecycle::State & previous_state) override;

  // ④ 매 주기 호출 — 실시간
  controller_interface::return_type update(
      const rclcpp::Time & time, const rclcpp::Duration & period) override;

private:
  std::vector<std::string> joint_names_;

  // 비실시간 → 실시간 전달
  realtime_tools::RealtimeBuffer<CmdMsg> rt_command_;
  rclcpp::Subscription<CmdMsg>::SharedPtr cmd_sub_;

  // 실시간 → 비실시간 전달
  std::shared_ptr<realtime_tools::RealtimePublisher<StateMsg>> rt_state_pub_;
  rclcpp::Publisher<StateMsg>::SharedPtr state_pub_;

  // 사전 할당 작업 공간 (2.6.3)
  std::vector<double> error_, output_;
};

}  // namespace my_controllers

6.4.3 인터페이스 선언

컨트롤러가 무엇을 점유할지 선언한다(6.3.2).

controller_interface::InterfaceConfiguration
MyController::command_interface_configuration() const
{
  controller_interface::InterfaceConfiguration config;
  config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
  for (const auto & joint : joint_names_) {
    config.names.push_back(joint + "/" + hardware_interface::HW_IF_EFFORT);
  }
  return config;
}

controller_interface::InterfaceConfiguration
MyController::state_interface_configuration() const
{
  controller_interface::InterfaceConfiguration config;
  config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
  for (const auto & joint : joint_names_) {
    config.names.push_back(joint + "/" + hardware_interface::HW_IF_POSITION);
    config.names.push_back(joint + "/" + hardware_interface::HW_IF_VELOCITY);
  }
  return config;
}

세 가지 설정 타입:

joint_state_broadcaster가 ALL을 쓰는 예다 — 어떤 인터페이스가 있는지 모르는 채로 전부 읽어 발행한다.

함정: 이 함수들은 const이며 on_configure 이후에 호출된다. 즉 joint_names_가 그 시점에 채워져 있어야 한다. on_init이나 on_configure에서 파라미터를 읽어 설정한다.

접근:

// 활성화 후 command_interfaces_ · state_interfaces_ 멤버로 접근
double pos = state_interfaces_[0].get_value();
command_interfaces_[0].set_value(effort);

순서는 선언 순서와 같다 — 하지만 인덱스를 하드코딩하는 것은 취약하다. 이름으로 찾아 인덱스를 캐시하는 편이 안전하다.

// on_activate에서 인덱스를 한 번 찾아 캐시 (실시간 경로에서 탐색 금지)
controller_interface::CallbackReturn MyController::on_activate(
    const rclcpp_lifecycle::State &)
{
  pos_idx_.clear();
  for (const auto & joint : joint_names_) {
    auto it = std::find_if(state_interfaces_.begin(), state_interfaces_.end(),
      [&](const auto & si) {
        return si.get_prefix_name() == joint &&
               si.get_interface_name() == hardware_interface::HW_IF_POSITION;
      });
    if (it == state_interfaces_.end()) {
      RCLCPP_ERROR(get_node()->get_logger(),
                   "상태 인터페이스 없음: %s/position", joint.c_str());
      return controller_interface::CallbackReturn::ERROR;
    }
    pos_idx_.push_back(std::distance(state_interfaces_.begin(), it));
  }
  // ...
}

정직: 인터페이스 접근 API(get_value·set_value·get_prefix_name)는 최신 배포판에서 optional 반환이나 다른 형태로 변경되었을 수 있다(확인 필요).

6.4.4 update() — 실시간 경로

controller_interface::return_type MyController::update(
    const rclcpp::Time & time, const rclcpp::Duration & period)
{
  // ① 최신 명령 읽기 — 락프리
  const auto * cmd = rt_command_.readFromRT();
  if (cmd == nullptr) {
    return controller_interface::return_type::OK;   // 아직 명령 없음
  }

  // ② 명령 나이 검사 — 워치독 (1.4.8)
  if ((time - cmd->stamp).seconds() > command_timeout_) {
    hold_position();                                 // 안전 정지
    return controller_interface::return_type::OK;
  }

  // ③ 실제 dt 사용 (2.5.3)
  const double dt = period.seconds();
  if (dt <= 0.0 || dt > max_dt_) {
    return controller_interface::return_type::OK;    // 비정상 주기 건너뜀
  }

  // ④ 제어 계산 — 할당·락·로깅 없이
  for (size_t i = 0; i < joint_names_.size(); ++i) {
    const double pos = state_interfaces_[pos_idx_[i]].get_value();
    const double vel = state_interfaces_[vel_idx_[i]].get_value();

    error_[i] = cmd->positions[i] - pos;
    integral_[i] = std::clamp(integral_[i] + error_[i] * dt, -i_max_, i_max_);
    output_[i] = kp_[i] * error_[i] + ki_[i] * integral_[i] - kd_[i] * vel;
    output_[i] = std::clamp(output_[i], -effort_max_[i], effort_max_[i]);

    command_interfaces_[i].set_value(output_[i]);
  }

  // ⑤ 상태 발행 — 논블로킹
  if (rt_state_pub_ && rt_state_pub_->trylock()) {
    rt_state_pub_->msg_.header.stamp = time;
    for (size_t i = 0; i < joint_names_.size(); ++i) {
      rt_state_pub_->msg_.error[i] = error_[i];
      rt_state_pub_->msg_.output[i] = output_[i];
    }
    rt_state_pub_->unlockAndPublish();
  }

  return controller_interface::return_type::OK;
}

금지 목록(6.2.7과 동일):

주목할 세 가지:

② 명령 워치독 — 1.4.8에서 "명령 토픽에는 워치독이 필수"라고 한 것이 여기서 구현된다. 통신이 끊겨도 마지막 명령을 계속 실행하면 로봇이 폭주한다.

③ 실제 dt 사용 — period 인자가 실측 주기다. 설정 주기 상수를 쓰면 지터가 제어에 그대로 유입된다(2.5.3).

⑤ trylock() — 발행 버퍼가 사용 중이면 이번 주기는 건너뛴다. 블로킹하지 않으므로 제어 주기를 지킨다. 상태 발행 한 번을 놓치는 것이 주기를 놓치는 것보다 낫다.

6.4.5 realtime_tools — 두 방향의 다리

헤더 확장자에 주의(90차 실행 확인) — Jazzy의 realtime_tools는 realtime_buffer.h·realtime_publisher.h를 포함하면 #warning으로 폐기를 알린다: "This header include is deprecated. Please update your code to use 'realtime_buffer.hpp' header." .hpp를 쓰라. 오류가 아니라 경고라서 빌드 로그에 묻히기 쉽다.

같은 컴파일에서 hardware_interface::LoanedCommandInterface(CommandInterface&, Deleter&&) 생성자도 폐기 예정임이 드러났다("Replaced by the new version using shared_ptr") — 컨트롤러가 인터페이스를 직접 빌려 쓰는 코드를 옮길 때 확인하라(6.3).

RealtimeBuffer — 비실시간 → 실시간:

// 구독 콜백 (executor 스레드)
cmd_sub_ = get_node()->create_subscription<CmdMsg>(
    "~/commands", rclcpp::SystemDefaultsQoS(),
    [this](const CmdMsg::SharedPtr msg) {
      // 검증은 여기서 — 실시간 경로 밖
      if (msg->positions.size() != joint_names_.size()) {
        RCLCPP_WARN(get_node()->get_logger(), "명령 크기 불일치 — 무시");
        return;
      }
      rt_command_.writeFromNonRT(*msg);
    });

// update() (제어 스레드)
const auto * cmd = rt_command_.readFromRT();

동작 원리: 내부적으로 이중 버퍼를 쓰며(2.9.4), 읽는 쪽이 블로킹되지 않는다. 쓰는 쪽이 갱신 중이면 읽는 쪽은 이전 값을 본다.

견해: 명령 검증을 구독 콜백에서 하라. 크기 불일치·NaN·범위 초과를 실시간 경로에서 검사하면 분기와 로깅이 늘어난다. 비실시간 쪽에서 걸러 update()에 도달하는 것은 항상 유효한 명령이 되도록 하는 것이 깔끔하다.

RealtimePublisher — 실시간 → 비실시간:

// on_configure에서 생성
state_pub_ = get_node()->create_publisher<StateMsg>(
    "~/state", rclcpp::SystemDefaultsQoS());
rt_state_pub_ = std::make_shared<realtime_tools::RealtimePublisher<StateMsg>>(state_pub_);

// 메시지 크기를 미리 확보 (실시간에서 resize 금지)
rt_state_pub_->msg_.error.resize(joint_names_.size());
rt_state_pub_->msg_.output.resize(joint_names_.size());

동작 원리: 별도 스레드가 실제 발행을 담당하고, update()는 버퍼에 쓰고 신호만 보낸다. 직렬화·네트워크 비용이 제어 스레드 밖에서 일어난다.

함정: msg_의 크기를 on_configure에서 확보하지 않으면 update()에서 resize가 일어나 할당이 발생한다. 이것이 RealtimePublisher를 쓰면서도 실시간을 깨는 흔한 실수다.

RealtimeBox — 포인터·객체를 안전하게 교환하는 용도(확인 필요: Jazzy에서의 API 형태).

6.4.6 파라미터

generate_parameter_library가 보일러플레이트를 없애 준다.

# src/my_controller_parameters.yaml
my_controller:
  joints:
    type: string_array
    default_value: []
    description: "제어할 관절 목록"
    validation:
      unique<>: null
      not_empty<>: null

  command_timeout:
    type: double
    default_value: 0.5
    description: "명령 타임아웃 (초)"
    validation:
      gt<>: [0.0]

  gains:
    __map_joints:
      p:
        type: double
        default_value: 1.0
        validation: { gt_eq<>: [0.0] }
      i:
        type: double
        default_value: 0.0
      d:
        type: double
        default_value: 0.0

find_package(generate_parameter_library REQUIRED)
generate_parameter_library(my_controller_parameters
  src/my_controller_parameters.yaml)
target_link_libraries(my_controller PRIVATE my_controller_parameters)

#include "my_controller_parameters.hpp"

// 멤버
std::shared_ptr<my_controller::ParamListener> param_listener_;
my_controller::Params params_;

// on_init
param_listener_ = std::make_shared<my_controller::ParamListener>(get_node());

// on_configure
params_ = param_listener_->get_params();
joint_names_ = params_.joints;

// update() — 갱신 확인은 간단히
if (param_listener_->is_old(params_)) {
  params_ = param_listener_->get_params();     // (확인 필요: 실시간 안전성)
}

이점:

정직: get_params()의 실시간 안전성은 확인이 필요하다(확인 필요). 문자열·벡터 복사가 일어날 수 있으므로, 안전하게는 비실시간에서 갱신해 RealtimeBuffer로 넘기는 구조가 맞다. 대부분의 컨트롤러가 update()에서 직접 호출하지만, 엄격한 실시간 요구가 있다면 검증하기 바란다.

6.4.7 활성화 시 초기화

6.3.5에서 "전환 순간 명령 연속성은 컨트롤러 책임"이라고 했다.

controller_interface::CallbackReturn MyController::on_activate(
    const rclcpp_lifecycle::State &)
{
  // ① 인터페이스 인덱스 캐시 (6.4.3)
  if (!cache_interface_indices()) {
    return controller_interface::CallbackReturn::ERROR;
  }

  // ② 현재 상태를 읽어 명령 초기값으로 — ★ 튀지 않게
  for (size_t i = 0; i < joint_names_.size(); ++i) {
    hold_positions_[i] = state_interfaces_[pos_idx_[i]].get_value();
  }

  // ③ 적분항 등 상태 초기화
  std::fill(integral_.begin(), integral_.end(), 0.0);

  // ④ 명령 버퍼 초기화 — 옛 명령이 남아 있으면 위험
  rt_command_.reset();     // (확인 필요: API 이름)

  RCLCPP_INFO(get_node()->get_logger(), "활성화 — 현재 위치 유지로 시작");
  return controller_interface::CallbackReturn::SUCCESS;
}

네 단계 모두 중요하다:

함정: ④가 특히 위험하다. 컨트롤러를 비활성화했다가 몇 분 뒤 다시 활성화하면, 버퍼에 남은 옛 명령이 즉시 적용될 수 있다. 로봇이 예상치 못한 위치로 이동한다. 명령 워치독(6.4.4의 ②)이 이것도 막아 주지만, 버퍼 초기화가 더 직접적이다.

6.4.8 체이닝

컨트롤러를 겹쳐 계층 제어를 구성한다.

핵심 개념: ChainableControllerInterface는 자기 자신이 명령 인터페이스를 "내보낸다". 상위 컨트롤러가 그것을 하위의 입력으로 쓴다.

class MyChainableController : public controller_interface::ChainableControllerInterface
{
protected:
  // 이 컨트롤러가 상위에게 제공할 참조 인터페이스
  std::vector<hardware_interface::CommandInterface>
      on_export_reference_interfaces() override
  {
    std::vector<hardware_interface::CommandInterface> refs;
    for (size_t i = 0; i < joint_names_.size(); ++i) {
      refs.emplace_back(hardware_interface::CommandInterface(
          get_node()->get_name(),                       // 접두어
          joint_names_[i] + "/" + hardware_interface::HW_IF_POSITION,
          &reference_interfaces_[i]));
    }
    return refs;
  }

  // 체인 모드 전환 허용 여부
  bool on_set_chained_mode(bool chained_mode) override
  {
    // 체인 모드에서는 토픽 구독 대신 참조 인터페이스를 쓴다
    return true;
  }

  // 체인 모드일 때의 update
  controller_interface::return_type update_reference_from_subscribers(
      const rclcpp::Time & time, const rclcpp::Duration & period) override
  {
    // 비체인 모드: 토픽에서 참조값 갱신
    const auto * cmd = rt_command_.readFromRT();
    if (cmd) {
      for (size_t i = 0; i < reference_interfaces_.size(); ++i) {
        reference_interfaces_[i] = cmd->positions[i];
      }
    }
    return controller_interface::return_type::OK;
  }

  controller_interface::return_type update_and_write_commands(
      const rclcpp::Time & time, const rclcpp::Duration & period) override
  {
    // 참조값(reference_interfaces_)을 읽어 실제 명령 계산
    for (size_t i = 0; i < joint_names_.size(); ++i) {
      double target = reference_interfaces_[i];
      double actual = state_interfaces_[pos_idx_[i]].get_value();
      command_interfaces_[i].set_value(compute_effort(target, actual, period));
    }
    return controller_interface::return_type::OK;
  }
};

정직: 체이닝 API는 이 책에서 가장 확신이 낮은 부분이다. 함수 이름·시그니처·동작이 배포판마다 다르며(확인 필요), 위 코드는 개념 구조를 보이기 위한 것이다. 실제 작성 시 controller_interface 헤더와 예제 컨트롤러(ros2_controllers 저장소)를 참조하기 바란다.

체이닝이 값을 하는 경우:

체이닝의 대가:

견해: 체이닝은 정말 필요할 때만 쓰라. 계층 제어가 요구사항이 아니라면, 하나의 컨트롤러 안에서 계층을 구현하는 편이 단순하고 디버깅이 쉽다. 체이닝의 실제 가치는 계층을 런타임에 재구성해야 할 때(같은 하위 컨트롤러 위에 다른 상위를 교체하기) 나온다.

진단 도구(실행 확인 2026-08-16): 체이닝 관계는 CLI로 바로 볼 수 있다.

ros2 control view_controller_chains

ros2 control 서브커맨드로 실재한다. 체인이 의도대로 이어졌는지, 어느 컨트롤러가 어느 참조 인터페이스를 물고 있는지 확인하는 첫 명령이다(6.3.6).

6.4.9 등록과 테스트

#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(
    my_controllers::MyController,
    controller_interface::ControllerInterface)

<!-- my_controllers.xml -->
<library path="my_controllers">
  <class name="my_controllers/MyController"
         type="my_controllers::MyController"
         base_class_type="controller_interface::ControllerInterface">
    <description>커스텀 PID 토크 컨트롤러</description>
  </class>
</library>

pluginlib_export_plugin_description_file(controller_interface my_controllers.xml)

하드웨어 없이 테스트:

// controller_manager 테스트 유틸을 활용한 단위 테스트 (개념)
class MyControllerTest : public ::testing::Test {
protected:
  void SetUp() override {
    controller_ = std::make_unique<my_controllers::MyController>();
    // 모의 인터페이스 구성
    pos_values_ = {0.0, 0.0};
    cmd_values_ = {0.0, 0.0};
    // state/command interface 를 직접 만들어 주입
  }
  std::vector<double> pos_values_, cmd_values_;
  std::unique_ptr<my_controllers::MyController> controller_;
};

TEST_F(MyControllerTest, HoldsPositionOnActivate) {
  pos_values_ = {1.5, -0.8};
  // 활성화 후 명령이 현재 위치를 유지하는지 (6.4.7의 ②)
  // ...
}

TEST_F(MyControllerTest, WatchdogStopsOnStaleCommand) {
  // 명령이 오래되면 정지하는지 (6.4.4의 ②)
  // ...
}

견해: 컨트롤러는 단위 테스트하기 좋은 대상이다. 순수 계산이 대부분이고, 인터페이스가 double 배열이라 모의하기 쉽다. 특히 활성화 초기화·워치독·한계 클램프 같은 안전 로직은 반드시 테스트하라 — 이것들은 실기에서 처음 실패하면 물리적 사고가 된다.

통합 테스트는 mock 하드웨어로(6.2.8):

# mock 하드웨어 + 내 컨트롤러로 전체 스택 시험
ros2 launch my_robot_bringup test.launch.py use_mock:=true
ros2 control list_controllers
ros2 topic pub /my_controller/commands ...

6.4.10 함정 카탈로그

5·7번을 강조한다. 활성화 관련 초기화 누락은 하드웨어 컴포넌트(6.2.11의 1번)와 컨트롤러 양쪽에서 같은 방식으로 위험하다. 시뮬에서는 잘 드러나지 않고 실기에서 물리적 사고가 된다.

6.4.11 체크포인트

• 컨트롤러가 구현할 함수 네 부류를 말할 수 있는가.
• 인터페이스 설정 타입 세 가지와 각각의 용도는? joint_state_broadcaster가 ALL을 쓰는 이유는?
• update()에서 금지되는 여섯 가지와 각각의 대안을 말할 수 있는가.
• 명령 워치독이 필요한 이유를 1.4.8과 연결해 설명할 수 있는가.
• period 인자를 dt로 써야 하는 이유는?
• RealtimeBuffer와 RealtimePublisher가 각각 어느 방향의 다리인지, 동작 원리를 말할 수 있는가.
• RealtimePublisher를 쓰면서도 실시간을 깨는 흔한 실수는 무엇인가.
• trylock()이 실패했을 때 건너뛰는 것이 왜 올바른 선택인가.
• 명령 검증을 구독 콜백에서 하라는 조언의 근거는?
• on_activate의 네 단계와, 각각을 빠뜨렸을 때의 증상을 말할 수 있는가.
• 체이닝의 핵심 개념(참조 인터페이스)과, 값을 하는 경우·대가를 말할 수 있는가. 실제 가치는 언제 나오는가.
• 컨트롤러에서 반드시 단위 테스트해야 할 로직 세 가지는?

다음 장 6.5는 직접 쓰기 전에 확인해야 할 것 — 표준 컨트롤러 카탈로그다. 무엇이 이미 있고, 어떤 것을 언제 쓰며, 설정은 어떻게 하는가.