環境

この記事は以下の環境で動いています。

項目	値
CPU	Core i5-8250U
Ubuntu	22.04
ROS2	Humble

概要

feetech製の安いシリアル通信方式のサーボが秋月で販売さてています。これを使ってパンチルトカメラを作ります。

秋月電子webより

feetech STS3215

機能

STS3215はシリアル通信方式のサーボで以下の特徴があります。

電圧6~7.4V
最大トルク19.5kg・cm
最大速度0.192sec/60deg
360deg回転
両軸（出力軸の逆側にもフリー軸がある）
3線シリアル接続（デイジーチェーン可）
速度制御可能

秋月電子Webで3000円弱で販売されていて、この性能のサーボにしては大変安価です。
またUSBシリアル変換基板であるインターフェイス基板が同様に秋月電子webにて1500円弱で売っていると、まさにROSとつなぐのにうってつけの部品です。

HW接続

本体はM2タッピングねじで止めるのですがねじ穴の周りが複雑な形状をしていて、それをよける板を作成する必要があります。Top側とBottom側で形が違うのですがそれぞれ以下のような切り欠きがある板を作る必要があります

↓Top側

↓Bottom側

また付属のケーブルが15cmで今回の形に組み立てようとすると長さが足りませんでした。3Pinシリアルのコネクタは以下のMolexの物を使っています。

ハウジング Molex5264-03 千石web
ピン Molex5263 千石web

通信方式

Servoとはシリアル通信で接続します。通信内容の資料がまとまっていない、かつ公式サイトにまともなドキュメントがなく秋月の標品ページにしかまともな情報がないのですが、これを読み解いていきます。

レジスタ説明

基本的なコンセプトとして、サーボモーター自体にレジスタがあり、これを書き込んだり、読み込んだりして操作を行います。

秋月のサイトのzipの中の資料に詳細なものがありますが、ここではよく使うものを抜粋して紹介します。

アドレス	項目	値の範囲	デフォルト値	R/W	説明
0x05	ID	0~253	1	R/W	サーボモーターのID

0x2A	目標位置(L)	0~255	?	W	値の下8bit
0x2B	目標位置(H)	0~255?	?	W	値の上8bit

0x2E	目標速度(L)	0~255	?	W	値の下8bit
0x2F	目標速度(H)	0~255?	?	W	値の上8bit

0x38	現在位置(L)	0~255	?	R	値の下8bit
0x39	現在位置(H)	0~255?	?	R	値の上8bit
0x3A	現在速度(L)	0~255	?	R	値の下8bit
0x3B	現在速度(H)	0~255?	?	R	値の上8bit

目標位置/現在位置は2Byteを使って0~4095までの値をとります。0~4095までで丁度1回転します。
目標速度は2Byteを使って表して、例えば100としたら、1秒間で現在位置が100だけ変化する速度を最大とします。実際に軸が正負どちらの方向で回るとしても、目標速度には正の値を入れます。この値が0の時はサーボモーターの最大速度で動きます。
現在速度は2Byteを使って表します。こちらは目標速度と違い正負の値をとります。

通信プロトコル

こちら秋月の「FEETECH 社デジタルサーボの使用方法」にしか詳細が載っていないのですが、以下のようなパケットの形式をとります。

書き込みの例

バイト位置	項目	説明
1	ヘッダー	0xFF固定
2	ヘッダー	0xFF固定
3	ID番号	サーボモーターのIDを指定
4	パケットデータ長さ	IDからチェックサム手前まで-1で、N+2になる
5	コマンド	3:書き込み
6	レジスタ先頭アドレス
7~	データの内容
7+N	チェックサム	ID番号からチェックサム手前までの値から生成する

連続したアドレスのレジスタなら1つのパケットでまとめて書き込むことが出来ます。
チェックサムは「ID番号」から「チェックサム手前」までの値を合計した下8bitをビット反転した値です。

読み込みの例

読み込み時にサーボモーターに送るパケットは以下

バイト位置	項目	説明
1	ヘッダー	0xFF固定
2	ヘッダー	0xFF固定
3	ID番号	サーボモーターのIDを指定
4	パケットデータ長さ	4固定
5	コマンド	4:読み込み
6	レジスタ先頭アドレス
7	読み込みバイト数
8	チェックサム	ID番号からチェックサム手前までの値から生成する

サーボモーターからの返答は以下

