IMU (on Arduino) でros_serialのテスト [ROSパッケージ作成初心者]

# IMUからデータを書き出すROSノードを作りたい
こういうのやりたい
![imu_test_demo_resized.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/282390/7dee2f69-dee5-5789-d1fb-4ce5c72b3a5c.gif)

もう少し具体的な仕様は、

- IMUからの時系列の速度加速度データをPCで受け取ってcsvに書き出したい
- ソースコードはC++
- ros_serialで通信

ROSが導入済みで、チュートリアルに目を通した方を対象としています。ちょっと雑ですが、ROSパッケージの作成方法も順に説明します。
Arduinoの導入とros_serialの導入は前回の記事をご覧ください。
https://qiita.com/moriitkys/items/0b83fc6f99947059a34a
今回作成するROSパッケージやArduinoプログラムはgithubに投稿しています。
https://github.com/moriitkys/imu_check

※注意　本記事で紹介する技術は著者が経験済みですが、これらを実行することで生じるいかなる損害などに対しても著者は責任を負いません。（例えばデータ損失や半田付けの怪我など）

# ハードウェア構築
PC側

 - Ubuntu 16.04 (64bit)
 - CPU: Intel core i7 7700HQ
 - Memory: 16GB
 - Graphic Board: GTX1060
 - 2020年8月9日現在（重要）

Arduino側

