提交串口读取代码,更新ros中imu数据输出

rasp_imu_hat_6dof/CMakeLists.txt

@@ -3,36 +3,30 @@ project(rasp_imu_hat_6dof)
 
 find_package(catkin REQUIRED COMPONENTS 
     rospy
-    roscpp
-    sensor_msgs
+    std_msgs
     dynamic_reconfigure
-    cmake_modules
-    )
+    message_generation)
 
-find_package(Eigen REQUIRED)
-
-include_directories(
-    include
-    ${catkin_INCLUDE_DIRS}
-    ${Eigen_INCLUDE_DIRS}
-    )
+add_service_files(
+    FILES
+    GetYawData.srv
+)
 
-add_definitions(${EIGEN_DEFINITIONS})
+generate_messages(
+    DEPENDENCIES
+    std_msgs
+)
 
 ## Generate dynamic reconfigure parameters in the 'cfg' folder
 generate_dynamic_reconfigure_options(
     cfg/imu.cfg
 )
-catkin_package(
-    INCLUDE_DIRS include
-    CATKIN_DEPENDS roscpp
-    )
 
-add_executable(imu_pub_node src/imu_data.cpp src/imu_node.cpp src/iic_dev.cpp)
-target_link_libraries(imu_pub_node
-    ${catkin_LIBRARIES}
-    libbcm2835.a
-    )
+catkin_package(
+    CATKIN_DEPENDS
+    std_msgs
+    message_runtime
+)
 
 include_directories(
     ${catkin_INCLUDE_DIRS}
@@ -54,7 +48,8 @@ install(DIRECTORY src
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
 )
 
-catkin_install_python(PROGRAMS 
+catkin_install_python(PROGRAMS
 	nodes/imu_node.py
     DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/nodes
 )
+

rasp_imu_hat_6dof/cfg/param.yaml

@@ -1,4 +1,15 @@
+##################################################
+# Copyright: 2016-2020 www.corvin.cn ROS小课堂
+# Description:使用IIC总线来读取IMU模块的数据,并
+#   通过话题将imu数据发送出来.这里是可以配置的
+#   一些参数.
+# Author: corvin
+# History:
+#   20200406:init this file.
+#   20200410:更新默认发布imu话题数据频率为20hz.
+###################################################
 pub_data_topic: imu_data
 pub_temp_topic: imu_temp
+yaw_topic: yaw_data
 link_name: base_imu_link
-pub_hz: 10
+pub_hz: 20

rasp_imu_hat_6dof/include/iic_dev.h

@@ -1,48 +0,0 @@
-#ifndef _I2CDEV_H_
-#define _I2CDEV_H_
-
-#ifndef __cplusplus
-#error A C++ compiler is required!
-#endif
-
-#include <bcm2835.h>
-#include <math.h> // required for BMP180
-#include <stdlib.h> // required for MPU6060
-#include <string.h> // required for MPU6060
-
-
-#define set_I2C_pins  false
-/* used to boolean for setting RPi I2C pins P1-03 (SDA) and P1-05 (SCL) to alternate function ALT0, which enables those pins for I2C interface.
-   setI2Cpin should be false, if the I2C are already configured in alt mode ... */
-
-#define i2c_baudrate 100000  //100 kHz
-//uint32_t i2c_baudrate = 400000 ; //400 kHz,
-
-class I2Cdev {
- public:
-        I2Cdev();
-
-        static void initialize();
-        static void enable(bool isEnabled);
-
-        static int8_t readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data);
-        //TODO static int8_t readBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t *data);
-        static int8_t readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data);
-        //TODO static int8_t readBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t *data);
-        static int8_t readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data);
-        static int8_t readWord(uint8_t devAddr, uint8_t regAddr, uint16_t *data);
-        static int8_t readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data);
-        static int8_t readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data);
-
-        static bool writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data);
-        //TODO static bool writeBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t data);
-        static bool writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data);
-        //TODO static bool writeBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t data);
-        static bool writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data);
-        static bool writeWord(uint8_t devAddr, uint8_t regAddr, uint16_t data);
-        static bool writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data);
-        static bool writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data);
-};
-
-#endif /* _I2CDEV_H_ */
-

rasp_imu_hat_6dof/include/imu_data.h

@@ -1,50 +0,0 @@
-#ifndef _IMU_DATA_H_
-#define _IMU_DATA_H_
-
-#define I2C_DEV_ADDR  0x50
-
-#define ACC_X_REG  0x34
-#define ACC_Y_REG  0x35
-#define ACC_Z_REG  0x36
-
-#define GYO_X_REG  0x37
-#define GYO_Y_REG  0x38
-#define GYO_Z_REG  0x39
-
-#define ROLL_REG   0x3d
-#define PITCH_REG  0x3e
-#define YAW_REG    0x3f
-
-class myIMU
-{
-    public:
-        int initDev();
-
-        float getRollData();
-        float getPitchData();
-        float getYawData();
-
-        float getAccXData();
-        float getAccYData();
-        float getAccZData();
-
-        float getGyoXData();
-        float getGyoYData();
-        float getGyoZData();
-
-    private:
-        unsigned char accX[2];
-        unsigned char accY[2];
-        unsigned char accZ[2];
-
-        unsigned char gyoX[2];
-        unsigned char gyoY[2];
-        unsigned char gyoZ[2];
-
-        unsigned char roll[2];
-        unsigned char pitch[2];
-        unsigned char yaw[2];
-};
-
-#endif
-

rasp_imu_hat_6dof/launch/imu_data_pub.launch

@@ -10,11 +10,8 @@
 <launch>
   <arg name="imu_cfg_file" default="$(find rasp_imu_hat_6dof)/cfg/param.yaml"/>
 
-  <!---
   <node pkg="rasp_imu_hat_6dof" type="imu_node.py" name="imu_node" output="screen">
       <rosparam file="$(arg imu_cfg_file)" command="load"/>
   </node>
-  -->
-  <node pkg="rasp_imu_hat_6dof" type="imu_pub_node" name="imu_pub_node" output="screen" />
 </launch>
 

rasp_imu_hat_6dof/nodes/imu_data.py

@@ -5,12 +5,13 @@
 # Author: corvin
 # Description: 为树莓派IMU扩展板所使用的配套代码，由于默认
 #    扩展板与树莓派使用IIC连接。所以这里的代码是直接从IIC接口
-#    中读取IMU模块的三轴加速度、角度、四元数。
+#    中读取IMU模块的三轴加速度、三轴角速度、四元数。
 # History:
 #    20191031: Initial this file.
-#    20191209： Add get imu chip temperature function-get_temp().
-
+#    20191209：Add get imu chip temperature function-get_temp().
+#    20200702: 读取各数据后需要使用np.short转换才能进行后续数据处理.
 import smbus
+import rospy
 import numpy as np
 
 class MyIMU(object):
