更新iic获取imu代码,新增iic_bus,iic_addr可以配置的功能

iic_6dof_imu/cfg/param.yaml

@@ -1,18 +1,24 @@
 ###################################################
 # Copyright: 2016-2021 www.corvin.cn ROS小课堂
 # Description:使用IIC总线来读取IMU模块的数据,并
-#   通过话题将imu数据发送出来.这里是可以配置的
-#   一些参数.各参数意义如下:
+#   通过话题将imu数据发送出来.这里是可以配置的一些参数,
+#   各参数意义如下:
+#   iic_bus:当前使用的设备上哪一条iic总线,默认1.
+#   iic_addr:当前IMU板的IIC地址,默认0x50.
+#   topic_pub_hz:上述各话题发布的频率,默认100hz.
 #   pub_data_topic:发布imu数据的话题名.
 #   yaw_topic:发布当前yaw偏航角的话题,单位弧度.
 #   pitch_topic:发布俯仰角的话题名,单位弧度.
 #   roll_topic:发布横滚角的话题名,单位弧度.
 #   link_name:imu模块的urdf中link名字.
-#   pub_hz:上述各话题发布的频率,默认100hz.
+#   get_yaw_service:可以获取到yaw角度信息的service名称.
 # Author: corvin
 # History:
 #   20211122:init this file.
+#   20211202:新增iic_bus,iic_addr配置参数.
 ###################################################
+iic_bus: 1
+iic_addr: 0x50
 topic_pub_hz: 100
 pub_data_topic: imu_data
 yaw_topic: yaw_data

iic_6dof_imu/src/iic_6dof_imu_data.py

@@ -4,16 +4,21 @@
 # Copyright: 2016-2021 https://www.corvin.cn ROS小课堂
 # Author: corvin
 # Description: 从IIC接口中读取IMU模块的三轴加速度、三轴角速度、四元数。
+#    这里注意self.RPY_CONST表示的是180.0/32768.0,self.ACC_CONST
+#    表示16.0/32768.0，self.ANG_CONST表示2000.0/32768.0.
 # History:
-#    20211122: Initial this file.
-import smbus
-import rospy
+#    20211122:Initial this file.
+#    20211202:init函数增加iic_bus,iic_addr参数，同时优化get_YPRAG函数.
+import smbus,rospy
 import numpy as np
 
 class MyIMU(object):
-    def __init__(self, addr):
-        self.addr = addr
-        self.i2c = smbus.SMBus(1)
+    def __init__(self, iic_bus, iic_addr):
+        self.i2c  = smbus.SMBus(iic_bus)
+        self.addr = iic_addr
+        self.RPY_CONST = 0.00549316
+        self.ACC_CONST = 0.00048828 
+        self.ANG_CONST = 0.06103516
 
     def get_YPRAG(self):
         try:
@@ -23,17 +28,17 @@ class MyIMU(object):
         except IOError:
             rospy.logerr("Read IMU RPYAG date error !")
         else:
-            self.raw_roll  = float(np.short(np.short(rpy_data[1]<<8)|rpy_data[0])/32768.0*180.0)
-            self.raw_pitch = float(np.short(np.short(rpy_data[3]<<8)|rpy_data[2])/32768.0*180.0)
-            self.raw_yaw   = float(np.short(np.short(rpy_data[5]<<8)|rpy_data[4])/32768.0*180.0)
+            self.raw_roll  = float(np.short(np.short(rpy_data[1]<<8)|rpy_data[0])*self.RPY_CONST)
+            self.raw_pitch = float(np.short(np.short(rpy_data[3]<<8)|rpy_data[2])*self.RPY_CONST)
+            self.raw_yaw   = float(np.short(np.short(rpy_data[5]<<8)|rpy_data[4])*self.RPY_CONST)
 
-            self.raw_ax = float(np.short(np.short(axyz_data[1]<<8)|axyz_data[0])/32768.0*16.0)
-            self.raw_ay = float(np.short(np.short(axyz_data[3]<<8)|axyz_data[2])/32768.0*16.0)
-            self.raw_az = float(np.short(np.short(axyz_data[5]<<8)|axyz_data[4])/32768.0*16.0)
+            self.raw_ax = float(np.short(np.short(axyz_data[1]<<8)|axyz_data[0])*self.ACC_CONST)
+            self.raw_ay = float(np.short(np.short(axyz_data[3]<<8)|axyz_data[2])*self.ACC_CONST)
+            self.raw_az = float(np.short(np.short(axyz_data[5]<<8)|axyz_data[4])*self.ACC_CONST)
 
-            self.raw_gx = float(np.short(np.short(gxyz_data[1]<<8)|gxyz_data[0])/32768.0*2000.0)
-            self.raw_gy = float(np.short(np.short(gxyz_data[3]<<8)|gxyz_data[2])/32768.0*2000.0)
-            self.raw_gz = float(np.short(np.short(gxyz_data[5]<<8)|gxyz_data[4])/32768.0*2000.0)
+            self.raw_gx = float(np.short(np.short(gxyz_data[1]<<8)|gxyz_data[0])*self.ANG_CONST)
+            self.raw_gy = float(np.short(np.short(gxyz_data[3]<<8)|gxyz_data[2])*self.ANG_CONST)
+            self.raw_gz = float(np.short(np.short(gxyz_data[5]<<8)|gxyz_data[4])*self.ANG_CONST)
 
     def get_quatern(self):
         try:

