cvl-robot's diary

研究ノート メモメモ https://github.com/dotchang/

Emlid Navio2で動かすardupilotにxwindowを入れる

要らないものもあるかもしれないけれど、とりあえず起動するのでOK。

sudo apt-get update
sudo apt-get install lxde
sudo apt-get install lightdm
sudo apt-get install xinit
sudo apt-get install xutils
sudo apt-get install xserver-xorg

startxでLXDEというデスクトップが立ち上がります。

lxdeは古いので、新しいPIXELを使いたい、という場合はこちら。

sudo apt-get install raspberrypi-ui-mods

sudo apt-get install firefox-esr

単にxwindowを入れた状態ではlibEGLの設定がおかしな状態になっていて、openFrameworksやQtが立ち上がらないという問題があります。
[1]のサイトを参考に、libEGLのライブラリへのリンクを張りなおすと立ち上がるようになります。
raspi-configでGL DriverをLegacyに設定するのを忘れずに。

# sudo ln -fs /opt/vc/lib/libEGL.so /usr/lib/arm-linux-gnueabihf/libEGL.so
# sudo ln -fs /opt/vc/lib/libGLESv2.so /usr/lib/arm-linux-gnueabihf/libGLESv2.so
# sudo ldconfig

[1] raspbian - Qt applications don't work due to libEGL - Raspberry Pi Stack Exchange

掃除機の買い方の研究

掃除機は、多くの人には数年に一度しか買い替える機会がないものかと思います。
急に掃除機が欲しくなるというよりも、そろそろ買い替えたいなー、という気分になったときしばらく時間をかけて検討して買い替えするかと思います。
ということは、時々安くなるセールを狙って買えるということです!定期的に安くなるモデルを見つけたので、紹介します。

何を買うか

ダイソンのハンディのバッテリー駆動モデルと、ロボット掃除機のルンバを狙います。

ビックカメラの量販店モデル
ダイソンDC74 MH EX: 

ルンバ631: 

ジャパネットたかたの量販店モデル
ルンバ626: 

いずれもお買い得感のあるエントリーモデルです。

どこで買うか

ビックカメラ・コジマか、ジャパネットたかたをチェックします。
店頭でもネットでも価格は同じでしたが、ポイントの付き方が違います。
アマゾンポイントが安く買えて使えるときは、アマゾンがお得かと思います。

いつ買うか

ジャパネットたかたのセールの日に、ビックカメラがえげつなくセールをぶつけてくることが多いので
半期に一度ぐらいのジャパネットたかたのセールをテレビで見かけたら、ビックカメラのセール品をチェックします。

いくらで買うか

半年に一度ぐらい、どちらも\29800で販売されることがあります。
去年の実績は10月ごろと3月ごろに、それぞれ一日だけその価格で販売されていました。

LAUNCHXL-F28069MとBOOSTXL-DRV8305EVMでSCIを使うとき、問題を避けるためにADCの割り込み優先順位を上げる方法

TiのC2000Piccoloの割り込みは、独自のPIEテーブルとかいうものに基づいたもので、とても分かりにくく使いにくいです。
C2000シリーズではPIEテーブルの順序の変更も容易ではなく(掲示板にはデキナイ、との書き込みも)、さらにその定義も小首を傾げたくなるような順序に並んでいます。
InstaSPINモータドライバを使うとき、ADCがフル稼働しているのですが、標準だとこの優先順位がSCI(UART)よりも低く設定されているため、SCI割り込みが入るとモータが動かせないという謎仕様です。
ただし、これは問題を知っていれば簡単に回避できます。

割り込みトリガーをADC_IntNumber_1からADC_IntNumber_1HP(High Priority)に変更

なぜか、同じADCに優先順位の設定が2つ割り当てられていて、10番(標準)と1番(優先)です。
デフォルトで10番に割り当てられているものを1番のPIEで駆動されるように変更します。

hal.c

