はじめに

ロボットを自律走行させるため、前回の記事の続きです。
IMU（9軸IMUモジュールHWT901B-TTL）を拾ったので、ROS1で使ってみました。

参考

porizou1さんの記事を参考にさせていただきました。ありがとうございました。
この記事はWT901のUSB版でしたが、シリアル接続後はこの記事を参考にしました。

環境

RasberryPi 4B(Ubuntu 20.04.4 LTS)

項目	バージョン
Ubuntu	20.04
ROS	Noetic

IMU： HWT901B-TTL

公式ドキュメント：Witmotion WT901 Datasheet.pdf (elettra.eu)

amazonで1万円以下で購入できるみたいですね。

やったこと

シリアル接続

手元にあるWT901はTTL版のため、シリアル接続させます。

本来はUSB変換が同梱されているみたいですが、無かったので、ラズパイのGPIOポートを使って接続します。

ラズパイのGPIOでシリアル通信するための設定は、以下の記事に別で記載しました。

結線図を見ていきます。4本線が出ており、図のようにVCC、Rx, Tx, GNDとなっているようです。

HWT901B-TTL本体の裏にも記載されてました。

ラズパイ側のGPIO14(TXD)とGPIO15(RXD)と接続していきます。

ラズパイ
GPIO14(TXD)	Green(RX)
GPIO15(RXD)	Yellow(TX)
GND	Black(RX)

5Vはラズパイから供給できない(動いたけど不安定だった)ので、別の基板(OpenCR)から拝借しました。

受信信号の確認

受信してみます。何かしら受信してるようですが、ボーレートが合ってないのか、文字化けしています。

