WebでIMU取得::Raspberry Pi4 + Ubuntu22.04 + ROS2 Humble でロボット作り（その７）

# ロボットの位置情報を取得する

今回は、「BNO055使用　9軸センサーフュージョンモジュールキット」（秋月電子で購入）を使って、ロボットの姿勢情報を取得＆表示してみます。

https://akizukidenshi.com/catalog/g/gK-16996/

## 追記(2023/02/11)
Javascript　index.jsの画像処理を修正しました。（async function setBlobの部分）
これで、ブラウザが固まることはなくなる（はず）

## 接続
ラズパイのGPIOは3.3vなので、3.3vでBNO055とI2Cで接続します。
ラズパイのGPIOとの接続は以下のとおりです。

| AE-BNO055-BO | Raspberry Pi |
|:-|:-|
|1. VIN   |1. 3V3 power   |
|2. GND   |6. GND  |
|3. SDA/T |3. GPIO2(SDA)   |
|4. SCL/R |5. GPIO3(SCL)   |
|5. RESET | NC   |
|6. INT   | NC  |
|7. GND   | NC  |
|8. VOUT  | NC  |

## I2Cの有効化

接続したら、I2Cを有効化します。（すでに有効化している場合は不要です）

```bash
$ sudo raspi-config
```

[3 Interface Options] を選択して [I5 I2C] を Enableにして再起動します。

## i2c-tools等のインストールと権限の付与

以下のコマンドでI2Cを使うためのツール類をインストールします。
```bash
$ sudo apt update
$ sudo apt install i2c-tools
$ sudo apt install python3-smbus
```

インストールが出来たら、一度以下のコマンドでIMUモジュールが接続できているか確認します。
```bash
$ sudo i2cdetect -y 1
    0  1  2  3  4  5  6  7  8  9  a  b  c  d  e  f
00:                         -- -- -- -- -- -- -- -- 
10: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- 
20: -- -- -- -- -- -- -- -- 28 -- -- -- -- -- -- -- 
30: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- 
40: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- 
50: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- 
60: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- 
70: -- -- -- -- -- -- -- --           
```

**28**（または29）の数字が表示されていれば接続はOKです。

ROS2では sudo は使わないので、スーパーユーザーにならなくてもi2cが使えるようにします。
まずは、i2cの状態を確認します。
```bash
$ ls -al /dev/i2c-*
crw-rw---- 1 root dialout 89, 1  9月 10 03:48 /dev/i2c-1
```
上記では root:dialout になってますが場合によっては root:i2c とかになってたりするようです。

なので、念の為以下のコマンドで、i2c dialout 両方をユーザーのグループに追加します。
```bash
$ sudo groupmod -aG i2c [ユーザー名]
$ sudo groupmod -aG dialout [ユーザー名]
```
追加したら、以下のコマンドで**i2c**と**dialout**がグループに入っているか確認します。
```bash
$ groups [ユーザー名]
```
確認ができたら、次はsudoを入れずに以下のコマンドを入力して表示されるか確認します。
```bash
$ i2cdetect -y 1
```
## BNO055用のROS2パッケージの取得

すばらしい先達の方たちに感謝しつつ、BNO055用のROSパッケージを以下のサイトからダウンロードします。

Gitを使うので、Gitをインストールしていない場合は以下のコマンドでインストールしてからクローンします。
ただ、上記パッケージのマスターはi2c未対応なので、プルリクからi2c対応版をもらってきます。
```bash
$ sudo apt install git
$ cd ~/[ワークスペース]/src
$ git clone https://github.com/flynneva/bno055
$ git fetch origin pull/52/head:add_i2c_implementation
$ git branch -a
  add_i2c_implementation
* main
  remotes/origin/HEAD -> origin/main
  remotes/origin/gh-pages
  remotes/origin/main
$ git checkout add_i2c_implementation
```
次は設定ファイルを修正します。
```bash
$ nano bno055/bno055/bno055_params.yaml
```
```diff
-    i2c_bus: 0
+    i2c_bus: 1
```
Ctrl+Xで保存終了。

では、ビルドしましょう。
```bash
$ cd ~/[ワークスペース]
$ colcon build --packages-select bno055
```

ビルドが終わったら、前回作ったカメラ用ローンチファイルに追加します。
```python camera.launch.py
from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    v4l2_node = Node(
        package='v4l2_camera',
        executable='v4l2_camera_node',
        output='screen',
        parameters=[{'image_size': [640, 480]}]
    )

    image_transport = Node(
        package='image_transport',
        executable='republish',
        arguments=["raw"],
        remappings=[('in', '/image_raw'),
                    ('out', '/image_raw/compressed')
                   ]
    )

    web_image = Node(
        package='web_image',
        executable='web_image_node'
    )

    bno055 = Node(
        package='bno055',
        executable='bno055'
    )

    return LaunchDescription([
        v4l2_node,
        image_transport,
        web_image,
        bno055
    ])
```
## Web側の準備