@@ -27,6 +28,9 @@ class MyIMU(object):
             ax_tmp  = self.i2c.read_i2c_block_data(self.addr, 0x34, 2)
             ay_tmp  = self.i2c.read_i2c_block_data(self.addr, 0x35, 2)
             az_tmp  = self.i2c.read_i2c_block_data(self.addr, 0x36, 2)
+            #rospy.loginfo("Read i2c accX:" + str(ax_tmp))
+            #rospy.loginfo("Read i2c accY:" + str(ay_tmp))
+            #rospy.loginfo("Read i2c accZ:" + str(az_tmp))
 
             gx_tmp  = self.i2c.read_i2c_block_data(self.addr, 0x37, 2)
             gy_tmp  = self.i2c.read_i2c_block_data(self.addr, 0x38, 2)
@@ -38,13 +42,16 @@ class MyIMU(object):
             self.raw_pitch = float(((pitch_tmp[1]<<8)|pitch_tmp[0])/32768.0*180.0)
             self.raw_yaw  = (yaw_tmp[1] << 8 | yaw_tmp[0])/32768.0*180
 
-            self.raw_ax  = float(((ax_tmp[1]<<8)|ax_tmp[0])/32768.0*16.0)
-            self.raw_ay  = float(((ay_tmp[1]<<8)|ay_tmp[0])/32768.0*16.0)
-            self.raw_az  = float(((az_tmp[1]<<8)|az_tmp[0])/32768.0*16.0)
+            self.raw_ax = float(np.short(np.short(ax_tmp[1]<<8)|ax_tmp[0])/32768.0*16.0)
+            self.raw_ay = float(np.short(np.short(ay_tmp[1]<<8)|ay_tmp[0])/32768.0*16.0)
+            self.raw_az = float(np.short(np.short(az_tmp[1]<<8)|az_tmp[0])/32768.0*16.0)
+            #rospy.loginfo("Read raw accX:" + str(self.raw_ax))
+            #rospy.loginfo("Read raw accY:" + str(self.raw_ay))
+            #rospy.loginfo("Read raw accZ:" + str(self.raw_az))
 
-            self.raw_gx  = float(((gx_tmp[1]<<8)|gx_tmp[0])/32768.0*2000.0)
-            self.raw_gy  = float(((gy_tmp[1]<<8)|gy_tmp[0])/32768.0*2000.0)
-            self.raw_gz  = float(((gz_tmp[1]<<8)|gz_tmp[0])/32768.0*2000.0)
+            self.raw_gx = float(np.short(np.short(gx_tmp[1]<<8)|gx_tmp[0])/32768.0*2000.0)
+            self.raw_gy = float(np.short(np.short(gy_tmp[1]<<8)|gy_tmp[0])/32768.0*2000.0)
+            self.raw_gz = float(np.short(np.short(gz_tmp[1]<<8)|gz_tmp[0])/32768.0*2000.0)
 
     def get_quatern(self):
         try:

rasp_imu_hat_6dof/nodes/imu_node.py

@@ -1,7 +1,7 @@
 #!/usr/bin/env python
 # -*- coding: UTF-8 -*-
 
-# Copyright: 2016-2019 https://www.corvin.cn ROS小课堂
+# Copyright: 2016-2020 https://www.corvin.cn ROS小课堂
 # Author: corvin
 # Description: 为树莓派IMU扩展板所使用的配套代码，由于默认
 #    扩展板与树莓派使用IIC连接。所以这里的代码是直接从IIC接口
@@ -9,15 +9,15 @@
 #    中的IMU消息格式，发布到/imu话题中，这样有需要的节点可以
 #    直接订阅该话题即可获取到imu扩展板当前的数据。
 # History:
-#    20191031: Initial this file.
-#    20191209: 新增发布IMU芯片温度的话题，发布频率与发布imu数据频率相同.
-
+#    20191031:Initial this file.
+#    20191209:新增发布IMU芯片温度的话题，发布频率与发布imu数据频率相同.
+#    20200406:增加可以直接发布yaw数据的话题.
+#    20200410:增加通过service方式发布yaw数据,方便其他节点调用.
+#    20200703:修改三轴线性加速度的方向,沿着各轴正方向数据为正,否则加速度就为负值,单位m/s2.
 import rospy
-import string
 import math
-import time
-import sys
 
+from rasp_imu_hat_6dof.srv import GetYawData,GetYawDataResponse
 from tf.transformations import quaternion_from_euler
 from dynamic_reconfigure.server import Server
 from rasp_imu_hat_6dof.cfg import imuConfig
@@ -28,12 +28,17 @@ from imu_data import MyIMU
 
 degrees2rad = math.pi/180.0
 imu_yaw_calibration = 0.0
+yaw = 0.0
+
+# ros server return Yaw data
+def return_yaw_data(req):
+    return GetYawDataResponse(yaw)
 
 # Callback for dynamic reconfigure requests
 def reconfig_callback(config, level):
     global imu_yaw_calibration
     imu_yaw_calibration = config['yaw_calibration']
-    rospy.loginfo("Set imu_yaw_calibration to %d" % (imu_yaw_calibration))
+    rospy.loginfo("Set imu_yaw_calibration to %f" % (imu_yaw_calibration))
     return config
 
 rospy.init_node("imu_node")
@@ -42,12 +47,17 @@ rospy.init_node("imu_node")
 data_topic_name = rospy.get_param('~pub_data_topic', 'imu_data')
 temp_topic_name = rospy.get_param('~pub_temp_topic', 'imu_temp')
 link_name  = rospy.get_param('~link_name', 'base_imu_link')
-pub_hz = rospy.get_param('~pub_hz', '10')
+pub_hz = rospy.get_param('~pub_hz', '20')
+yaw_topic_name = rospy.get_param('~yaw_topic', 'yaw_topic')
 
 data_pub = rospy.Publisher(data_topic_name, Imu, queue_size=1)
 temp_pub = rospy.Publisher(temp_topic_name, Float32, queue_size=1)
+yaw_pub = rospy.Publisher(yaw_topic_name, Float32, queue_size=1)
 srv = Server(imuConfig, reconfig_callback)  # define dynamic_reconfigure callback
+yaw_srv = rospy.Service('imu_node/GetYawData', GetYawData, return_yaw_data)
+
 imuMsg = Imu()
