ROS 最低限のセットアップ

出先でROSを入れまくっているのでそろそろその度に公式documentをたどるのを辞めたいところ．

ROSのインストール

anacondaなどで自前のpython環境をつくっているひとは，その環境とは別にROSの環境を立てるのをおすすめします．
ユーザ変えるとかしていいかも？

自分の過去記事見てもいいですし，普通に公式Document見てもいいです．
ossyaritoori.hatenablog.com

dockerの上にROSを立てるというのもあったりするので猛者はぜひ挑戦してください．
docker/Tutorials/Docker - ROS Wiki

catkin_wsの作成

公式の手順をかいつまんでいきます．
ROS/Tutorials/InstallingandConfiguringROSEnvironment - ROS Wiki

次にはワークスペースを作成します．

$ mkdir -p ~/catkin_ws/src
$ cd ~/catkin_ws/
$ catkin_make
$ source devel/setup.bash

例のごとくsetup.bashは面倒ならbashrcに書いておきましょう．
（catkin_wsを複数作る予定が有るなら別）

ここまできたら正直準備は完了です．

Tutorialを導入

大して意義は無いですが，大方のROSのパッケージ説明などにおいてTutorialがこなされていることを前提に話が進む場合が多いです．やっぱりドキュメントは読んでおきましょう．

また，チュートリアル用のファイルも一応持っておいて損はないです．

$ sudo apt-get install ros-<distro>-ros-tutorials

ROSを新しくインストールする （含 Anacondaとの競合解決）

ROSのバージョンが古くなったり，ある程度いじった後いろいろとセットアップが面倒になった際に綺麗サッパリ掃除して最初からやり直したいという時にこの手順を踏みました．

アンインストール

以下のガイドに従って，
Uninstall and reinstall ROS - ROS Answers: Open Source Q&A Forum

sudo apt-get purge ros-<distribution>-*

この後，sudo apt autoremove を要求されたので実行して関連パッケージを削除しました．

インストール

ROS のサーバからのデータの受信を許可

sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'

鍵の設定？

sudo apt-key adv --keyserver hkp://ha.pool.sks-keyservers.net:80 --recv-key 421C365BD9FF1F717815A3895523BAEEB01FA116

この後一度，sudo apt-get update をかまします．

kineticのインストール＜2017年9月現在＞
ディストリビューションのおすすめは年度が進むごとに変わります．
また，そろそろROS2が出るのでその頃にはこの記事は時代遅れになっているかもしれません．

sudo apt-get install ros-kinetic-desktop-full

rosdepの設定です（私はここでエラーが発生しました（後述））

sudo rosdep init   
rosdep update

setupをします．

source /opt/ros/kinetic/setup.bash

このコードは毎回必要になるのでbashrcに記述しておくのが良いでしょう．

その後，pythonのパッケージをインストールします．

sudo apt-get install python-rosinstall  

ここまでなんの問題もなければ順調でしょう．後はチュートリアルやってね．

エラーが起きる場合：AnacondaとROSのコンフリクト

さて，AnacondaとROSが共存している私の環境では非常にこれは辛い問題です．
ROSは近々２になるしTensorflowを使わない限りROSのPythonパッケージを使えば十分な気もしますが，果たしてどうしのいだものか．

どうやら似たようなことをStackOverflowにて発見．
Conflict with ROS Python and Conda - ROS Answers: Open Source Q&A Forum

Try1: Remove Conda from PATH

上の参考記事において最も簡単と書いてあったこの手法を試してみるとします．

Path の消し方

stackoverflow.com

まずはanacondaのPathを調べます．

echo $PATH

私の場合は以下にありました．は自分のユーザ名です．多分晒しても平気だと思われます．

/opt/ros/kinetic/bin:/home/<user>/anaconda3/bin:/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin:/usr/games:/usr/local/games:/snap/bin

