ros接入IMU数据，打包发布topic

1 串口读入IMU数据

1.1 serial工具

IMU接入ros系统可以使用I2C，串口或者其他方式实现，这里考虑使用usb转TTL解决。
参考博客ROS使用官方包进行串口通信

几点说明：

• 在catkin_workspace/src 在创建package

$ cd ~/catkin_ws/src
$ catkin_create_pkg imu_com std_msgs rospy roscpp serial

• 在~/catkin_ws/src/imu_com/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})

• 在~/catkin_ws/在编译

$ catkin_make

1.2 查看usb挂载情况

ls -l /dev/ttyUSB*

修改串口权限

但是一般情况下会出现打不开串口的情况，在确认驱动安装正常的情况下，可能是操作权限不够导致的，赋予权限就好了

 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

2 IMU数据打包发布topic

在imu_com.cpp文件中

#include "ros/ros.h"
#include "std_msgs/String.h"
#include  
#include 
# include 
#include 

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);   //发送串口数据 
} 

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", 1000); 

    //打开串口
    try 
    { 
    //设置串口属性，并打开串口 
        ser.setPort("/dev/ttyUSB0"); 
        ser.setBaudrate(115200); 
        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; 
    } 


    //消息发布频率
	ros::Rate loop_rate(100);
    while (ros::ok()){

		//处理从串口来的Imu数据
		//串口缓存字符数
	     unsigned char  data_size;
        if(data_size = ser.available()){ //ser.available(当串口没有缓存时，这个函数会一直等到有缓存才返回字符数
 
            unsigned char  tmpdata[data_size] ;
            ser.read(tmpdata, data_size);
            for (int i = 0; i< data_size; i++){
                JY901.CopeSerialData( tmpdata[i] );   //JY901 imu 库函数
            }

           //打包IMU数据
            sensor_msgs::Imu imu_data;
            
            imu_data.header.stamp = ros::Time::now();
            imu_data.header.frame_id = "base_link";
            imu_data.orientation.x = (float)JY901.stcAngle.Angle[0]/32768*180;
            imu_data.orientation.y = (float)JY901.stcAngle.Angle[1]/32768*180;
            imu_data.orientation.z = (float)JY901.stcAngle.Angle[1]/32768*180;
       
           imu_data.angular_velocity.x = (float)JY901.stcGyro.w[0]/32768*2000;
           imu_data.angular_velocity.y = (float)JY901.stcGyro.w[0]/32768*2000;
           imu_data.angular_velocity.z = (float)JY901.stcGyro.w[0]/32768*2000;
 
           imu_data.linear_acceleration.x = (float)JY901.stcAcc.a[0]/32768*16;
           imu_data.linear_acceleration.y = (float)JY901.stcAcc.a[1]/32768*16;
           imu_data.linear_acceleration.z = (float)JY901.stcAcc.a[2]/32768*16;
           
 
           //发布topic
           IMU_read_pub.publish(imu_data);
        }
        //处理ROS的信息，比如订阅消息,并调用回调函数 
        ros::spinOnce(); 
        loop_rate.sleep(); 
  }
  	return 0;
 }

/imu_data　通过　rviz 显示

$ cd ~/catkin_ws
$ source devel/setup.bash
$ rosrun rviz rviz 

最后在rviz 界面将Global OPtions 下面的Fixed Frame 修改base_link
在下面的add添加话题/imu_data 即可显示