近藤科学のサーボモータ（Ｂ３Ｍシリーズ）をROSで動かせるようパッケージ化していました。
ktd-prototype.hatenablog.com
このパッケージについて、新たに
- 接続しているサーボのIDをROSパラメータを使って手動で指定する
- 同じくROSパラメータを使って、それぞれのサーボについて個別に制御モードを設定する
機能を追加しました。
これまでのパッケージの概要と欠点
何を以って「ROS化した」と言えるのか、この界隈のお作法的なところは正直まだ把握しきれていませんが・・・ とにかく、
- ROSメッセージを購読してサーボが動くようにした
- ROSメッセージを発行してサーボの情報を取れるようにした
- 既存のROSパッケージ（joyノード）と連接して動くようにした
- 以上の機能を１つのパッケージで再現できるlaunchファイルを整備した
のだから、バチは当たらないでしょう。
一方でこのパッケージ、接続されているサーボの個数やIDを自動検出して、一括で初期化・制御モード変更を行ってしまうため、
- 一度ノードを終了して、別の制御モード用のノードを立ち上げ直さない限り、制御モード（位置制御、速度制御等）を切り替えらない
- 全てのサーボを同じ制御モードで動かす必要がある
というのが欠点であったなあと反省しています。
一括・自動の初期化は便利だろうと思ったし、確かに便利なんですが、複数のサーボを同じ制御モードで、しかも全く同じ初期状態から動かしたいことってあまりないんですよね。
それこそ、以前製作を試みた倒立振子ロボットver1くらいで・・・
この辺は、実際に自分でこのパッケージを使って何種類かロボット製作を試みたことで初めて気がついたことです。
慣れてくれば、実際に自分で使わなくても、ある程度は具備すべき機能のアタリが付くのかも知れませんが・・・・
これらの欠点を解決した新しいノードと、サンプル用のlaunchファイルを作りましたので、その概要を説明いたします。
ノード起動後の流れ
新しく追加したノードは、起動後、以下の流れに沿って動きます。
今までのものとそう大きくは変わりません。
- ノード初期化、Subscriber、Publisherの設定
- 初期設定関数の起動
- launchファイルでROSパラメータとして指定されたサーボIDのリストと対応する制御モードを取得
- 他のノードで利用できるよう、指定されたサーボの個数をPublish
- 指定された全IDのサーボに対して、サーボの初期化を実施
- 同じく、対応する制御モードへの変更を実施
- 以下、ROSメッセージとしてSubscribeした指令値通りにサーボを動かす
- 各サーボごとに、そのサーボの制御モードを確認し、対応する指令値を取得
- その制御モードと指令値に従ってサーボを駆動
- 各サーボの情報を取得し、Publish
- 指令値のうち制御モードに変更があったら、対応する制御モードに変更
サーボの個数をPublishしているのは、実際にサーボへの指令値を計算する他のノード（倒立振子ロボットで言えば車体コントローラ）が、指令値を何個分出せばいいのかわかるようにするためですが、個数だけわかってもあまり意味ないというか、それくらいなら最初から決め打ちにしておけばいいので、IDと制御モードをリストとしてPulishするくらいが良いかもしれません。
launchファイルでパラメータの指定が無かった場合や、あってもそれが不適切な値であった場合のエラー処理についても、今後追加する必要がありますね・・・
現在、位置フィードバックによるトルク制御をスレーブ側、普通のトルク制御（電流制御？）をマスタ側とした、力覚フィードバックが可能なマスタ・スレーブ式のロボットハンドを作成しており、その制御にこのパッケージを使うつもりですので、その製作を通じて改良点を洗い出していければと考えています。
使い方
サーボ、シリアルアダプタ、バッテリ等の電源なんかをPCに繋いでいくところや、シリアル変換アダプタをLinuxで使うための初期設定等は、今までと同じです。
繋いで、シリアルアダプタをきちんとするようになったら、launchディレクトリにあるmulti_servo_control.launch というlaunchファイルのパラメータ部分を変更します。
| パラメータ名 | 形式 | 例 |
| param_servo_id | 整数のリスト | [3, 4] （制御したいサーボがID3、ID4の2個の場合） |
| param_control_mode | 整数のリスト | [4, 8] （ID3を速度制御、ID4をトルク制御する場合。0なら位置制御、16ならトルク制御による位置決め。） |
所望のパラメータに更新したら、このLaunchファイルを起動すれば、サーボが動きます。
一緒にジョイスティックのノード（joy_node）と、ジョイスティックの値からサーボへの指令値を生成しPublishするノード（generate_command_autodetect_joy）を起動するようにしてありますので、お持ちであれば、適宜スティックを使って動かすことができます。
スティックは、位置制御とトルク制御による位置決めが左スティック左右、速度制御が右スティック左右、トルク制御が左スティック上下と割り当てています。
デフォルトではID3、ID4のサーボをそれぞれ位置制御、トルク制御による位置決めで動かすように設定していますので、左スティックの左右を動かすと、2個のサーボが同じ角度になるように動きます。
ただ、ID3は位置制御なのでキビキビ動きます。
ID4はトルク制御による位置決め（アウターループに位置フィードバックを入れたトルク制御）なので、そのゲインに依ってはヌルヌル動きます。（ゲインは、呼び出している駆動関数drive_function.pyの中のcontrol_servo_position_by_Torqueに適当に決めた値が入っています。
今までの使い方も、もちろん引き続き可能です。
この記事自体ももう少し整理したい気もしますが、とりあえず以上です。