void HAL_enableAdcInts(HAL_Handle handle)
{
  HAL_Obj *obj = (HAL_Obj *)handle;

  // enable the PIE interrupts associated with the ADC interrupts
  PIE_enableAdcInt(obj->pieHandle,ADC_IntNumber_1HP); // ADC_IntNumber_1 to ADC_IntNumber_1HP

  // enable the ADC interrupts
  ADC_enableInt(obj->adcHandle,ADC_IntNumber_1);

  // enable the cpu interrupt for ADC interrupts
  CPU_enableInt(obj->cpuHandle,CPU_IntNumber_1); // CPU_IntNumber_10 to CPU_IntNumber_1

  return;
} // end of HAL_enableAdcInts() function

hal.h

static inline void HAL_initIntVectorTable(HAL_Handle handle)
{
  HAL_Obj *obj = (HAL_Obj *)handle;
  PIE_Obj *pie = (PIE_Obj *)obj->pieHandle;

  ENABLE_PROTECTED_REGISTER_WRITE_MODE;

  pie->ADCINT1_HP = &mainISR; // pie->ADCINT1 to pie->ADCINT1_HP
  //pie->SCIRXINTA = &sciarxISR;

  DISABLE_PROTECTED_REGISTER_WRITE_MODE;

  return;
} // end of HAL_initIntVectorTable() function

hal.h

static inline void HAL_acqAdcInt(HAL_Handle handle,const ADC_IntNumber_e intNumber)
{
  HAL_Obj *obj = (HAL_Obj *)handle;

  // clear the ADC interrupt flag
  ADC_clearIntFlag(obj->adcHandle,intNumber);

  // Acknowledge interrupt from PIE group 1
  PIE_clearInt(obj->pieHandle,PIE_GroupNumber_1); // PIE_GroupNumber_10 to PIE_GroupNumber_1

  return;
} // end of HAL_acqAdcInt() function

これでも、SCITX割り込みがうまく動かない。なんでた”~。

SLAM実験に便利そうな移動ロボット用プラットフォーム

筑波大の油田研の移動ロボットの研究成果を元に、ハードウェアやソフトウェアが整備されているそうです。
現在は、東北大の渡辺先生を中心にメンテナンスされているとのことです。

ソフトウェア

openspurはskid型の移動ロボットを座標指令で動かしたいときに便利そうなライブラリです。
Spurはシュプールと発音するそうです。
OpenSpur.org

Google グループ

YP-Spur [Robot Platform Project Wiki]

ハードウェア

エンコーダ付きACモータや、モータドライバなどのハードウェアを購入できるようになっています。
t-frog.com

ROS連携なども整備されているようですから、
つくばチャレンジ的な本格的な実験や、ちょっとしたSLAMの実験にも便利に使えそうです。
論文や資料も充実していますので、勉強のための教科書的に一度目を通しておくと良さそうです。

ただ願わくば、
もう少し明確にハードウェアとソフトウェアの切り分けをして欲しかった。

Emlid Navio2で動かすardupilotにもzeroMQを導入する

navio2はEmlid社の開発するRaspberry Piベースのオートパイロット用ボードで、オープンソースのドローンシステムardupilotに対応しています。
emlid.com
詳細はこちら。
Navio2 docs

まず、マニュアルに従ってAPMを普通にインストールしてください。
ここでは、planeでもcopterでもなくroverをターゲットとすることを前提にします。

基本的にRCサーボモータを出力対象として設計されていますので、ロボット用コマンドサーボやもっと大型の汎用モータを動かすためにはインターフェースを自分で用意してやる必要があります。
navio2ボード上にI2CやUARTのコネクタがありますので、それを使って出力を取っても良いですし、RaspberryPiのUSBやLANも使えますのでそれらを使って外部のデバイスとつなげても良いです。

ただ、あらゆるデバイスはzeroMQで繋げてしまえばいいじゃん、という思想があるので、このページではardupilotでzeroMQを使う方法を調査します。

準備

1. wafを見てみる

新しいビルドシステムwafで、ライブラリを追加する方法がわからない。
[1]を勉強して、

def build(bld):
bld(...
lib = ['m'],
libpath = ['/usr/lib'],
...)

のように書けばいいのだろうな、などと推測はつくものの、
ardupilotのwscriptが大きくて、あちらこちらに散在するので、何がどういう順番で呼ばれるのかよくわからない。
*1

2. Configure/Makeを見てみる

