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

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

追記(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を有効化します。（すでに有効化している場合は不要です）

$ sudo raspi-config

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

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

以下のコマンドでI2Cを使うためのツール類をインストールします。

$ sudo apt update
$ sudo apt install i2c-tools
$ sudo apt install python3-smbus

インストールが出来たら、一度以下のコマンドでIMUモジュールが接続できているか確認します。

$ 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の状態を確認します。

$ ls -al /dev/i2c-*
crw-rw---- 1 root dialout 89, 1  9月 10 03:48 /dev/i2c-1

上記では root:dialout になってますが場合によっては root:i2c とかになってたりするようです。

なので、念の為以下のコマンドで、i2c dialout 両方をユーザーのグループに追加します。

$ sudo groupmod -aG i2c [ユーザー名]
$ sudo groupmod -aG dialout [ユーザー名]

追加したら、以下のコマンドでi2cとdialoutがグループに入っているか確認します。

$ groups [ユーザー名]

確認ができたら、次はsudoを入れずに以下のコマンドを入力して表示されるか確認します。

$ i2cdetect -y 1

BNO055用のROS2パッケージの取得

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

Gitを使うので、Gitをインストールしていない場合は以下のコマンドでインストールしてからクローンします。
ただ、上記パッケージのマスターはi2c未対応なので、プルリクからi2c対応版をもらってきます。

$ 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

次は設定ファイルを修正します。

$ nano bno055/bno055/bno055_params.yaml

-    i2c_bus: 0
+    i2c_bus: 1

Ctrl+Xで保存終了。

では、ビルドしましょう。

$ 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以下に保存してください。

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

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

動作確認

ラズパイ側でローンチファイルを2つ起動します（それぞれのターミナルで起動）

$ ros2 launch camera.launch.py

$ ros2 launch rosbridge_server rosbridge_websocket_launch.xml

今日はここまで〜

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

目次