倒立振子ロボットを作る（５）-ソフトの全体構造とマイコンとのI/Fノード

ソフトの全体構造のイメージ

若干順番が前後したような気もしましたが、倒立振子ロボットを作る（２）-姿勢制御とそのためのパーツ - 技術開発日記＠KTDの記事で、Arduino互換のマイコンであるLambda Plusを使って、姿勢角を取得できるようになりました。
このままLambda Plusに遠隔操縦指令を受け付ける機能とか障害物を回避する機能とかを付け加えていくことも可能だとは思うのですが、最初に述べた通り、「ROSを使ってみたい」という副目標のようなものがありますし、上位計算機としてUbuntuが動いているような大きなマシンがあるのは、重い処理を受け持たせられるというメリットもあります。
せっかく持っている近藤科学のサーボモータ＋ROS対応ドライバを使ってみたいというのもありますが。

そんなわけで、
Lambda Plus：

• 姿勢角検出
• 上位PCへ転送
• その他、LEDや簡単なセンサのインターフェースとしての機能

上位PC

• 姿勢角に基づくモータ制御（サーボのROSパッケージ）
• ジョイスティックやその他環境認識結果などからの速度指令生成、モータ制御への添加
• カメラなどの高度なセンサとのインターフェース

みたいな感じで役割分担をさせることにしました。

Lambda PlusとROSのインターフェース

いきなり悲しい話ですが、本来であればArduino自体がROSのノードとして動くはずなのですが、残念ながら僕には上手くできませんでした。
メモリ（？）がいっぱいですみたいなことを言われて怒られてしまい、まともに書き込めない。

Arduino IDEの設定ファイル？みたいのを書き換えてメモリ空き容量を増やすことはできたけど、安定稼働には至らず。
ROSノードとして動くPythonスクリプトに、普通にシリアル通信で送る形を取ることにしました。

インターフェースノードに求められる機能

• Lambda Plusにデータ送信をリクエストする

高度な同期処理や、通信バッファに溜まったデータを処理する自信がないため、１回PCからリクエスト→１回Lambdaから返信の繰り返しにする

• Lambda PlusからIMUデータを受け取り、適宜変換してROSメッセージとしてパブリッシュ

とこれだけです。簡単。
ホイールオドメトリのためのエンコーダデータはサーボ制御ノードから取りますので、このノードが処理する情報はIMU関係のみ。
IMU関係データは今の所モータへの指令値を決める制御ノードだけの予定ですが、折角なのでsensor_msg/Imu型のメッセージでパブリッシュすることにしました。

ROSのImu型メッセージはクオータニオンで姿勢角を表現しますが、デバッグの時とか、手軽に車体の傾き角を見られるよう、オリジナルのPosture_angleというメッセージを定義して、オイラー角をパブリッシュしています。
その他、なぜこの書き方にしたのか覚えていない部分も結構あるのですが、一応コードも上げてみようと思います・・・

実際のコード

#!/usr/bin/env python
# -*- coding: utf-8 -*-
# code for python2.7

#
# this node is a interface node to arduino
# it collects IMU data from arduino.
#

import rospy
import serial
import time
import signal
import sys
import tf
import math
from sensor_msgs.msg import Imu

# message for publishing auler angle, so that I can recognize robot's posture intuitively
from whipbot.msg import Posture_angle

# start serial communication
# using symbolic link, Lmabda will recognized as "/dev/Lambda" whereever/whenever port you connect
ser = serial.Serial('/dev/Lambda', 115200)

# prepare command to request data send to Lambda Plus
# I forgot why I write as bellow?
send_command = []
send_command += [chr(1)]

# prepare data list sent from Lambda Plus
data = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]


def get_data_from_Lambda():  # function to receive IMU data from Lambda. Executed at 100 Hz by ROS node
    # reset input buffer
    ser.reset_input_buffer()

    # send data request to Lambda
    ser.write(send_command)

    # wait until the reply has reached to this node (36 must be inappropriate...)
    while ser.inWaiting() < 36:
        pass

    # i:0 to 8, roll, pitch, heading, accelX,Y,Z, gyroX,Y,Z
    for i in range(9):
        data[i] = ser.readline()  # read data line by line
        # remove return code and end point null
        data[i] = data[i].replace('\r\n', '')
        data[i] = float(data[i])  # convert into float type parameter

    # contain into ROS message
    posture.roll = data[0]
    posture.pitch = data[1]
    posture.heading = data[2]

    # transform posture angle[deg] to [radian]
    for i in range(3):
        data[i] = data[i] * math.pi / 180.0

    # transform posture from euler to quaternion
    posture_angle_quaternion = tf.transformations.quaternion_from_euler(
        data[0], data[1], data[2])

    # contain to the message : /imu
    imu_data.orientation.x = posture_angle_quaternion[0]
    imu_data.orientation.y = posture_angle_quaternion[1]
    imu_data.orientation.z = posture_angle_quaternion[2]
    imu_data.orientation.w = posture_angle_quaternion[3]

    # transform to [m/s^2] from [g] and contain to the message : /imu
    imu_data.linear_acceleration.x = data[3] * 9.81
    imu_data.linear_acceleration.y = data[4] * 9.81
    imu_data.linear_acceleration.z = data[5] * 9.81

    # transform to [rad/sec] from [deg/sec] and contain to the message : /imu
    imu_data.angular_velocity.x = data[6] * math.pi / 180
    imu_data.angular_velocity.y = data[7] * math.pi / 180
    imu_data.angular_velocity.z = data[8] * math.pi / 180


if __name__ == '__main__':
    rospy.init_node('get_sernsor')  # initialize node

    posture_angle_pub = rospy.Publisher(
        'posture_angle', Posture_angle, queue_size=1)  # define auler angle publisher

    # define /Imu publisher
    imu_pub = rospy.Publisher('/imu', Imu, queue_size=1)

    # declare publisher
    posture = Posture_angle()
    imu_data = Imu()
    imu_data.header.frame_id = 'map'
    # you should wait for a while until your Lambda Plus is ready
    time.sleep(5)

    # set the loop rate at 100Hz
    rate = rospy.Rate(100)

    while not rospy.is_shutdown():
        try:
            get_data_from_Lambda()  # function to get data from Lambda
            # publish posture angle at auler angle
            posture_angle_pub.publish(posture)
            imu_pub.publish(imu_data)  # publish as sensor_msgs/Imu

        except IOError:
            pass

        rate.sleep()

次回は、今回実装したノードがパブリッシュしたsensor_msgs/Imuデータをサブスクライブして、その情報に基づいてモータを制御するノードを実装していきます。
ktd-prototype.hatenablog.com