【備忘録】【Navio2✖️ROS】PC上でセンサデータを共有
開発環境
・OS : Linux (Ubuntu14.04 LTS)
・Raspberry Pi + Navio2
事前準備
1. Raspberry Piの有線接続時のIPアドレス固定
1) デフォルトゲートウェイを確認
$ route
結果は、192.168.20.20と仮定
2) Raspberry Piの設定ファイルを変更
$nano /etc/dhcpcd.conf 下記内容を追記し保存 interface eth0 static ip_address=192.168.20.30(←任意）/24 static routers=192.168.20.20 static domain_name_servers=192.168.20.20 $sudo nano /boot/cmdline.txt 末尾に「net.ifnames=0」を追記し保存$ ＄sudo reboot 再起動後、次のコマンドでログイン可能 $ sudo ssh pi@192.168.20.30（←/etc/dhcpcd.conf設定内容を参照）
2. Raspberry PiにNavio2操作用ライブラリをインストール
$ git clone https://github.com/emlid/Navio2.git $ cd Navio2/C++/Navio $ make all ライブラリ(libnavio.a)が生成される
3. Raspberry Piで取得したセンサデータをROS上に書き出す(パブリッシュ)
1) ROSプログラムのビルド
ワークスペースの作成
$ echo "source /opt/ros/kinetic/setup.bash" >> ~/.bashrc
$ mkdir ~/catkin_ws
$ mkdir ~/catkin_ws/src
パッケージの作成
$ cd ~/catkin_ws/src
$ catkin_create_pkg accel_gyro_mag std_msgs roscpp rospy
$ cd ~/catkin_ws
$ catkin_make
メッセージファイルの作成
※ メッセージが定義されているヘッダーファイルは/opt/ros/kinetic/includeにある。
$ cd ~/catkin_ws/src/accel_gyro_mag
$ mkdir msg
$ cd msg
$ touch sensor_9dof.msg
$ nano sensor_9dof.msg
下記内容を追記
float32 temperature
geometry_msgs/Vector3 acc
geometry_msgs/Vector3 gyro
geometry_msgs/Vector3 mag
プログラムの作成
$ cd ~/catkin_ws/src/accel_gyro_mag/src/
$ nano accel_gyro_mag.cpp
CMakeLists.txtの編集
$ cd ~/catkin_ws/src/accel_gyro_mag
$ nano CMakeLists.txt
ファイル内の似た記述に下記を追加し、保存
cmake_minimum_required(VERSION 2.8.3)
project(accel_gyro_mag)
find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
std_msgs
geometry_msgs
message_generation
)
add_message_files(
FILES
sensor_9dof.msg
)
generate_messages(
DEPENDENCIES
std_msgs
geometry_msgs
)
include_directories(~/(環境依存)/Navio2/C++/Navio)
link_directories(~/Drivers/(環境依存)/C++/Navio)
add_executable(accel_gyro_mag src/accel_gyro_mag.cpp)
target_link_libraries(accel_gyro_mag ${catkin_LIBRARIES})
target_link_libraries(accel_gyro_mag navio)
ビルド
$ cd ~/catkin_ws
$ catkin_make
※ make後エラーが発生したが、~/catkin_ws/src/accel_gyro_mag/msg/._sensor_9dof.msgを削除後に解決
環境のセットアップ
source ~/catkin_ws/devel/setup.bash
（~/.bashrcの最終行に追加しておくと、手動セットアップを省略可） 2) ROSプログラム
accel_gyro_mag.cppは下記の通り
#include <ros/ros.h> // ROSヘッダファイル #include "Common/MPU9250.h" // For Navio #include "Navio2/LSM9DS1.h" // For Navio #include "Common/Util.h" // For Navio #include "accel_gyro_mag/sensor_9dof.h" // For ROS Message #include <unistd.h> #include <string> #include <memory> std::unique_ptr <InertialSensor> get_inertial_sensor( std::string sensor_name) { if (sensor_name == "mpu") { printf("Selected: MPU9250\n"); auto ptr = std::unique_ptr <InertialSensor>{ new MPU9250() }; return ptr; } else { return NULL; } } int main(int argc, char **argv) { // Init for ROS------------------------------------------------------------------------------------------------------ ros::init(argc, argv, "accel_gyro_mag"); // ROS初期化（ノード名を"accel_gryo_mag"に設定） ros::NodeHandle nh; // ノードハンドルの生成 ros::Publisher pub = nh.advertise<accel_gyro_mag::sensor_9dof>("Sensor9Dof_data", 1000); //Publisherとしての定義 ros::Rate rate(1); // 周期設定（1Hzに設定） // Init for Navio----------------------------------------------------------------------------------------------------- if (check_apm()) { return 1; } //MPU Sensor---------------------------------------------------------- auto mpu_sensor = get_inertial_sensor("mpu"); if (!mpu_sensor->probe()) { printf("MPU Sensor not enabled\n"); return EXIT_FAILURE; } mpu_sensor->initialize(); accel_gyro_mag::sensor_9dof mpu_data; // accel_gyro_mag::sensor_9dof型のオブジェクトを定義 float ax, ay, az; float gx, gy, gz; float mx, my, mz; while(ros::ok()) { // Ctrl+Cが押されるまで繰り返し // Get Sensor data------------------------------------------------------------------------------------- mpu_sensor->update(); mpu_sensor->read_accelerometer(&ax, &ay, &az); mpu_sensor->read_gyroscope(&gx, &gy, &gz); mpu_sensor->read_magnetometer(&mx, &my, &mz); // Publisherへのデータ格納------------------------------------------------------------------------- mpu_data.acc.x = ax; mpu_data.acc.y = ay; mpu_data.acc.z = az; mpu_data.gyro.x = gx; mpu_data.gyro.y = gy; mpu_data.gyro.z = gz; mpu_data.mag.x = mx; mpu_data.mag.y = my; mpu_data.mag.z = mz; mpu_data.temperature = 1.1; //センサ値の代入が必要 pub.publish(mpu_data); ros::spinOnce(); printf("Acc: %+7.3f %+7.3f %+7.3f ", ax, ay, az); printf("Gyr: %+8.3f %+8.3f %+8.3f ", gx, gy, gz); printf("Mag: %+7.3f %+7.3f %+7.3f\n", mx, my, mz); //ROS_INFO_STREAM("Acc: %+7.3f, %+7.3f, %+7.3f", ax, ay, az); //ROS_INFO_STREAM("Acc:", ax, ",", ay, ",", az); //ROS_INFO_STREAM("Gyr:", gx, ",", gy, ",", gz); //ROS_INFO_STREAM("Mag:", mx, ",", my, ",", mz); rate.sleep(); // 指定された周期待ち（ループ内の他処理時間を引いた時間分を待つ） } }
実行
ROSマスターの起動 $ roscore ROSプログラムの実行 $ rosrun accel_gyro_mag accel_gyro_mag ROSトピックの確認 $ rostopic echo /Sensor9Dof_data ROSプログラムの停止 Ctl+C or $ rosnode kill accel_gyro_mag
参考URL
1) ROSの使い方:http://forestofazumino.web.fc2.com/ros/ros_using_two_machines.html
2) Navio2サンプルコード①:https://docs.emlid.com/navio2/common/dev/navio-repository-cloning/
3) Navio2サンプルコード②:https://docs.emlid.com/navio2/common/dev/imu-mpu9250_lsm9ds1/
4) CMakeLists.txt:ja/catkin/CMakeLists.txt - ROS Wiki
5) CMakeLists.txtの設定:http://www.wagavulin.jp/entry/2011/11/27/222642
6) std_msgs:std_msgs - ROS Wiki
7) ros::spin or ros::spinOnce:https://cse.sc.edu/~jokane/agitr/agitr-letter-pubsub.pdf
8) ROSについて：ワークスペースとパッケージ
9) IP固定：http://hirazakura.hatenablog.com/entry/raspberrypi/setup/last
10) ROS メッセージ：https://kazuyamashi.github.io/ros_lecture/ros_study.html
11) USBカメラ:http://zumashi.blogspot.com/2016/12/ros-kinetic-usb-cam.html
最後まで読んで頂き、ありがとうございました。