バイト位置	項目	説明
1	ヘッダー	0xFF固定
2	ヘッダー	0xFF固定
3	ID番号	サーボモーターのIDを指定
4	パケットデータ長さ	N+2になる
5	空白	0固定
6~6+N-1	内容	レジスタの内容(N Byte)
6+N	チェックサム	ID番号からチェックサム手前までの値から生成する

タイミング

インターフェイスボードの回路では送信したコマンドがループバックして自分で受信しないようになっています。
読み込みのパケットを送信してから、応答が返るまでに数msのラグがあります。この時間waitする必要があります。

FT SCServo Debugを使った試運転(on Windows)

Feetech純正のサーボモーターの試運転や設定をする純正アプリがあります。

HW接続

USBをPCと接続
電源(G & V1)を7.2V電源と接続
SCSと書かれた側のコネクタをサーボモーターと接続
スイッチは5V側に倒す

ソフトウェアの起動

インターフェイス基板はUSB-シリアル変換ICの「CH340」が使われています。最新のWindowsならドライバーはすでに入っているはずです。

秋月webより「FT-SCServo Debug V1.9.8(FD1.9.8-EN_200923.zip)」をダウンロードして解凍します。
中の「FD1.9.8-EN_200923」の「FD.exe」を起動します。

左の「Com Setting」の「Com」でポートを選択します。
「BaudR」は「1000000」を選択して、「Open」ボタンを押します。
「Servo List」の「Search」ボタンを押すとつながっているサーボモーターのIDをスキャンします。
出てきたリストの中から操作したい行を選択します。
「Servo Control」のスライダーを操作すると、サーボの軸が動きます。

IDの書き換え

「Programming」タブに切り替えます。
ここの「ID」の部分の値を書き換えます。

PanTiltノードの作成(ROS2 on Ubuntu)

過去の記事で作成した「HeadCamera」を操作するROSノードを作成します。

事前準備

インターフェースボードをUbuntuに接続すると、ドライバが認識されません。sudo dmesgで調べてみます。

[ 3072.516464] usb 3-2.4: New USB device found, idVendor=1a86, idProduct=7523, bcdDevice= 2.64
[ 3072.516473] usb 3-2.4: New USB device strings: Mfr=0, Product=2, SerialNumber=0
[ 3072.516477] usb 3-2.4: Product: USB Serial
[ 3072.523490] ch341 3-2.4:1.0: ch341-uart converter detected
[ 3072.524302] usb 3-2.4: ch341-uart converter now attached to ttyUSB0
[ 3073.063633] input: BRLTTY 6.4 Linux Screen Driver Keyboard as /devices/virtual/input/input24
[ 3073.151669] usb 3-2.4: usbfs: interface 0 claimed by ch341 while 'brltty' sets config #1
[ 3073.152413] ch341-uart ttyUSB0: ch341-uart converter now disconnected from ttyUSB0
[ 3073.152437] ch341 3-2.4:1.0: device disconnected

brlttyというモジュールが悪さをしているみたいです。sudo apt purge brlttyで削除します。

この後インターフェースボードを接続すると/dev/ttyUSB0が出現します。しかしttyUSB0の名前はほかのUSBシリアル変換を刺した場合に読み込み順で変わってしまう場合があります。udevルールを追加して別名にしましょう。以下のファイルを追加します。

/etc/udev/rules.d/99-feetech.rules

SUBSYSTEM=="tty", ATTRS{idVendor}=="1a86", ATTRS{idProduct}=="7523", SYMLINK+="feetech"

これで/dev/feetechでアクセスできるようになります。
上記ではCH340系のモジュールを認識しているだけなのでFeetechのインターフェイス以外もこの名前になる可能性があります（ジェネリックArduinoなどで使われています）。ほかに特定する手掛かりがないのでこの粒度での指定になります。

ソースコード

4つのクラスに分かれています。

SerialPotHandlerはシリアルポートのOpenやRead、Write処理を行います。
FeetechHandlerは上記で解説した送信パケットの生成や受信パケットの解釈を行います。
PanTiltRosIfはROSのpub/Subを行うクラスです。
PanTiltNodeはPanTiltRosIfを継承、FeetechHandlerをメンバ変数として持つROSノード本体です。

SerialPotHandler

feetech_ros/include/feetech_ros/serial_port_handler.hpp

#pragma once

