陀螺仪试用：
执行：sudo chmod 777 /dev/ttyUSB0
执行：./serial
就可以看到数据了。
1、创建
cd ~/workspace/catkin_ws/src
#创建功能包
catkin_create_pkg mytest std_msgs rospy roscpp
2、编译(catkin_make)
3、编辑或复制源码
4、修改下面两个文件
1）修改CMakeLists.txt（发现包）
2）修改配置（catkin_package）
3）增加可执行文件
取消“include”的注释，可以将头文件放在include目录了。
5、修改package.xml
1）原文件
2）修改后
5）编译成功
6）执行
source ~/.bashrc
rosrun imu_wt901c_node imu_wt901c_node
6、增加usb驱动
1）查看U口设备
2）增加规则
3）重启系统，修改源码U口设置
4）串口打开成功
5）原始数据说明
6）数据说明
6）环境建成，开始编程。查看原始数据（加速度、角速度，角度欧拉角、角度四元数）
7) 串口读写。
原文链接：
命令行发布消息：
格式：rostopic pub 话题名 消息类型名 消息内容 -r [频率]
例如：rostopic pub /speak_string std_msgs/String “hello” -r 1
含义：在/speak_string上发布一条字符串消息，消息内容为"hello"，频率为1Hz。
rostopic pub /turtle1/cmd_vel geometry_msgs/Twist -r 1 – ‘[2.0, 0.0, 0.0]’ ‘[0.0, 0.0, 1.8]’
rostopic pub 这条命令将会发布消息到某个给定的话题
-1 (单个破折号）这个参数选项使rostopic发布一条消息后马上退出
/turtle1/cmd_vel 这是消息所发布到的话题名称
geometry_msgs/Twist 这是所发布消息的类型
–（双破折号）这会告诉命令选项解析器接下来的参数部分都不是命令选项。这在参数里面包含有破折号 -（比如负号）时是必须要添加的
修改程序增加如下：订阅Imu复位消息
插入文件：include
int fd;//ttyusb0
void callback_imu_reset(const std_msgs::Int16& msg);
void reset_imu(int fd);
void initinal_imu(int fd);
ros::Subscriber Imu_rst;
插入文件：执行
#include <std_msgs/Int16.h>
插入文件：imu.c
void callback_imu_reset(const std_msgs::Int16& msg)
{
if(msg.data == 99){
ROS_INFO("RST topic cmd = %d" , msg.data);
initinal_imu(fd);
}
else{
ROS_INFO("Imu Reset code error = %d" , msg.data);
}
}
void reset_imu(int fd)
{
int len;
char buf[] = "AT+RST";
len = write(fd, buf, sizeof(buf));
if (len < 0) {
ROS_INFO("Write usart data error!");
}
else{
ROS_INFO("Write usart data success!");
}
}
void initinal_imu(int fd)
{
int16_t cnt_ok = 0;
int16_t cnt_error = 0;
int16_t i = 0;
bool rst_ok_flag = 0;
int16_t rst_m = 0;
ros::Duration(0.25).sleep();reset_imu(fd);ros::Duration(0.5).sleep();
while(rst_ok_flag == 0){
getImuData(fd);
switch(rst_m){
case 0:if(RecEular[2] <= 0.5){
if(++cnt_ok >= 10){
rst_ok_flag = 1;
ROS_INFO("RST IMU SUCCESS!");
ROS_INFO("Reset ok! Eular(P R Y):%0.2f %0.2f %0.2f",RecEular[0], RecEular[1], RecEular[2]);
}
}
else{
cnt_ok = 0;
rst_m = 1;
}
break;
case 1:reset_imu(fd);
ros::Duration(0.5).sleep();
if(++cnt_error > 3){
rst_ok_flag = 2;
ROS_INFO("RST IMU Error!");
printf("Reset fail! Eular(P R Y):%0.2f %0.2f %0.2f\r\n",RecEular[0], RecEular[1], RecEular[2]);
}
else{
rst_m = 0;
}
default:break;
}
ros::Duration(0.1).sleep();
}
}
//初始化节点
ros::init(argc, argv, "Imu_hi216_node");
//声明节点句柄
ros::NodeHandle nh;
//订阅主题，并配置回调函数
IMU_pub = nh.advertise<sensor_msgs::Imu>("imu_hi216", 20);
Imu_rst = nh.subscribe("Imu_reset", 1000, callback_imu_reset);
fd=open_port("/dev/ttyUSB0");
imu_data_decode_init();
initinal_imu(fd);
//指定循环的频率5、测试：发布复位消息
rostopic pub -1 /Imu_reset std_msgs/Int16 “99”
rosrun imu_hi216_node imu_hi216_node
模拟测试完成，确实收到消息，如果发的数字是其他，则收不到消息，消息订阅测试完成。
继续上机测试。