> cd ardupilot
> make -C APMrover2 navio2

で試してみるとまだ使える。廃止されると書かれているが。こちらなら使い慣れていてどこに何を書けばいいのかもわかるので、当面こちらで乗り切ることにする。

3. zeroMQのインストール

zmqは普通にライブラリをインストールしてください。ソースからが無難だと思います。
c++用のbindingを/usr/local/includeに置いてください。

Makefileの編集

ardupilotのmakefileが存在する場所は、~/ardupilot/mkの下です。
zeroMQを追加するときに編集するべきファイルは、board_native.mkです。

CXXOPTS = -ffunction-sections -fdata-sections -fno-exceptions -fsigned-char

と書かれた一文を探して、zmqを使うのに都合がいいように次のように変更します。

CXXOPTS = -ffunction-sections -fdata-sections -fexceptions -fsigned-char
LIBS += -lzmq -lpthread

no-exceptionsを外してしまったので、予期せずエラー時の挙動が何か変わってしまうかもしれません。注意してください。
とりあえず、これでbuildは通るようになります。

モータの出力をpubで垂れ流す準備

APMrover2/Rover.hにコンテキストとソケットの定義を追加

#include <zmq.hpp> // 追加

class Rover
{
・・・

public:
    zmq::context_t *context;
    zmq::socket_t *publisher;
};

APMrover2/Rover.cppのRoverコンストラクタ内で初期化

Rover::Rover(void) :
・・・
{
    context = new zmq::context_t(1);
    publisher = new zmq::socket_t(*context, ZMQ_PUB);
    int v = 1;
    size_t size = sizeof(v);
    publisher->setsockopt(ZMQ_SNDHWM, &v, size);
    publisher->bind("tcp://*:6636"); // 送信先IPとポート番号 
}

RCサーボモータへの出力をzmqで流す

出力っぽい名前の関数呼び出しを探していくと、サーボモータへの出力はStering.cpp内で行われてることが見つかります。

void Rover::set_servos(void){
・・・
channel_steer->output();
channel_throttle->output();
・・・
}

ここのPWM出力していると思しき値の、元になった値をもらって来ればよさそうです。

channel_steerもchannel_throttleもRC_Channelというクラスのインスタンスのようなので、RC_Channelのクラスの定義を探しますが、APMrover2の中には見当たりません。
探すと、~/ardupilot/libraries/RC_Channelの下に関連ファイルがありそうです。
RC_Channel.hの中を見ていくと、

int_16t get_servo_out() const { return _servo_out; }
void set_servo_out(int_16t val) {_servo_out = val; }

というのがあり、RC_Channel.cppの中でoutput()から辿ってcalc_pwmの中を見ていくと、range_to_pwm()などの中で_servo_out変数が随所で使われていますので、とりあえずコレで良さそうです。
レンジをpwm用に整形された値を得たい場合は、get_pwm_out()関数を使います。

ということで、
Stering.cppのser_servos(void)関数の中に、zeroMQ出力を追加します。

channel_steer->output();
channel_throttle->output();

int16_t l = channel_steer->get_servo_out();
int16_t r = channel_throttle->get_servo_out();

int msg_size = 20;
zmq::message_t msg(msg_size);
memset (msg.data (), 0, msg_size);
snprintf((char*)msg.data(), msg_size,
"/servo_out %d %d", l, r);
publisher->send(msg);

これでひとまず行けそうです。

お片付け

行儀よくデストラクタで、contextとsocketを削除するようにしないといけません。
Rover::~Rover(void);の定義をRover.hの中に追加して、

Rover::~Rover(void)
{
    if(publisher){
         int v = 0;
         size_t size = sizeof(v);
         publisher->setsockopt(ZMQ_LINGER, &v, size);
         // publisher->close();
         delete publisher;
    }

    if(context){
         // context->close();
         delete context;
    }
}

とします。

実行は、別のコントローラ用PCを用意してAPMPlanner2などを立ち上げておいてから、
raspberrypi側で、

> cd ardupilot/APMrover2
> sudo ./APMrover2.elf -A udp:192.168.10.41:14550

のようにします。14550は規定のport番号、192.168.10.41はコントローラ用PCのIPアドレスの例です。

