ROS & Robotics

Gazebo 에서 IMU 데이터 받아와서 csv 파일로 저장하고 데이터 시각화하기

지난 번에 사용했던 Warthog 로봇과는 달리, Husky 로봇 시뮬레이터에는 IMU가 탑재되어 있어서 따로 urdf 를 수정해 줄 필요가 없습니다. (야호)

다음과 같이 Husky robot 을 Gazebo 에 불러와주고 조종할 수 있게

$ rosrun teleop_twist_keyboard teleop_twist_keyboard.py

rostopic list 로 토픽을 확인해 보면 다음과 같이 토픽이 있는 것을 확인할 수 있습니다.

/clock
/cmd_vel
/diagnostics
/e_stop
/gazebo/link_states
/gazebo/model_states
/gazebo/parameter_descriptions
/gazebo/parameter_updates
/gazebo/performance_metrics
/gazebo/set_link_state
/gazebo/set_model_state
/husky_velocity_controller/cmd_vel
/husky_velocity_controller/odom
/husky_velocity_controller/parameter_descriptions
/husky_velocity_controller/parameter_updates
/imu/data
/imu/data/accel/parameter_descriptions
/imu/data/accel/parameter_updates
/imu/data/bias
/imu/data/rate/parameter_descriptions
/imu/data/rate/parameter_updates
/imu/data/yaw/parameter_descriptions
/imu/data/yaw/parameter_updates
/imu_data
/joint_states
/joy_teleop/cmd_vel
/joy_teleop/joy
/joy_teleop/joy/set_feedback
/kb_teleop/cmd_vel
/navsat/fix
/navsat/fix/position/parameter_descriptions
/navsat/fix/position/parameter_updates
/navsat/fix/status/parameter_descriptions
/navsat/fix/status/parameter_updates
/navsat/fix/velocity/parameter_descriptions
/navsat/fix/velocity/parameter_updates
/navsat/vel
/odometry/filtered
/rosout
/rosout_agg
/set_pose
/tf
/tf_static
/twist_marker_server/cmd_vel
/twist_marker_server/feedback
/twist_marker_server/update
/twist_marker_server/update_full

여기서 사용할 토픽은 1) /gazebo/model_states 2) /imu/data 입니다.

해당 토픽을 구독해서 imu 데이터를 기록해 줄 새로운 패키지를 생성합니다.

imu_data_logger 패키지 제작

$ catkin_create_pkg imu_data_logger roscpp sensor_msgs gazebo_msgs

그리고 CMakeLists.txt 파일에 다음 줄을 추가해줍니다.

add_executable(imu_data_logger_node src/imu_data_logger_node.cpp)
target_link_libraries(imu_data_logger_node ${catkin_LIBRARIES})

imu_data_logger.cpp

이 코드는 위에서 말한 토픽을 구독해서

seq,angular_velocity_x,angular_velocity_y,angular_velocity_z,linear_acceleration_x,linear_acceleration_y,linear_acceleration_z,position_x,position_y,position_z

의 순서로 해당 값을 저장한 후 이를 Csv 파일로 저장해 주는 코드입니다.

#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <gazebo_msgs/GetModelState.h>
#include <fstream>

// Global variables
std::ofstream csv_file;
ros::ServiceClient model_state_client;
std::string filename;

// Frequency variables
double desired_frequency = 10.0;  // Desired frequency in Hz
ros::Rate* rate;

// Callback function for the IMU data subscriber
void imuDataCallback(const sensor_msgs::Imu::ConstPtr& msg) {
    // Get the current location of the "husky" robot from Gazebo
    gazebo_msgs::GetModelState model_state;
    model_state.request.model_name = "husky";
    model_state_client.call(model_state);

    // Write the IMU data and robot location to the CSV file
    csv_file << msg->header.seq << ","
             << msg->angular_velocity.x << "," << msg->angular_velocity.y << "," << msg->angular_velocity.z << ","
             << msg->linear_acceleration.x << "," << msg->linear_acceleration.y << "," << msg->linear_acceleration.z << ","
             << model_state.response.pose.position.x << "," << model_state.response.pose.position.y << "," << model_state.response.pose.position.z << "\n";

    // Sleep to achieve the desired frequency
    rate->sleep();
}

int main(int argc, char** argv) {
    // Initialize the ROS node and create a node handle
    ros::init(argc, argv, "imu_data_logger_node");
    ros::NodeHandle nh;

    // Create a service client to call the GetModelState service
    model_state_client = nh.serviceClient<gazebo_msgs::GetModelState>("/gazebo/model_states");

    // Get the filename from the launch argument
    if (argc < 2) {
        ROS_ERROR("Please provide the filename as an argument!");
        return 1;
    }
    filename = argv[1];

    // Open the CSV file for writing
    csv_file.open(filename);
    csv_file << "seq,angular_velocity_x,angular_velocity_y,angular_velocity_z,linear_acceleration_x,linear_acceleration_y,linear_acceleration_z,position_x,position_y,position_z\n";

    // Subscribe to the IMU data topic
    ros::Subscriber imu_data_sub = nh.subscribe("/imu/data", 10, imuDataCallback);

    // Set the desired frequency
    rate = new ros::Rate(desired_frequency);

    // Spin the node and wait for callbacks
    ros::spin();

    // Close the CSV file
    csv_file.close();

    delete rate;

    return 0;
}

컴파일한 후, 다음과 같은 command line 으로 실행시킵니다.

$ rosrun imu_data_logger imu_data_logger_node 0917_test1.csv

그러면 다음과 같은 형태로 Imu 데이터가 저장됩니다.

Data visualization

csv 파일을 읽어들여 값을 시계열로 시각화하는 ipynb코드입니다.

import matplotlib.pyplot as plt
import csv

with open('../imu_data/0917_test1.csv', 'r') as csvfile:
    data_reader = csv.reader(csvfile)
    headers = next(data_reader)  # skip the header row
    seqs = []
    ang_vel_x = []
    ang_vel_y = []
    ang_vel_z = []
    lin_acc_x = []
    lin_acc_y = []
    lin_acc_z = []
    # pos_x = []
    # pos_y = []
    # pos_z = []
    for i, row in enumerate(data_reader):
        # if i > 100:
        #     break
        seqs.append(int(row[0]))
        ang_vel_x.append(float(row[1]))
        ang_vel_y.append(float(row[2]))
        ang_vel_z.append(float(row[3]))
        lin_acc_x.append(float(row[4]))
        lin_acc_y.append(float(row[5]))
        lin_acc_z.append(float(row[6]))
        # pos_x.append(float(row[7]))
        # pos_y.append(float(row[8]))
        # pos_z.append(float(row[9]))
        
  fig, axs = plt.subplots(2, 1, figsize=(10, 8))

axs[0].plot(seqs, ang_vel_x, 'r', label='Angular Velocity X')
axs[0].plot(seqs, ang_vel_y, 'g', label='Angular Velocity Y')
axs[0].plot(seqs, ang_vel_z, 'b', label='Angular Velocity Z')
axs[0].set_xlabel('Seq')
axs[0].set_ylabel('Angular Velocity')
axs[0].set_title('IMU Data: Angular Velocity')
axs[0].legend()


axs[1].plot(seqs, lin_acc_x, 'r', label='Linear Acceleration X')
axs[1].plot(seqs, lin_acc_y, 'g', label='Linear Acceleration Y')
axs[1].plot(seqs, lin_acc_z, 'b', label='Linear Acceleration Z')
axs[1].set_xlabel('Seq')
axs[1].set_ylabel('Linear Acceleration')
axs[1].set_title('IMU Data: Linear Acceleration')
axs[1].legend()