- Arduino MEGA 2560
- LSM9DS1搭載 9軸センサモジュール (https://www.switch-science.com/catalog/5384/)
- ジャンパワイヤ、ミニブレッドボード

補足　もともとは数年前に購入したAdafruit 10-DOF IMU - (L3GD20 + LSM303 + BMP180)でimuのテストを行っていました。しかし、現在は販売終了となっているので上記のものを使用しました。ちなみに販売終了は以下ページ。
https://www.switch-science.com/catalog/1641/

まず、センサモジュールのはんだ付けと配線です。Adafruit公式ページに詳しく書いてあります。Adafruit大好きです。
[Adafruit公式ページ-Overview](https://learn.adafruit.com/adafruit-lsm9ds1-accelerometer-plus-gyro-plus-magnetometer-9-dof-breakout/overview)

<img width="450" alt="imu_build_7.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/282390/987d5e50-1874-1148-7afa-b9fee1ad4118.png">

配線
<img width="450" alt="imu_haisen_1.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/282390/602a9016-e688-cc59-9583-494213590c9a.png">



# ROSパッケージ作成
ROS wikiのチュートリアルでもありますが、初心者にとってはそれぞれの要求に応じたパッケージを１から作るのは戸惑うことが多いと思います。今回はgeometry_msgsやsensor_msgsのpub/sub、CMakeLists.txtの書き方など、軽く記しておきます。

## 1, catkin_create_pkg
home直下で以下のコマンド

```
$ catkin_create_pkg imu_check roscpp std_msgs geometry_msgs sensor_msgs
```
以下のようにSuccessfully created ~となったら成功です。
<img width="400" alt="imu_check_1.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/282390/03f61457-1511-7bd4-482a-35d2a5fceff3.png">

## 2, edit source code (imu_check_3topics.cpp)
以下コマンドで以下の画像のようにcppファイルにプログラムを書きこみ（コピペ）、保存。このコードがROSノードで実行されるメインのプログラムです。

```
$ cd ~/catkin_ws/src/imu_check/src
$ code imu_check_3topics.cpp
```

コピペするソースコードは以下。

<details><summary>imu_check_3topics.cpp</summary><div>

```c++

//ros basic
#include "ros/ros.h"
#include <std_msgs/Float64.h>
#include <geometry_msgs/Vector3.h>

#include <string>
#include <iostream>
#include <fstream>
#include <vector>
#include <time.h>

using namespace std;

vector<float> t_array;//time array
vector<float> ax_array;//acceleration array
vector<float> ay_array;
vector<float> az_array;
vector<float> gx_array;//gyro array
vector<float> gy_array;
vector<float> gz_array;

int arr_fin = 0;
int a_cnt = 0;
int g_cnt = 0;
int set_condition = 0;

void tdataCallback(const std_msgs::Float64::ConstPtr& getdata){

    arr_fin = arr_fin + 1;
    float t = getdata->data;
    t_array.push_back(t);
    //ROS_INFO("ok");
}

void adataCallback(const geometry_msgs::Vector3::ConstPtr& getdata){
    a_cnt = a_cnt + 1;
    
    float a_x = getdata->x;
    float a_y = getdata->y;
    float a_z = getdata->z;
    ax_array.push_back(a_x);
    ay_array.push_back(a_y);
    az_array.push_back(a_z);
    while( a_cnt < arr_fin ){
        a_cnt = a_cnt + 1;
        ax_array.push_back(0);
        ay_array.push_back(0);
        az_array.push_back(0);
    }
    
}

void gdataCallback(const geometry_msgs::Vector3::ConstPtr& getdata){
    g_cnt = g_cnt + 1;

    float g_x = getdata->x;
    float g_y = getdata->y;
    float g_z = getdata->z;
    gx_array.push_back(g_x);
    gy_array.push_back(g_y);
    gz_array.push_back(g_z);
    while( g_cnt < arr_fin ){
        g_cnt = g_cnt + 1;
        gx_array.push_back(0);
        gy_array.push_back(0);
        gz_array.push_back(0);
    }
}


int main(int argc, char **argv) 
{
	ros::init(argc, argv, "imu_check_3topics_node");
	ros::NodeHandle n;
	ros::Subscriber tsub = n.subscribe<std_msgs::Float64>("time", 1, tdataCallback);
    ros::Subscriber asub = n.subscribe<geometry_msgs::Vector3>("acceldata", 1, adataCallback);
    ros::Subscriber gsub = n.subscribe<geometry_msgs::Vector3>("gyrodata", 1, gdataCallback);

	ros::Rate loop_rate(10);

	while(ros::ok())
	{

        ofstream outputfile("imu_result.csv");
        if ( set_condition == 0 ){
            outputfile << "time" << "," << "a_x" << "," << "a_y" << "," << "a_z" << "," << "g_x" << "," << "g_y" << "," << "g_z" <<endl;
        }
        for ( int i = 0; i < arr_fin; i++){
            outputfile << t_array[i] << "," << ax_array[i] << "," << ay_array[i] << "," << az_array[i] << "," << gx_array[i] << "," << gy_array[i] << "," << gz_array[i] << endl;
        }
        outputfile.close();


        ros::spinOnce(); //subscribe
        loop_rate.sleep(); 
    
	}
	return 0; 

}
```

</div></details>   

![imu_pkg_2.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/282390/d588204e-7bbb-4030-0ee7-2c2d2bb0b1d1.png)


**軽くコードの解説**
この**cppファイルはROSトピックをSubscribeしかしません**（Publishなし）。
システムの流れとしてはArduinoから出たトピックをこのC++コードで読み取るということで、Arduinoのプログラムを先に紹介した方が良いですが、まあ**システムの内容が把握できていればSubscribeを先に作っても良いでしょう**。
・ROSトピックを扱うためのライブラリの読み込みは#include "ros/ros.h"や#include <std_msgs/Float64.h>の宣言
・ROSノードの初期化にはメインループ内のros::init(argc, argv, "imu_check_3topics_node");
・Subscriberの宣言が以下のようなところ

```c++
    ros::Subscriber tsub = n.subscribe<std_msgs::Float64>("time", 1, tdataCallback);
    ros::Subscriber asub = n.subscribe<geometry_msgs::Vector3>("acceldata", 1, adataCallback);
    ros::Subscriber gsub = n.subscribe<geometry_msgs::Vector3>("gyrodata", 1, gdataCallback);
```
・ros::spinOnce();の部分では、その行が実行されたタイミングで1回コールバック関数(void adataCallbackなど)にアクセス。ループ自体はwhile(ros::ok())で繰り返され、ros::Rate loop_rate(10);で指定した周波数のループとなるようにloop_rate.sleep(); で時間を調整する
・トピックをSubscribeするとコールバック関数が実行されます。例えば"acceldata"が来るとadataCallbackが呼び出されます。
・Subscribeしたデータをcsvに書き出すため、ofstream outputfile("imu_result.csv");を使っています。詳しくは以下などを参考にしてください。
https://qiita.com/NickTominaga/items/7e01b7eb0b67ac791ec6

## 3, edit CMakeLists.txt

以下のようにCMakeListsを変更します。CMakeLists.txtの編集ではVScodeを使ってもいいですが、設定しないと見にくいです。デフォルトの場合はgeditの方が色がついて見やすいです。
注意としては、project名をimu_checkにしているところです。後でimu_check.cppを作りimu_checkノードを作成するのでこのような名前としています。
「CMakeLists.txtは、ソフトウエアパッケージをビルドするためのCMakeビルドシステムへのインプット」です（ROS.orgより）。

```CMakeLists.txt
cmake_minimum_required(VERSION 3.0.2)
project(imu_check)

find_package(catkin REQUIRED COMPONENTS
  geometry_msgs
  roscpp
  sensor_msgs
  std_msgs
)
catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES imu_check
#  CATKIN_DEPENDS geometry_msgs roscpp sensor_msgs std_msgs
#  DEPENDS system_lib
)

include_directories(
# include
  ${catkin_INCLUDE_DIRS}
)

# ~ 省略 ~
add_executable(${PROJECT_NAME}_3topics_node src/imu_check_3topics.cpp)

# ~ 省略 ~
target_link_libraries(
  ${PROJECT_NAME}_3topics_node
  ${catkin_LIBRARIES}
)
```

![imu_pkg_3.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/282390/a0b62be9-db60-3adc-d77e-ef8a8c88b972.png)

今回はpackage.xmlを編集しませんが、必要に応じて編集する必要があります。

## 4, edit launch file
以下のようにlaunchフォルダ、ファイルの作成をします。記載内容も折り畳みのところに示してあります。
launchファイルはROSノードを複数同時に立ち上げてくれるものです。同時にroscoreも立ち上がります。

```
$ cd ~/ catkin_ws/src/imu_check
$ mkdir launch
$ gedit imu_check_3topics.launch
```

<details><summary>imu_check_3topics.launch</summary><div>

```
<launch>
  <node name="serial_node" pkg="rosserial_python" type="serial_node.py" output="screen" />
    <param name = "port" value = "/dev/ttyACM0" /> 
  <node name="rqt_plot"
        pkg="rqt_plot"
        type="rqt_plot"
        args="/acceldata" />
        args="/gyrodata" />
  <node name="imu_check_3topics" pkg="imu_check" type="imu_check_3topics_node" output="screen" />
</launch>

```
</div>
</details>

このコードはstd_msgs/Float64で時間を、geometry_msgs/Vector3で、加速度、角速度をそれぞれ別々に取得するコードです。特に理由がなければこのようにトピックを分割することは推奨できません（同期しなくなる）。今回はいろんなトピックを使ってみるということでコード作成しましたが、sensor_msgs/ImuでIMUデータを取得するコードも記載します。

## 5, catkin_make
パッケージビルドをするためのコマンドはcatkin_makeです。以下のコマンド、画像のようにします。

```
$ cd ~/catkin_ws/src
$ catkin_make
```

![imu_pkg_5.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/282390/db2ac196-5b62-13c5-cbcc-d71efe3db50f.png)

画像の右のように何のエラーも出なければビルド成功です。もしエラーが出たら、コードの内容やファイル階層などを見直してください。

# Arduinoプログラム作成・書き込み
まず、必要なライブラリを揃えます。前回のArduinoインストールにより、以下のようにros_libが使えるようになっているとは思います。
また、IMUのセンサを読み取るために以下のようにしてAdafruit LSM9DS1とAdafruit Unified Sensorライブラリを追加します。詳しくはAdafruit公式ページを見てください。Adafruit大好きです。
[Adafruit公式ページ-libraries](https://learn.adafruit.com/adafruit-lsm9ds1-accelerometer-plus-gyro-plus-magnetometer-9-dof-breakout/arduino-code)

<img width="350" alt="Arduino_roslib.PNG" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/282390/dad896cf-3d7c-1c6b-3c2c-02f4f6b0747e.png">

<img width="400" alt="arduino_adafruit_install1.gif" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/282390/9809151a-adad-5e93-e743-7150f380d74c.gif">


次にコードを書いていきます。Arduinoのプログラムは同名のフォルダに入っている必要があるので、注意してください。（imu_check/imu_check.inoみたいに）
コードは以下の折り畳みのようなものです。

<details><summary>imu_check_lsm9ds1_3topics.ino</summary><div>

```c
#include <Adafruit_Sensor.h>  // not used in this demo but required!
#include <Adafruit_LSM9DS1.h>

#if (ARDUINO >= 100)
 #include <Arduino.h>
#else
 #include <WProgram.h>
#endif

#include <ros.h>
#include <std_msgs/Float64.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/Vector3.h>
#include <std_msgs/UInt32.h>

#include <Wire.h>

#define LSM9DS1_SCK A5
#define LSM9DS1_MISO 12
#define LSM9DS1_MOSI A4
#define LSM9DS1_XGCS 6
#define LSM9DS1_MCS 5
Adafruit_LSM9DS1 lsm = Adafruit_LSM9DS1();  // i2c sensor


ros::NodeHandle  nh;//ros node

//geometry_msgs::Twist imu_msg;
geometry_msgs::Vector3 accel_msg;
geometry_msgs::Vector3 gyro_msg;
//std_msgs::UInt32 t_msg;
std_msgs::Float64 t_msg;

ros::Publisher accel_pub("acceldata", &accel_msg);
ros::Publisher gyro_pub("gyrodata", &gyro_msg);
ros::Publisher t_pub("time", &t_msg);

unsigned long now_time;

void setupSensor()
{
  // 1.) Set the accelerometer range
  lsm.setupAccel(lsm.LSM9DS1_ACCELRANGE_2G);
  //lsm.setupAccel(lsm.LSM9DS1_ACCELRANGE_4G);
  //lsm.setupAccel(lsm.LSM9DS1_ACCELRANGE_8G);
  //lsm.setupAccel(lsm.LSM9DS1_ACCELRANGE_16G);
  
  // 2.) Set the magnetometer sensitivity
  lsm.setupMag(lsm.LSM9DS1_MAGGAIN_4GAUSS);
  //lsm.setupMag(lsm.LSM9DS1_MAGGAIN_8GAUSS);
  //lsm.setupMag(lsm.LSM9DS1_MAGGAIN_12GAUSS);
  //lsm.setupMag(lsm.LSM9DS1_MAGGAIN_16GAUSS);

  // 3.) Setup the gyroscope
  lsm.setupGyro(lsm.LSM9DS1_GYROSCALE_245DPS);
  //lsm.setupGyro(lsm.LSM9DS1_GYROSCALE_500DPS);
  //lsm.setupGyro(lsm.LSM9DS1_GYROSCALE_2000DPS);
}

void setup(void)
{
  
  Serial.begin(57600);

  nh.initNode();
  nh.advertise( accel_pub );
  nh.advertise( gyro_pub );
  nh.advertise( t_pub );

  while (!Serial) {
    delay(1); // will pause Zero, Leonardo, etc until serial console opens
  }
  
  Serial.println("LSM9DS1 data read demo");
  
  // Try to initialise and warn if we couldn't detect the chip
  if (!lsm.begin())
  {
    Serial.println("Oops ... unable to initialize the LSM9DS1. Check your wiring!");
    while (1);
  }
  Serial.println("Found LSM9DS1 9DOF");

  setupSensor();
}

void loop(void)
{
  now_time = millis();
  
  lsm.read();  /* ask it to read in the data */ 

  sensors_event_t accel, mag, gyro, temp;

  lsm.getEvent(&accel, &mag, &gyro, &temp);

  
  float a_x = accel.acceleration.x;
  float a_y = accel.acceleration.y;
  float a_z = accel.acceleration.z;
  
  accel_msg.x= a_x;
  accel_msg.y= a_y;
  accel_msg.z= a_z;


  float g_x = gyro.gyro.x;
  float g_y = gyro.gyro.y;
  float g_z = gyro.gyro.z;
  
  gyro_msg.x= g_x;
  gyro_msg.y= g_y;
  gyro_msg.z= g_z;

  Serial.print("Accel X: "); Serial.print(accel.acceleration.x); Serial.print(" m/s^2");
  Serial.print("\tY: "); Serial.print(accel.acceleration.y);     Serial.print(" m/s^2 ");
  Serial.print("\tZ: "); Serial.print(accel.acceleration.z);     Serial.println(" m/s^2 ");

  Serial.print("Mag X: "); Serial.print(mag.magnetic.x);   Serial.print(" uT");
  Serial.print("\tY: "); Serial.print(mag.magnetic.y);     Serial.print(" uT");
  Serial.print("\tZ: "); Serial.print(mag.magnetic.z);     Serial.println(" uT");

  Serial.print("Gyro X: "); Serial.print(gyro.gyro.x);   Serial.print(" rad/s");
  Serial.print("\tY: "); Serial.print(gyro.gyro.y);      Serial.print(" rad/s");
  Serial.print("\tZ: "); Serial.print(gyro.gyro.z);      Serial.println(" rad/s");

  accel_pub.publish( &accel_msg );
  gyro_pub.publish( &gyro_msg );

  int nowtime = now_time;
  t_msg.data = nowtime;
  t_pub.publish( &t_msg );
  
  //Serial.println(F(""));
  nh.spinOnce();
  delay(100);
}

```
</div>
</details>

**軽くコードを解説**
ROSパッケージのimu_check_3topics.cppと同様にnh.spinOnce();を使っていますが、今回はPublicherを使います。
以下のようにPublisherを用意して、

```c
// ~ 略 ~
ros::NodeHandle  nh;//ros node

geometry_msgs::Vector3 accel_msg;
geometry_msgs::Vector3 gyro_msg;
std_msgs::Float64 t_msg;

ros::Publisher accel_pub("acceldata", &accel_msg);
ros::Publisher gyro_pub("gyrodata", &gyro_msg);
ros::Publisher t_pub("time", &t_msg);
// ~ 略 ~
void setup(void)
{
  
  Serial.begin(57600);

  nh.initNode();
  nh.advertise( accel_pub );
  nh.advertise( gyro_pub );
  nh.advertise( t_pub );

  // ~ IMUモジュール準備部分　略 ~
}
```

センサーからの値を受け取ったら、

```c
  lsm.read();  /* ask it to read in the data */ 
  sensors_event_t accel, mag, gyro, temp;
  lsm.getEvent(&accel, &mag, &gyro, &temp);

  float a_x = accel.acceleration.x;
  float a_y = accel.acceleration.y;
  float a_z = accel.acceleration.z;
  
  accel_msg.x= a_x;
  accel_msg.y= a_y;
  accel_msg.z= a_z;
```

トピックをPublishします。

```c
accel_pub.publish( &accel_msg );
```

プログラムを書き込むボードがご使用のものであることを確認します（今回はArduino Mega 2560）。ポートもACM0やUSB0など、適切なものになっていることを確認します。
最後にArduino IDEの右矢印ボタンでコンパイルと書き込みです。

<img width="400" alt="Arduino_port_write.PNG" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/282390/64b40972-28ce-dfa7-5cb7-5b647112fd35.png">


# 同様にsensor_msgs/Imuでトピックやりとりするパッケージの作成
以上は先述の通り時間、加速度、角速度をそれぞれ別々に取得するコードでした。IMUデータをまとめて取得するにはsensor_msgs/Imuを使います。ROSパッケージのimu_check/srcにおいてimu_check.cppを作ります。コードは以下の折り畳みです。
<details><summary>imu_check.cpp</summary><div>

```c++
//ros basic
#include "ros/ros.h"
#include <std_msgs/Float64.h>
#include <ros/time.h>

#include <sensor_msgs/Imu.h>

#include <string>
#include <iostream>
#include <fstream>
#include <vector>
#include <time.h>

using namespace std;

vector<float> t_array;//time array
vector<float> ax_array;//acceleration array
vector<float> ay_array;
vector<float> az_array;
vector<float> gx_array;//gyro array
vector<float> gy_array;
vector<float> gz_array;

int set_condition = 0;
int arr_fin = 0;
float t_sec = 0.0;
unsigned int t_pre;
unsigned int dt;


void imuCallback(const sensor_msgs::Imu::ConstPtr& getdata){
    
    float a_x = getdata->linear_acceleration.x;
    float a_y = getdata->linear_acceleration.y;
    float a_z = getdata->linear_acceleration.z;
    float g_x = getdata->angular_velocity.x;
    float g_y = getdata->angular_velocity.y;
    float g_z = getdata->angular_velocity.z;

    unsigned int t = getdata->header.stamp.toNSec();

    if ( t < t_pre ){
        dt = 4294967295 - t_pre + t;
    }else{
        dt = t - t_pre;
    }
    
    t_pre = t;
    t_sec = t_sec + dt / 1000000000.0;

    ax_array.push_back(a_x);
    ay_array.push_back(a_y);
    az_array.push_back(a_z);
    gx_array.push_back(g_x);
    gy_array.push_back(g_y);
    gz_array.push_back(g_z);
    t_array.push_back(t_sec);

    arr_fin = arr_fin + 1;
    
}

int main(int argc, char **argv) 
{
	ros::init(argc, argv, "imu_check_node");
	ros::NodeHandle n;
    ros::Subscriber imusub = n.subscribe<sensor_msgs::Imu>("imu/data", 1, imuCallback);
    
    
	ros::Rate loop_rate(10);

	while(ros::ok())
	{

        ofstream outputfile("imu_result.csv");
        if ( set_condition == 0 ){
            outputfile << "time" << "," << "a_x" << "," << "a_y" << "," << "a_z" << "," << "g_x" << "," << "g_y" << "," << "g_z" <<endl;
        }
        for ( int i = 0; i < arr_fin; i++){
            outputfile << t_array[i] << "," << ax_array[i] << "," << ay_array[i] << "," << az_array[i] << "," << gx_array[i] << "," << gy_array[i] << "," << gz_array[i] << endl;
        }
        outputfile.close();


        ros::spinOnce(); //subscribe
        loop_rate.sleep(); 
    
	}
	return 0; 

}

```
</div>
</details>

上述のトピックのSubscribe方法とは異なり、以下のように加速度、角速度をまとめてimuCallbackで受け取ることができます。

```c
void imuCallback(const sensor_msgs::Imu::ConstPtr& getdata){
    
    float a_x = getdata->linear_acceleration.x;
    float a_y = getdata->linear_acceleration.y;
    float a_z = getdata->linear_acceleration.z;
    float g_x = getdata->angular_velocity.x;
    float g_y = getdata->angular_velocity.y;
    float g_z = getdata->angular_velocity.z;

    // ~ 略 ~

    ax_array.push_back(a_x);
    ay_array.push_back(a_y);
    az_array.push_back(a_z);
    gx_array.push_back(g_x);
    gy_array.push_back(g_y);
    gz_array.push_back(g_z);
    t_array.push_back(t_sec);

    // ~ 略 ~
}
```

二つのソースコードをビルドできるようにします。
ソースコードを複数作成した場合は、CMakeLists.txtを以下のようにadd_executableとtarget_link_librariesを変更します。このときadd_executableなどの中に二つ記述しないよう気を付けてください。

<details><summary>CMakeLists.txt</summary><div>

```
cmake_minimum_required(VERSION 3.0.2)
project(imu_check)

## Compile as C++11, supported in ROS Kinetic and newer
# add_compile_options(-std=c++11)

## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
  geometry_msgs
  roscpp
  sensor_msgs
  std_msgs
)

###################################
## catkin specific configuration ##
###################################
catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES imu_check
#  CATKIN_DEPENDS geometry_msgs roscpp sensor_msgs std_msgs
#  DEPENDS system_lib
)

###########
## Build ##
###########
include_directories(
# include
  ${catkin_INCLUDE_DIRS}
)

## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
add_executable(${PROJECT_NAME}_node src/imu_check.cpp)
add_executable(${PROJECT_NAME}_3topics_node src/imu_check_3topics.cpp)

## Add cmake target dependencies of the executable
## same as for the library above
# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

## Specify libraries to link a library or executable target against
target_link_libraries(
  ${PROJECT_NAME}_node
  ${catkin_LIBRARIES}
)
target_link_libraries(
  ${PROJECT_NAME}_3topics_node
  ${catkin_LIBRARIES}
)
```
</div>
</details>

<img width="400" alt="imu_check_11.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/282390/9dfdea2d-34af-9502-bf4b-56389e3cb2dd.png">

また、sensor_msgs/Imu用のlaunchファイルは以下です。
<details><summary>imu_check.launch</summary><div>

```
<launch>
  <node name="serial_node" pkg="rosserial_python" type="serial_node.py" output="screen" />
    <param name = "port" value = "/dev/ttyACM0" /> 
  <node name="rqt_plot"
        pkg="rqt_plot"
        type="rqt_plot"
        args="/imu/data/linear_acceleration" />
        args="/imu/data/angular_velocity" />
  <node name="imu_check" pkg="imu_check" type="imu_check_node" output="screen" />
</launch>

```
</div>
</details>

最後に、sensor_msgs/Imu用のArduinoファイルです。こちらもimu_pub.publish( &imu_msg );でまとめて情報をPublishできていることがわかると思います。
<details><summary>imu_check_lsm9ds1.ino</summary><div>

```c
#include <Adafruit_Sensor.h>  // not used in this demo but required!
#include <Adafruit_LSM9DS1.h>

#if (ARDUINO >= 100)
 #include <Arduino.h>
#else
 #include <WProgram.h>
#endif

#include <ros.h>

#include <sensor_msgs/Imu.h>
#include <std_msgs/Float64.h>

#include <Wire.h>

#define LSM9DS1_SCK A5
#define LSM9DS1_MISO 12
#define LSM9DS1_MOSI A4
#define LSM9DS1_XGCS 6
#define LSM9DS1_MCS 5
Adafruit_LSM9DS1 lsm = Adafruit_LSM9DS1();  // i2c sensor

ros::NodeHandle  nh;//ros node

sensor_msgs::Imu imu_msg;

ros::Publisher imu_pub("imu/data", &imu_msg);

void setupSensor()
{
  // 1.) Set the accelerometer range
  lsm.setupAccel(lsm.LSM9DS1_ACCELRANGE_2G);
  //lsm.setupAccel(lsm.LSM9DS1_ACCELRANGE_4G);
  //lsm.setupAccel(lsm.LSM9DS1_ACCELRANGE_8G);
  //lsm.setupAccel(lsm.LSM9DS1_ACCELRANGE_16G);
  
  // 2.) Set the magnetometer sensitivity
  lsm.setupMag(lsm.LSM9DS1_MAGGAIN_4GAUSS);
  //lsm.setupMag(lsm.LSM9DS1_MAGGAIN_8GAUSS);
  //lsm.setupMag(lsm.LSM9DS1_MAGGAIN_12GAUSS);
  //lsm.setupMag(lsm.LSM9DS1_MAGGAIN_16GAUSS);

  // 3.) Setup the gyroscope
  lsm.setupGyro(lsm.LSM9DS1_GYROSCALE_245DPS);
  //lsm.setupGyro(lsm.LSM9DS1_GYROSCALE_500DPS);
  //lsm.setupGyro(lsm.LSM9DS1_GYROSCALE_2000DPS);
}


void setup(void)
{
  Serial.begin(57600);
  
  nh.initNode();
  nh.advertise( imu_pub );
  
  while (!Serial) {
    delay(1); // will pause Zero, Leonardo, etc until serial console opens
  }
  
  Serial.println("LSM9DS1 data read demo");
  
  // Try to initialise and warn if we couldn't detect the chip
  if (!lsm.begin())
  {
    Serial.println("Oops ... unable to initialize the LSM9DS1. Check your wiring!");
    while (1);
  }
  Serial.println("Found LSM9DS1 9DOF");

  setupSensor();

}

void loop(void)
{

  lsm.read();  /* ask it to read in the data */ 

  sensors_event_t accel, mag, gyro, temp;

  lsm.getEvent(&accel, &mag, &gyro, &temp);
  
  imu_msg.header.frame_id = "imu";
  imu_msg.header.stamp = nh.now();
  //imu_msg.child_frame_id = "base_link";

  imu_msg.linear_acceleration.x = accel.acceleration.x;
  imu_msg.linear_acceleration.y = accel.acceleration.y;
  imu_msg.linear_acceleration.z = accel.acceleration.z;

  imu_msg.angular_velocity.x = gyro.gyro.x;
  imu_msg.angular_velocity.y = gyro.gyro.y;
  imu_msg.angular_velocity.z = gyro.gyro.z;
    

  imu_pub.publish( &imu_msg );


  nh.spinOnce();
  delay(100);
}

```
</div>
</details>

# 実行の様子
## sensor_msgs/Imuのデータを見る
作成したパッケージをlaunchで実行すると、rqtが自動的に立ち上がり、Arduinoからのデータを見ることができます。

```
roslaunch imu_check imu_check_3topics.launch
```

グラフが表示されないときは以下のように「Topics」にスラッシュを入れて該当トピックを追加したり、右上あたりのバッテン左矢印をクリックしたり、何かしらrqtの操作が必要です。
<img width="430" alt="imu_check_rqt.gif" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/282390/98c71322-62d1-596d-3dbb-f6f29739124e.gif">

z軸の加速度が10付近をうろうろしているのが見えますでしょうか。多分重力加速度だと思うのですが、否定されました。まあ確かにプラスになってるから変ではあります（向きが逆？）。

<img width="430" alt="imu_test_z+10.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/282390/3320cc16-bcbf-bb45-ec7a-b3bf0c4ed3f7.png">


その他はIMUモジュール基板に書いてある通りの軸に対応した加速度、角速度が得られていると思います。

<img width="400" alt="imu_test_4.gif" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/282390/8c570989-9ca7-4923-1375-dece7e29a506.gif">


## csvファイル書き出し結果を見る
このパッケージでは時系列データをcsvに記録できるので、home直下の.ros内に保存されます。上記でも述べた通り、launchで実行しているので.ros内に保存されてしまいます。これ、避ける方法をご存じの方は教えてほしいです。
以下のようにしてグラフ表示できます。
![imu_check_csv.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/282390/d7748ffc-d589-48cf-e584-ae2b380a58fb.png)

# 廃盤の方について
もう使ってる人もいないと思いますが一応使っていた記念に。
ライブラリは以下のような感じで揃えます。
![imu_check_lib_imu.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/282390/6985fa0d-3d19-238c-6082-88402e902de6.png)


Arduinoのプログラムはこちら。ROSパッケージは同じでＯＫです（トピックを受け取るだけなので）。
<details><summary>imu_check.ino (old version)</summary><div>

```
#if (ARDUINO >= 100)
 #include <Arduino.h>
#else
 #include <WProgram.h>
#endif

#include <ros.h>

#include <sensor_msgs/Imu.h>
#include <std_msgs/Float64.h>

#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_LSM303_U.h>
#include <Adafruit_BMP085_U.h>
#include <Adafruit_L3GD20_U.h>
#include <Adafruit_10DOF.h>

/* Assign a unique ID to the sensors */
Adafruit_LSM303_Accel_Unified accel = Adafruit_LSM303_Accel_Unified(30301);
Adafruit_LSM303_Mag_Unified   mag   = Adafruit_LSM303_Mag_Unified(30302);
Adafruit_BMP085_Unified       bmp   = Adafruit_BMP085_Unified(18001);
Adafruit_L3GD20_Unified       gyro  = Adafruit_L3GD20_Unified(20);

ros::NodeHandle  nh;//ros node

sensor_msgs::Imu imu_msg;


ros::Publisher imu_pub("imu/data", &imu_msg);


void displaySensorDetails(void)
{
  sensor_t sensor;
  accel.getSensor(&sensor);
  gyro.getSensor(&sensor);
  mag.getSensor(&sensor);
  bmp.getSensor(&sensor);
  delay(500);
}

void setup(void)
{
  Serial.begin(57600);
  Serial.println(F("Adafruit 10DOF Tester")); Serial.println("");
  
  /* Initialise the sensors */
  if(!accel.begin())
  {
    /* There was a problem detecting the ADXL345 ... check your connections */
    //Serial.println(F("Ooops, no LSM303 detected ... Check your wiring!"));
    while(1);
  }
  if(!mag.begin())
  {
    /* There was a problem detecting the LSM303 ... check your connections */
    //Serial.println("Ooops, no LSM303 detected ... Check your wiring!");
    while(1);
  }
  if(!bmp.begin())
  {
    /* There was a problem detecting the BMP085 ... check your connections */
    //Serial.print("Ooops, no BMP085 detected ... Check your wiring or I2C ADDR!");
    while(1);
  }
  if(!gyro.begin())
  {
    /* There was a problem detecting the L3GD20 ... check your connections */
    //Serial.print("Ooops, no L3GD20 detected ... Check your wiring or I2C ADDR!");
    while(1);
  }
  
  /* Display some basic information on this sensor */
  displaySensorDetails();
  nh.initNode();
  nh.advertise( imu_pub );
}

void loop(void)
{
  imu_msg.header.frame_id = "imu";
  imu_msg.header.stamp = nh.now();
  //imu_msg.child_frame_id = "base_link";
  
  /* Get a new sensor event */
  sensors_event_t event;
  
  /* Display the results (acceleration is measured in m/s^2) */
  accel.getEvent(&event);
  /*
  float a_x = event.acceleration.x;
  float a_y = event.acceleration.y;
  float a_z = event.acceleration.z;
  */

  imu_msg.linear_acceleration.x = event.acceleration.x;
  imu_msg.linear_acceleration.y = event.acceleration.y;
  imu_msg.linear_acceleration.z = event.acceleration.z;
  
  /* Display the results (magnetic vector values are in micro-Tesla (uT)) */
  mag.getEvent(&event);

  /* Display the results (gyrocope values in rad/s) */
  gyro.getEvent(&event);
  
  float g_x = event.gyro.x;
  float g_y = event.gyro.y;
  float g_z = event.gyro.z;

  imu_msg.angular_velocity.x = g_x;
  imu_msg.angular_velocity.y = g_y;
  imu_msg.angular_velocity.z = g_z;

  /* Display the pressure sensor results (barometric pressure is measure in hPa) */
  bmp.getEvent(&event);
  if (event.pressure)
  {
    /* Display atmospheric pressure in hPa */
    /*
    Serial.print(F("PRESS "));
    Serial.print(event.pressure);
    Serial.print(F(" hPa, "));
    */
    /* Display ambient temperature in C */
    float temperature;
    bmp.getTemperature(&temperature);
    /*
    Serial.print(temperature);
    Serial.print(F(" C, "));
    */
    /* Then convert the atmospheric pressure, SLP and temp to altitude    */
    /* Update this next line with the current SLP for better results      */
    float seaLevelPressure = SENSORS_PRESSURE_SEALEVELHPA;
    /*
    Serial.print(bmp.pressureToAltitude(seaLevelPressure,
                                        event.pressure,
                                        temperature)); 
    Serial.println(F(" m"));
    */
    
  }
  imu_pub.publish( &imu_msg );
  nh.spinOnce();
  delay(100);
}

```
</div>
</details>

# Arduino系のエラーについて
追記予定。ポートのエラー関連が出ると思われますがネット調べたらだいたいは出てきます。

# 最後に
ROSパッケージ作成の練習、ArduinoのROSトピックを受け取る練習をしました。ROS Serialなどは簡単なロボット作成には使える技術ですが、やはり手軽に入手できる方法では安定性に難ありです。

# 参考
http://wiki.ros.org/ja/ROS/Tutorials/CreatingPackage
https://learn.adafruit.com/adafruit-lsm9ds1-accelerometer-plus-gyro-plus-magnetometer-9-dof-breakout/overview