+yawMsg = Float32()
 
 # Orientation covariance estimation
 imuMsg.orientation_covariance = [
@@ -71,16 +81,18 @@ imuMsg.linear_acceleration_covariance = [
 ]
 
 seq=0
-# sensor reports accel as 256.0 = 1G (9.8m/s^2). Convert to m/s^2.
-accel_factor = 9.806/256.0
+# sensor accel g convert to m/s^2.
+accel_factor = 9.806
 
 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()
 
+    #rospy.loginfo("yaw:%f pitch:%f  roll:%f", myIMU.raw_yaw,
+    #              myIMU.raw_pitch, myIMU.raw_roll)
     yaw_deg = float(myIMU.raw_yaw)
     yaw_deg = yaw_deg + imu_yaw_calibration
     if yaw_deg >= 180.0:
@@ -90,12 +102,16 @@ while not rospy.is_shutdown():
     yaw = yaw_deg*degrees2rad
     pitch = float(myIMU.raw_pitch)*degrees2rad
     roll  = float(myIMU.raw_roll)*degrees2rad
-    #rospy.loginfo("yaw:%f pitch:%f  rool:%f", yaw, pitch, roll)
+
+    yawMsg.data = yaw
+    yaw_pub.publish(yawMsg)
 
     # Publish imu message
-    imuMsg.linear_acceleration.x = float(myIMU.raw_ax)*accel_factor
-    imuMsg.linear_acceleration.y = float(myIMU.raw_ay)*accel_factor
-    imuMsg.linear_acceleration.z = float(myIMU.raw_az)*accel_factor
+    #rospy.loginfo("acc_x:%f acc_y:%f acc_z:%f", myIMU.raw_ax,
+    #              myIMU.raw_ay, myIMU.raw_az)
+    imuMsg.linear_acceleration.x = -float(myIMU.raw_ax)*accel_factor
+    imuMsg.linear_acceleration.y = -float(myIMU.raw_ay)*accel_factor
+    imuMsg.linear_acceleration.z = -float(myIMU.raw_az)*accel_factor
 
     imuMsg.angular_velocity.x = float(myIMU.raw_gx)*degrees2rad
     imuMsg.angular_velocity.y = float(myIMU.raw_gy)*degrees2rad

rasp_imu_hat_6dof/package.xml

@@ -15,13 +15,15 @@
 
   <buildtool_depend>catkin</buildtool_depend>
 
+  <build_depend>std_msgs</build_depend>
   <build_depend>dynamic_reconfigure</build_depend>
-  <build_depend>roscpp</build_depend>
+  <build_depend>message_generation</build_depend>
 
   <run_depend>rospy</run_depend>
-  <run_depend>roscpp</run_depend>
   <run_depend>tf</run_depend>
   <run_depend>sensor_msgs</run_depend>
+  <run_depend>std_msgs</run_depend>
   <run_depend>dynamic_reconfigure</run_depend>
+  <run_depend>message_runtime</run_depend>
 </package>
 

rasp_imu_hat_6dof/src/iic_dev.cpp

@@ -1,233 +0,0 @@
-#include "iic_dev.h"
-#include <stdio.h>
-
-I2Cdev::I2Cdev() { }
-
-void I2Cdev::initialize() {
-  bcm2835_init();
-  bcm2835_i2c_set_baudrate( i2c_baudrate  );
-}
-
-/** Enable or disable I2C,
- * @param isEnabled true = enable, false = disable
- */
-void I2Cdev::enable(bool isEnabled) {
-  if ( set_I2C_pins ){
-    if (isEnabled)
-      bcm2835_i2c_end();
-    else
-      bcm2835_i2c_begin() ;
-  }
-}
-
-char sendBuf[256];
-char recvBuf[256];
-
-
-
-/** Read a single bit from an 8-bit device register.
- * @param devAddr I2C slave device address
- * @param regAddr Register regAddr to read from
- * @param bitNum Bit position to read (0-7)
- * @param data Container for single bit value
- * @return Status of read operation (true = success)
- */
-int8_t I2Cdev::readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data) {
-  bcm2835_i2c_setSlaveAddress(devAddr);
-  sendBuf[0] = regAddr;
-  uint8_t response = bcm2835_i2c_write_read_rs(sendBuf, 1, recvBuf, 1);
-  *data = recvBuf[1] & (1 << bitNum);
-  return response == BCM2835_I2C_REASON_OK ;
-}
-
-/** Read multiple bits from an 8-bit device register.
- * @param devAddr I2C slave device address
- * @param regAddr Register regAddr to read from
- * @param bitStart First bit position to read (0-7)
- * @param length Number of bits to read (not more than 8)
- * @param data Container for right-aligned value (i.e. '101' read from any bitStart position will equal 0x05)
- * @return Status of read operation (true = success)
- */
-int8_t I2Cdev::readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data) {
-  // 01101001 read byte
-  // 76543210 bit numbers
-  //    xxx   args: bitStart=4, length=3
-  //    010   masked
-  //   -> 010 shifted
-  bcm2835_i2c_setSlaveAddress(devAddr);
-  sendBuf[0] = regAddr;
-  uint8_t response = bcm2835_i2c_write_read_rs(sendBuf, 1, recvBuf, 1);
-  uint8_t b = (uint8_t) recvBuf[0];
-  if (response == BCM2835_I2C_REASON_OK) {
-    uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1);
-    b &= mask;
-    b >>= (bitStart - length + 1);
-    *data = b;
-  }
-  return response == BCM2835_I2C_REASON_OK;
-}
-
-/** Read single byte from an 8-bit device register.
- * @param devAddr I2C slave device address
- * @param regAddr Register regAddr to read from
- * @param data Container for byte value read from device
- * @return Status of read operation (true = success)
- */
-int8_t I2Cdev::readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data) {
-  bcm2835_i2c_setSlaveAddress(devAddr);
-  sendBuf[0] = regAddr;
-  uint8_t response = bcm2835_i2c_write_read_rs(sendBuf, 1, recvBuf, 1);
-  data[0] = (uint8_t) recvBuf[0];
-  return response == BCM2835_I2C_REASON_OK;
-}
-
-/** Read multiple bytes from an 8-bit device register.
- * @param devAddr I2C slave device address
- * @param regAddr First register regAddr to read from
- * @param length Number of bytes to read
- * @param data Buffer to store read data in
- * @return I2C_TransferReturn_TypeDef http://downloads.energymicro.com/documentation/doxygen/group__I2C.html
- */
-int8_t I2Cdev::readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data) {
-  bcm2835_i2c_setSlaveAddress(devAddr);
-  sendBuf[0] = regAddr;
-  uint8_t response = bcm2835_i2c_write_read_rs(sendBuf, 1, recvBuf, length);
-  int i ;
-  for (i = 0; i < length ; i++) {
-    data[i] = (uint8_t) recvBuf[i];
-  }
-  return response == BCM2835_I2C_REASON_OK;
-}
-
-/** write a single bit in an 8-bit device register.
- * @param devAddr I2C slave device address
- * @param regAddr Register regAddr to write to
- * @param bitNum Bit position to write (0-7)
- * @param value New bit value to write
- * @return Status of operation (true = success)
- */
-bool I2Cdev::writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data) {
-  bcm2835_i2c_setSlaveAddress(devAddr);
-  //first reading registery value
-  sendBuf[0] = regAddr;
-  uint8_t response = bcm2835_i2c_write_read_rs(sendBuf, 1, recvBuf, 1 );
-  if ( response == BCM2835_I2C_REASON_OK ) {
-    uint8_t b = recvBuf[0] ;
-    b = (data != 0) ? (b | (1 << bitNum)) : (b & ~(1 << bitNum));
-    sendBuf[1] = b ;
-    response = bcm2835_i2c_write(sendBuf, 2);
-  }
-  return response == BCM2835_I2C_REASON_OK;
-}
-
-/** Write multiple bits in an 8-bit device register.
- * @param devAddr I2C slave device address
- * @param regAddr Register regAddr to write to
- * @param bitStart First bit position to write (0-7)
- * @param length Number of bits to write (not more than 8)
- * @param data Right-aligned value to write
- * @return Status of operation (true = success)
- */
-bool I2Cdev::writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data) {
-  //      010 value to write
-  // 76543210 bit numbers
-  //    xxx   args: bitStart=4, length=3
-  // 00011100 mask byte
-  // 10101111 original value (sample)
-  // 10100011 original & ~mask
-  // 10101011 masked | value
-  bcm2835_i2c_setSlaveAddress(devAddr);
-  //first reading registery value
-  sendBuf[0] = regAddr;
-  uint8_t response = bcm2835_i2c_write_read_rs(sendBuf, 1, recvBuf, 1 );
-  if ( response == BCM2835_I2C_REASON_OK ) {
-    uint8_t b = recvBuf[0];
-    uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1);
-    data <<= (bitStart - length + 1); // shift data into correct position
-    data &= mask; // zero all non-important bits in data
-    b &= ~(mask); // zero all important bits in existing byte
-    b |= data; // combine data with existing byte
-    sendBuf[1] = b ;
-    response = bcm2835_i2c_write(sendBuf, 2);
-    }
-  return response == BCM2835_I2C_REASON_OK;
-}
-
-/** Write single byte to an 8-bit device register.
- * @param devAddr I2C slave device address
- * @param regAddr Register address to write to
- * @param data New byte value to write
- * @return Status of operation (true = success)
- */
-bool I2Cdev::writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data) {
-  bcm2835_i2c_setSlaveAddress(devAddr);
-  sendBuf[0] = regAddr;
-  sendBuf[1] = data;
-  uint8_t response = bcm2835_i2c_write(sendBuf, 2);
-  return response == BCM2835_I2C_REASON_OK ;
-}
-
-/** Read single word from a 16-bit device register.
- * @param devAddr I2C slave device address
- * @param regAddr Register regAddr to read from
- * @param data Container for word value read from device
- * @return Status of read operation (true = success)
- */
-int8_t I2Cdev::readWord(uint8_t devAddr, uint8_t regAddr, uint16_t *data) {
-  bcm2835_i2c_setSlaveAddress(devAddr);
-  sendBuf[0] = regAddr;
-  uint8_t response = bcm2835_i2c_write_read_rs(sendBuf, 1, recvBuf, 2 );
-  data[0] = (recvBuf[0] << 8) | recvBuf[1] ;
-  return  response == BCM2835_I2C_REASON_OK ;
-}
-
-/** Read multiple words from a 16-bit device register.
- * @param devAddr I2C slave device address
- * @param regAddr First register regAddr to read from
- * @param length Number of words to read
- * @param data Buffer to store read data in
- * @return Number of words read (-1 indicates failure)
- */
-int8_t I2Cdev::readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data) {
-  bcm2835_i2c_setSlaveAddress(devAddr);
-  sendBuf[0] = regAddr;
-  uint8_t response = bcm2835_i2c_write_read_rs(sendBuf, 1, recvBuf, length*2 );
-  uint8_t i;
-  for (i = 0; i < length; i++) {
-    data[i] = (recvBuf[i*2] << 8) | recvBuf[i*2+1] ;
-  }
-  return  response == BCM2835_I2C_REASON_OK ;
-}
-
-bool I2Cdev::writeWord(uint8_t devAddr, uint8_t regAddr, uint16_t data){
-  bcm2835_i2c_setSlaveAddress(devAddr);
-  sendBuf[0] = regAddr;
-  sendBuf[1] = (uint8_t) (data >> 8); //MSByte
-  sendBuf[2] = (uint8_t) (data >> 0); //LSByte
-  uint8_t response = bcm2835_i2c_write(sendBuf, 3);
-  return response == BCM2835_I2C_REASON_OK ;
-}
-
-bool I2Cdev::writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data){
-  bcm2835_i2c_setSlaveAddress(devAddr);
-  sendBuf[0] = regAddr;
-  uint8_t i;
-  for (i = 0; i < length; i++) {
-    sendBuf[i+1] = data[i] ;
-  }
-  uint8_t response = bcm2835_i2c_write(sendBuf, 1+length);
-  return response == BCM2835_I2C_REASON_OK ;
-}
-
-bool I2Cdev::writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data){
-  bcm2835_i2c_setSlaveAddress(devAddr);
-  sendBuf[0] = regAddr;
-  uint8_t i;
-  for (i = 0; i < length; i++) {
-    sendBuf[1+2*i] = (uint8_t) (data[i] >> 8); //MSByte
-    sendBuf[2+2*i] = (uint8_t) (data[i] >> 0); //LSByte
-  }
-  uint8_t response = bcm2835_i2c_write(sendBuf, 1+2*length);
-  return response == BCM2835_I2C_REASON_OK ;
-}
-