#include <string>
#include <unistd.h>
#include <fcntl.h>
#include <termios.h>
#include <vector>

class SerialPortHandler
{
public:
  SerialPortHandler() {}

  bool Open(std::string device_name)
  {
    fd_ = open(device_name.c_str(), O_RDWR | O_NOCTTY | O_NONBLOCK);
    fcntl(fd_, F_SETFL, 0);

    // load configuration
    struct termios conf_tio;
    tcgetattr(fd_, &conf_tio);
    // set baudrate
    speed_t BAUDRATE;
    BAUDRATE = B1000000;

    cfsetispeed(&conf_tio, BAUDRATE);
    cfsetospeed(&conf_tio, BAUDRATE);

    // non canonical, non echo back
    conf_tio.c_lflag &= CS8;
    // conf_tio.c_lflag &= ~(ECHO | ICANON);
    // non blocking
    conf_tio.c_cc[VMIN] = 0;
    conf_tio.c_cc[VTIME] = 0;
    // store configuration
    tcsetattr(fd_, TCSANOW, &conf_tio);

    if (fd_ < 0)
    {
      printf("fail to open port\n");
      return false;
    }

    return true;
  }

  void SetVerbose(void) {
    verbose_ = true;
  }

  bool Write(const std::vector<unsigned char> buffer)
  {
    if (fd_ < 0)
    {
      printf("fd not initialized\n");
      return false;
    }
    if (verbose_) {
      printf("write[%lu] ", buffer.size());
      for (auto b : buffer)
      {
        printf("%x ", b);
      }
      printf("\n");
    }

    write(fd_, buffer.data(), (int)buffer.size());
    return true;
  }

  std::vector<unsigned char> Read(void)
  {
    if (fd_ < 0)
    {
      printf("fd not initialized\n");
      return {};
    }
    std::vector<unsigned char> buffer;
    buffer.resize(256);
    int recv_num = read(fd_, buffer.data(), buffer.size());
    if (0 < recv_num)
    {
      if (verbose_) {
        printf("recv[%lu] ", (size_t)recv_num);
        for (size_t i = 0; i < (size_t)recv_num; i++)
        {
          printf("%x ", buffer[i]);
        }
        printf("\n");
      }
    }
    buffer.resize(recv_num);
    return buffer;
  }

  void CloseSerial(void) { close(fd_); }

private:
  int fd_{0};
  bool verbose_{false};
};

Open()の処理ではシリアルポートの設定をしています。通常と違うのは受信処理で以下の設定をしています。
- 非カノニカル：通常は\nまでを1単位をして扱うところを、\nが無くても1文字単位で受信する
- 非ブロッキング：通常はRead()を呼ぶと受信データが来るまで処理をブロックするところを、一定時間でタイムアウトするようにする。

FeetechHandler

feetech_ros/include/feetech_ros/feetech_handler.hpp

#pragma once
#include "serial_port_handler.hpp"
#include <map>
#include <numeric>
#include <optional>

struct ServoStatus
{
  int position{2048};
  int velocity{0};
};

struct ServoConfig
{
  int min_tick{1024};
  int max_tick{3072};
};

class FeetechHandler
{
public:
  FeetechHandler(void) {}

  bool Initialize(const std::map<int, ServoConfig> &config_list)
  {
    serial_port_handler_.Open("/dev/feetech");
    config_list_ = config_list;
    return true;
  }

  void Close(void) { serial_port_handler_.CloseSerial(); }

  bool SetCommand(const int id, const int position, const int velocity)
  {
    if (!config_list_.count(id))
    {
      printf("id[%d] not found in config_list\n", id);
      return false;
    }
    const ServoConfig config = config_list_.at(id);
    int regulated_position = std::min(std::max(position, config.min_tick), config.max_tick);
    int regulated_velocity = std::min(std::max(velocity, 0), max_tick_per_sec_);
    std::vector<unsigned char> send_data = GenerateSetPositionVelocityCommand(id, regulated_position, regulated_velocity);
    SendCommandAndWait(send_data);
    return true;
  }

  bool RequestStatus(void)
  {
    for (auto config_pair : config_list_)
    {
      int id = config_pair.first;
      std::vector<unsigned char> send_data = GenerateRequestPositionVelocityCommand(id);
      SendCommandAndWait(send_data);
    }
    std::vector<unsigned char> buffer = serial_port_handler_.Read();
    std::vector<std::vector<unsigned char>> split_data = SplitInput(buffer);
    for (auto msg : split_data)
    {
      std::optional<std::pair<int, ServoStatus>> result_opt = ParseInput(msg);
      if (result_opt)
      {
        status_list_[result_opt.value().first] = result_opt.value().second;
      }
    }
    return true;
  }