一応中身を確認すると，condaのコマンドやpython3，ipythonなどがありまして...
Pathを削除したらこれらに絶対Pathを与えてアクセスする必要があります．うーんこれは良い解決策か？

次にこのPathを削除するにはescape sequenceを交えて以下のように書きます．（ちなみにうまく行かなかった）

PATH=`echo $PATH | sed -e 's/:\/home\/<user>\/anaconda3\/bin\/$//'`

確認してみるとん？消えてない．ということでより確実な方法を使います．先ほどのPATHから該当箇所をぬいて

export PATH=/opt/ros/kinetic/bin:/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin:/usr/games:/usr/local/games:/snap/bin

これで完了です．さようならanaconda

.bashrcの中身を無効化

以下の部分をコメントアウト．

# added by Anaconda3 4.2.0 installer
#export PATH="/home/<user>/anaconda3/bin:$PATH"
# added by user 
#export PYTHONPATH="/home/<user>/anaconda3/lib/python3.5/site-packages:$PYTHONPATH"

これを忘れると次回起動時に同じ感じになります．

Try1の続き：ワークスペースを一度綺麗にする

未だにPythonのPathが残っている模様．．．？

Pythonのパッケージの確認

>> which python
/usr/bin/python

一応pythonがシステム標準のものになっているということを確認．

この時点でうまく行くと思われるので以下の解説に従い，一度catkin_ws以下をお掃除．
https://groups.google.com/forum/#!topic/ros-japan-users/DwmvcM9X5jU

rm -fr ~/catkin_ws/devel ~/catkin_ws/build

この後，もう一度catkin_makeを行うことで成功．

余談：anacondaは動くのか？

試しにfullpathを与えてpythonを駆動してみました．

user:~/catkin_ws⟫ /home/<user>/anaconda3/bin/python
Python 3.5.2 |Anaconda 4.2.0 (64-bit)| (default, Jul  2 2016, 17:53:06) 
[GCC 4.4.7 20120313 (Red Hat 4.4.7-1)] on linux
Type "help", "copyright", "credits" or "license" for more information.
>>> import cv2
Traceback (most recent call last):
  File "<stdin>", line 1, in <module>
ImportError: /opt/ros/kinetic/lib/python2.7/dist-packages/cv2.so: undefined symbol: PyCObject_Type
>>> import numpy

ふぅ〜．cvやtensorflowへはアクセスできないみたい？ですね．これはおそらくバージョンや参照先の違いとかのせいでしょう．numpyとかは正しくimportできているように見えます．見えるだけかも．
Python2系なのは不満ですがros2が出るまでの辛抱です．

一応メモとして残しておきました．

ROS and Arduino serial communication setup

Supposed to use ros kinetic on ubuntu 16.04LTS.
And currently it is just a memo.

Download Arduino IDE

Install Software (Linux) | Ladyada's Learn Arduino - Lesson #0 | Adafruit Learning System

Ancient (Un reccomended)

Following step you do not need to do, but I mistaken done that.

Arduino Playground - Ubuntu

sudo apt-get update && sudo apt-get install arduino arduino-core 

Download from HP and Install

Choose your OS. I use Ubuntu 16 on jetson TX1 (arm64) so choose "Linux ARM".
Arduino - Software

after you unzip it,
I got many warning but I could see that.

touch: cannot touch '/home/ubuntu/.local/share/icons/hicolor/.xdg-icon-resource-dummy': No such file or directory

And my desktop icon do not work...

Checking if it works

ls /dev/ttyUSB*

rosserial_arduino/Tutorials/Arduino IDE Setup - ROS Wiki

Sample Code?

Too long to explain. See the official tutorial.< source cord cannot be pasted...>

ROS tf and geometry_msgs mutual translation

I tried convert tf::Pose to geomtery_msgs and memorize it in here.
But I don't think this is best way.
tf looks great.
tf/Tutorials - ROS Wiki

tf::Quaternion to geometry_msgs::Pose

geometry_msgs::Quaternion  msgqt;
tf::quaternionTFToMsg(qt, msgqt);