rasp_imu_hat_6dof/src/imu_data.cpp

@@ -1,243 +0,0 @@
-#include "imu_data.h"
-#include "iic_dev.h"
-#include <ros/ros.h>
-#include <unistd.h>
-#include <fcntl.h>
-#include <stdio.h>
-#include <linux/i2c-dev.h>
-#include <sys/ioctl.h>
-#include <bcm2835.h>
-#include <math.h>
-
-int myIMU::initDev()
-{
-//I2Cdev::initialize();
-}
-
-float myIMU::getAccXData()
-{
-    float data = 0;
-#if 0
-    int len = 0;
-    len = I2Cdev::readBytes(I2C_DEV_ADDR, ACC_X_REG, 2, accX);
-    if(len > 0)
-    {
-        data = (((short)accX[1]<<8)|accX[0])/32768.0*16.0;
-    }
-    else
-    {
-        ROS_ERROR_STREAM("Read accX data error !");
-    }
-#endif
-    return data;
-}
-
-float myIMU::getAccYData()
-{
-    float data = 0;
-    int fd = 0;
-    fd = open("/dev/i2c-1", O_RDWR);
-    if(fd < 0)
-    {
-        ROS_ERROR_STREAM("Open IIC device error !");
-        return -1;
-    }
-    if(ioctl(fd, I2C_SLAVE, I2C_DEV_ADDR) < 0)
-    {
-        ROS_ERROR_STREAM("IIC device ioctl error !");
-    }
-    if(read(fd, accY, 2))
-    {
-        data = (((short)accY[1]<<8)|accY[0])/32768.0*16.0;
-    }
-    else
-    {
-        ROS_ERROR_STREAM("Read accY data error !");
-    }
-    close(fd);
-    return data;
-}
-
-float myIMU::getAccZData()
-{
-    float data = 0;
-    int fd = 0;
-    fd = open("/dev/i2c-1", O_RDWR);
-    if(fd < 0)
-    {
-        ROS_ERROR_STREAM("Open IIC device error !");
-        return -1;
-    }
-    if(ioctl(fd, I2C_SLAVE, I2C_DEV_ADDR) < 0)
-    {
-        ROS_ERROR_STREAM("IIC device ioctl error !");
-    }
-    if(read(fd, accZ, 2))
-    {
-        data = (((short)accZ[1]<<8)|accZ[0])/32768.0*16.0;
-    }
-    else
-    {
-        ROS_ERROR_STREAM("Read accZ data error !");
-    }
-    close(fd);
-
-    return data;
-}
-
-float myIMU::getGyoXData()
-{
-    float data = 0;
-    int fd = 0;
-    fd = open("/dev/i2c-1", O_RDWR);
-    if(fd < 0)
-    {
-        ROS_ERROR_STREAM("Open IIC device error !");
-        return -1;
-    }
-    if(ioctl(fd, I2C_SLAVE, I2C_DEV_ADDR) < 0)
-    {
-        ROS_ERROR_STREAM("IIC device ioctl error !");
-    }
-    if(read(fd, gyoX, 2))
-    {
-        data = (((short)gyoX[1]<<8)|gyoX[0])/32768.0*2000.0;
-    }
-    {
-        ROS_ERROR_STREAM("Read gyoX data error !");
-    }
-    close(fd);
-
-    return data;
-}
-
-float myIMU::getGyoYData()
-{
-    float data = 0;
-    int fd = 0;
-    fd = open("/dev/i2c-1", O_RDWR);
-    if(fd < 0)
-    {
-        ROS_ERROR_STREAM("Open IIC device error !");
-        return -1;
-    }
-    if(ioctl(fd, I2C_SLAVE, I2C_DEV_ADDR) < 0)
-    {
-        ROS_ERROR_STREAM("IIC device ioctl error !");
-    }
-    if(read(fd, gyoY, 2))
-    {
-        data = (((short)gyoY[1]<<8)|gyoY[0])/32768.0*2000.0;
-    }
-    {
-        ROS_ERROR_STREAM("Read gyoY data error !");
-    }
-    close(fd);
-
-    return data;
-}
-
-float myIMU::getGyoZData()
-{
-    float data = 0;
-    int fd = 0;
-    fd = open("/dev/i2c-1", O_RDWR);
-    if(fd < 0)
-    {
-        ROS_ERROR_STREAM("Open IIC device error !");
-        return -1;
-    }
-    if(ioctl(fd, I2C_SLAVE, I2C_DEV_ADDR) < 0)
-    {
-        ROS_ERROR_STREAM("IIC device ioctl error !");
-    }
-    if(read(fd, gyoZ, 2))
-    {
-        data = (((short)gyoZ[1]<<8)|gyoZ[0])/32768.0*2000.0;
-    }
-    {
-        ROS_ERROR_STREAM("Read gyoZ register data error !");
-    }
-    close(fd);
-
-    return data;
-}
-
-float myIMU::getRollData()
-{
-    float data = 0;
-    int fd = 0;
-    fd = open("/dev/i2c-1", O_RDWR);
-    if(fd < 0)
-    {
-        ROS_ERROR_STREAM("Open IIC device error !");
-        return -1;
-    }
-    if(ioctl(fd, I2C_SLAVE, I2C_DEV_ADDR) < 0)
-    {
-        ROS_ERROR_STREAM("IIC device ioctl error !");
-    }
-    if(read(fd, &roll, 2))
-    {
-        data = (((short)roll[1]<<8)|roll[0])/32768.0*180.0;
-    }
-    {
-        ROS_ERROR_STREAM("Read roll register data error !");
-    }
-    close(fd);
-
-    return data;
-}
-
-float myIMU::getPitchData()
-{
-    float data = 0;
-    int fd = 0;
-    fd = open("/dev/i2c-1", O_RDWR);
-    if(fd < 0)
-    {
-        ROS_ERROR_STREAM("Open IIC device error !");
-        return -1;
-    }
-    if(ioctl(fd, I2C_SLAVE, I2C_DEV_ADDR) < 0)
-    {
-        ROS_ERROR_STREAM("IIC device ioctl error !");
-    }
-    if(read(fd, &pitch, 2))
-    {
-        data = (((short)pitch[1]<<8)|pitch[0])/32768.0*180.0;
-    }
-    {
-        ROS_ERROR_STREAM("Read pitch register data error !");
-    }
-    close(fd);
-
-    return data;
-}
-
-float myIMU::getYawData()
-{
-    float data = 0;
-    int fd = 0;
-    fd = open("/dev/i2c-1", O_RDWR);
-    if(fd < 0)
-    {
-        ROS_ERROR_STREAM("Open IIC device error !");
-        return -1;
-    }
-    if(ioctl(fd, I2C_SLAVE, I2C_DEV_ADDR) < 0)
-    {
-        ROS_ERROR_STREAM("IIC device ioctl error !");
-    }
-    if(read(fd, &yaw, 2))
-    {
-        data = (((short)yaw[1]<<8)|yaw[0])/32768.0*180.0;
-    }
-    {
-        ROS_ERROR_STREAM("Read yaw register data error !");
-    }
-    close(fd);
-
-    return data;
-}
-