これとは別に、zeromqのsubを受け取るプログラムを書いて受け取ってみると、

/servo_out -15796 -4

といった感じでデータが送られてきていることが分かります。

zeroMQで繋がってさえしまえば、あとはもう自由に料理できますね!

追記

最近、ソースコードをダウンロードした場合、mavgenに必要なpythonライブラリが正しくインストールされなくてエラーが出ます。
解決方法は、こちらのページを参照してください。
ArduPilot 入門 (7):ビルドが失敗する、シミュレーターが起動しない場合のトラブルシューティング – Drone Japan

> sudo pip install future
> sudo pip install lxml


[1] wafチュートリアル
waf チュートリアル - 純粋関数型雑記帳

*1:プログラムをビルドをするためだけに、また大きなプログラムを書かなければいけない本末転倒な仕組みな気がするので、wafは今のところ評価できない。 そのうち、wafを書くためだけのツールが出てきそうな気がする。

pyzmqとpyOSCでOSC over ZeroMQっぽいもの

普段はc++ばかりなのですが、少しだけpythonで書く必要ができました。
極力、普段と同じようにぬるく書きたいので、使い慣れたZeroMQとOSCを使えるようにしたいと思います。

1.準備

zeromqとoscをpipでインストールします。

> pip install pyzmq
> pip install pyosc

2.テストプログラム

[2][3]のexamplesと[1]のpyOSCの実装を参考に、OSCMessageのbinaryをzeroMQを通して送受信するサンプルを書きたいと思います。

実装のポイント

OSCMessageをバイナリ形式で送受信することだけを考えます。
どの環境の、どのOSCの実装でもネットワーク上を飛び交うパケットは同じバイナリのはず、なので。

OSCMessageのバイナリを取得する方法は、普通に書くだけです。
送られてきたバイナリ形式のOSCMessageを、OSCMessageに戻すには(効率悪いですが、手を抜いて)

recv = decodeOSC(recv_binary)

で文字列や数値にデコードした結果を元に、

recv_osc = OSCMessage(recv[0])
recv_osc += recv[2:]

OSCMessageを作り直しているだけです。イベントハンドラ等は自分で好きに書いてください。

ソースコード

req.py

import zmq
from OSC import decodeOSC, OSCMessage

ctx = zmq.Context()
sock = ctx.socket(zmq.REQ)
sock.connect('tcp://127.0.0.1:5555')

# Prepairing an OSCMessage
msg = OSCMessage("/my/osc/address")
msg.append('something')
msg.insert(0, 'something else')
msg[1] = 'entirely'
msg.extend([1,2,3.])
msg += [4,5,6]
del msg[3:6]
msg.pop(-2)
print 'send'
print 'OSCMessage:\t', msg
print 'OSCMessage(binary):\t', msg.getBinary()

# Send the OSCMessage
sock.send(msg.getBinary())

# Receive the echo
recv_binary = sock.recv()
print 'echo'
print 'binary:\t', recv_binary
recv = decodeOSC(recv_binary)
print 'string:\t', recv
recv_osc = OSCMessage(recv[0])
recv_osc += recv[2:]
print 'OSCMessage:\t', recv_osc
print 'OSCMessage(binary):\t', recv_osc.getBinary()

rep.py

import zmq
from zmq.eventloop import ioloop
from OSC import decodeOSC, OSCMessage

loop = ioloop.IOLoop.instance()

ctx = zmq.Context()
sock = ctx.socket(zmq.REP)
sock.bind('tcp://127.0.0.1:5555')

def rep_handler(sock, events):
  # Receive an OSC packet
  osc_binary = sock.recv()
  print 'received'
  print 'binary:\t', osc_binary
  # Decode the binary to OSCMessage
  msg = decodeOSC(osc_binary)
  osc_msg = OSCMessage(msg[0])
  osc_msg += msg[2:]
  print 'OSCMessage:\t', osc_msg
  # Reply the decoded OSCMessage
  sock.send(osc_msg.getBinary())

loop.add_handler(sock, rep_handler, zmq.POLLIN)

loop.start()

実行結果

