ROS IMU 센서

세 방향의 축

ROLL : 오른쪽, 왼쪽 바퀴의 들림 정도

PITCH : 앞, 뒤의 들림 정도

YAW : 좌회전 우회전 정도

 

/imu 토픽

- 타입 : sensor_msgs/imu

- 구성

std_msgs/Header header
	unit32 seq
    time stamp
    string frame_id
geometry_msgs/Quaternion orientation
	float64 x
    float64 y
    float64 z
    float64 w // Quaternion 기울어짐 정도

---

RVIZ에서 IMU 시각화하기

 

rviz_imu_plugin 을 catkin_make한 후 rviz 실행 후 add를 눌러 rviz_imu_plugin의 imu 선택 후 추가

imu를 아래와 같이 설정

 

IMU 데이터로 RVIZ 뷰어의 박스 조작

#! /usr/bin/env python

import rospy, math, os
from sensor_msgs.msg import Imu
from tf.transformations import euler_from_quaternion 

degrees2rad = float(math.pi)/float(180.0)
rad2degrees = float(180.0)/float(math.pi)
name = " >> ./imu_data.txt"

def call_back(data):
    global degrees2rad
    global rad2degrees

    euler = euler_from_quaternion((data.orientation.x, data.orientation.y, data.orientation.z, data.orientation.w))

    euler = [euler[0], euler[1], euler[2]]

    save_str = "roll : " + str(euler[0]) + ", " + "pitch : " + str(euler[1]) + ", " + "yaw : "+ str(euler[2])

    command = 'echo "' + save_str + " >> ./imu_data.txt"

    print(command)
    os.system(command)

def listener():
    rospy.init_node('imu_data_maker', anonymous = False)
    rospy.Subscriber('imu', Imu, call_back)

if __name__ == "__main__":
    listener()
    try:
        rospy.spin()
    except KeyboardInterrupt:
        print("Shutting down")

위 코드는 IMU 토픽을 받아 imu_data.txt 파일을 만드는 코드이다. 이 코드로 만들어진 파일의 일부는 다음과 같다.

roll : -3.11715804406, pitch : -0.0532325421858, yaw : -0.365821011218
roll : -3.11733257699, pitch : -0.0532325421858, yaw : -0.365821011218
roll : -3.11733257699, pitch : -0.0528834763354, yaw : -0.365821011218
roll : -3.11820524161, pitch : -0.0514872129338, yaw : -0.365821011218
roll : -3.11977603794, pitch : -0.0492182849062, yaw : -0.365646478293
roll : -3.12099776842, pitch : -0.0486946861306, yaw : -0.365646478293
roll : -3.12221949889, pitch : -0.047298422729, yaw : -0.365646478293

목표:  imu_Data.txt의 데이터를 읽어 imu_generator.py로 가공하여 rviz 뷰어에 표시

 

1. 패키지 생성: catkin_create_pkg rviz_imu_cpp roscpp tf geometry_msgs urdf rviz xacro

2. rviz 파일 복사 저장

3. launch 파일 작성

 

rviz_imu_cpp
│   ├── CMakeLists.txt
│   ├── include
│   │   └── rviz_imu_cpp
│   ├── launch
│   │   └── imu_generator_cpp.launch
│   ├── package.xml
│   ├── rviz
│   │   └── imu_generator.rviz
│   └── src
│       ├── imu_cpp.cpp
│       └── imu_data.txt

// launch 파일
<launch>
	<!-- rviz display -->
	<node name="rviz_visualizer" pkg="rviz" type="rviz" required="true" 
              args="-d $(find rviz_imu_cpp)/rviz/imu_generator.rviz"/>
	<node name="imu_generator" pkg="rviz_imu_cpp" type="imu_cpp" />
</launch>

#include <iostream>
#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <tf2/LinearMath/Quaternion.h>
#include <fstream>
#include <vector>

using namespace std;
using namespace ros;

int main(int argc, char** argv)
{
    init(argc, argv, "imu_generator");
    NodeHandle nh;

    Publisher pub = nh.advertise<sensor_msgs::Imu>("imu", 1);
    sensor_msgs::Imu imumsg;
    imumsg.header.frame_id = "map";
    Rate rate(5);

    ifstream file("/home/jagbum/xycar_ws/src/rviz_imu_cpp/src/imu_data.txt");

    string line;
    while (getline(file, line))
    {
        istringstream iss(line);
        vector<double> extract;
        string token;

        while (getline(iss, token, ','))
        {
            extract.push_back(stof(token.substr(token.find(":") + 1)));
        }

        tf2::Quaternion quaternion;
        quaternion.setRPY(extract[0], extract[1], extract[2]);

        cout << quaternion.x()<<" "<< quaternion.y() <<" "<< quaternion.z()<<" "<< quaternion.w()<< endl;
        
        imumsg.orientation.x = quaternion.x();
        imumsg.orientation.y = quaternion.y();
        imumsg.orientation.z = quaternion.z();
        imumsg.orientation.w = quaternion.w();

        pub.publish(imumsg);

        extract.clear();
        rate.sleep();
    }
    file.close();
    return 0;
}