rasp_imu_hat_6dof/src/imu_node.cpp

@@ -1,81 +0,0 @@
-#include <ros/ros.h>
-#include "imu_data.h"
-#include <Eigen/Eigen>
-#include <Eigen/Geometry>
-#include <Eigen/Core>
-#include <sensor_msgs/Imu.h>
-
-using namespace Eigen;
-using namespace std;
-
-Eigen::Quaterniond euler2Quater(float roll, float yaw, float pitch)
-{
-    Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitZ());
-    Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitY());
-    Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitX());
-
-    Eigen::Quaterniond q = rollAngle * yawAngle * pitchAngle;
-    cout << "Euler2Quaternion result is:" <<endl;
-    cout << "x = " << q.x() <<endl;
-    cout << "y = " << q.y() <<endl;
-    cout << "z = " << q.z() <<endl;
-    cout << "w = " << q.w() <<endl<<endl;
-
-    return q;
-}
-
-int main(int argc, char **argv)
-{
-    ros::init(argc, argv, "imu_data_pub_node");
-    ros::NodeHandle handle;
-    myIMU my_imu;
-    my_imu.initDev();
-
-    ros::Publisher imu_pub = handle.advertise<sensor_msgs::Imu>("imu_data", 2);
-    ros::Rate loop_rate(10);
-    sensor_msgs::Imu imu_msg;
-    float yaw_tmp;
-    float pitch_tmp;
-    float roll_tmp;
-
-    while(ros::ok())
-    {
-        imu_msg.header.stamp = ros::Time::now();
-        imu_msg.header.frame_id = "base_link";
-
-        roll_tmp  = my_imu.getRollData();
-        pitch_tmp = my_imu.getPitchData();
-        yaw_tmp   = my_imu.getYawData();
-
-        Eigen::Quaterniond q = euler2Quater(roll_tmp, pitch_tmp, yaw_tmp);
-        imu_msg.orientation.x = q.x();
-        imu_msg.orientation.y = q.y();
-        imu_msg.orientation.z = q.z();
-        imu_msg.orientation.w = q.w();
-        imu_msg.orientation_covariance = {0.0025, 0, 0,
-                                          0, 0.0025, 0,
-                                          0, 0, 0.0025};
-
-        imu_msg.angular_velocity.x = my_imu.getGyoXData();
-        imu_msg.angular_velocity.y = my_imu.getGyoYData();
-        imu_msg.angular_velocity.z = my_imu.getGyoZData();
-        imu_msg.angular_velocity_covariance = {0.02, 0, 0,
-                                               0, 0.02, 0,
-                                               0, 0, 0.02};
-
-        imu_msg.linear_acceleration.x = my_imu.getAccXData();
-        imu_msg.linear_acceleration.y = my_imu.getAccYData();
-        imu_msg.linear_acceleration.z = my_imu.getAccZData();
-        imu_msg.linear_acceleration_covariance = {0.04, 0, 0,
-                                                   0, 0.04, 0,
-                                                   0, 0, 0.04};
-
-        imu_pub.publish(imu_msg);
-
-        ros::spinOnce();
-        loop_rate.sleep();
-    }
-
-    return 0;
-}
-