$ sudo cat /dev/ttyAMA0
z pb     s ޚ*b )(  
*j )( ߆ ;8   T b  ߆ ;8븒 b )( ߊu _    C b  ߊu _    C b  
 ";8 äz b  
 ";8  æz b  
a  ;8     C`b  
a  ;8 b: b  
 E`   Z bR)( 
tݬ b (8 ߊ" ( ݬ bv(8 ߊ" ( ݬ b (8     ݬ b8 j        Ub8 j   
     Ub8 j   
      b9(8 Њ       b[(8 Њ       b8 j   
     b+(8 
     j[(8        j[          b8 j     j    b8 j     j    j8 j    "j    j8 j    "j    j8 j         j8 j         j    u      #   u     #   
   U  #   
    U   #   
 " (    j

ラズパイ側のボーレートの設定は38400となっていました。別の速度に設定していたはずですが、接続するたびに自動で変更されるのでしょうか。よくわからないので後で調べます。

$ sudo stty -F /dev/ttyAMA0
speed 38400 baud; line = 0;
-brkint ixoff iutf8
echoprt

ROS動作の準備

ここから、参考記事9軸IMUモジュールWT901CをROS Noeticで使用するのように進めていきます。

ROSパッケージのインストール

ros-ecl関係のインストール

$ sudo apt install ros-noetic-ecl*

wit_nodeのインストールとBuild

$ cd ~/catkin_ws/src
$ git clone https://github.com/yowlings/wit_node.git
$ cd ~/catkin_ws
$ catkin build

ボーレート、ポート設定の変更

ROSノード側の設定は wit_node/include/wit_node/parameter.hppを変更します。
私の環境では、接続先のシリアルポートは"/dev/ttyAMA0"でした。

parameter.hpp

#include <string>
#ifndef PARAMETER_HPP
#define PARAMETER_HPP
using namespace std;
namespace wit {
class Parameter {
 public:
  Parameter() : port_("/dev/ttyAMA0"), baut_rate_(38400), ns("wit") {}
  string port_;
  int baut_rate_;
  string ns;
};
}
#endif  // PARAMETER_HPP

また、launchファイルwit_node/launch/wit.launchを変更します。

wit.launch

<launch>
  <arg name="port"                  default = "/dev/ttyAMA0" />
  <node pkg="nodelet" type="nodelet" output="screen" name="wit_nodelet_manager" args="manager"/>
  <node pkg="nodelet" type="nodelet" output="screen" name="wit" args="load wit_node/WitNodelet wit_nodelet_manager" >
    <param name="port" value="$(arg port)" />
  </node>
</launch>

$ cd ~/catkin_ws
$ catkin build

Pose Topicを出力するノードの作成（必要なら）

porizou1さんの記事をまるまる参考に、/imu_poseトピックを出力するノードを作成しました。

rvizのIMUプラグインは回転が分かりづらいので、imuトピックの回転をPoseStamped型に変換してPulishするノードを作成する。

imu_to_pose.py

#!/usr/bin/env python3

import rospy
from geometry_msgs.msg import PoseStamped
from sensor_msgs.msg import Imu

def imu_callback(imu_msg):
    ps_msg = PoseStamped()
    ps_msg.header=imu_msg.header
    ps_msg.pose.orientation=imu_msg.orientation
    posestamped_pub.publish(ps_msg)

if __name__ == "__main__":
    rospy.init_node("imu_to_pose")
    #Publisher
    posestamped_pub = rospy.Publisher("imu_pose", PoseStamped, queue_size=10)
    #Subscriber
    rospy.Subscriber("imu", Imu, imu_callback)
    rospy.spin()

実行

権限を付与しておきます。

$ sudo chmod 777 /dev/ttyAMA0

実行します。

$ roslaunch wit_node wit.launch
~~~
[ INFO] [1703815988.845498160]: Wit : initialised.

Wit : initialised.まで表示されたら、無事実行できているようです。
別のターミナルトピックを確認します。

$ rostopic list
/gps
/imu
/rosout
/rosout_agg
/wit/raw_data
/wit/related_yaw
/wit/reset_offset
/wit_nodelet_manager/bond

/imu トピックが流れていることも確認できます。

$ rostopic echo /imu
header:
  seq: 167
  stamp:
    secs: 1703816001
    nsecs: 645507122
  frame_id: "imu_link"
orientation:
  x: -0.0024576369886855193
  y: -0.03853854587173703
  z: 0.033045750629375904
  w: 0.998707524187069
orientation_covariance: [0.001, 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.001]
angular_velocity:
  x: 0.0
  y: 0.0
  z: 0.0
angular_velocity_covariance: [1e-05, 0.0, 0.0, 0.0, 1e-05, 0.0, 0.0, 0.0, 1e-05]
linear_acceleration:
  x: 0.6986328125000001
  y: -0.052636718750000006
  z: 9.766503906250001
linear_acceleration_covariance: [0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01]

rvizで回転を見る

先ほど作成したPythonノードを実行します。

$ python3 imu_to_pose.py

rvizを起動してADDからPoseを選択、PoseのTopicを/imu_poseに変更、Global OptionsのFixed Rrameをimu_linkに変更することで、pose表示されました。ここではShapeをAxesに変更しています。

失敗例

ラズパイの5V Powerに接続

これでも動いたけど、不安定。途中でラズパイがシャットダウンしてしまいました。ラズパイから直接5Vを取ってくるのは諦めました。

失敗 Witmotion_ros 公式ページの手順

witmotion_ros - ROS Wiki

インストールまではできましたが、message-enumeratorが存在しないとのこと。/opt/ros/noetic/share/witmotion_rosを見ても空でした。よくわからないので放置しましたが、別で
message-enumeratorを準備する手順があるのかもしれません。

$ sudo apt install ros-${ROS_DISTRO}-witmotion-ros
$ rosrun witmotion_ros message-enumerator -d ttyAMA0 -b 38400 -p 10
[rosrun] Couldn't find executable named message-enumerator below /opt/ros/noetic/share/witmotion_ros

権限付与できてなかった

権限付与したつもりでwit.launch を実行したら、Initializeのエラーが発生。権限付与できてなかったみたいです。

$ roslaunch wit_node wit.launch
~~~
[ERROR] [1703815890.517596984]: Wit : initialisation failed [/wit].
[ERROR] [1703815890.517767212]: Wit : could not initialise! Please restart.

同様のエラーを踏んでいる方のIssueがありました。

まとめ

本記事ではROS1(Noetic)で9軸IMUモジュール HWT901B-TTLを使用したときのメモを記載WIしました。いつもそうですが、先人の記事に感謝です。