# ROS2 Driver Demo

**What you’ll do**
- ROS2 노드를 생성하여 `KMC_HARDWARE::Driver`를 구동합니다.
- ROS2 메시지인 `cmd_vel`(geometry_msgs/msg/Twist)을 subscribe하여 차량에 **명령 입력**을 전달합니다.
- 마지막으로 수신된 명령을 주기적으로 재전송(Latch)하는 기능을 구현합니다.
- 디버깅을 위해 현재 적용 중인 명령을 `cmd_echo` 토픽으로 발행합니다.

**Prerequisites**
- ROS2 환경
- {doc}`Driver Tutorials </page/SDK/tutorials/driver_tutorials>`

**Next**
- {doc}`ROS2 Driver Observe </page/SDK/tutorials/ros2_driver_observe>`
- {doc}`ROS2 Driver Read AllState </page/SDK/tutorials/ros2_driver_read_allstate>`
- {doc}`ROS2 High Rate Control </page/SDK/tutorials/ros2_high_rate_control>`

---

이 튜토리얼은 기본 SDK의 `driver_demo` 예제를 ROS2 노드 구조로 변환하는 과정을 다룹니다. 

ROS2의 토픽 시스템(`cmd_vel`)과 SDK의 `Driver` 클래스(UART 통신 스레드)를 결합하여 차량을 제어하는 가장 기초적인 노드입니다.

## 목표
1. **노드 구성**: `rclcpp::Node`를 상속받아 UART 포트를 열고 `Driver`를 시작합니다.
2. **명령 Subscribe**: `/cmd_vel` 토픽을 구독하고, 수신된 메시지를 D`river::setCommand(v, omega)`로 전달합니다.
3. **명령 유지** (Optional): `/cmd_vel` 발행이 일시적으로 끊겨도 차량이 멈추지 않도록, 마지막 명령을 주기적으로 `Driver`에 갱신하는 타이머를 구현합니다.
4. **피드백 Publish**: 현재 `Driver`에 설정된 목표값을 `/cmd_echo`로 발행하여 동작을 검증합니다.

---

## 준비물
- ROS2 환경 사용 가능
- `KMC_HARDWARE::Driver` 기본 동작 이해
- UART 장치 접근 권한 확보 (`/dev/ttyKMC`)

---

## 단계별 구현 가이드

### 단계 1) include 준비
SDK의 드라이버 헤더와 ROS2 필수 헤더를 포함합니다.

```cpp
#include "KMC_driver.hpp"

#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/twist.hpp>

#include <chrono>
#include <optional>
#include <string>
```

---

### 단계 2) 노드 클래스 정의
`rclcpp::Node`를 상속받는 클래스를 정의하고, `Driver` 객체를 멤버로 가집니다. 소멸자에서는 반드시 `driver_.stop()`을 호출하여 UART 스레드를 안전하게 종료해야 합니다.

```cpp
class KmcHardwareDriverDemoNode final : public rclcpp::Node {
public:
  KmcHardwareDriverDemoNode() : Node("kmc_hardware_driver_demo_node") {
    // 생성자에서 파라미터 선언 및 Driver 시작 로직을 구현합니다.
  }

  ~KmcHardwareDriverDemoNode() override { driver_.stop(); } // 노드 종료 시 안전 정지 및 포트 닫기

private:
  KMC_HARDWARE::Driver driver_;
};
```

---

### 단계 3) 멤버 변수 선언

ROS 파라미터로 받을 설정 변수들과 Publisher/Subscription 객체를 선언합니다.

```cpp
  // ROS2 통신 객체
  rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_echo_pub_;
  rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_sub_;
  rclcpp::TimerBase::SharedPtr cmd_timer_;

  // 설정 파라미터
  std::string port_;
  int baud_{115200};
  double control_rate_hz_{100.0};
  double vehicle_speed_rate_hz_{1.0};
  int command_timeout_ms_{200};
  double command_refresh_hz_{50.0};
  int realtime_priority_{-1};
  int cpu_affinity_{-1};

  // 명령 상태 저장
  float last_cmd_v_{0.0f};
  float last_cmd_w_{0.0f};
  std::optional<rclcpp::Time> last_cmd_received_;
```

---

### 단계 4) 파라미터 선언

런타임에 `yaml` 파일이나 `ros2` `run` 인자로 설정을 변경할 수 있도록 파라미터를 선언합니다.