rasp_imu_hat_6dof/srv/GetYawData.srv

@@ -0,0 +1,2 @@
+---
+float32 yaw

serial_imu_hat_6dof/CMakeLists.txt

@@ -0,0 +1,206 @@
+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)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  dynamic_reconfigure
+  roscpp
+  sensor_msgs
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   sensor_msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES serial_imu_hat_6dof
+#  CATKIN_DEPENDS dynamic_reconfigure roscpp sensor_msgs
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/serial_imu_hat_6dof.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+#add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+add_executable(serial_imu_node src/proc_serial_data.cpp src/serial_imu_node.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+target_link_libraries(serial_imu_node
+  ${catkin_LIBRARIES}
+)
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
+# install(TARGETS ${PROJECT_NAME}_node
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark libraries for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+# install(TARGETS ${PROJECT_NAME}
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_serial_imu_hat_6dof.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

serial_imu_hat_6dof/cfg/param.yaml

@@ -0,0 +1,32 @@
+################################################
+# Copyright: 2016-2020 www.corvin.cn ROS小课堂
+# Description:使用串口进行通信时的配置信息.
+#   imu_dev:指示串口设备挂在点,如果使用usb转串口
+#     模块,都是写ttyUSB0,1,2等.如果使用树莓派的
+#     GPIO排针处串口,则使用ttyS0.
+#   baud_rate:串口通信波特率,旧模块的波特率为9600,
+#     新版本的波特率为57600,如果串口没数据可以修改.
+#   data_bits:数据位
+#   parity:数据校验位
+#   stop_bits:停止位
+#   pub_data_topic:发布imu数据的话题名.
+#   pub_temp_topic:发布imu模块温度的话题名.
+#   link_name: imu模块的link名.
+#   pub_hz:话题发布的数据频率.
+# Author: corvin
+# History:
+#   20200402: init this file.
+#   20200406: 增加直接发布yaw数据的话题.
+################################################
+imu_dev: /dev/ttyS0
+baud_rate: 57600
+data_bits: 8
+parity: N
+stop_bits: 1
+
+pub_data_topic: imu_data
+pub_temp_topic: imu_temp
+yaw_topic: yaw_data
+link_name: base_imu_link
+pub_hz: 10
+

serial_imu_hat_6dof/include/imu_data.h

@@ -0,0 +1,22 @@
+#ifndef _IMU_DATA_H_
+#define _IMU_DATA_H_
+
+int initSerialPort(const char* path, const int baud, const int dataBits,
+        const char* parity, const int stopBit);
+
+int getImuData();
+int closeSerialPort();
+
+float getAccX();
+float getAccY();
+float getAccZ();
+
+float getAngularX();
+float getAngularY();
+float getAngularZ();
+
+float getAngleX();
+float getAngleY();
+float getAngleZ();
+
+#endif

serial_imu_hat_6dof/launch/serial_imu_hat.launch

@@ -0,0 +1,14 @@
+<!--
+  Copyright: 2016-2020 https://www.corvin.cn ROS小课堂
+  Description:使用串口来读取imu模块的数据,并在ros中发布使用话题发布出来.
+  Author: corvin
+  History:
+    20200401: init this file.
+-->
+<launch>
+  <arg name="imu_cfg_file" default="$(find serial_imu_hat_6dof)/cfg/param.yaml" />
+
+  <node pkg="serial_imu_hat_6dof" type="serial_imu_node" name="serial_imu_node" output="screen">
+    <rosparam file="$(arg imu_cfg_file)" command="load" />
+  </node>
+</launch>

serial_imu_hat_6dof/package.xml

@@ -0,0 +1,68 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>serial_imu_hat_6dof</name>
+  <version>0.0.0</version>
+  <description>The serial_imu_hat_6dof package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="corvin_zhang@corvin.cn">corvin</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/serial_imu_hat_6dof</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>dynamic_reconfigure</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_export_depend>dynamic_reconfigure</build_export_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>sensor_msgs</build_export_depend>
+  <exec_depend>dynamic_reconfigure</exec_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

serial_imu_hat_6dof/src/proc_serial_data.cpp

