目錄
重新編譯：catkin_make, 然後 source ~/.bashrc
1 前言
imu_filter_madgwick：一種濾波器，可將來自常規IMU設備的角速度，加速度和磁力計讀數（可選）融合到一個方向中。基於工作：http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/
imu_complementary_filter：一種濾波器，它使用一種基於互補融合的新穎方法，將來自通用IMU設備的角速度，加速度和磁力計讀數（可選）融合到方向四元數中。基於文獻：http://www.mdpi.com/1424-8220/15/8/19302
rviz_imu_plugin：rviz插件，可顯示sensor_msgs::Imu消息。
ROS org imu_tools官方介紹地址：http://wiki.ros.org/imu_tools
github源代碼地址：https://github.com/ccny-ros-pkg/imu_tools
2. 安裝
From source files
Create a catkin workspace (e.g., ~/ros-hydro-ws/) and source the devel/setup.bash file.
創建一個 catkin 工作區(例如 ~ / ros-hydro-ws /) ，併爲 devel / setup. bash 文件提供源代碼。
Make sure you have git installed:
確保你已經安裝了 git:
sudo apt-get install git-core
Download the stack from our repository into your catkin workspace (e.g., ros-hydro-ws/src; use the proper branch for your distro, e.g., groovy, hydro...):
從我們的存儲庫中下載堆棧到您的 catkin 工作區(例如，ros-hydro-ws / src; 對您的發行版本使用適當的分支，例如，groovy，hydro...) :
git clone -b <distro> https://github.com/ccny-ros-pkg/imu_tools.git
Install any dependencies using rosdep.
使用 rosdep 安裝任何依賴項。
rosdep install imu_tools
Compile the stack:
編譯堆棧:
cd ~/ros-hydro-ws
catkin_make
3. IMU原始數據測試
3.1 文件系統
./src/imu_read.cpp
/*
HI219出廠默認輸出協議接收:
輸出 sum = 41
0x5A+0xA5+LEN_LOW+LEN_HIGH+CRC_LOW+CRC_HIGH+ 0x90+ID(1字節) + 0xA0+Acc(加速度6字節) + 0xB0+Gyo(角速度6字節) + 0xC0+Mag(地磁6字節) + 0xD0 +AtdE(歐拉角6字節) + 0xF0+Pressure(壓力4字節)
*/
#include "ros/ros.h"
#include "sensor_msgs/Imu.h"
#include "tf/transform_broadcaster.h"
#include <iostream>
#include <boost/asio.hpp>
#include <boost/bind.hpp>
#include <string>
using namespace std;
using namespace boost::asio;
#define MAX_PACKET_LEN (41)// length of the data
typedef enum
{
kItemID = 0x90, /* user programed ID size: 1 */
kItemIPAdress = 0x92, /* ip address size: 4 */
kItemAccRaw = 0xA0, /* raw acc size: 3x2 */
kItemAccRawFiltered = 0xA1,
kItemAccDynamic = 0xA2,
kItemGyoRaw = 0xB0, /* raw gyro size: 3x2 */
kItemGyoRawFiltered = 0xB1,
kItemMagRaw = 0xC0, /* raw mag size: 3x2 */
kItemMagRawFiltered = 0xC1,
kItemAtdE = 0xD0, /* eular angle size:3x2 */
kItemAtdQ = 0xD1, /* att q, size:4x4 */
kItemTemp = 0xE0,
kItemPressure = 0xF0, /* pressure size:1x4 */
kItemEnd = 0xFF,
}ItemID_t;
uint8_t ID;
int16_t AccRaw[3];
int16_t GyoRaw[3];
int16_t MagRaw[3];
float Eular[3];
int32_t Pressure;
int main(int argc, char **argv)
{
ros::init(argc, argv, "imu_read_node");
ros::NodeHandle n;
//發佈主題, 消息格式使用sensor_msg::Imu標準格式（topic名稱，隊列長度）
ros::Publisher imu_pub = n.advertise<sensor_msgs::Imu>("imu_raw", 1000);
//parameters
string com_port = "/dev/ttyUSB0";
string imu_frame_id = "imu_link";
ros::param::get("~com_port",com_port);
ros::param::get("~imu_frame_id",imu_frame_id);
io_service iosev;
serial_port sp(iosev, com_port);
sp.set_option(serial_port::baud_rate(115200));
sp.set_option(serial_port::flow_control(serial_port::flow_control::none));
sp.set_option(serial_port::parity(serial_port::parity::none));
sp.set_option(serial_port::stop_bits(serial_port::stop_bits::one));
sp.set_option(serial_port::character_size(8));
int count = 0;
ros::Rate loop_rate(100);
while (ros::ok())
{
// 向串口寫數據
// write(sp, buffer("Hello world", 12));
// 向串口讀數據
uint8_t buf_tmp[1];
uint8_t buf[MAX_PACKET_LEN-1];
read(sp, buffer(buf_tmp));
if(buf_tmp[0] == 0x5A )
{
read(sp, buffer(buf));
sensor_msgs::Imu imu_msg;
imu_msg.header.stamp = ros::Time::now();
imu_msg.header.seq = count;
imu_msg.header.frame_id = imu_frame_id;
/*
按出廠默認輸出協議接收:
0x5A+0xA5+LEN_LOW+LEN_HIGH+CRC_LOW+CRC_HIGH+ 0x90+ID(1字節) + 0xA0+Acc(加速度6字節) + 0xB0+Gyo(角速度6字節) + 0xC0+Mag(地磁6字節) + 0xD0 +AtdE(歐拉角6字節) + 0xF0+Pressure(壓力4字節)
*/
int i=0;
if(buf[i] == 0xA5) /* user ID */
{
i+=5;//moving right 5bit to 0x90
//user ID
if(buf[i+0] == kItemID)
{
ID = buf[i+1];
}
//Acc value
if(buf[i+2] == kItemAccRaw)
{
memcpy(AccRaw, &buf[i+3], 6);
imu_msg.linear_acceleration.x =AccRaw[0]* 9.7887/1000.0;
imu_msg.linear_acceleration.y =AccRaw[1]* 9.7887/1000.0;
imu_msg.linear_acceleration.z =AccRaw[2]* 9.7887/1000.0;
}
//Gyro value
if(buf[i+9] == kItemGyoRaw)
{
memcpy(GyoRaw, &buf[i+10], 6);
imu_msg.angular_velocity.x = GyoRaw[0]*M_PI/10.0/180.0;
imu_msg.angular_velocity.y = GyoRaw[1]*M_PI/10.0/180.0;
imu_msg.angular_velocity.z = GyoRaw[2]*M_PI/10.0/180.0;
}
//Mag value
if(buf[i+16] == kItemMagRaw)
{
memcpy(MagRaw, &buf[i+17], 6);
}
//atd E
if(buf[i+23] == kItemAtdE)
{
Eular[0] = ((float)(int16_t)(buf[i+24] + (buf[i+25]<<8)))/100;
Eular[1] = ((float)(int16_t)(buf[i+26] + (buf[i+27]<<8)))/100;
Eular[2] = ((float)(int16_t)(buf[i+28] + (buf[i+29]<<8)))/10;
geometry_msgs::Quaternion quat = tf::createQuaternionMsgFromRollPitchYaw(Eular[0], Eular[1], Eular[2]);
imu_msg.orientation = quat;
//imu_msg.linear_acceleration_covariance=boost::array<double, 9>
}
//Pressure value
if(buf[i+30] == kItemPressure)
{
memcpy(&Pressure, &buf[i+31], 4);
}
//debug
/*
printf("ID: %d \r\n", ID);
printf("AccRaw: %d %d %d\r\n", AccRaw[0], AccRaw[1], AccRaw[2]);
printf("GyoRaw: %f %f %f\r\n", GyoRaw[0], GyoRaw[1], GyoRaw[2]);
printf("MagRaw: %d %d %d\r\n", MagRaw[0], MagRaw[1], MagRaw[2]);
printf("Eular: %0.2f %0.2f %0.2f\r\n", Eular[1], Eular[0], Eular[2]);
printf("Pressure: %d Pa\r\n\n", Pressure);
*/
imu_pub.publish(imu_msg);
//ros::spinOnce();
//loop_rate.sleep();
count++;
}
}
}//while end
iosev.run();
return 0;
}
./launch/imu.launch文件
<launch>
<node name="imu_read_node" pkg="miiboo_imu" type="imu_read" output="screen">
<param name="com_port" value="/dev/ttyUSB0"/>
<param name="imu_frame_id" value="imu_link"/>
</node>
</launch>
注：遇到串口權限問題，請安裝此步驟解決
一般情況下會出現打不開串口的情況，在確認驅動安裝正常的情況下，可能是操作權限不夠導致的，賦予權限就好了
sudo chmod a+rw /dev/ttyUSB0
但是這種方式只是臨時的，每次啓動都需輸入一次，永久性解決辦法：
可以通過增加udev規則來實現：
/etc/udev/rules.d/70-ttyusb.rules
創建文件
sudo gedit /etc/udev/rules.d/70-ttyusb.rules
文件內容爲：
KERNEL=="ttyUSB[0-9]*", MODE="0666
增加訪問權限：
sudo chmod a+rw /dev/ttyUSB0
————————————————
3.2 運行imu_read_node
運行節點launch文件
roslaunch miiboo_imu imu.launch
查看節點和話題信息
查看原始數據：
rostopic echo /imu_data
3.3 打開rviz查看原始的imu數據
修改Fixed Frame選項：
1. 
（注意，座標系應該與你定義的imu節點中的link一致。）
添加IMU數據：
2. 點擊左下方的Add按鈕，往下翻找到rviz_imu_plugin插件中的imu選項
3. 修改imu訂閱的話題：根據你定義的imu發佈的sensor msg topic名稱，選擇對應的話題即可
4. 可視化，在實際動畫中可以明顯的觀察到imu的飄逸和晃動，切位姿不準確。
4. imu_tools濾波及可視化
4.1 修改imu_tools文件
打開文件：~/imu_tools_ws/src/imu_tools/imu_complementary_filter/src/complementary_filter_ros.cpp，有如下代碼：
// Register IMU raw data subscriber.
imu_subscriber_.reset(new ImuSubscriber(nh_, ros::names::resolve("imu") + "/data_raw", queue_size));
可以看出，imu_tools訂閱的topic爲imu/data_raw，而IMU發佈的topic爲/imu_data，因此需要修改代碼，使topic一致：
將imu訂閱的話題改爲自定義的話題名稱即可：
// Register IMU raw data subscriber.
imu_subscriber_.reset(new ImuSubscriber(nh_, "/imu_data", queue_size));
然後，修改launch文件
打開launch文件：~/imu_tools_ws/src/imu_tools/imu_complementary_filter/launch/complementary_filter.launch，進行一些修改：
前半部分已省略
重點修改以下部分的內容
<!-- ComplementaryFilter launch file -->
<launch>
#### Complementary filter
<node pkg="imu_complementary_filter" type="complementary_filter_node"
name="complementary_filter_gain_node" output="screen">
<param name="do_bias_estimation" value="true"/>
<param name="do_adaptive_gain" value="true"/>
<param name="use_mag" value="false"/>
<param name="gain_acc" value="0.01"/>
<param name="gain_mag" value="0.01"/>
<param name="publish_debug_topics" value="false"/>
<param name="publish_tf" value="true"/>
</node>
</launch>
重新編譯：catkin_make, 然後 source ~/.bashrc
4.2 測試
1. 先開一個終端，打開imu讀取節點，具體命令根據不同package節點的命名自行修改，
roslaunch miiboo_imu imu.launch
2. 再開一個終端運行imu_tools中的launch文件，若正常運行不報錯則已經開始了濾波操作
roslaunch imu_complementary_filter complementary_filter.launch
3. 再開一個終端打開rviz，並把imu接收到topic改爲imu_tools濾波後發佈的消息
再觀察右邊可視化窗口
對比濾波前的imu姿態數據，爲了形成對照，imu設備的姿態在此過程中均未發生移動，左圖爲原始位姿，右圖爲濾波之後位姿。較爲準確的對imu的姿態濾波和估計。
動畫演示：中間突然報錯退出了rviz，不知怎麼回事。
參考：https://github.com/ccny-ros-pkg/imu_tools
https://www.cnblogs.com/21207-iHome/p/7832355.html