```cpp
    port_ = declare_parameter<std::string>("port", "/dev/ttyKMC"); //포트 설정
    baud_ = declare_parameter<int>("baud", 115200); //baudrate 설정
    control_rate_hz_ = declare_parameter<double>("control_rate_hz", 100.0);  // Driver 송신 주기
    vehicle_speed_rate_hz_ = declare_parameter<double>("vehicle_speed_rate_hz", 1.0); // 속도 요청 주기
    command_timeout_ms_ = declare_parameter<int>("command_timeout_ms", 200); // Driver 타임아웃
    command_refresh_hz_ = declare_parameter<double>("command_refresh_hz", 50.0); // 명령 재전송(Latch) 주기
    realtime_priority_ = declare_parameter<int>("realtime_priority", -1);
    cpu_affinity_ = declare_parameter<int>("cpu_affinity", -1);
```

---

### 단계 5) Driver 옵션 구성 + 시작

ROS 파라미터 값을 `Driver::Options` 구조체에 매핑하고, `driver_.start()`를 호출하여 통신 스레드를 시작합니다.
옵션 필드 의미는 {ref}`Driver::Options <sdk-driver-options>`를 기준으로 합니다.

```cpp
    KMC_HARDWARE::Driver::Options opt;
    opt.port = port_;
    opt.serial.baudrate = baud_;
    opt.serial.hw_flow_control = true;
    opt.control_rate_hz = control_rate_hz_;
    opt.vehicle_speed_rate_hz = vehicle_speed_rate_hz_;
    opt.command_timeout_ms = command_timeout_ms_;
    opt.realtime_priority = realtime_priority_;
    opt.cpu_affinity = cpu_affinity_;

    if (!driver_.start(opt)) {
      throw std::runtime_error("Failed to open UART port: " + port_);
    }
```

---

### 단계 6) 토픽 연결 (cmd_vel 구독 + cmd_echo 발행)

`/cmd_vel` 토픽을 구독하여 수신된 메시지(`Twist`)의 선속도와 각속도를 추출하고,`Driver::setCommand()` 함수를 통해 하드웨어 제어 스레드로 전달합니다.

```cpp
    cmd_echo_pub_ = create_publisher<geometry_msgs::msg::Twist>("cmd_echo", 10);
    cmd_sub_ = create_subscription<geometry_msgs::msg::Twist>(
        "cmd_vel", 10, [this](geometry_msgs::msg::Twist::SharedPtr msg) {
          const float v = static_cast<float>(msg->linear.x);
          const float w = static_cast<float>(msg->angular.z);
          last_cmd_v_ = v;
          last_cmd_w_ = w;
          last_cmd_received_ = now();
          driver_.setCommand(v, w);
          cmd_echo_pub_->publish(*msg);
        });
```

매핑 규칙:
- `msg->linear.x` → `v` (m/s)
- `msg->angular.z` → `omega` (rad/s)

---

### 단계 7) 명령 재전송 타이머 (선택)

`Driver`는 `command_timeout_ms` 동안 `setCommand` 호출이 없으면 안전을 위해 차량에 정지 명령을 전송합니다. 

- 만약 ROS2 네트워크 지연 등으로 `/cmd_vel` 주기가 불안정하더라도 차량이 멈추지 않게 하려면, 마지막 명령을 주기적으로 다시 설정해주는 타이머가 필요합니다.
- `command_timeout_ms = 0`이면 정지명령을 전송하지 않습니다.

```cpp
    if (command_refresh_hz_ > 0.0) {
      const auto period = std::chrono::duration_cast<std::chrono::nanoseconds>(
          std::chrono::duration<double>(1.0 / command_refresh_hz_));
      cmd_timer_ = create_wall_timer(period, [this]() {
        if (!last_cmd_received_.has_value()) return;
        driver_.setCommand(last_cmd_v_, last_cmd_w_);
        geometry_msgs::msg::Twist msg;
        msg.linear.x = last_cmd_v_;
        msg.angular.z = last_cmd_w_;
        cmd_echo_pub_->publish(msg);
      });
    }
```


---

### 단계 8) main 함수
노드 생성 후 spin으로 유지합니다.