@@ -0,0 +1,327 @@
+/*****************************************************************
+ * Copyright:2016-2020 www.corvin.cn ROS小课堂
+ * Description:使用串口方式读取IMU模块信息.
+ * Author: corvin
+ * History:
+ *   20200401:init this file.
+ *   20200702:将读取到的各数据的高低自己合成后要转换为short类型,
+ *     这样才能进行/32768.0*16.0运算.
+******************************************************************/
+#include<ros/ros.h>
+#include<stdio.h>
+#include<stdlib.h>
+#include<fcntl.h>
+#include<unistd.h>
+#include<termios.h>
+#include<string.h>
+#include<sys/time.h>
+#include<sys/types.h>
+
+
+char r_buf[512];
+int port_fd = 0;
+float acce[3],angular[3],angle[3],quater[4];
+
+/**********************************************
+ * Description:打开串口,输入各参数即可.
+ *********************************************/
+int openSerialPort(const char *path, int baud, int dataBits,
+                const char* parity, int stopBit)
+{
+    int fd = 0;
+    struct termios terminfo;
+    bzero(&terminfo, sizeof(terminfo));
+
+    fd = open(path, O_RDWR|O_NOCTTY);
+    if (-1 == fd)
+    {
+        ROS_ERROR("Open imu dev error:%s", path);
+        return -1;
+    }
+
+    if(isatty(STDIN_FILENO) == 0)
+    {
+        ROS_ERROR("IMU dev isatty error !");
+        return -2;
+    }
+
+    /*设置串口通信波特率-bps*/
+    switch(baud)
+    {
+        case 9600:
+           cfsetispeed(&terminfo, B9600); //设置输入速度
+           cfsetospeed(&terminfo, B9600); //设置输出速度
+           break;
+
+        case 19200:
+           cfsetispeed(&terminfo, B19200);
+           cfsetospeed(&terminfo, B19200);
+           break;
+
+        case 38400:
+           cfsetispeed(&terminfo, B38400);
+           cfsetospeed(&terminfo, B38400);
+           break;
+
+        case 57600:
+           cfsetispeed(&terminfo, B57600);
+           cfsetospeed(&terminfo, B57600);
+           break;
+
+       case 115200:
+           cfsetispeed(&terminfo, B115200);
+           cfsetospeed(&terminfo, B115200);
+           break;
+
+       default://设置默认波特率9600
+           cfsetispeed(&terminfo, B9600);
+           cfsetospeed(&terminfo, B9600);
+           break;
+    }
+
+    //设置数据位
+    terminfo.c_cflag |= CLOCAL|CREAD;
+    terminfo.c_cflag &= ~CSIZE;
+    switch(dataBits)
+    {
+        case 7:
+           terminfo.c_cflag |= CS7;
+           break;
+
+        case 8:
+           terminfo.c_cflag |= CS8;
+           break;
+
+        default:
+           ROS_ERROR("set dataBit error !");
+           return -3;
+    }
+
+    //设置奇偶校验位
+    switch(*parity)
+    {
+        case 'o': //设置奇校验
+        case 'O':
+           terminfo.c_cflag |= PARENB;
+           terminfo.c_cflag |= PARODD;
+           terminfo.c_iflag |= (INPCK|ISTRIP);
+           break;
+
+        case 'e': //设置偶校验
+        case 'E':
+           terminfo.c_iflag |= (INPCK|ISTRIP);
+           terminfo.c_cflag |= PARENB;
+           terminfo.c_cflag &= ~PARODD;
+           break;
+
+        case 'n': //不设置奇偶校验位
+        case 'N':
+           terminfo.c_cflag &= ~PARENB;
+           break;
+
+        default:
+           ROS_ERROR("set parity error !");
+          return -4;
+    }
+
+    //设置停止位
+    switch(stopBit)
+    {
+        case 1:
+            terminfo.c_cflag &= ~CSTOPB;
+            break;
+
+        case 2:
+            terminfo.c_cflag |= CSTOPB;
+            break;
+
+        default:
+            ROS_ERROR("set stopBit error !");
+            return -5;
+    }
+
+    terminfo.c_cc[VTIME] = 10;
+    terminfo.c_cc[VMIN]  = 0;
+    tcflush(fd, TCIFLUSH);
+
+    if((tcsetattr(fd, TCSANOW, &terminfo)) != 0)
+    {
+        ROS_ERROR("set imu serial port attr error !");
+        return -6;
+    }
+
+    return fd;
+}
+
+/**************************************
+ * Description:关闭串口文件描述符.
+ *************************************/
+int closeSerialPort()
+{
+    int ret = close(port_fd);
+    return ret;
+}
+
+/*****************************************************
+ * Description:向串口中发送数据.
+ ****************************************************/
+int send_data(int fd, char *send_buffer, int length)
+{
+	length = write(fd, send_buffer, length*sizeof(unsigned char));
+
+	return length;
+}
+
+/*******************************************************
+ * Description:从串口中接收数据.
+ *******************************************************/
+int recv_data(int fd, char* recv_buffer, int len)
+{
+    int length = read(fd, recv_buffer, len);
+    return length;
+}
+
+/************************************************************
+ * Description:根据串口数据协议来解析有效数据,
+ ***********************************************************/
+void parse_serialData(char chr)
+{
+    static unsigned char chrBuf[100];
+    static unsigned char chrCnt = 0;
+    signed short sData[4]; //save 8 Byte valid info
+    unsigned char i = 0;
+    char frameSum = 0;
+
+    chrBuf[chrCnt++] = chr; //保存当前字节,字节计数加1
+
+    //判断是否满一个完整数据帧11个字节
+    if(chrCnt < 11)
+        return;
+
+    //计算数据帧的前十个字节的校验和
+    for(i=0; i<10; i++)
+    {
+        frameSum += chrBuf[i];
+    }
+
+    //找到数据帧第一个字节是0x55,校验和是否正确,若两者有一个不正确,
+    //都需要移动到最开始的字节,再读取新的字节进行判断数据帧完整性
+    if ((chrBuf[0] != 0x55)||(frameSum != chrBuf[10]))
+    {
+        memcpy(&chrBuf[0], &chrBuf[1], 10); //将有效数据往前移动1字节位置
+        chrCnt--; //字节计数减1,需要再多读取一个字节进来,重新判断数据帧
+        return;
+    }
+
+    //for(i=0;i<11; i++)
+    //  printf("0x%x ",chrBuf[i]);
+    //printf("\n");
+
+    memcpy(&sData[0], &chrBuf[2], 8);
+    switch(chrBuf[1])
+    {
+        case 0x51: //x,y,z轴 加速度输出
+            acce[0] = ((short)(((short)chrBuf[3]<<8)|chrBuf[2]))/32768.0*16.0;
+            acce[1] = ((short)(((short)chrBuf[5]<<8)|chrBuf[4]))/32768.0*16.0;
+            acce[2] = ((short)(((short)chrBuf[7]<<8)|chrBuf[6]))/32768.0*16.0;
+            break;
+
+        case 0x52: //角速度输出
+            angular[0] = ((short)(((short)chrBuf[3]<<8)|chrBuf[2]))/32768.0*2000.0;
+            angular[1] = ((short)(((short)chrBuf[5]<<8)|chrBuf[4]))/32768.0*2000.0;
+            angular[2] = ((short)(((short)chrBuf[7]<<8)|chrBuf[6]))/32768.0*2000.0;
+            break;
+
+        case 0x53: //欧拉角度输出, roll, pitch, yaw
+            angle[0] = ((short)(((short)chrBuf[3]<<8)|chrBuf[2]))/32768.0*180.0;
+            angle[1] = ((short)(((short)chrBuf[5]<<8)|chrBuf[4]))/32768.0*180.0;
+            angle[2] = ((short)(((short)chrBuf[7]<<8)|chrBuf[6]))/32768.0*180.0;
+            break;
+
+        case 0x59: //四元素输出
+            quater[0] = ((short)(((short)chrBuf[3]<<8)|chrBuf[2]))/32768.0;
+            quater[1] = ((short)(((short)chrBuf[5]<<8)|chrBuf[4]))/32768.0;
+            quater[2] = ((short)(((short)chrBuf[7]<<8)|chrBuf[6]))/32768.0;
+            quater[3] = ((short)(((short)chrBuf[9]<<8)|chrBuf[8]))/32768.0;
+            break;
+    }
+    chrCnt = 0;
+}
+
+/*****************************************************************
+ * Description:根据串口配置信息来配置打开串口,准备进行数据通信
+ ****************************************************************/
+int initSerialPort(const char* path, const int baud, const int dataBits,
+          const char* parity, const int stopBit)
+{
+    bzero(r_buf, 512);
+    port_fd = openSerialPort(path, baud, dataBits, parity, stopBit);
+    if(port_fd < 0)
+    {
+        ROS_ERROR("openSerialPort error !");
+        return -1;
+    }
+
+    return 0;
+}
+
+float getAccX()
+{
+    return acce[0];
+}
+float getAccY()
+{
+    return acce[1];
+}
+float getAccZ()
+{
+    return acce[2];
+}
+
+float getAngularX()
+{
+    return angular[0];
+}
+
+float getAngularY()
+{
+    return angular[1];
+}
+
+float getAngularZ()
+{
+    return angular[2];
+}
+
+float getAngleX()
+{
+    return angle[0];
+}
+float getAngleY()
+{
+    return angle[1];
+}
+float getAngleZ()
+{
+    return angle[2];
+}
+
+int getImuData()
+{
+    int ret = 0;
+    ret = recv_data(port_fd, r_buf,  44);
+    if(ret == -1)
+    {
+        ROS_ERROR("read serial port data failed !\n");
+        closeSerialPort();
+
+        return -1;
+    }
+
+    for (int i=0; i<ret; i++)
+    {
+        parse_serialData(r_buf[i]);
+    }
+
+    return 0;
+}

