はじめに
ロボットを自律走行させるため、前回の記事の続きです。
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"でした。
#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を変更します。
<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するノードを作成する。
#!/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 公式ページの手順
インストールまではできましたが、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しました。いつもそうですが、先人の記事に感謝です。