  std::optional<ServoStatus> GetStatus(const int id) const
  {
    if (status_list_.count(id))
    {
      return status_list_.at(id);
    }
    return std::nullopt;
  }

private:
  bool SendCommandAndWait(const std::vector<unsigned char> &buffer)
  {
    std::vector<unsigned char> send_command;
    send_command.push_back(0xff);
    send_command.push_back(0xff);
    for (auto b : buffer)
    {
      send_command.push_back(b);
    }
    send_command.push_back(GetCheckSum(buffer));
    bool ret = serial_port_handler_.Write(send_command);
    usleep(4 * 1000); // wait 4[ms] for finish sending
    return ret;
  }

  static unsigned char GetCheckSum(const std::vector<unsigned char> &buffer)
  {
    int sum = std::accumulate(buffer.begin(), buffer.end(), 0);
    return 0xff - (0xff & sum);
  }

  static std::vector<std::vector<unsigned char>> SplitInput(const std::vector<unsigned char> &buffer)
  {
    std::vector<std::vector<unsigned char>> split_output;
    for (size_t i = 0; i + 3 < buffer.size();)
    {
      bool h0_valid = (buffer[i + 0] == 0xff);
      bool h1_valid = (buffer[i + 1] == 0xff);
      int length = buffer[i + 3];
      bool ln_valid = (size_t)(i + 3 + length) < buffer.size();
      if (h0_valid && h1_valid && ln_valid)
      {
        unsigned char checksum = buffer[i + 3 + length];

        std::vector<unsigned char> msg;
        for (size_t j = 2; j < (size_t)(length + 3); j++)
        {
          msg.push_back(buffer[i + j]);
        }
        unsigned char expected_check_sum = GetCheckSum(msg);

        if (checksum == expected_check_sum)
        {
          split_output.push_back(msg);
        }

        i += (length + 4);
        continue;
      }
      i++;
    }
    return split_output;
  }

  static std::optional<std::pair<int, ServoStatus>> ParseInput(const std::vector<unsigned char> &msg)
  {
    if (msg.size() != 7)
    {
      return std::nullopt;
    }
    std::pair<int, ServoStatus> output;
    // set id
    output.first = msg[0];
    // set position
    output.second.position = msg[4] << 8 | msg[3];
    // set velocity
    uint16_t vel_tmp = msg[6] << 8 | msg[5];
    if (vel_tmp & 0x8000)
    {
      output.second.velocity = -((int)vel_tmp - 0x8000);
    }
    else
    {
      output.second.velocity = vel_tmp;
    }
    return output;
  }

  static std::vector<unsigned char> GenerateSetPositionVelocityCommand(const int id, const int pos, const int vel)
  {
    std::vector<unsigned char> buffer;
    buffer.push_back(id);
    buffer.push_back(9);                 // len
    buffer.push_back(3);                 // write command
    buffer.push_back(42);                // command_pos_time_vel address
    buffer.push_back(0xff & (pos >> 0)); // pos
    buffer.push_back(0xff & (pos >> 8));
    buffer.push_back(0x00); // time
    buffer.push_back(0x00);
    buffer.push_back(0xff & (vel >> 0)); // vel
    buffer.push_back(0xff & (vel >> 8));
    return buffer;
  }

  static std::vector<unsigned char> GenerateRequestPositionVelocityCommand(const int id)
  {
    std::vector<unsigned char> buffer;
    buffer.push_back(id);
    buffer.push_back(4);    // len
    buffer.push_back(2);    // write command
    buffer.push_back(0x38); // current_pos
    buffer.push_back(4);
    return buffer;
  }

private:
  SerialPortHandler serial_port_handler_;
  std::map<int, ServoConfig> config_list_;
  std::map<int, ServoStatus> status_list_;
  static constexpr int max_tick_per_sec_ = 4095;
};

PanTiltRosIf

feetech_ros/src/pan_tilt/pan_tilt_ros_if.hpp

#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/twist_stamped.hpp>
#include <sensor_msgs/msg/joint_state.hpp>