serial_imu_hat_6dof/src/serial_imu_node.cpp

@@ -0,0 +1,113 @@
+/****************************************************
+ * Copyright:2016-2020 www.corvin.cn ROS小课堂
+ * Description:使用串口来读取IMU模块的数据,并通过
+ *   topic将数据发布出来.芯片中默认读取出来的加速度数据
+ *   单位是g,需要将其转换为ROS中加速度规定的m/s2才能发布.
+ * Author: corvin
+ * History:
+ *   20200403: init this file.
+ *   20200406:增加发布yaw数据的话题.
+ *   20200703:修改三轴加速度数据正负,按照ROS规定沿着各轴
+ *     正方向时数据为正,否则为负.单位m/s2。
+*****************************************************/
+#include <ros/ros.h>
+#include <tf/tf.h>
+#include "imu_data.h"
+#include <sensor_msgs/Imu.h>
+#include <std_msgs/Float32.h>
+
+
+int main(int argc, char **argv)
+{
+    float yaw, pitch, roll;
+    std::string imu_dev;
+    int baud = 0;
+    int dataBit = 0;
+    std::string parity;
+    int stopBit = 0;
+
+    std::string link_name;
+    std::string imu_topic;
+    std::string temp_topic;
+    std::string yaw_topic;
+    int pub_hz = 0;
+    float degree2Rad = 3.1415926/180.0;
+    float acc_factor = 9.806;
+
+    ros::init(argc, argv, "imu_data_pub_node");
+    ros::NodeHandle handle;
+
+    ros::param::get("~imu_dev", imu_dev);
+    ros::param::get("~baud_rate", baud);
+    ros::param::get("~data_bits", dataBit);
+    ros::param::get("~parity", parity);
+    ros::param::get("~stop_bits", stopBit);
+
+    ros::param::get("~link_name", link_name);
+    ros::param::get("~pub_data_topic", imu_topic);
+    ros::param::get("~pub_temp_topic", temp_topic);
+    ros::param::get("~yaw_topic", yaw_topic);
+    ros::param::get("~pub_hz", pub_hz);
+
+    int ret = initSerialPort(imu_dev.c_str(), baud, dataBit, parity.c_str(), stopBit);
+    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, 1);
+    ros::Publisher yaw_pub = handle.advertise<std_msgs::Float32>(yaw_topic, 1);
+    ros::Rate loop_rate(pub_hz);
+
+    sensor_msgs::Imu imu_msg;
+    imu_msg.header.frame_id = link_name;
+    std_msgs::Float32 yaw_msg;
+    while(ros::ok())
+    {
+        if(getImuData() < 0)
+            break;
+
+        imu_msg.header.stamp = ros::Time::now();
+        roll  = getAngleX()*degree2Rad;
+        pitch = getAngleY()*degree2Rad;
+        yaw   = getAngleZ()*degree2Rad;
+        if(yaw >= 3.1415926)
+            yaw -= 6.2831852;
+
+        //publish yaw data
+        yaw_msg.data = yaw;
+        yaw_pub.publish(yaw_msg);
+
+        //ROS_INFO("yaw:%f pitch:%f roll:%f",yaw, pitch, roll);
+        imu_msg.orientation = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw);
+        imu_msg.orientation_covariance = {0.0025, 0, 0,
+                                          0, 0.0025, 0,
+                                          0, 0, 0.0025};
+
+        imu_msg.angular_velocity.x = getAngularX()*degree2Rad;
+        imu_msg.angular_velocity.y = getAngularY()*degree2Rad;
+        imu_msg.angular_velocity.z = getAngularZ()*degree2Rad;
+        imu_msg.angular_velocity_covariance = {0.02, 0, 0,
+                                               0, 0.02, 0,
+                                               0, 0, 0.02};
+
+        //ROS_INFO("acc_x:%f acc_y:%f acc_z:%f",getAccX(), getAccY(), getAccZ());
+        imu_msg.linear_acceleration.x = -getAccX()*acc_factor;
+        imu_msg.linear_acceleration.y = -getAccY()*acc_factor;
+        imu_msg.linear_acceleration.z = -getAccZ()*acc_factor;
+        imu_msg.linear_acceleration_covariance = {0.04, 0, 0,
+                                                   0, 0.04, 0,
+                                                   0, 0, 0.04};
+
+        imu_pub.publish(imu_msg);
+        ros::spinOnce();
+        loop_rate.sleep();
+    }
+
+    closeSerialPort();
+    return 0;
+}
+