UartClient Teleop and Speed

UartClient Teleop and Speed#

What you’ll do

  • UartClient로 명령 입력 송신과 속도 요청/출력을 한 루프에서 직접 스케줄링합니다.

Prerequisites

Next

이 튜토리얼은 UartClient로 직접 스케줄링하여 명령 입력 송신 + 속도 요청/출력을 구현합니다.

참고

UartClient.sendPcControl(v, kappa)의 두 번째 인자는 \(\omega\)가 아니라 곡률 \(\kappa\)(\(\mathrm{m}^{-1}\))입니다. 필요하면 Vehicle Control Model의 \(\kappa=\omega/v\) 변환을 적용 후 전송합니다.

1. 목표#

  • UartClient로 명령 입력 송신과 속도 요청/출력을 구현

  • 스케줄링/타이밍을 직접 관리하는 루프 구성

2. 단계별 구현 가이드#

단계 1) 포트 열기#

UartClient는 저수준이기 때문에 먼저 포트를 열고, 초기 입력을 비웁니다.

  KMC_HARDWARE::UartClient client;
  if (!client.open(port, 115200)) {
    // 에러 처리
    return 1;
  }
  client.flushInput();

단계 2) 스케줄링을 위한 타이머 설정#

여기서 “다음에 보낼 시간(next)”을 잡고, 루프에서 시간이 되면 동작합니다.

  const auto t0 = std::chrono::steady_clock::now();
  auto next_ctrl = t0;
  auto next_b3 = t0;

단계 3) 메인 루프 구현 (직접 주기 관리)#

  while (...) {
    const auto now = std::chrono::steady_clock::now();

    // 1. 명령 입력 송신 (100 Hz)
    if (now >= next_ctrl) {
      client.sendPcControl(v, k);
      next_ctrl += std::chrono::milliseconds(10); // 10ms = 100Hz
    }

    // 2. 속도 요청 (20 Hz)
    if (now >= next_b3) {
      client.requestVehicleSpeed();
      next_b3 += std::chrono::milliseconds(50); // 50ms = 20Hz
    }

    // 3. 수신 (Non-blocking poll)
    // poll(0)을 사용하여 대기 없이 버퍼에 있는 데이터를 즉시 확인합니다.
    if (auto msg = client.poll(0)) {
      if (auto* vs = std::get_if<KMC_HARDWARE::VehicleSpeed>(&*msg)) {
        std::printf("VehicleSpeed: %.3f m/s\n", vs->mps);
      }
    }

    // 4. CPU 사용률 제어
    // 저수준 루프는 쉽게 바쁘게 돌아 CPU를 태우기 때문에, 짧은 sleep을 넣습니다.
    std::this_thread::sleep_for(std::chrono::milliseconds(1));
  }

3. Result#

아래 코드는 SDK 리포지토리의 examples/UartClient_Advanced/teleop_and_speed.cpp와 동일한 내용입니다.

#include "KMC_uart_client.hpp"

#include <chrono>
#include <cstdio>
#include <cstdlib>
#include <string>
#include <thread>
#include <variant>

int main(int argc, char **argv) {
  const std::string port = (argc > 1) ? argv[1] : "/dev/ttyKMC";
  int seconds = (argc > 2) ? std::atoi(argv[2]) : 5;
  if (seconds <= 0) seconds = 5;

  KMC_HARDWARE::UartClient client;
  if (!client.open(port, 115200)) {
    std::fprintf(stderr, "Failed to open port: %s\n", port.c_str());
    return 1;
  }
  client.flushInput();

  const auto t0 = std::chrono::steady_clock::now();
  auto next_ctrl = t0;
  auto next_b3 = t0;
  const float v = 0.5f;
  const float k = 0.0f;

  while (std::chrono::steady_clock::now() - t0 < std::chrono::seconds(seconds)) {
    const auto now = std::chrono::steady_clock::now();

    if (now >= next_ctrl) {
      client.sendPcControl(v, k);
      next_ctrl += std::chrono::milliseconds(10);
    }

    if (now >= next_b3) {
      client.requestVehicleSpeed();
      next_b3 += std::chrono::milliseconds(50);
    }

    if (auto msg = client.poll(0)) {
      if (auto* vs = std::get_if<KMC_HARDWARE::VehicleSpeed>(&*msg)) {
        std::printf("VehicleSpeed: %.3f m/s\n", vs->mps);
      }
    }

    std::this_thread::sleep_for(std::chrono::milliseconds(1));
  }

  client.sendPcControl(0.0f, 0.0f);
  client.close();
  return 0;
}
Contents