geometry_msgs::PoseStamped to tf::Transform

const geometry_msgs::PoseStamped::ConstPtr& Pose
tf::Transform pose;
tf::poseMsgToTF(Pose->pose,pose);

tf::Transform to tf::Matrix3x3

tf::Matrix3x3 Rot;
Rot = pose.getBasis();

get each element of tf::Matrix3x3

std::cout <<  Rot.getRow(0).getX() << Rot.getRow(0).getY() << Rot.getRow(0).getZ() << std::endl; 
std::cout <<  Rot.getRow(1).getX() << Rot.getRow(1).getY() << Rot.getRow(1).getZ() << std::endl; 
std::cout <<  Rot.getRow(2).getX() << Rot.getRow(2).getY() << Rot.getRow(2).getZ() << std::endl; 

Ref:

tf: tf::Transform Class Reference
tf2: tf2::Matrix3x3 Class Reference
tf: tf::Vector3 Class Reference

Numpyで*を使わないほうが良い理由

それはnp.arrayとnp.matrixに依って計算内容が異なるからです。
以下にもっと詳しい記事があるので時間のある人はこちらを参照されたし。
ksknw.hatenablog.com

numpyの型について

numpyを使うことでmatlabとほぼ同様に計算をすることができます。
主にarray型とmatrix型がありますが、どちらもnp.asmatrixやnp.asarrayで相互変換可能です。
stackoverflow.com

MATLAB使いの方はmatrix型で使ったほうが混乱が少なくていいと思います。

なぜならarrayの型では少なくとも行列の積において掛け算に混乱が生じるからです。

やってみる

import numpy as np
A = np.array([1,2,3,4]).reshape(2,2)
B = np.matrix([1,2,3,4]).reshape(2,2)

A2 = A*A
B2 = B*B

結果はこちら

   A2
=> array([[ 1,  4],
       [ 9, 16]])
   B2
=> matrix([[ 7, 10],
        [15, 22]])

「＊」オペレータは
「array」だと「要素ごとの積」
「matrix」だと「行列の積」
を表すことがわかります。

numpyには
「np.multiply(X,Y)」（要素ごとの積）
と
「np.dot(X,Y)」（通常の掛け算）
という関数があるので誤解を避けたければこういった関数を使ったほうがいいのではないかと思います。

特に何らかの返り値としてnumpyのオブジェクトを受け取ったときはしばしばarray型になっていることもあるので、デバッグの際にも非常に見つかりにくいです。

全部matrixとしてしまう解決策

それでも「＊」のかわりに「np.dot()」と書くのは少し手間なので、
以下のように変換をしてしまうのも良いかと思います。
matrixからmatrixの変換でも特に怒られないのでとりあえず変換をはさめば安心ではないかと。

   np.asmatrix(A2)
=> matrix([[ 1,  4],
        [ 9, 16]])
   np.asmatrix(B2)
=> matrix([[ 7, 10],
        [15, 22]])

落ちたことのある罠集

外積が計算できない

np.crossは横長のベクトルをよこせとほざくので横長で渡す必要があります。
計算式を考えると見通し悪いですがそこは我慢するしかないのか…

行列とベクトルの積

np.arrayだとベクトルの転置とか気にせずうまいことやってくれるっぽいんですがとても不安。
サイズのミスマッチによる行列演算のミスに気づくなんていうのはmatlabあるあるですがこちらでは通用しません。
くわばらくわばら。

Does Linear or Spherical quaternion interpolation make large difference? / Python

As a result, it seems NOT so much.
Using my own library for right handed quaternion caluclation.
ossyaritoori.hatenablog.com

Online iPython console

I tried online iPython console. It is pretty good.
Try IPython from your Browser: PythonAnywhere

Comparison

Try interpolating quaternions below:

q = [0.7071067811865475, 0, 0, 0.7071067811865475]
qq = [-0.006504044267823542,
 0.47180695486352564,
 -0.024006722434494504,
 0.8813509925271104] 

