ROS 新建包 陀螺仪

陀螺仪试用：

执行：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) 串口读写。
原文链接：https://blog.csdn.net/morixinguan/article/details/80898172



命令行发布消息：
格式：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

模拟测试完成，确实收到消息，如果发的数字是其他，则收不到消息，消息订阅测试完成。
继续上机测试。