```cpp
int main(int argc, char** argv) {
  rclcpp::init(argc, argv);
  try {
    auto node = std::make_shared<KmcHardwareDriverDemoNode>();
    rclcpp::spin(node);
  } catch (const std::exception& e) {
    RCLCPP_ERROR(rclcpp::get_logger("kmc_hardware_driver_demo_node"), "Fatal: %s",
                 e.what());
  }
  rclcpp::shutdown();
  return 0;
}
```

---

## Result
완성된 소스는 `examples/Driver_ROS2/src/driver_demo_node.cpp`에 있습니다.

```cpp
#include "KMC_driver.hpp"

#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/twist.hpp>

#include <chrono>
#include <optional>
#include <string>

class KmcHardwareDriverDemoNode final : public rclcpp::Node {
public:
  KmcHardwareDriverDemoNode() : Node("kmc_hardware_driver_demo_node") {
    port_ = declare_parameter<std::string>("port", "/dev/ttyKMC");
    baud_ = declare_parameter<int>("baud", 115200);
    control_rate_hz_ = declare_parameter<double>("control_rate_hz", 100.0);
    vehicle_speed_rate_hz_ = declare_parameter<double>("vehicle_speed_rate_hz", 1.0);
    command_timeout_ms_ = declare_parameter<int>("command_timeout_ms", 200);
    command_refresh_hz_ = declare_parameter<double>("command_refresh_hz", 50.0);
    realtime_priority_ = declare_parameter<int>("realtime_priority", -1);
    cpu_affinity_ = declare_parameter<int>("cpu_affinity", -1);

    KMC_HARDWARE::Driver::Options opt;
    opt.port = port_;
    opt.serial.baudrate = baud_;
    opt.serial.hw_flow_control = true;
    opt.control_rate_hz = control_rate_hz_;
    opt.vehicle_speed_rate_hz = vehicle_speed_rate_hz_;
    opt.command_timeout_ms = command_timeout_ms_;
    opt.realtime_priority = realtime_priority_;
    opt.cpu_affinity = cpu_affinity_;

    if (!driver_.start(opt)) {
      throw std::runtime_error("Failed to open UART port: " + port_);
    }

    cmd_echo_pub_ = create_publisher<geometry_msgs::msg::Twist>("cmd_echo", 10);
    cmd_sub_ = create_subscription<geometry_msgs::msg::Twist>(
        "cmd_vel", 10, [this](geometry_msgs::msg::Twist::SharedPtr msg) {
          const float v = static_cast<float>(msg->linear.x);
          const float w = static_cast<float>(msg->angular.z);
          last_cmd_v_ = v;
          last_cmd_w_ = w;
          last_cmd_received_ = now();
          driver_.setCommand(v, w);
          cmd_echo_pub_->publish(*msg);
        });

    if (command_refresh_hz_ > 0.0) {
      const auto period = std::chrono::duration_cast<std::chrono::nanoseconds>(
          std::chrono::duration<double>(1.0 / command_refresh_hz_));
      cmd_timer_ = create_wall_timer(period, [this]() {
        if (!last_cmd_received_.has_value()) return;
        driver_.setCommand(last_cmd_v_, last_cmd_w_);
        geometry_msgs::msg::Twist msg;
        msg.linear.x = last_cmd_v_;
        msg.angular.z = last_cmd_w_;
        cmd_echo_pub_->publish(msg);
      });
    }
  }

  ~KmcHardwareDriverDemoNode() override { driver_.stop(); }

private:
  KMC_HARDWARE::Driver driver_;

  rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_echo_pub_;
  rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_sub_;
  rclcpp::TimerBase::SharedPtr cmd_timer_;

  std::string port_;
  int baud_{115200};
  double control_rate_hz_{100.0};
  double vehicle_speed_rate_hz_{1.0};
  int command_timeout_ms_{200};
  double command_refresh_hz_{50.0};
  int realtime_priority_{-1};
  int cpu_affinity_{-1};

  float last_cmd_v_{0.0f};
  float last_cmd_w_{0.0f};
  std::optional<rclcpp::Time> last_cmd_received_;
};

int main(int argc, char** argv) {
  rclcpp::init(argc, argv);
  try {
    auto node = std::make_shared<KmcHardwareDriverDemoNode>();
    rclcpp::spin(node);
  } catch (const std::exception& e) {
    RCLCPP_ERROR(rclcpp::get_logger("kmc_hardware_driver_demo_node"), "Fatal: %s",
                 e.what());
  }
  rclcpp::shutdown();
  return 0;
}
```