Figure below represents the idea of two interpolation method.

Linear Interpolation (LERP)

Linear Interpolation is quite simple.
Just do interpolation and then normalize.

Half point:

In [32]: q_LERP(q,qq,0.5)
Out[32]: (0.5820332949500574, 0.12762550407838416, 0.0, 0.8030871523554078)

Quater point:

In [33]: q_LERP(q,qq,0.25)
Out[33]: (0.6485637875878557, 0.06347682311265765, 0.0, 0.7585088703220412)

Spherical Interpolation (SLERP)

Spherical Interpolation is little bit complicated but graphically it is easy to understand.
Slerp - Wikipedia

Half point:

In [34]: q_SLERP(q,qq,0.5)
Out[34]: (0.5820332949500575, 0.12762550407838413, 0.0, 0.8030871523554077)

Quater point:

In [35]: q_SLERP(q,qq,0.25)
Out[35]: (0.6479108467340242, 0.06414349374669522, 0.0, 0.759010636878277)

Difference

So, it seems that if interpolating point is far from center, the result become worse.

Half point:

q_LERP(q,qq,0.5) =  [0.5820332949500574, 0.12762550407838416, 0.0, 0.8030871523554078]
q_SLERP(q,qq,0.5) =  [0.5820332949500575, 0.12762550407838413, 0.0, 0.8030871523554077]

Quater point:

q_LERP(q,qq,0.25) =  [0.6485637875878557, 0.06347682311265765, 0.0, 0.7585088703220412]
q_SLERP(q,qq,0.25) =  [0.6479108467340242, 0.06414349374669522, 0.0, 0.759010636878277]

1/10 point:

q_LERP(q,qq,0.1) = [0.6847348182509574, 0.02518528264165945, 0.0, 0.728357007389294]
q_SLERP(q,qq,0.1)= [0.6842708912820434, 0.025694514075164966, 0.0, 0.7287750951360228]

The below graph will be helpful to understand these difference.

Python Quaternion Calculation Function

I will translate this article to Japanese sooner or later.

This is just for my study and I recommend you to use other well-done python libraries.

Using tf library for handling Quaternion in python

There are already "tf" library to do some kind of these job.
tf (Python) — tf 0.1.0 documentation

Quaternion to Euler angle

For example, you can get euler angle from quaternion q = [x,y,z,w]

import tf

euler = tf.transformations.euler_from_quaternion(q)
roll = euler[0]
pitch = euler[1]
yaw = euler[2]

Euler angle to DCM( Direct Cosine Matrix )

transformations — tf 0.1.0 documentation

Also, you can get 3D rotational matrix from euler angle.

Re = tf.transformations.euler_matrix(roll, pitch, yaw, 'rxyz')

Now ,you can combine upper transformation to get DCM from quaternion!

Test

Let's check this DCM with a simple example.
90 degree rotation around x axis is like below:

q = [1.0/math.sqrt(2) ,0 ,0 , 1.0/math.sqrt]

Then we can get Rotation matrix:

e = tf.transformations.euler_from_quaternion(q)
Re = tf.transformations.euler_matrix(e[0], e[1], e[2], 'rxyz')

Results in,

matrix([[ 1.,  0.,  0.],
        [ 0.,  0., -1.],
        [ 0.,  1.,  0.]])

It convert z axis to -y and y axis to z axis.

My Python Functions

I just need more compact one and write myself for my study.
I use right handed coordinate and scalar part of quaternion is in the 4th element of quaternion.
Like: q = (qx, qy, qz, qw).

I used math class to calculate some sin and cos.

import math

Basic quaternion calculations here: (this is not so compact...)

#### ----- Quaternion Calculation ----- ####

def q_mult(q1, q2):
    x1, y1, z1 ,w1 = q1
    x2, y2, z2 ,w2 = q2
    w = w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2
    x = w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2
    y = w1 * y2 + y1 * w2 + z1 * x2 - x1 * z2
    z = w1 * z2 + z1 * w2 + x1 * y2 - y1 * x2
    return x, y, z ,w