次はWeb側の受信処理を作っていきます。
Webのディレクトリ（ユーザーディレクトリ/www/html）に移動して以下のファイルを修正していきます。

```html index.html
<!DOCTYPE html>
<html lang="ja">
<head>
    <meta charset="UTF-8">
    <meta name="viewport" content="width=device-width">
    <title>WebCameraWithIMU</title>
    <link rel="stylesheet" href="css/index.css">
    <script src="https://cdnjs.cloudflare.com/ajax/libs/jquery/3.6.1/jquery.min.js"></script>
    <script src="http://static.robotwebtools.org/EventEmitter2/current/eventemitter2.min.js"></script>
    <script src="http://static.robotwebtools.org/roslibjs/current/roslib.min.js"></script>
    <script src="js/index.js"></script>
</head>
<body>
    <div id="main-contents">
        <img id="camera_view" src="">
        <div id="camera_frame">
            <img id="camera_roll" src="img/roll_white.svg">
            <img id="camera_yaw" src="img/yaw_white.svg">
        </div>
        <div id="camera_pitch">
            <img src="img/pitch_white.svg">
        </div>
    </div>
    <div id="top-left">
        <button id="start">start</button>
        <button id="stop">stop</button>
    </div>
    <div id="bottom-left">
        <div id="imu"></div>
    </div>
</body>
</html>
```

```css index.css
body{
    background: gray;
    color: white;
}

#main-contents {
    margin: 0;
    padding: 0;
    position: relative;
    top: 0;
    left: 0;
    text-align: center;
}

#camera_view {
    width: auto;
    height: 100vh;
}

#camera_frame {
    position: absolute;    
    left: 0;
    right: 0;
    top: 0;
    bottom: 0;
    width: auto;
    margin: auto;
    height: 80vh;
}

#camera_roll {
    position:absolute;
    left: 0;
    right: 0;
    top: 0;
    bottom: 0;
    width: auto;
    margin: auto;
    height: 100%;
}

#camera_pitch {
    position:absolute;
    left: 0;
    right: 0;
    top: 0;
    bottom: 0;
    height: 50%;
    margin: auto;
    overflow: hidden;
}
#camera_pitch img {
    position: absolute;
    left: 0;
    right: 0;
    width: auto;
    margin: auto;
    height: 80vh;
}

#camera_yaw {
    position:absolute;
    right: 0;
    top: 0;
}

#top-left {
    position: absolute;
    top: 0;
    left: 0;
}

#bottom-left {
    position: absolute;
    bottom: 0;
    left: 0;
}
```