using namespace std::chrono_literals;
using namespace std::placeholders; // NOLINT

class PanTiltRosIf : public rclcpp::Node
{
public:
  PanTiltRosIf() : Node("pan_tilt_node")
  {
    joint_state_pub_ = this->create_publisher<sensor_msgs::msg::JointState>("/joint_states", 1);
    twist_sub_ = this->create_subscription<geometry_msgs::msg::TwistStamped>("cmd_rate", 10,
        std::bind(&PanTiltRosIf::onTwistReceived, this, std::placeholders::_1));
    timer_ = this->create_wall_timer(50ms, std::bind(&PanTiltRosIf::onTimer, this));
  }

protected:
  virtual void onTimer() = 0;
  virtual void onTwistReceived(const geometry_msgs::msg::TwistStamped::SharedPtr msg) = 0;

  void publishJointState(const sensor_msgs::msg::JointState &msg)
  {
    joint_state_pub_->publish(msg);
  }

private:
  rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr joint_state_pub_{nullptr};
  rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr twist_sub_{nullptr};
  rclcpp::TimerBase::SharedPtr timer_{nullptr};
};

PanTiltNode

feetech_ros/src/pan_tilt/pan_tilt_node.cpp

#include <feetech_ros/feetech_handler.hpp>
#include "pan_tilt_ros_if.hpp"

class PanTiltNode : public PanTiltRosIf
{
public:
  PanTiltNode(void) : PanTiltRosIf{}
  {
    printf("start feetech\n");
    std::map<int,ServoConfig> config_list;
    config_list[1] = {1024, 3072};
    config_list[2] = {1024, 2400};
    bool open_success = feetech_handler_.Initialize(config_list);
    if (!open_success) {
      printf("fail to open serial\n");
      throw;
    }
  }

private:
  void onTimer() override
  {
    feetech_handler_.RequestStatus();
    
    sensor_msgs::msg::JointState joint_state;
    joint_state.header.stamp = now();

    auto s1_opt = feetech_handler_.GetStatus(1);
    if (s1_opt) {
      auto& status = s1_opt.value();
      joint_state.name.push_back("turret_pan_joint");
      joint_state.position.push_back(((float)(status.position - center_tick_)) / tick_per_rad_);
      joint_state.velocity.push_back((float)status.velocity / tick_per_rad_);
    }
    auto s2_opt = feetech_handler_.GetStatus(2);
    if (s2_opt) {
      auto& status = s2_opt.value();
      joint_state.name.push_back("turret_tilt_joint");
      joint_state.position.push_back(((float)(status.position - center_tick_)) / tick_per_rad_);
      joint_state.velocity.push_back((float)status.velocity / tick_per_rad_);
    }
    publishJointState(joint_state);
  }

  void onTwistReceived(const geometry_msgs::msg::TwistStamped::SharedPtr msg) override {
    setCommand(1, msg->twist.angular.z);
    setCommand(2, msg->twist.angular.y);
  }

  void setCommand(const int id, const float value) {
    float tick_per_s = value * tick_per_rad_;
    if (std::abs(tick_per_s) < 5) { // hold
      auto status_opt = feetech_handler_.GetStatus(id);
      if (status_opt) {
        feetech_handler_.SetCommand(id, status_opt.value().position, 1);
      } else {
        printf("id[%d] no status for hold\n", id);
      }
    } else {
      int temporary_position = 0 < tick_per_s ? 4096 : 0;
      feetech_handler_.SetCommand(id, temporary_position, std::abs(tick_per_s));
    }
    usleep(1000); // guard wait
  }

private:
  FeetechHandler feetech_handler_;
  static constexpr int center_tick_ = 2048;
  static constexpr int tick_per_rad_ = 651.9f;
};

int main(int argc, char *argv[]) {
  rclcpp::init(argc, argv);
  auto pan_tilt_node = std::make_shared<PanTiltNode>();
  rclcpp::spin(pan_tilt_node);
  rclcpp::shutdown();
  return 0;
}

他のコード

以下のサンプルを実行するために

joy->twistの変換
rviz用のurdf
上記を起動するlaunchファイル

が必要です。

ビルド&実行

ビルド

source /opt/ros/humble/setup.bash
cd ros2_ws
colcon build

ビルド

source /opt/ros/humble/setup.bash
ros2 launch feetech_ros turret.launch.py

目次ページへのリンク

ROS2講座の目次へのリンク