#维特智能IMU 接入ROS发布IMU数据类型话题

1.准备工作

1.1安装串口功能包

sudo apt-get install ros-melodic-serial

1.2创建功能包

$ cd ~/catkin_ws/src
$ catkin_create_pkg robot_imu std_msgs rospy roscpp serial tf

在~/catkin_ws/src/robot_imu/src/创建imu_com.cpp,编辑程序
在CMakeList.txt中添加

include_directories(include ${catkin_INCLUDE_DIRS})
add_executable(imu_com src/imu_com.cpp src/JY901.cpp)
target_link_libraries(imu_com ${catkin_LIBRARIES})

我使用的是维特智能的HWT901B的IMU
这里要用到他官方C++驱动程序的JY901.cpp和JY901.h文件，后文会给出。

1.3查看串口挂载情况

ls -l /dev/ttyUSB*

如果只有一个USB设备的话，默认情况下是USB0，需要打开串口权限。注意这种方法只是临时的，并不是永久的。

sudo chmod 777 /dev/ttyUSB0

2.读取IMU数据并发布话题

编辑imu_com.cpp文件

#include "ros/ros.h"
#include "std_msgs/String.h"
#include <serial/serial.h>
#include <robot_imu/JY901.h>
# include <sensor_msgs/Imu.h>
#include <sstream>
#include "tf/transform_datatypes.h"
serial::Serial ser; //声明串口对象 //回调函数
void write_callback(const std_msgs::String::ConstPtr& msg)
{ ROS_INFO_STREAM("Writing to serial port" <<msg->data); ser.write(msg->data);   //发送串口数据
}
extern char sum;
int main(int argc, char **argv)
{//初始化节点 ros::init(argc, argv, "serial_imu_node"); //声明节点句柄 ros::NodeHandle nh; //订阅主题，并配置回调函数 ros::Subscriber IMU_write_pub = nh.subscribe("imu_command", 1000, write_callback); //发布主题, 消息格式使用sensor_msg::Imu标准格式（topic名称，队列长度）ros::Publisher IMU_read_pub = nh.advertise<sensor_msgs::Imu>("imu/data_raw", 1000);//打开串口try { //设置串口属性，并打开串口 ser.setPort("/dev/ttyUSB0"); ser.setBaudrate(9600); serial::Timeout to = serial::Timeout::simpleTimeout(1000); ser.setTimeout(to); ser.open(); } catch (serial::IOException& e) { ROS_ERROR_STREAM("Unable to open port "); return -1; } //检测串口是否已经打开，并给出提示信息 if(ser.isOpen()) { ROS_INFO_STREAM("Serial Port initialized"); } else { return -1; } unsigned short  data_size;unsigned char   tmpdata[2000] ;float           roll,pitch,yaw;//消息发布频率ros::Rate loop_rate(100);while (ros::ok()){//处理从串口来的Imu数据//串口缓存字符数if(data_size = ser.available()){ //ser.available(当串口没有缓存时，这个函数会一直等到有缓存才返回字符数ser.read(tmpdata, data_size);JY901.CopeSerialData( tmpdata,data_size);   //JY901 imu 库函数//打包IMU数据sensor_msgs::Imu imu_data;imu_data.header.stamp = ros::Time::now();imu_data.header.frame_id = "imu_link";roll  =  (float)JY901.stcAngle.Angle[0]/32768*180;pitch =  (float)JY901.stcAngle.Angle[1]/32768*180;yaw   =  (float)JY901.stcAngle.Angle[2]/32768*180;//    ROS_INFO("roll:%.3f  pitch:%.3f  yaw:%.3f  \r\n",roll ,pitch,yaw);//打印欧拉角imu_data.orientation.x = (double)JY901.stcQ.Q[1]/32768;imu_data.orientation.y = (double)JY901.stcQ.Q[2]/32768;imu_data.orientation.z = (double)JY901.stcQ.Q[3]/32768;imu_data.orientation.w = (double)JY901.stcQ.Q[0]/32768;//      ROS_INFO("QX:%.3f  QY:%.3f  QZ:%.3f QW:%.3f \r\n\r\n", imu_data.orientation.x,imu_data.orientation.y,imu_data.orientation.z,imu_data.orientation.w);imu_data.angular_velocity.x = (double)JY901.stcGyro.w[0]/32768*2000*3.1415/180;imu_data.angular_velocity.y = (double)JY901.stcGyro.w[1]/32768*2000*3.1415/180;imu_data.angular_velocity.z = (double)JY901.stcGyro.w[2]/32768*2000*3.1415/180;imu_data.linear_acceleration.x = (double)JY901.stcAcc.a[0]/32768*16*9.8;imu_data.linear_acceleration.y = (double)JY901.stcAcc.a[1]/32768*16*9.8;imu_data.linear_acceleration.z = (double)JY901.stcAcc.a[2]/32768*16*9.8;//发布topicIMU_read_pub.publish(imu_data);}//处理ROS的信息，比如订阅消息,并调用回调函数 ros::spinOnce(); loop_rate.sleep(); }return 0;}

官方的JY901.cpp JY901.h稍微做了修改。

#include "robot_imu/JY901.h"
#include "string.h"CJY901 ::CJY901 ()
{}void CJY901 ::CopeSerialData(unsigned char ucData[],unsigned short usLength)
{static unsigned char chrTemp[2000];static unsigned char ucRxCnt = 0;  static unsigned short usRxLength = 0;memcpy(chrTemp,ucData,usLength);usRxLength += usLength;while (usRxLength >= 11){if (chrTemp[0] != 0x55){usRxLength--;memcpy(&chrTemp[0],&chrTemp[1],usRxLength);                        continue;}switch(chrTemp[1]){case 0x50:    memcpy(&stcTime,&chrTemp[2],8);break;case 0x51: memcpy(&stcAcc,&chrTemp[2],8);break;case 0x52:  memcpy(&stcGyro,&chrTemp[2],8);break;case 0x53: memcpy(&stcAngle,&chrTemp[2],8);break;case 0x54:    memcpy(&stcMag,&chrTemp[2],8);break;case 0x55:  memcpy(&stcDStatus,&chrTemp[2],8);break;case 0x56:  memcpy(&stcPress,&chrTemp[2],8);break;case 0x57:    memcpy(&stcLonLat,&chrTemp[2],8);break;case 0x58:   memcpy(&stcGPSV,&chrTemp[2],8);break;case 0x59: memcpy(&stcQ,&chrTemp[2],8);break;}usRxLength -= 11;memcpy(&chrTemp[0],&chrTemp[11],usRxLength);                     }
}
CJY901 JY901 = CJY901();

#ifndef JY901_h
#define JY901_h#define SAVE             0x00
#define CALSW       0x01
#define RSW             0x02
#define RRATE           0x03
#define BAUD            0x04
#define AXOFFSET    0x05
#define AYOFFSET    0x06
#define AZOFFSET    0x07
#define GXOFFSET    0x08
#define GYOFFSET    0x09
#define GZOFFSET    0x0a
#define HXOFFSET    0x0b
#define HYOFFSET    0x0c
#define HZOFFSET    0x0d
#define D0MODE      0x0e
#define D1MODE      0x0f
#define D2MODE      0x10
#define D3MODE      0x11
#define D0PWMH      0x12
#define D1PWMH      0x13
#define D2PWMH      0x14
#define D3PWMH      0x15
#define D0PWMT      0x16
#define D1PWMT      0x17
#define D2PWMT      0x18
#define D3PWMT      0x19
#define IICADDR     0x1a
#define LEDOFF      0x1b
#define GPSBAUD     0x1c#define YYMM                0x30
#define DDHH                0x31
#define MMSS                0x32
#define MS                  0x33
#define AX                  0x34
#define AY                  0x35
#define AZ                  0x36
#define GX                  0x37
#define GY                  0x38
#define GZ                  0x39
#define HX                  0x3a
#define HY                  0x3b
#define HZ                  0x3c
#define Roll                0x3d
#define Pitch               0x3e
#define Yaw                 0x3f
#define TEMP                0x40
#define D0Status        0x41
#define D1Status        0x42
#define D2Status        0x43
#define D3Status        0x44
#define PressureL       0x45
#define PressureH       0x46
#define HeightL         0x47
#define HeightH         0x48
#define LonL                0x49
#define LonH                0x4a
#define LatL                0x4b
#define LatH                0x4c
#define GPSHeight   0x4d
#define GPSYAW      0x4e
#define GPSVL               0x4f
#define GPSVH               0x50#define DIO_MODE_AIN 0
#define DIO_MODE_DIN 1
#define DIO_MODE_DOH 2
#define DIO_MODE_DOL 3
#define DIO_MODE_DOPWM 4
#define DIO_MODE_GPS 5      struct STime
{unsigned char ucYear;unsigned char ucMonth;unsigned char ucDay;unsigned char ucHour;unsigned char ucMinute;unsigned char ucSecond;unsigned short usMiliSecond;
};
struct SAcc
{short a[3];short T;
};
struct SGyro
{short w[3];short T;
};
struct SAngle
{short Angle[3];short T;
};
struct SMag
{short h[3];short T;
};struct SDStatus
{short sDStatus[4];
};struct SPress
{long lPressure;long lAltitude;
};struct SLonLat
{long lLon;long lLat;
};struct SGPSV
{short sGPSHeight;short sGPSYaw;long lGPSVelocity;
};struct SQUA
{short Q[4];
};
class CJY901
{public: struct STime        stcTime;struct SAcc         stcAcc;struct SGyro         stcGyro;struct SAngle       stcAngle;struct SMag        stcMag;struct SDStatus  stcDStatus;struct SPress        stcPress;struct SLonLat         stcLonLat;struct SGPSV      stcGPSV;struct SQUA         stcQ;CJY901 (); void CopeSerialData(unsigned char ucData[],unsigned short usLength);
};
extern CJY901 JY901;
#endif

3.在rviz中显示

运行imu_com.cpp文件，注意要先打开串口权限

rosrun robot_imu imu_com

可以先使用rostopic echo /imu/data_raw查看一下话题内容，可以看到有数据输出。

再打开rviz添加rviz_imu_plugin,可以看见有坐标系出现。（要先 sudo apt-get install ros-melodic-imu-tools 安装imu功能包）

到此结束，有问题欢迎提问。

维特智能IMU 接入ROS发布IMU数据类型话题相关推荐

维特WT931——制作支持ROS的IMU惯性导航传感器
维特WT931--制作支持ROS的IMU惯性导航传感器本来自己想玩一个ros小车进行slam建图和导航,结果发现由于地形原因,导致建图不稳定.这是因为在某些情况下,机器人可能出现轮子在动,本体却不动 ...
ROS2手写接收IMU数据(Imu)代码并发布
目录前言接收IMU数据 IMU的串口连接问题 python接收串口数据 python解析数据 ROS2发布IMU数据可视化IMU数据效果前言在前面测试完了单独用激光雷达建图之后,一直想把 ...
使用ros发布UVC相机和串口IMU数据
1．目的:为了可以标定普通USB相机和固定在相机上的外置IMU的外参,我希望通过ROS获取更高分辨率和更高频率的图像数据,并且可以将图像和imu的topic发布出来,直接使用rosbag record ...
ros --- 录制imu bag包和 imu标定
ros --- base notes ( tf ...) 1. ros 录制imu bag包 2. imu标定创建 launch 文件 1. ros 录制imu bag包 rosbag 指令 ros ...
IMU的ROS调试开发工具包：imu_tools
目录 imu_tool包问题:参数配置便利性问题实例:调试microstrain 3dm-gx5-25 imu 问题:发布的imu姿态与实际imu姿态不一致问题 imu_tool包 http:// ...
ubuntu 18.04 LORD 3DM-GX5-45 IMU ros_mscl ros驱动安装
LORD 3DM-GX5-45 IMU ros_mscl ros驱动安装 drivers 驱动安装安装ros_mscl ROS-MSCL Examples 测试 lord的ros_mscl包的安装网 ...
IMU(LPMS-B2) ROS下使用教程
一.基本信息 http://www.alubi.cn/lpms-b2/ 安装ros教程 http://wiki.ros.org/lpms_imu https://lp-research.com/ros ...
维特智能高精度三轴加速度计ros角度倾角传感器陀螺仪震动HWT9053
1.项目介绍随着我国新能源技术的大力发展,风力发电的装机量也在快速增长.现代大型风力发电机组的轮毂高度一般均超过80米,基础微小的不均匀沉降将引起塔筒倾斜,机舱.轮毂.叶片发生较大的水平偏移.基础严 ...
树莓派pico使用维特智能jy61/jy901/jy61p陀螺仪
材料: 树莓派pico开发板(焊好排针方便插线) jy61或jy61p或jy901 杜邦线4根,分别插rx.tx.vcc.gnd(具体见后文) 一根microusb数据线连接树莓派pico和电脑 ...

维特智能IMU 接入ROS发布IMU数据类型话题