環境
この記事は以下の環境で動いています。
項目 | 値 |
---|---|
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側で形が違うのですがそれぞれ以下のような切り欠きがある板を作る必要があります
また付属のケーブルが15cmで今回の形に組み立てようとすると長さが足りませんでした。3Pinシリアルのコネクタは以下のMolexの物を使っています。
通信方式
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ルールを追加して別名にしましょう。以下のファイルを追加します。
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
#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
#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
#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
#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