iic_6dof_imu/src/iic_6dof_imu_node.py

@@ -8,6 +8,7 @@
 # Author: corvin
 # History:
 #    20211122:Initial this file.
+#    20211203:增加imu_iic_bus,imu_iic_addr配置参数.
 import rospy
 import math
 
@@ -26,6 +27,8 @@ def return_yaw_data(req):
 rospy.init_node("iic_6dof_imu_node")
 
 # Get DIY config param
+imu_iic_bus     = rospy.get_param('~iic_bus', 1)
+imu_iic_addr    = rospy.get_param('~iic_addr', '0x50')
 topic_pub_hz    = rospy.get_param('~topic_pub_hz', '100')
 data_topic_name = rospy.get_param('~pub_data_topic', 'imu_data')
 link_name       = rospy.get_param('~link_name', 'base_imu_link')
@@ -34,6 +37,7 @@ pitch_topic_name= rospy.get_param('~pitch_topic', 'pitch_topic')
 roll_topic_name = rospy.get_param('~roll_topic', 'roll_topic')
 get_yaw_srv     = rospy.get_param('~get_yaw_service', '/iic_imu_service/getYawData_service')
 
+
 #创建各话题发布者和自定义服务
 data_pub  = rospy.Publisher(data_topic_name,  Imu,     queue_size=1)
 yaw_pub   = rospy.Publisher(yaw_topic_name,   Float32, queue_size=1)
@@ -47,31 +51,31 @@ yawMsg   = Float32()
 pitchMsg = Float32()
 rollMsg  = Float32()
 
-# Orientation covariance estimation
+#Orientation covariance estimation
 imuMsg.orientation_covariance = [
-0.0025 , 0 , 0,
-0, 0.0025, 0,
-0, 0, 0.0025
+0, 0, 0,
+0, 0, 0,
+0, 0, 0
 ]
 
-# Angular velocity covariance estimation
+#Angular velocity covariance estimation
 imuMsg.angular_velocity_covariance = [
-0.02, 0 , 0,
-0 , 0.02, 0,
-0 , 0 , 0.02
+0, 0, 0,
+0, 0, 0,
+0, 0, 0
 ]
 
-# linear acceleration covariance estimation
+#linear acceleration covariance estimation
 imuMsg.linear_acceleration_covariance = [
-0.04 , 0 , 0,
-0 , 0.04, 0,
-0 , 0 , 0.04
+0, 0, 0,
+0, 0, 0,
+0, 0, 0
 ]
 
 seq = 0
 accel_factor = 9.806  #sensor accel g convert to m/s^2.
 
-myIMU = MyIMU(0x50)
+myIMU = MyIMU(imu_iic_bus, int(imu_iic_addr))
 rate = rospy.Rate(topic_pub_hz)
 
 rospy.loginfo("Now 6DOF IMU Module is working ...")
@@ -89,7 +93,7 @@ while not rospy.is_shutdown():
     pitch = float(myIMU.raw_pitch)*degrees2rad
     roll  = float(myIMU.raw_roll)*degrees2rad
 
-    #填充话题消息,然后通过话题发布
+    #填充话题消息内容,然后通过话题发布
     yawMsg.data   = yaw
     pitchMsg.data = pitch
     rollMsg.data  = roll
@@ -97,7 +101,6 @@ while not rospy.is_shutdown():
     pitch_pub.publish(pitchMsg)
     roll_pub.publish(rollMsg)
 
-    # Publish imu message
     #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

serial_6dof_imu/src/serial_imu_node.cpp

@@ -152,24 +152,24 @@ int main(int argc, char **argv)
         imu_msg.orientation.y = getQuat(2);
         imu_msg.orientation.z = getQuat(3);
         imu_msg.orientation.w = getQuat(0);
-        imu_msg.orientation_covariance = {0.0025, 0, 0,
-                                          0, 0.0025, 0,
-                                          0, 0, 0.0025};
-
+        imu_msg.orientation_covariance = {0, 0, 0,
+                                          0, 0, 0,
+                                          0, 0, 0};
+        //三轴角速度
         imu_msg.angular_velocity.x = getAngular(0)*degree2Rad;
         imu_msg.angular_velocity.y = getAngular(1)*degree2Rad;
         imu_msg.angular_velocity.z = getAngular(2)*degree2Rad;
-        imu_msg.angular_velocity_covariance = {0.02, 0, 0,
-                                               0, 0.02, 0,
-                                               0, 0, 0.02};
-
+        imu_msg.angular_velocity_covariance = {0, 0, 0,
+                                               0, 0, 0,
+                                               0, 0, 0};
+        //三轴线加速度
         //ROS_INFO("acc_x:%f acc_y:%f acc_z:%f",getAcc(0), getAcc(1), getAcc(2));
         imu_msg.linear_acceleration.x = -getAcc(0)*acc_factor;
         imu_msg.linear_acceleration.y = -getAcc(1)*acc_factor;
         imu_msg.linear_acceleration.z = -getAcc(2)*acc_factor;
-        imu_msg.linear_acceleration_covariance = {0.04, 0, 0,
-                                                  0, 0.04, 0,
-                                                  0, 0, 0.04};
+        imu_msg.linear_acceleration_covariance = {0, 0, 0,
+                                                  0, 0, 0,
+                                                  0, 0, 0};
 
         imu_pub.publish(imu_msg); //将imu数据通过话题发布出去
         ros::spinOnce();