def q_inv(q):
    x, y, z ,w = q
    n = x*x + y*y +z*z + w*w
    return [-x/n, -y/n, -z/n, w/n]

# inner product
def q_iprod(q1,q2):
    q = [x*y for (x,y) in zip(q1,q2)] 
    return sum(q)
    
def q_conjugate(q):
    x, y, z ,w = q
    return [-x, -y, -z, w]

def q_normalize(q):
    x, y, z ,w = q
    n = math.sqrt(x*x + y*y +z*z + w*w)
    return [x/n, y/n, z/n, w/n]

# Linear interpolate with rate: [t,(1-t)] (with normalization)     
def q_LERP(q1,q2,t):
    q = [x*(1.0-t)+y*t for (x,y) in zip(q1,q2)]    
    return q_normalize(q)
    
# Sphere interpolate with rate: [t,(1-t)]   
def q_SLERP(q1,q2,t):
    w = math.acos(q_iprod(q1,q2))
    a = math.sin((1.0-t)*w)/ math.sin(w)
    b = math.sin(t*w)/ math.sin(w)
    q = [x*a+y*b for (x,y) in zip(q1,q2)]    
    return q

# Rotate vector using quaternion        
def qv_mult(q1, v1):
    q2 =  v1 + [0.0]
    return q_mult(q_mult(q1, q2), q_conjugate(q1))[:3]

#### ---------------------------------- ####

Remember the scalar part is the 4th element in this definition.
You can just put returning scalar:w to be a first to convert the other formulation.

Quaternion to DCM

DCM can be directly get from quaternion.
Here shows the function:

def q2Rmat(q):
    x,y,z,w = q
    Rmat = np.matrix([ [x*x-y*y-z*z+w*w, 2.0*(x*y-w*z), 2.0*(x*z+w*y) ],\
              [2.0*(x*y+w*z), y*y+w*w-x*x-z*z, 2.0*(y*z-w*x)],\
              [2.0*(x*z-w*y),  2.0*(y*z+w*x), z*z+w*w-x*x-y*y]] )
    return Rmat

Remember the rotational matrix is different depending on vector or coordinate you want to rotate.
This equation is tend to rotate vector.

Test

Let's check this DCM with a simple example.
90 degree rotation around x axis is like below:

q = [1.0/math.sqrt(2) ,0 ,0 , 1.0/math.sqrt]

Then we can get Rotation matrix:

q2Rmat(q)

Results in,

matrix([[ 1.,  0.,  0.],
        [ 0.,  0., -1.],
        [ 0.,  1.,  0.]])

It convert z axis to -y and y axis to z axis.

Comparison Tests

Consider you have orientation of some robot in a quaternion form:

q = [math.sqrt(3)/2 * 0.5 , 0.5*0.5,0,math.sqrt(3)/2] 

then convert to rpy

rpy = [-0.061428016242791844, 0.9815194409145511, -0.08729595308817747]

Using tf.transformation function

array([[ 0.875     ,  0.21650635,  0.4330127 ,  0.        ],
       [ 0.21650635,  0.625     , -0.75      ,  0.        ],
       [-0.4330127 ,  0.75      ,  0.5       ,  0.        ],
       [ 0.        ,  0.        ,  0.        ,  1.        ]])

This output 4 times 4 homogeneous matrix.

Also try my sample,

matrix([[ 0.875     ,  0.21650635,  0.4330127 ],
        [ 0.21650635,  0.625     , -0.75      ],
        [-0.4330127 ,  0.75      ,  0.5       ]])

(for vector rotation)

It is almost the same result, but I think my function is more precise because it directly convert the quaternion.

coding tech memo: zip in python

I thought this kind of description is cool:

q = [x*(1.0-t)+y*t for (x,y) in zip(q1,q2)]