```javascript index.js
$(function() {
    let ros = new ROSLIB.Ros({
        url : 'ws://[ホスト名].local:9090'
    });

    ros.on('connection', function() {
        console.log('Connected to websocket server.');
    });
    ros.on('error', function(error) {
        console.log('Error connecting to websocket server: ', error);
    });
    ros.on('close', function() {
         console.log('Connection to websocket server closed.');
    });

    // IMU
    let imu_listener = new ROSLIB.Topic({
        ros : ros,
        name : '/bno055/imu',
        messageType : 'sensor_msgs/Imu'
    });

    let prevRoll = 0;
    let prevPitch = 0;
    let prevYaw = 0;

    let limitRoll = 30;
    let limitPitch = 30;

    let initFlag = true;
    function ros_imu_topic_subscribe() {
        imu_listener.subscribe(function(message) {
            let o = message.orientation;

            let x = o.x;
            let y = o.y;
            let z = o.z;
            let w = o.w;

            let result = convertQt2Eu(x,y,z,w);

            roll = result.roll;
            pitch = result.pitch;
            yaw = result.yaw;

            if (initFlag || checkAllowLimitImu(roll, pitch, yaw)) {
                $('#imu').html('roll: ' + parseInt(roll) + '<br>' + 'pitch: '+ parseInt(pitch) + '<br>' + 'yaw: ' + parseInt(yaw));
                prevRoll = roll;
                prevPitch = pitch;
                prevYaw = yaw;
                rollRender(roll);
                pitchRender(pitch);
                yawRender(yaw);
                initFlag = false;
            }

        });
    }

    function checkAllowLimitImu(roll, pitch, yaw)
    {
        // ノイズデータが出るときは、roll, pitch, yaw 全体に出る感じなので、rollとpitchdだけチェックする
        // yawは 角度が変わる際、-179から179に飛ぶので、チェックがめんどくさいという理由

        let diffRoll = Math.abs(prevRoll - roll);
        let diffPitch = Math.abs(prevPitch - pitch);
        if (diffRoll > limitRoll || diffPitch > limitPitch) {
            return false;
        }else{
            return true;
        }
    }

    function convertQt2Eu(x,y,z,w)
    {
        let ysqr = y * y;

        // roll (x-axis rotation)
        t0 = +2.0 * (w * x + y * z);
        t1 = +1.0 - 2.0 * (x * x + ysqr);
        roll = Math.atan2(t0, t1);
        
        // pitch (y-axis rotation)
        t2 = +2.0 * (w * y - z * x);
        t2 = t2 > 1.0 ? 1.0 : t2;
        t2 = t2 < -1.0 ? -1.0 : t2;
        pitch = Math.asin(t2);
        
        // yaw (z-axis rotation)
        t3 = +2.0 * (w * z + x * y);
        t4 = +1.0 - 2.0 * (ysqr + z * z);  
        yaw = Math.atan2(t3, t4);

        roll *= 57.2957795131;
        pitch *= 57.2957795131;
        yaw *= 57.2957795131;

        let result = {};
        result.roll = roll;
        result.pitch = pitch;
        result.yaw = yaw;
        return result;
    }

    function rollRender(roll)
    {
        let intRoll = -(parseInt(roll));

        $('#camera_roll').css({transform: 'rotate(' + intRoll + 'deg)'});
    }

    function pitchRender(pitch)
    {
        let intPitch = parseInt(pitch) * 10;
        let cameraGuageHeight = $('#camera_pitch').height();

        let movePitch = -(cameraGuageHeight / 4) - intPitch;

        $('#camera_pitch').find('img').css({top: movePitch + 'px'});

    }
    function yawRender(yaw)
    {
        let intYaw = parseInt(yaw);

        $('#camera_yaw').css({transform: 'rotate(' + intYaw + 'deg)'});
    }

    $(window).resize(function() {
        cameraFrameResize();
    });

    function cameraFrameResize()
    {
        let cameraWidth = $('#camera_roll').width();
        $('#camera_frame').width(cameraWidth);
        let scale = cameraWidth / 800;
        let yawImageWidth = 130 * scale;
        $('#camera_yaw').width(yawImageWidth);
    }
    $('#camera_roll').on('load', function(){
        cameraFrameResize();
    });

    // Camera
    let camera_listener = new ROSLIB.Topic({
        ros : ros,
        name : '/web_image',
        messageType : 'std_msgs/msg/String'
    });

    function ros_camera_topic_subscribe() {
        camera_listener.subscribe(function(message) {
            const imagedata = "data:image/jpeg;base64," + message.data;
            setBlob(imagedata);
        });
    }

    let prevSrc = '';

    async function setBlob(data_uri){
        let img = document.getElementById('camera_view');
        const blob = await (await fetch(data_uri)).blob();
        let imageURL = window.URL.createObjectURL(blob);
        img.onload = ()=>{
            URL.revokeObjectURL(imageURL);
        };
        img.onerror = e=>{
            URL.revokeObjectURL(imageURL);
        };
        img.src = imageURL;
    }

    $('#start').on('click', function() {
        ros_camera_topic_subscribe();
        ros_imu_topic_subscribe();
    });

    $('#stop').on('click', function() {
        camera_listener.unsubscribe();
        imu_listener.unsubscribe();
    });
});
```
次に以下のサイトから３つの画像を保存（右クリックして画像保存）して、~/www/html/img以下に保存してください。

:::note info
画像は
roll_white.svg
pitch_white.svg
yaw_white.svg
の３つです
:::

http://c2z.biz/ros2_web/no7/index.html

これで準備完了です。
ローンチファイルを起動して動作確認してみましょう。

## 動作確認

ラズパイ側でローンチファイルを2つ起動します（それぞれのターミナルで起動）
```bash
$ ros2 launch camera.launch.py
```
```bash
$ ros2 launch rosbridge_server rosbridge_websocket_launch.xml
```

https://twitter.com/shoichi4411/status/1603754096460124160

今日はここまで〜

次はバッテリー監視あたりを書く予定です。

## 目次

* [[←前回]Webでカメラ表示::Raspberry Pi4 + Ubuntu22.04 + ROS2 Humble でロボット作り（その６）](https://qiita.com/shoichi4411/items/e0aa33d9ce286485c5a8)
* [[次回→]バッテリー監視::Raspberry Pi4 + Ubuntu22.04 + ROS2 Humble でロボット作り（その８）](https://qiita.com/shoichi4411/items/a38261c921746c5d71d5)