> E:\workspace\python\req.py
send
OSCMessage: /my/osc/address ['something else', 'entirely', 1, 6]
OSCMessage(binary): /my/osc/address ,ssii something else entirely
echo
binary: /my/osc/address ,ssii something else entirely
string: ['/my/osc/address', ',ssii', 'something else', 'entirely', 1, 6]
OSCMessage: /my/osc/address ['something else', 'entirely', 1, 6]
OSCMessage(binary): /my/osc/address ,ssii something else entirely

> E:\workspace\python\rep.py
received
binary: /my/osc/address ,ssii something else entirely
OSCMessage: /my/osc/address ['something else', 'entirely', 1, 6]

c++の実装と繋いでみる

cvl-robot.hateblo.jp
ここで昔書いたテストプログラムのpub,subと、pythonで書いたpub,subとを繋いでみます。

sub.py

import zmq
from zmq.eventloop import ioloop
from OSC import decodeOSC, OSCMessage

loop = ioloop.IOLoop.instance()

ctx = zmq.Context()
sock = ctx.socket(zmq.SUB)
sock.connect('tcp://127.0.0.1:5555')

sock.setsockopt(zmq.SUBSCRIBE, '')

def sub_handler(sock, events):
  # Receive an OSC packet
  osc_binary = sock.recv()
  print 'received'
  print 'binary:\t', osc_binary
  # Decode the binary to OSCMessage
  msg = decodeOSC(osc_binary)
  osc_msg = OSCMessage(msg[0])
  osc_msg += msg[2:]
  print 'OSCMessage:\t', osc_msg

loop.add_handler(sock, sub_handler, zmq.POLLIN)

loop.start()

pub.py

import time
import zmq
from OSC import decodeOSC, OSCMessage

ctx = zmq.Context()
sock = ctx.socket(zmq.PUB)
sock.bind('tcp://127.0.0.1:9999')

# Prepairing an OSCMessage
msg = OSCMessage("/my/osc/address")
msg.append('something')
msg.insert(0, 'something else')
msg[1] = 'entirely'
msg.extend([1,2,3.])
msg += [4,5,6]
del msg[3:6]
msg.pop(-2)
print 'send'
print 'OSCMessage:\t', msg
print 'OSCMessage(binary):\t', msg.getBinary()

# Send the OSCMessage
time.sleep(1.0)
sock.send(msg.getBinary())

c++の方も,送受信先のフィルタを

publisher->bind("tcp://*:5555");
subscriber->connect("tcp://127.0.0.1:9999");

適当に調整してください。

実行結果

received
binary: /mouse/button ,siii pressed $ [
OSCMessage: /mouse/button ['pressed', 548, 347, 2]
received
binary: /mouse/button ,siii pressed ( _
OSCMessage: /mouse/button ['pressed', 296, 351, 0]
received
binary: /mouse/button ,siii pressed \
OSCMessage: /mouse/button ['pressed', 260, 604, 0]
received
binary: /key/pressed ,i c
OSCMessage: /key/pressed [99]
received
binary: /key/pressed ,i v
OSCMessage: /key/pressed [118]
received
binary: /key/pressed ,i l
OSCMessage: /key/pressed [108]

Received: /my/osc/address something else entirely 1 6
Received: /my/osc/address something else entirely 1 6

問題なく動きますね。

[1] pythonでOSC Messageのデコード
https://github.com/ptone/pyosc/blob/master/OSC.py
[2] pyzmqのreq,repのexample
zmq の python binding で echo テスト - Twisted Mind
[3] pyOSCのexample
pyosc/examples at master · ptone/pyosc · GitHub

[4] pyzmqのpub, subのexample
pyzmqでZeroMQを触ってみる (PUB/SUB) - YAMAGUCHI::weblog
[5] int64を送受信したいとき
pyOSCでint64が受け取れなくてはまったメモ - Qiita

ロボットには出来ないこと

(編集中)
慣用句として、ロボットには出来ない、という言葉が良くつかわれます。

とくに伝統工芸や芸能などに多いような気がします。
本当に出来ないのかどうか検証したいので、ロボットには出来ない、という言葉を聞いた時にここにメモしておこうと思います。

・ジーンズを作れない