优化串口读取imu代码

src/rasp_imu_hat_6dof/rasp_imu_hat_6dof/nodes/imu_node.py

@@ -74,7 +74,7 @@ accel_factor = 9.806/256.0
 myIMU = MyIMU(0x50)
 rate = rospy.Rate(pub_hz)
 
-rospy.loginfo("Rasp IMU Module is woking ...")
+rospy.loginfo("IMU Module is working ...")
 while not rospy.is_shutdown():
     myIMU.get_YPRAG()
 

src/rasp_imu_hat_6dof/serial_imu_hat_6dof/CMakeLists.txt

@@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 2.8.3)
 project(serial_imu_hat_6dof)
 
 ## Compile as C++11, supported in ROS Kinetic and newer
-# add_compile_options(-std=c++11)
+add_compile_options(-std=c++11)
 
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)

src/rasp_imu_hat_6dof/serial_imu_hat_6dof/cfg/param.yaml

@@ -1,10 +1,10 @@
-###############################################
+################################################
 # Copyright: 2016-2020 www.corvin.cn ROS小课堂
 # Description:使用串口进行通信时的配置信息.
 # Author: corvin
 # History:
 #   20200402: init this file.
-###############################################
+################################################
 imu_dev: /dev/ttyUSB0
 baud_rate: 9600
 data_bits: 8
@@ -15,4 +15,3 @@ pub_data_topic: imu_data
 pub_temp_topic: imu_temp
 link_name: base_imu_link
 pub_hz: 10
-

src/rasp_imu_hat_6dof/serial_imu_hat_6dof/launch/serial_imu_hat.launch

@@ -1,5 +1,5 @@
 <!--
-  Copyright: 2016-2019 https://www.corvin.cn ROS小课堂
+  Copyright: 2016-2020 https://www.corvin.cn ROS小课堂
   Description:使用串口来读取imu模块的数据,并在ros中发布使用话题发布出来.
   Author: corvin
   History:

src/rasp_imu_hat_6dof/serial_imu_hat_6dof/src/proc_serial_data.cpp

@@ -1,10 +1,10 @@
-/*************************************************************************
+/*****************************************************************
  * Copyright:2016-2020 www.corvin.cn ROS小课堂
  * Description:使用串口方式读取IMU模块信息.
  * Author: corvin
  * History:
  *   20200401:init this file.
- *************************************************************************/
+******************************************************************/
 #include<ros/ros.h>
 #include<stdio.h>
 #include<stdlib.h>
@@ -24,7 +24,7 @@ float acce[3],angular[3],angle[3],quater[4];
  * Description:打开串口,输入各参数即可.
  *********************************************/
 int openSerialPort(const char *path, int baud, int dataBits,
-   const char* parity, int stopBit)
+                const char* parity, int stopBit)
 {
     int fd = 0;
     struct termios terminfo;
@@ -179,7 +179,6 @@ int recv_data(int fd, char* recv_buffer, int len)
     return length;
 }
 
-
 /************************************************************
  * Description:根据串口数据协议来解析有效数据,
  ***********************************************************/
@@ -324,4 +323,3 @@ int getImuData()
 
     return 0;
 }
-

src/rasp_imu_hat_6dof/serial_imu_hat_6dof/src/serial_imu_node.cpp

@@ -1,3 +1,11 @@
+/**************************************************
+ * Copyright:2016-2020 www.corvin.cn ROS小课堂
+ * Description:使用串口来读取IMU模块的数据,并通过
+ *   topic将数据发布出来.
+ * Author: corvin
+ * History:
+ *   20200403: init this file.
+ **************************************************/
 #include <ros/ros.h>
 #include <tf/tf.h>
 #include "imu_data.h"
@@ -17,7 +25,7 @@ int main(int argc, char **argv)
     std::string link_name;
     std::string imu_topic;
     std::string temp_topic;
-    int pub_hz;
+    int pub_hz = 0;
     float degree2Rad = 3.1415926/180.0;
     float acc_factor = 9.806/256.0;
 
@@ -39,10 +47,12 @@ int main(int argc, char **argv)
     if(ret < 0)
     {
         ROS_ERROR("init serial port error !");
+        closeSerialPort();
         return -1;
     }
+    ROS_INFO("IMU module is working... ");
 
-    ros::Publisher imu_pub = handle.advertise<sensor_msgs::Imu>(imu_topic, 2);
+    ros::Publisher imu_pub = handle.advertise<sensor_msgs::Imu>(imu_topic, 5);
     ros::Rate loop_rate(pub_hz);
 
     sensor_msgs::Imu imu_msg;
@@ -90,7 +100,6 @@ int main(int argc, char **argv)
     }
 
     closeSerialPort();
-
     return 0;
 }