#维特智能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::